move_base custom global path planning plugin

Create feature packfixed_route

1. Custom path planning header filemy_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. Customized path planning algorithmmy_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. Configuration CMakeLists.txtfile

add_library(my_global_planner_lib src/my_global_planner.cpp)

4. Configuration package.xmlfile

<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. Custom plugin description filemy_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.xmlPaths are at the same level as CMakeLists.txtfiles and package.xmlfiles.

After compiling and running, use the following command to view the registered plug-ins

rospack plugins --attrib=plugin nav_core

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

6. Use plugins in move_base

(1) move_base_params.yaml uses plugins

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

(2) global_planner_params.yaml calls the configuration parameters of the plug-in

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 Find ns in the launch file as the configuration parameter of 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. Verification

catkin_make

source devel/setup.bash
roslaunch simulation_launch gmapping_navigation.launch

The following red error message appears, indicating that the customized global path planning has been loaded and run.