在ROS navigation stack中,关于机器人全局路径规划BaseGlobalPlanner的plugin有三种实现,分别是:
"navfn/NavfnROS","global_planner/GlobalPlanner","carrot_planner/CarrotPlanner"
其中常用的是
global_planner/GlobalPlanner
,它是
navfn/NavfnROS
的改进版本。关于两者是什么关系,以及为什么都存在于ROS navigation中,可以参考该链接:
http://answers.ros.org/question/98511/global_planner-package-with-a-planner-question/
本文分析其中的一种实现
"
global_planner/GlobalPlanner
"
。源码位于navigation stack中的
global_planner
源码包,包含了
A*算法
和
Dijkstra算法
的实现。
首先,move base是通过plugin调用它的:文件
bgp_plugin.xml
<library path="lib/libglobal_planner">
<class name="global_planner/GlobalPlanner" type="global_planner::GlobalPlanner" base_class_type="nav_core::BaseGlobalPlanner">
<description>
A implementation of a grid based planner using Dijkstras or A*
</description>
</class>
</library>
在
package.xml
的配置中,如下:
<export>
<nav_core plugin="${prefix}/bgp_plugin.xml" />
</export>
global_planner main函数的入口,核心代码行是:
global_planner::PlannerWithCostmap pppp(“planner”, &lcr);
int main(int argc, char** argv) {
ros::init(argc, argv, "global_planner");
tf2_ros::Buffer buffer(ros::Duration(10));
tf2_ros::TransformListener tf(buffer);
costmap_2d::Costmap2DROS lcr("costmap", buffer);
global_planner::PlannerWithCostmap pppp("planner", &lcr);
ros::spin();
return 0;
}
PlannerWithCostmap构造方法如下:
PlannerWithCostmap::PlannerWithCostmap(string name, Costmap2DROS* cmap) :
GlobalPlanner(name, cmap->getCostmap(), cmap->getGlobalFrameID())
{
ros::NodeHandle private_nh("~");
cmap_ = cmap;
make_plan_service_ = private_nh.advertiseService("make_plan", &PlannerWithCostmap::makePlanService, this);
pose_sub_ = private_nh.subscribe<rm::PoseStamped>("goal", 1, &PlannerWithCostmap::poseCallback, this);
}
第一:发布一个
make_plan
的service,一旦有请求,就回调
PlannerWithCostmap::makePlanService
方法,执行
makePlan(req.start, req.goal, path)
; 如果makePlan成功,将
vector<PoseStamped> path
赋值到
resp.path
中,并返回true。
bool PlannerWithCostmap::makePlanService(navfn::MakeNavPlan::Request& req, navfn::MakeNavPlan::Response& resp) {
vector<PoseStamped> path;
req.start.header.frame_id = "map";
req.goal.header.frame_id = "map";
bool success = makePlan(req.start, req.goal, path);
resp.plan_found = success;
if (success) {
resp.path = path;
}
return true;
}
第二: 订阅一个
goal
的topic,一旦收到goal的发布,就回调
PlannerWithCostmap::poseCallback
方法,根据机器人当前所在地图的位置和目标goal的位置,执行
makePlan(global_pose, *goal, path)
,计算出
vector<PoseStamped> path
数据。
void PlannerWithCostmap::poseCallback(const rm::PoseStamped::ConstPtr& goal) {
geometry_msgs::PoseStamped global_pose;
cmap_->getRobotPose(global_pose);
vector<PoseStamped> path;
makePlan(global_pose, *goal, path);
}
第三:主要的逻辑部分在于
class GlobalPlanner
中初始化函数
initialize
和路径规划函数
makePlan
。
GlobalPlanner(name, cmap->getCostmap(), cmap->getGlobalFrameID())
GlobalPlanner的构造方法有:
GlobalPlanner::GlobalPlanner() :
costmap_(NULL), initialized_(false), allow_unknown_(true) {
}
或者:
GlobalPlanner::GlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) :
costmap_(NULL), initialized_(false), allow_unknown_(true) {
//initialize the planner
initialize(name, costmap, frame_id);
}
初始化函数initialize
void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) {
if (!initialized_) {
ros::NodeHandle private_nh("~/" + name);
costmap_ = costmap;
frame_id_ = frame_id;
unsigned int cx = costmap->getSizeInCellsX(), cy = costmap->getSizeInCellsY();
private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
if(!old_navfn_behavior_)
convert_offset_ = 0.5;
else
convert_offset_ = 0.0;
bool use_quadratic;
private_nh.param("use_quadratic", use_quadratic, true);
if (use_quadratic)
p_calc_ = new QuadraticCalculator(cx, cy);
else
p_calc_ = new PotentialCalculator(cx, cy);
bool use_dijkstra;
private_nh.param("use_dijkstra", use_dijkstra, true);
if (use_dijkstra)
{
DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
if(!old_navfn_behavior_)
de->setPreciseStart(true);
planner_ = de;
}
else
planner_ = new AStarExpansion(p_calc_, cx, cy);
bool use_grid_path;
private_nh.param("use_grid_path", use_grid_path, false);
if (use_grid_path)
path_maker_ = new GridPath(p_calc_);
else
path_maker_ = new GradientPath(p_calc_);
orientation_filter_ = new OrientationFilter();
plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);
private_nh.param("allow_unknown", allow_unknown_, true);
planner_->setHasUnknown(allow_unknown_);
private_nh.param("planner_window_x", planner_window_x_, 0.0);
private_nh.param("planner_window_y", planner_window_y_, 0.0);
private_nh.param("default_tolerance", default_tolerance_, 0.0);
private_nh.param("publish_scale", publish_scale_, 100);
make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);
dsrv_ = new dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>(ros::NodeHandle("~/" + name));
dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>::CallbackType cb = boost::bind(
&GlobalPlanner::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
initialized_ = true;
} else
ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
}
参数配置:
参数一:
old_navfn_behavior
(若在某些情况下,想让global_planner完全复制navfn的功能,那就设置为true,但是需要注意navfn是非常旧的ROS系统中使用的,现在已经都用global_planner代替navfn了,所以不建议设置为true.)
private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
if(!old_navfn_behavior_)
convert_offset_ = 0.5;
else
convert_offset_ = 0.0;
参数二:
use_quadratic
(设置为true,将使用二次函数近似函数,否则使用更加简单的计算方式,这样节省硬件计算资源.)
bool use_quadratic;
private_nh.param("use_quadratic", use_quadratic, true);
if (use_quadratic)
p_calc_ = new QuadraticCalculator(cx, cy);
else
p_calc_ = new PotentialCalculator(cx, cy);
参数三:
use_dijkstra
(设置为true,将使用dijkstra算法,否则使用A*算法.)
bool use_dijkstra;
private_nh.param("use_dijkstra", use_dijkstra, true);
if (use_dijkstra)
{
DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
if(!old_navfn_behavior_)
de->setPreciseStart(true);
planner_ = de;
}
else
planner_ = new AStarExpansion(p_calc_, cx, cy);
参数四:
use_grid_path
(如果设置为true,则会规划一条沿着网格边界的路径,偏向于直线穿越网格,否则将使用梯度下降算法,路径更为光滑点.)
bool use_grid_path;
private_nh.param("use_grid_path", use_grid_path, false);
if (use_grid_path)
path_maker_ = new GridPath(p_calc_);
else
path_maker_ = new GradientPath(p_calc_);
参数五:
allow_unknown
(是否允许规划器规划穿过未知区域的路径,只设计该参数为true还不行,还要在costmap_commons_params.yaml中设置track_unknown_space参数也为true才行。)
参数六:
planner_window_x,planner_window_y
参数七:
default_tolerance
(当设置的目的地被障碍物占据时,需要以该参数为半径寻找到最近的点作为新目的地点.)
参数八:
publish_scale
参数九:
lethal_cost
致命代价值,默认是设置为253,可以动态来配置该参数.
参数十:
neutral_cost
中等代价值,默认设置是50,可以动态配置该参数.
参数十一:
cost_factor
代价地图与每个代价值相乘的因子.
参数十二:
publish_potential
是否发布costmap的势函数.
参数十三:
orientation_mode
如何设置每个点的方向(None = 0,Forward = 1,Interpolate = 2,
ForwardThenInterpolate = 3,Backward = 4,Leftward = 5,Rightward = 6)(可动态重新配置)
参数十四:
orientation_window_size
根据orientation_mode指定的位置积分来得到使用窗口的方向.默认值1,可以动态重新配置.
发布两个topic:
plan , potential ,
一个service :
make_plan,
并且回调
makePlanService
方法,执行
makePlan
函数。
plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);
make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);
路径规划函数makePlan()
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
boost::mutex::scoped_lock lock(mutex_);
if (!initialized_) {
ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
return false;
}
//clear the plan, just in case
plan.clear();
ros::NodeHandle n;
std::string global_frame = frame_id_;
//until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
if (goal.header.frame_id != global_frame) {
ROS_ERROR(
"The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", global_frame.c_str(), goal.header.frame_id.c_str());
return false;
}
if (start.header.frame_id != global_frame) {
ROS_ERROR(
"The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.", global_frame.c_str(), start.header.frame_id.c_str());
return false;
}
double wx = start.pose.position.x;
double wy = start.pose.position.y;
unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
double start_x, start_y, goal_x, goal_y;
if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {
ROS_WARN(
"The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
return false;
}
if(old_navfn_behavior_){
start_x = start_x_i;
start_y = start_y_i;
}else{
worldToMap(wx, wy, start_x, start_y);
}
wx = goal.pose.position.x;
wy = goal.pose.position.y;
if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
ROS_WARN_THROTTLE(1.0,
"The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
return false;
}
if(old_navfn_behavior_){
goal_x = goal_x_i;
goal_y = goal_y_i;
}else{
worldToMap(wx, wy, goal_x, goal_y);
}
//clear the starting cell within the costmap because we know it can't be an obstacle
clearRobotCell(start, start_x_i, start_y_i);
int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
//make sure to resize the underlying array that Navfn uses
p_calc_->setSize(nx, ny);
planner_->setSize(nx, ny);
path_maker_->setSize(nx, ny);
potential_array_ = new float[nx * ny];
outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);
bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
nx * ny * 2, potential_array_);
if(!old_navfn_behavior_)
planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);
if(publish_potential_)
publishPotential(potential_array_);
if (found_legal) {
//extract the plan
if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) {
//make sure the goal we push on has the same timestamp as the rest of the plan
geometry_msgs::PoseStamped goal_copy = goal;
goal_copy.header.stamp = ros::Time::now();
plan.push_back(goal_copy);
} else {
ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
}
}else{
ROS_ERROR("Failed to get a plan.");
}
// add orientations if needed
orientation_filter_->processPath(start, plan);
//publish the plan for visualization purposes
publishPlan(plan);
delete potential_array_;
return !plan.empty();
}
worldToMap
将start坐标点和goal的坐标点转换到map上;
clearRobotCell
清除Costmap中的起始单元格,因为我们知道它不会成为障碍。
outlineMap
将costmap的四个边的全部cell都设置为LETHAL_OBSTACLE
calculatePotentials
计算potential,调用的是:
virtual bool calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,
int cycles, float* potential) = 0;
getPlanFromPotential
提取plan数据
virtual bool getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path) = 0;
参考文章:
http://answers.ros.org/question/98511/global_planner-package-with-a-planner-question/
http://www.voidcn.com/article/p-fhhnkxfi-kk.html
http://answers.ros.org/question/11388/navfn-algorism/?answer=16891#answer-container-16891
http://answers.ros.org/question/28366/why-navfn-is-using-dijkstra/