1 概述

规划器服务器(Planner Server)负责处理规划器请求, 并托管插件实现的映射. 它会接收一个目标和一个要使用的规划器插件名称, 然后调用相应的插件来计算通往目标的路径. 它还托管全局成本图.

总得来说, 就是控制全局的导航路线.

2 参数解析

默认示例.

1
2
3
4
5
6
7
8
planner_server: # 负责调用规划插件
  ros__parameters:
    expected_planner_frequency: 20.0 # 规划器运行的频率
    costmap_update_timeout: 1.0 # 等待全局代价地图(global costmap)更新的超时时间
    introspection_mode: "disabled" # disabled(关闭) read_only(只读) full(完全记录)
    planner_plugins: ['GridBased'] # 定义要加载的规划器插件列表, GridBased和下面的插件对应, 名字自己配置就行.
    GridBased: # 具体插件
      plugin: 'nav2_navfn_planner::NavfnPlanner' # In Iron and older versions, "/" was used instead of "::"

下面列出一个全官方版本的插件列表.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    costmap_update_timeout: 1.0
    introspection_mode: "disabled"

    # 一次加载多个插件，运行时可切换
    planner_plugins: ["NavFn", "Smac2D", "SmacHybrid", "SmacLattice", "ThetaStar"]

    NavFn:
      plugin: "nav2_navfn_planner::NavfnPlanner"
      use_astar: true
      allow_unknown: true
      tolerance: 0.5

    Smac2D:
      plugin: "nav2_smac_planner::SmacPlanner2D"
      tolerance: 0.125
      allow_unknown: true

    SmacHybrid:
      plugin: "nav2_smac_planner::SmacPlannerHybrid"
      tolerance: 0.125
      allow_unknown: true

    SmacLattice:
      plugin: "nav2_smac_planner::SmacPlannerLattice"
      tolerance: 0.125
      allow_unknown: true
      primitive_filename: "/path/to/motion_primitives.yaml"  # 必须提前生成

    ThetaStar:
      plugin: "nav2_theta_star_planner::ThetaStarPlanner"
      how_many_corners: 8
      w_euc_cost: 1.0
      w_traversal_cost: 2.0

3 自定义规划器

例如这里创建一个直线的规划器.

目录建议.

1
2
3
4
5
6
nav2_straightline_planner/
├── CMakeLists.txt
├── package.xml
├── include/nav2_straightline_planner/straight_line_planner.hpp
├── src/straight_line_planner.cpp
└── global_planner_plugin.xml

头文件.

我们的示例插件继承自基础类 nav2_core::GlobalPlanner.

该基础类提供了 5 个纯虚函数, 用于实现规划器插件.

该插件将被规划器服务器用于计算轨迹, 让我们进一步了解编写规划器插件所需的方法.

方法名 调用时机（生命周期） 作用 关键入参 / 返回 必须重写
configure() planner_server 进入 on_configure 声明参数，初始化成员，抓取 TF/Costmap 句柄 入参：parent(node), name, tf, costmap_ros ；返回：无 是
activate() 进入 on_activate 进入活跃前的准备（如启动订阅/定时器等） 入参：无；返回：无 是
deactivate() 进入 on_deactivate 从活跃到非活跃的收尾（如停止订阅/定时器） 入参：无；返回：无 是
cleanup() 进入 on_cleanup 释放 configure() 中创建的资源 入参：无；返回：无 是
createPlan() 需要全局路径时调用 从起点到终点生成 nav_msgs::msg::Path 入参：start, goal, cancel_checker；返回：nav_msgs::msg::Path 是
方法名 签名
configure void configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, std::shared_ptr<tf2_ros::Buffer> tf, std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
activate void activate()
deactivate void deactivate()
cleanup void cleanup()
createPlan nav_msgs::msg::Path createPlan(const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal, std::function<bool()> cancel_checker) 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
#pragma once
#include <string>
#include <memory>
#include <functional>

#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav2_core/global_planner.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "tf2_ros/buffer.h"

namespace nav2_straightline_planner {

class StraightLine : public nav2_core::GlobalPlanner {
public:
  StraightLine() = default;
  ~StraightLine() override = default;

  void configure(
    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
    std::string name,
    std::shared_ptr<tf2_ros::Buffer> tf,
    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;

  void cleanup() override; // planner_server 从 active 退出到 inactive 触发
  void activate() override; // planner_server 进入 cleanup 触发
  void deactivate() override; // planner_server 进入 active 触发

  nav_msgs::msg::Path createPlan(
    const geometry_msgs::msg::PoseStamped & start,
    const geometry_msgs::msg::PoseStamped & goal,
    std::function<bool()> cancel_checker) override;

private:
  rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
  std::string name_;
  std::string global_frame_;
  std::shared_ptr<tf2_ros::Buffer> tf_;
  nav2_costmap_2d::Costmap2D * costmap_{nullptr};
  double interpolation_resolution_{0.1};
};

} // namespace nav2_straightline_planner

实现.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
#include <cmath>
#include "nav2_straightline_planner/straight_line_planner.hpp"
#include "pluginlib/class_list_macros.hpp"

namespace nav2_straightline_planner {

void StraightLine::configure(
  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, // 生命周期节点
  std::string name, // 插件映射名
  std::shared_ptr<tf2_ros::Buffer> tf, // TF 缓存
  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) // 全局代价地图包装
{
  node_ = parent.lock();
  name_ = name;
  tf_ = tf;
  costmap_ = costmap_ros->getCostmap(); // 原始代价图指针
  global_frame_ = costmap_ros->getGlobalFrameID(); // 全局坐标系

  // 声明并读取本插件的参数：<插件名>.interpolation_resolution
  // planner_server.ros__parameters:
  //   planner_plugins: ["StraightLine"]
  //   StraightLine:
  //     plugin: "nav2_straightline_planner::StraightLine"
  //     interpolation_resolution: 0.1

  node_->declare_parameter(name_ + ".interpolation_resolution", rclcpp::ParameterValue(0.1));
  node_->get_parameter(name_ + ".interpolation_resolution", interpolation_resolution_);
}

void StraightLine::activate() {}
void StraightLine::deactivate() {}
void StraightLine::cleanup() {}

nav_msgs::msg::Path StraightLine::createPlan(
  const geometry_msgs::msg::PoseStamped & start, // 起点姿态
  const geometry_msgs::msg::PoseStamped & goal, // 终点姿态
  std::function<bool()> cancel_checker) // jazzy 新增, 循环中调用它，若被取消则提前返回当前已生成的路径
{
  nav_msgs::msg::Path path;
  path.header.stamp = node_->now();
  path.header.frame_id = global_frame_;

  // 仅接受与全局代价地图同一坐标系
  if (start.header.frame_id != global_frame_ || goal.header.frame_id != global_frame_) {
    RCLCPP_ERROR(node_->get_logger(),
      "[StraightLine] start/goal frame must be %s", global_frame_.c_str());
    return path;
  }

  const double dx = goal.pose.position.x - start.pose.position.x;
  const double dy = goal.pose.position.y - start.pose.position.y;
  const double dist = std::hypot(dx, dy);
  // 步数 = 距离 / 插值分辨率(至少 1 步，防止除零)
  const int steps = std::max(1, static_cast<int>(dist / interpolation_resolution_));

  const double stepx = dx / steps;
  const double stepy = dy / steps;

  path.poses.reserve(steps + 1);
  for (int i = 0; i <= steps; ++i) {
    if (cancel_checker && cancel_checker()) {
      RCLCPP_WARN(node_->get_logger(), "[StraightLine] planning cancelled");
      return path;  // 返回已生成的前缀或空路径
    }
    geometry_msgs::msg::PoseStamped p;
    p.header = path.header;
    p.pose.position.x = start.pose.position.x + stepx * i;
    p.pose.position.y = start.pose.position.y + stepy * i;
    p.pose.position.z = 0.0;
    p.pose.orientation.w = 1.0;  // 平面路径, 单位四元数
    path.poses.push_back(p);
  }
  return path;
}

} // namespace nav2_straightline_planner

// 导出为 nav2_core::GlobalPlanner 插件
PLUGINLIB_EXPORT_CLASS(nav2_straightline_planner::StraightLine, nav2_core::GlobalPlanner)

xml 文件.

1
2
3
4
5
6
<library path="nav2_straightline_planner_plugin">
  <class type="nav2_straightline_planner::StraightLine"
         base_class_type="nav2_core::GlobalPlanner">
    <description>Straight-line global planner for Nav2.</description>
  </class>
</library>

package.xml 文件,需要加入一下的片段.

1
2
3
4
<export>
  <build_type>ament_cmake</build_type>
  <nav2_core plugin="${prefix}/global_planner_plugin.xml" />
</export>

CMakeLists.txt 需要导出插件.

1
pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml)

将插件放入 nav2 的 yaml 配置文件.

1
2
3
4
5
6
7
8
9
10
11
planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    costmap_update_timeout: 1.0
    introspection_mode: "disabled"

    planner_plugins: ["StraightLine"]

    StraightLine:
      plugin: "nav2_straightline_planner::StraightLine"
      interpolation_resolution: 0.1

4 References