move_base使用自己编写的全局规划器

编写全局规划器

首先在工作目录创建功能包

catkin_create_pkg relaxed_astar nav_core roscpp rospy std_msgs

在功能包下编写对应的头文件和源文件
具体的规划器代码根据自己的实际需求进行编写，本文为简化版，结尾处附有github 源代码链接。
RAstar_ros.h

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h> 
#include <string>

/** include ros libraries**********************/
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <move_base_msgs/MoveBaseGoal.h>
#include <move_base_msgs/MoveBaseActionGoal.h>
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud2.h"
#include <nav_msgs/Odometry.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/GetPlan.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
/** ********************************************/ 

#include <boost/foreach.hpp>
//#define forEach BOOST_FOREACH
/** for global path planner interface */
#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 <pcl_conversions/pcl_conversions.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/costmap_model.h>
#include <set>
using namespace std;
using std::string;

#ifndef RASTAR_ROS_CPP
#define RASTAR_ROS_CPP

namespace RAstar_planner {
   
  
class RAstarPlannerROS : public nav_core::BaseGlobalPlanner {
   
public:
  
  RAstarPlannerROS (ros::NodeHandle &); //this constructor is may be not needed
  RAstarPlannerROS ();
  RAstarPlannerROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros);

  /** overriden 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
	       );
};
};

• 需要包含路径规划器所需的核心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，这是插件必须要继承的父类。

然后在cpp文件中完善initialize 和 makePlan函数。
RAstar_ros.cpp

#include <stdio.h>
#include <stdlib.h>
#include