move_base 中使用自定义的全局/局部路径规划插件

个人学习记录，参考的博客文末都有链接

创建功能包 fixed_route

1. 自定义路径规划头文件 my_global_planner.h

/** include the libraries you need in your planner here */
/** for global path planner interface */
 #include <ros/ros.h>
 #include <costmap_2d/costmap_2d_ros.h>
 #include <costmap_2d/costmap_2d.h>
 #include <nav_core/base_global_planner.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <angles/angles.h>
 #include <base_local_planner/world_model.h>
 #include <base_local_planner/costmap_model.h>
 #include <tf/transform_broadcaster.h> 

 using std::string;

 #ifndef GLOBAL_PLANNER_CPP
 #define GLOBAL_PLANNER_CPP

 namespace my_global_planner {

 class MyGlobalPlanner : public nav_core::BaseGlobalPlanner {
 public:

  MyGlobalPlanner();
  MyGlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);

  /** overridden classes from interface nav_core::BaseGlobalPlanner **/
  void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
  bool makePlan(const geometry_msgs::PoseStamped& start,
                const geometry_msgs::PoseStamped& goal,
                std::vector<geometry_msgs::PoseStamped>& plan
               );
  };
 };
 #endif

• 需要包含路径规划器所需的核心 ROS 库。路径规划器需要使用<costmap_2d/costmap_2d_ros.h> 和 <costmap_2d/costmap_2d.h>, 而costmap_2d::Costmap2D 类作为输入的地图类。当定义为插件时，路径规划器类将自动访问此地图。没有必要订阅costmap2d来获取ROS的代价地图。
• <nav_core/base_global_planner.h> 用于导入接口 nav_core::BaseGlobalPlanner，这是插件必须要继承的父类。

2. 自定义路径规划算法 my_global_planner.cpp

 #include <pluginlib/class_list_macros.h>
 #include "fixed_route/my_global_planner.h"

 //register this planner as a BaseGlobalPlanner plugin
 PLUGINLIB_EXPORT_CLASS(my_global_planner::MyGlobalPlanner, nav_core::BaseGlobalPlanner)

 using namespace std;

 //Default Constructor
 namespace my_global_planner {

 MyGlobalPlanner::MyGlobalPlanner (){
	ROS_ERROR("hello MyGlobalPlanner");
 }

 MyGlobalPlanner::MyGlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
   	initialize(name, costmap_ros);
   	ROS_ERROR("hello MyGlobalPlanner initialize");
 }

 void MyGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
	ROS_ERROR("hello MyGlobalPlanner initialize");
 }

 bool MyGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,  std::vector<geometry_msgs::PoseStamped>& plan ){
	ROS_ERROR("hello MyGlobalPlanner makePlan");
	plan.push_back(start);
	for (int i=0; i<20; i++){
	geometry_msgs::PoseStamped new_goal = goal;
	tf::Quaternion goal_quat = tf::createQuaternionFromYaw(1.54);

	new_goal.pose.position.x = -2.5+(0.05*i);
	new_goal.pose.position.y = -3.5+(0.05*i);

	new_goal.pose.orientation.x = goal_quat.x();
	new_goal.pose.orientation.y = goal_quat.y();
	new_goal.pose.orientation.z = goal_quat.z();
	new_goal.pose.orientation.w = goal_quat.w();

	plan.push_back(new_goal);
   }
   plan.push_back(goal);
  return true;
 }
 };

在cpp文件中需要完善 initialize 和 makePlan 函数

• 构造函数 MyGlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros) 用于初始化代价地图，即将用于规划的代价地图（costmap_ros）以及规划器的名称（name）。
• 默认构造函数 MyGlobalPlanner() 即使用默认值作为初始化。方法 initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) 是 BaseGlobalPlanner 的初始化函数，用于初始化 costmap，参数为用于规划的代价地图（costmap_ros）和规划器的名称(name)。
• bool makePlan（start，goal，plan）的方法必须要重写。最终规划将存储在方法的参数 std::vector<geometry_msgs::PoseStamped>& plan 中。
• 注册规划器作为 BaseGlobalPlanner 插件：这是通过指令 PLUGINLIB_EXPORT_CLASS(my_global_planner::MyGlobalPlanner, nav_core::BaseGlobalPlanner) 完成的。为此，必须包含库 #include
  <pluginlib/class_list_macros.h
• makePlan() 方法实现：在开始和目标参数用来获取初始位置和目标位置。在该说明性实现中，以起始位置（plan.push_back(start))）作为开始的规划变量。然后将目标位置作为最后一个位置插入规划。然后，此规划路径将发送到 move_base 全局规划模块，该模块将通过 ROS 话题 nav_msgs/Path 进行发布，然后将由本地规划模块接收。

3. 配置 CMakeLists.txt 文件

要编译上面创建的全局规划库，必须将其添加到 CMakeLists.txt 中。添加代码：

add_library(my_global_planner_lib src/my_global_planner.cpp)

4. 配置 package.xml 文件

注册插件到ROS包系统

为了让 pluginlib 在所有 ROS 程序包上查询系统上的所有可用插件，每个程序包都必须明确指定其导出的插件，哪些程序包库包含这些插件。插件提供者必须在其导出标记块中的package.xml 中指向其插件描述文件。
在 package.xml 的文件中的 export 标签里加入下面这句

<nav_core plugin="${prefix}/my_global_planner_plugin.xml" />

注意：为了使上述导出命令正常工作，提供程序包必须直接依赖于包含插件接口的程序包，这在全局规划程序的情况下是 nav_core。因此，其 package.xml 中必须包含以下内容：

<build_depend>nav_core</build_depend>
<run_depend>nav_core</run_depend>

完整：

<build_depend>nav_core</build_depend>
<exec_depend>nav_core</exec_depend>

<export>
	<!-- Other tools can request additional information be placed here -->
	<nav_core plugin="${prefix}/my_global_planner_plugin.xml" />
</export>

回到工作目录 catkin_make 进行编译，完成后可以在 devel/lib 下有一个 libmy_global_planner_lib.so。

继续编写插件描述文件

5. 自定义插件描述文件 my_global_planner_plugin.xml

现在为全局规划类创建插件，将其集成到 move_base 包的全局规划模块nav_core::BaseGlobalPlanner 中。

 <!-- 注意修改lib的路径及名称 -->
 <library path="lib/libmy_global_planner_lib">  
  <!-- name是命名空间名/类名，move_base_param.yaml全局规划名称一致，type是命名空间名::类名 -->
  <class name="my_global_planner/MyGlobalPlanner" type="my_global_planner::MyGlobalPlanner" base_class_type="nav_core::BaseGlobalPlanner">
    <description>This is a global planner plugin by zhank fixed_route project.</description>
  </class>
 </library>

my_global_planner_plugin.xml 路径与 CMakeLists.txt 文件和 package.xml 文件处于同级目录。

编译运行后，使用如下命令可以查看注册的插件

rospack plugins --attrib=plugin nav_core

6. move_base 中使用插件

方法一

(1) 打开自己的 move_base.launch 配置文件，在 move_base 启动节点后添加

 <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
	 <param name="base_global_planner" value="my_global_planner/MyGlobalPlanner"/>

• 需要注意的是，规划器的名称是 my_global_planner/MyGlobalPlanner，跟指定在my_global_planner_plugin.xml 文件名称一致。

(2) global_planner_params.yaml 调用插件的配置参数

	 <rosparam file="$(find xxx)/config/global_planner_params.yaml" command="load" />

MyGlobalPlanner:
allow_unknown: true
default_tolerance: 0.0
visualize_potential: false
use_dijkstra: true
use_quadratic: true
use_grid_path: false
old_navfn_behavior: false

完整：

<?xml version="1.0"?>
<launch>

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    <!--  TebPlanner --> 
    <param name="base_local_planner" value="teb_planner/TebPlanner" />
    <rosparam file="$(find xxx)/config/teb_planner_params.yaml" command="load" /> 

    <!--  GlobalPlanner -->
    <param name="base_global_planner" value="my_global_planner/MyGlobalPlanner" />
    <rosparam file="$(find xxx)/config/global_planner_params.yaml" command="load" />

    <rosparam file="$(find xxx)/config/teb/move_base_params.yaml" command="load" />
    <rosparam file="$(find xxx)/config/teb/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find xxx)/config/teb/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find xxx)/config/teb/local_costmap_params.yaml" command="load" /> 
    <rosparam file="$(find xxx)/config/teb/global_costmap_params.yaml" command="load" />      
  </node>
</launch> 

方法二

(1) move_base_params.yaml 使用插件

# 命名空间名/类名 与my_global_planner_plugin.xml中的name一致
base_global_planner: "my_global_planner/MyGlobalPlanner"

(2) global_planner_params.yaml 调用插件的配置参数

ns: MyGlobalPlanner
allow_unknown: true
default_tolerance: 0.0
visualize_potential: false
use_dijkstra: true
use_quadratic: true
use_grid_path: false
old_navfn_behavior: false

(3) gmapping_navigation.launch 在 launch 文件里找到 ns 为 MyGlobalPlanner 的配置参数

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find xxx)/param/move_base_params.yaml" command="load" />
    <rosparam file="$(find xxx)/param/global_costmap_params.yaml" command="load" ns="global_costmap"/>
    <rosparam file="$(find xxx)/param/local_costmap_params.yaml" command="load" ns="local_costmap"/>
   
    <rosparam file="$(find xxx)/param/global_planner_params.yaml" command="load" ns="MyGlobalPlanner"/>
    
    <rosparam file="$(find xxx)/param/local_planner_params.yaml" command="load" ns="DWAPlannerROS"/>
  </node>

参考博客：

• move_base自定义全局路径规划插件
• move_base使用自己编写的全局规划器
• ROS-melodic：源码安裝teb_local_planner算法、替换DWA算法