编写全局规划器
首先在工作目录创建功能包
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