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

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;
 }
 };

3. 配置CMakeLists.txt文件

add_library(my_global_planner_lib src/my_global_planner.cpp)

4. 配置package.xml文件

<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>

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

 <!-- 注意修改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

fixed_route /home/lyp/Documents/gitee/path_ws/src/fixed_route/my_global_planner_plugin.xml

6. move_base中使用插件

(1) move_base_params.yaml 使用插件

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

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

ns: GlobalPlanner
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为GlobalPlanner的配置参数

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

7. 验证

catkin_make

source devel/setup.bash
roslaunch simulation_launch gmapping_navigation.launch

出现下面红色报错信息，说明已经加载运行自定义的全局路径规划了。