编写全局规划器
首先在工作目录创建功能包
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 <unistd.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <fstream>
#include <iostream>
#include <iomanip>
#include <string>
#include "RAstar_ros.h"
#include <pluginlib/class_list_macros.h>
//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(RAstar_planner::RAstarPlannerROS, nav_core::BaseGlobalPlanner)
int value;
int mapSize;
bool* OGM;
static const float INFINIT_COST = INT_MAX; //!< cost of non connected nodes
float infinity = std::numeric_limits< float >::infinity();
float tBreak; // coefficient for breaking ties
ofstream MyExcelFile ("RA_result.xlsx", ios::trunc);
namespace RAstar_planner
{
//Default Constructor
RAstarPlannerROS::RAstarPlannerROS()
{
}
RAstarPlannerROS::RAstarPlannerROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
{
initialize(name, costmap_ros);
}
void RAstarPlannerROS::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
{
if (!initialized_)
{
costmap_ros_ = costmap_ros;
costmap_ = costmap_ros_->getCostmap();
ros::NodeHandle private_nh("~/" + name);
originX = costmap_->getOriginX();
originY = costmap_->getOriginY();
width = costmap_->getSizeInCellsX();
height = costmap_->getSizeInCellsY();
resolution = costmap_->getResolution();
mapSize = width*height;
tBreak = 1+1/(mapSize);
value =0;
OGM = new bool [mapSize];
for (unsigned int iy = 0; iy < costmap_->getSizeInCellsY(); iy++)
{
for (unsigned int ix = 0; ix < costmap_->getSizeInCellsX(); ix++)
{
unsigned int cost = static_cast<int>(costmap_->getCost(ix, iy));
//cout<<cost;
if (cost == 0)
OGM[iy*width+ix]=true;
else
OGM[iy*width+ix]=false;
}
}
MyExcelFile << "StartID\tStartX\tStartY\tGoalID\tGoalX\tGoalY\tPlannertime(ms)\tpathLength\tnumberOfCells\t" << endl;
ROS_INFO("RAstar planner initialized successfully");
initialized_ = true;
}
else
ROS_WARN("This planner has already been initialized... doing nothing");
}
bool RAstarPlannerROS::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan)
{
if (!initialized_)
{
ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
return false;
}
ROS_DEBUG("Got a start: %.2f, %.2f, and a goal: %.2f, %.2f", start.pose.position.x, start.pose.position.y,
goal.pose.position.x, goal.pose.position.y);
plan.clear();
if (goal.header.frame_id != costmap_ros_->getGlobalFrameID())
{
ROS_ERROR("This planner as configured will only accept goals in the %s frame, but a goal was sent in the %s frame.",
costmap_ros_->getGlobalFrameID().c_str(), goal.header.frame_id.c_str());
return false;
}
tf::Stamped < tf::Pose > goal_tf;
tf::Stamped < tf::Pose > start_tf;
poseStampedMsgToTF(goal, goal_tf);
poseStampedMsgToTF(start, start_tf);
// convert the start and goal positions
float startX = start.pose.position.x;
float startY = start.pose.position.y;
float goalX = goal.pose.position.x;
float goalY = goal.pose.position.y;
getCorrdinate(startX, startY);
getCorrdinate(goalX, goalY);
int startCell;
int goalCell;
if (isCellInsideMap(startX, startY) && isCellInsideMap(goalX, goalY))
{
startCell = convertToCellIndex(startX, startY);
goalCell = convertToCellIndex(goalX, goalY);
MyExcelFile << startCell <<"\t"<< start.pose.position.x <<"\t"<< start.pose.position.y <<"\t"<< goalCell <<"\t"<< goal.pose.position.x <<"\t"<< goal.pose.position.y;
}
else
{
ROS_WARN("the start or goal is out of the map");
return false;
}
/
// call global planner
if (isStartAndGoalCellsValid(startCell, goalCell)){
vector<int> bestPath;
bestPath.clear();
bestPath = RAstarPlanner(startCell, goalCell);
//if the global planner find a path
if ( bestPath.size()>0)
{
// convert the path
for (int i = 0; i < bestPath.size(); i++)
{
float x = 0.0;
float y = 0.0;
int index = bestPath[i];
convertToCoordinate(index, x, y);
geometry_msgs::PoseStamped pose = goal;
pose.pose.position.x = x;
pose.pose.position.y = y;
pose.pose.position.z = 0.0;
pose.pose.orientation.x = 0.0;
pose.pose.orientation.y = 0.0;
pose.pose.orientation.z = 0.0;
pose.pose.orientation.w = 1.0;
plan.push_back(pose);
}
float path_length = 0.0;
std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
geometry_msgs::PoseStamped last_pose;
last_pose = *it;
it++;
for (; it!=plan.end(); ++it) {
path_length += hypot( (*it).pose.position.x - last_pose.pose.position.x,
(*it).pose.position.y - last_pose.pose.position.y );
last_pose = *it;
}
cout <<"The global path length: "<< path_length<< " meters"<<endl;
MyExcelFile << "\t" <<path_length <<"\t"<< plan.size() <<endl;
//publish the plan
return true;
}
else
{
ROS_WARN("The planner failed to find a path, choose other goal position");
return false;
}
}
else
{
ROS_WARN("Not valid start or goal");
return false;
}
}
}
-
构造函数GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros)用于初始化代价地图，即将用于规划的代价地图（costmap_ros）以及规划器的名称（name）。
-
默认构造函数GlobalPlanner()即使用默认值作为初始化。方法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(global_planner::GlobalPlanner,
nav_core::BaseGlobalPlanner)完成的。为此，必须包含库 #include
<pluginlib/class_list_macros.h> -
makePlan()方法实现：在开始和目标参数用来获取初始位置和目标位置。在该说明性实现中，以起始位置（plan.push_back(start))）作为开始的规划变量。然后，在for循环中，将在规划中静态插入20个新的虚拟位置，然后将目标位置作为最后一个位置插入规划。然后，此规划路径将发送到move_base全局规划模块，该模块将通过ROS话题nav_msgs/Path进行发布，然后将由本地规划模块接收。
现在，全局规划类已经完成，进行第二步，即为全局规划类创建插件，将其集成到move_base包的全局规划模块nav_core::BaseGlobalPlanner中。
要编译上面创建的全局规划库，必须将其添加到CMakeLists.txt中。添加代码：
add_library(global_planner_lib src/path_planner/global_planner/global_planner.cpp)
回到工作目录catkin_make进行编译，完成后可以在devel/lib下有一个librelaxed_astar_lib。
继续编写插件描述文件
插件描述文件
relaxed_astar_planner_plugin.xml
<library path="lib/librelaxed_astar_lib">
<class name="RAstar_planner/RAstarPlannerROS" type="RAstar_planner::RAstarPlannerROS" base_class_type="nav_core::BaseGlobalPlanner">
<description>This is Relaxed Astar global planner plugin by iroboapp project.</description>
</class>
</library>
注册插件到ROS包系统
为了让pluginlib在所有ROS程序包上查询系统上的所有可用插件，每个程序包都必须明确指定其导出的插件，哪些程序包库包含这些插件。插件提供者必须在其导出标记块中的package.xml中指向其插件描述文件。
在package.xml的文件中的 export 标签里加入下面这句
<nav_core plugin="${prefix}/relaxed_astar_planner_plugin.xml"/>
注意：为了使上述导出命令正常工作，提供程序包必须直接依赖于包含插件接口的程序包，这在全局规划程序的情况下是nav_core。因此，其package.xml中必须包含以下内容：
<build_depend>nav_core</build_depend>
<run_depend>nav_core</run_depend>
这将告诉编译器关于nav_core包的依赖性
回到工作目录在进行编译一次catkin_make，然后查询插件：
rospack plugins --attrib=plugin nav_core
可以看到 relaxed_astar的插件和其位置。
到此，插件编写完成，下面就是运用自己得全局规划器在move_base中。
打开自己的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="RAstar_planner/RAstarPlannerROS"/>
保存并关闭move_base.launch.xml。
- 需要注意的是，规划器的名称是RAstar_planner/RAstarPlannerROS，跟指定在relaxed_astar_planner_plugin.xml文件名称一致。
至此，完成配置。就可以运行了。
参考：
http://wiki.ros.org/navigation/Tutorials/Writing%20A%20Global%20Path%20Planner%20As%20Plugin%20in%20ROS
https://www.youtube.com/watch?v=We1gGDXAO_o
https://www.youtube.com/watch?v=t4A_niNlDdg
https://github.com/coins-lab/relaxed_astar
1476
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
