nvfn
如果我们没有修改过amcl_demo的配置， 那么它的move_base所用的全局规划器则是navfn包中定义和实现的NavfnROS， 这一配置可以在turtlebot_navigation包中的move_base_params.yaml文件中看到。
NavfnROS是对nav_core中用虚类BaseGlobalPlanner所定义的接口的实现。面向这一接口，move_base框架就不需要知道用户实际使用了什么全局规划算法， 只要有一个实现了该接口的对象就行了。NavfnROS是对规划器NavFn的封装，NavFn用Dijkstra算法提供了路径规划的功能，它也支持A*启发式规则的扩展。
本文分析NavfnROS和NavFn相关的源码，旨在了解全局路径规划所需要考虑的事情，以及一般流程。在详细分析这两个类之前，让我们先来看一下BaseGlobalPlanner的定义。
1. 接口——BaseGlobalPlanner
BaseGlobalPlanner是move_base获取全局规划能力的接口， 它是定义在包nav_core的头文件base_global_planner.h中的一个虚类， 其类定义如下:
class BaseGlobalPlanner {
public:
virtual bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan) = 0;
virtual bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan, double& cost)
{
cost = 0;
return makePlan(start, goal, plan);
}
virtual void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) = 0;
virtual ~BaseGlobalPlanner(){}
protected:
BaseGlobalPlanner(){}
};
从其定义中可以看到，BaseGlobalPlanner定义了两个纯虚函数，或者说是抽象函数，makePlan和initialize。在C++中纯虚函数的定义形式如下，不必提供该函数的实现。
virtual void fun() = 0;
如果在一个类定义中，有一个纯虚函数，那么这个类就是一个纯虚类。根据C++的语法规则，我们是不能够创建一个纯虚类的对象。必须定义一个子类继承这个纯虚类，并且在这个子类中实现所有的纯虚函数。 有时我们称这个纯虚类为接口，子类为接口的实现。我们可以创建一个实现了接口的对象，却不能够创建接口，但是可以通过接口来访问这一对象。比如说在move_base的源码中，通过如下的语句调用initialize函数， 完成对路径规划器的初始化。
planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
BaseGlobalPlanner中定义的纯虚函数makePlan的意义就是，给定给定起始点start和目标点goal，输出一条路径plan，如果能够找到一条可行的路径就返回true，否则返回false。 BaseGlobalPlanner还提供了一个makePlan的重载，它多了一个参数cost。这是一个输出参数，用于记录路径的代价，默认为0。
函数initialize是一个用于初始化规划器的函数，它有两个参数。其中参数name是规划器的名称，costmap_ros则是用于规划算法的输入——代价地图。这里还写了析构函数和构造函数， 但都没有实际的语句。
继承自BaseGlobalPlanner的全局规划器必须实现makePlan和initialize两个函数，才能摆脱纯虚函数的限制，实例化对象。下面我们深入研究这一接口的实现NavfnROS。
2. 封装——NavfnROS
NavfnROS是接口BaseGlobalPlanner的一种实现，本身并不提供算法，而是对NavFn的一个封装。下面是NavfnROS类定义片段，可以看到它继承自BaseGlobalPlanner，实现了initialize和makePlan两个接口函数。
class NavfnROS : public nav_core::BaseGlobalPlanner {
public:
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);
// 省略其它成员
};
2.1 构造与初始化
如下面代码所示，NavfnROS定义了三个重载的构造函数。它们首先对成员变量赋予必要的初值，其中costmap_是用于路径规划的代价地图对象，planner_是提供规划算法的NavFn对象。 initialized_是一个状态标志，它限定每个NavfnROS对象只能被初始化一遍。allow_unknown_限定了规划器是否可以从未知区域中经过。
除了默认构造函数，其余两个都调用了initialize函数完成对规划器的初始化工作。
NavfnROS::NavfnROS()
: costmap_(NULL), planner_(), initialized_(false), allow_unknown_(true) {}
NavfnROS::NavfnROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
: costmap_(NULL), planner_(), initialized_(false), allow_unknown_(true) {
initialize(name, costmap_ros);
}
NavfnROS::NavfnROS(std::string name, costmap_2d::Costmap2D* costmap, std::string global_frame)
: costmap_(NULL), planner_(), initialized_(false), allow_unknown_(true) {
initialize(name, costmap, global_frame);
}
NavfnROS提供了两个initialize的重载，其中有两个参数的initialize通过代价地图对象获取了全局坐标系，并调用了有三个参数的initialize函数。
void NavfnROS::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
initialize(name, costmap_ros->getCostmap(), costmap_ros->getGlobalFrameID());
}
下面是实际调用的初始化函数，它有三个参数。其中name是规划器的名称，costmap是进行路径规划的代价地图对象，global_frame是系统运行的全局坐标系名称。 在该函数中，检查初始化标志initialized_，如果之前已经完成了规划器的初始化，就报错退出。否则执行初始化操作，并标记initialized_。
void NavfnROS::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string global_frame) {
if (!initialized_) {
// 初始化
initialized_ = true;
} else
ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
}
为了方便本文写作，我们在不影响系统运行的前提下，对初始化的语句做些调整。下面是从ROS的参数服务器上获取参数的语句，首先创建一个ROS的句柄，再通过该句柄的接口获取参数。 各个参数的意义如下:
- visualize_potential:是否通过PointCloud2消息在rviz中可视化潜在的区域。这是1.3.1版的navigation添加的特性，实际上没啥用，默认是false。
- allow_unknown:是否允许NavFn规划的路径通过未知区域，默认true。
- planner_window_x, planner_window_y:规划窗口的大小，默认是0。(按:从源码中看，好像没用到)
- default_tolerance:到达终点的容忍度。当规划的路径点到目标点的距离小于该参数描述的值的时候，就认为路径点基本到达了目标点。
ros::NodeHandle private_nh("~/" + name);
private_nh.param("visualize_potential", visualize_potential_, false);
private_nh.param("allow_unknown", allow_unknown_, true);
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);
接下来，我们根据函数的输入参数构建规划器和代价地图对象。用成员变量costmap_记录代价地图对象的地址，字符串global_frame_记录全局坐标系名称， 根据代价地图的尺寸构建规划器对象，并用C++的boost库中的智能指针记录。
costmap_ = costmap;
global_frame_ = global_frame;
planner_ = boost::shared_ptr<NavFn>(new NavFn(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()));
接下来，我们发布记录路径规划结果的主题plan，根据需要发布可视化潜在区域的主题"potential"。并且注册"make_plan"服务。
plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
if(visualize_potential_)
potarr_pub_ = private_nh.advertise<sensor_msgs::PointCloud2>("potential", 1);
make_plan_srv_ = private_nh.advertiseService("make_plan", &NavfnROS::makePlanService, this);
2.2 规划路径
makePlan是继承自BaseGlobalPlanner的规划器所必须实现的接口函数，它用于实际的路径计算。NavfnROS为之提供了两个重载，它们只是参数列表不同，通过相互调用只保留了一个实现:
bool NavfnROS::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan) {
return makePlan(start, goal, default_tolerance_, plan);
}
下面是具体实现的makePlan，有四个参数。其中start和goal是两个输入参数，分别记录了规划轨迹的起始点和目标点。tolerance是目标点到达的容忍度，plan是一个输出参数记录了轨迹中的各个路径点。 由于在move_base中使用了多线程进行规划，为了线程安全，在函数的一开始对信号量mutex_加锁。
bool NavfnROS::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_);
在进行路径规划之前，我们必须保证规划器已经完成了初始化，否则报错退出。同时清空输出路径plan。
if(!initialized_) {
ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
return false;
}
plan.clear();
然后检查起始点和目标点的参考坐标系是全局坐标系，如不是则报错退出。
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;
}
接下来计算起始点坐标在代价地图中的坐标索引，并用临时的数组map_start记录它们。坐标索引是通过代价地图对象的worldToMap接口获得，这里的wx和wy是两个输入参数， 记录了物理世界中连续的位置坐标; mx和my则是输出参数，输出离散的删格索引。如果不能成功获取删格索引，则报错退出。
double wx = start.pose.position.x;
double wy = start.pose.position.y;
unsigned int mx, my;
if (!costmap_->worldToMap(wx, wy, mx, my)) {
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;
}
int map_start[2];
map_start[0] = mx;
map_start[1] = my;
下面这行代码，认为起始点出是不可能有障碍物的，所以它调用了clearRobotCell清除起始点代价。clearRobotCell是NavfnROS中定义的成员函数，在该函数中通过代价地图对象的接口清除指定删格索引的代价。
costmap_->setCost(mx, my, costmap_2d::FREE_SPACE);个人认为这点很不合理，代价地图对于规划器而言应当是只读的，不应该在这里修改它的内容。
clearRobotCell(start, mx, my);
类似的，用map_goal记录目标点坐标在代价地图中的删格索引。个人认为，这里应该与起始点的处理方式一致，如果不能成果获得索引，就报错退出，没必要在if语句中废话。
wx = goal.pose.position.x;
wy = goal.pose.position.y;
if(!costmap_->worldToMap(wx, wy, mx, my)){
if(tolerance <= 0.0){
ROS_WARN_THROTTLE(1.0, "The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal.");
return false;
}
mx = 0; my = 0;
}
int map_goal[2];
map_goal[0] = mx;
map_goal[1] = my;
然后，我们根据代价地图的属性，完成对规划器的配置。
planner_->setNavArr(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());
planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_);
planner_->setStart(map_goal);
planner_->setGoal(map_start);
完成了配置，我们就可以使用Dijkstra或者A*算法为每个删格计算一个代价或者说是潜力值(Potential)。
//bool success = planner_->calcNavFnAstar();
planner_->calcNavFnDijkstra(true);
下面我们定义一些临时的变量，用于在终点附近查找是否存在一个理想的点，能够连接到起始点上。
double resolution = costmap_->getResolution(); // 删格地图的分辨率
geometry_msgs::PoseStamped p = goal; // 当前考察点
geometry_msgs::PoseStamped best_pose; // 当前最理想的点
bool found_legal = false; // 是否找到一个可以接受的点
double best_sdist = DBL_MAX; // 当前最理想的点到终点的距离
我们在以终点为中心，tolerance为半边长的四方框内寻找最理想的点。这里的tolerance就是函数一开始传输的参数，到达终点的容忍度。对于每个删格点， 我们使用成员函数getPointPotential获取刚刚计算的代价。sq_distance则用于计算考察点与终点之间欧式距离的平方。
在第61行中的条件语句中，我们检查考察点的代价值，只要低于我们指定的一个阈值POT_HIGH我们就认为它是一个理想的点。同时我们通过刚刚定义的各个临时变量， 筛选出所有理想的点中距离终点最近的点。
p.pose.position.y = goal.pose.position.y - tolerance;
while(p.pose.position.y <= goal.pose.position.y + tolerance) {
p.pose.position.x = goal.pose.position.x - tolerance;
while(p.pose.position.x <= goal.pose.position.x + tolerance) {
double potential = getPointPotential(p.pose.position);
double sdist = sq_distance(p, goal);
if(potential < POT_HIGH && sdist < best_sdist) {
best_sdist = sdist;
best_pose = p;
found_legal = true;
}
p.pose.position.x += resolution;
}
p.pose.position.y += resolution;
}
如果我们找到了解，那么就通过成员函数getPlanFromPotential获取规划的路径。
if (found_legal) {
if (getPlanFromPotential(best_pose, plan)) {
geometry_msgs::PoseStamped goal_copy = best_pose;
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.");
}
如果在初始化NavfnROS对象的时候，设置了visualize_potential_变量，那么就需要发布关于potential的消息。这里与路径规划无关，略。
if (visualize_potential_) {
// 略
}
最后发布规划的路径并返回退出。
publishPlan(plan, 0.0, 1.0, 0.0, 0.0);
return !plan.empty();
}
NavfnROS只是对NavFn的一个封装而已，为NavFn提供了一个接入move_base框架的接口。NavFn实现了Dijkstra算法，并提供了回溯路径的方法。
3. 实现——NavFn
NavFn才是真正的路径规划器的实现。源码写的实在太烂，看起来很费劲。既然它说使用的是Dijkstra算法和A*算法进行路径规划的，我们就认为是吧。 我决定在数据结构与算法系列文章中添加这些经典算法的介绍。
Dijkstra算法原本是在图中求取两个节点之间的最短路径的一种算法。A*算法可以看作是Dijkstra算法的一种扩展，添加了一些启发式规则。算法的思想和我们的实现参见文章。
4. 完
本文中，我们分析了navfn的源码。它为move_base框架提供了Dijkstra和A*算法进行路径规划。本想在这里分析算法的实现，但源码写的很乱，就取消了。这里只是介绍了基本的工作流程。