在第一章中第四步执行了planThread函数的线程启动，该步骤会调用planThread函数。这里不是通过函数调用的形式实现的，而是通过线程开关的形式实现的:
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);//给该线程上锁
planner_goal_ = goal;//传入目标并开启线程
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();//打开互斥锁
看一下全局路径规划的定义函数：
planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
以及初始化定义：
bool runPlanner_;
boost::recursive_mutex planner_mutex_;
boost::condition_variable_any planner_cond_;
geometry_msgs::PoseStamped planner_goal_;
boost::thread* planner_thread_;
该线程创建以后会因为 runPlanner_ 值为false而挂起。当executeCB中唤醒线程前一刻时runPlanner_被设置为true。在这之后就正常情况下面没有被设置为fale过所以线程被唤醒以后就不在进入等待的循环中而是一直在大循环执行，每个大循环都会调用一次 makeplan并重新把新路径装入planner_plan_容器中。(更新了路径)在executeCB中没有意外发生时只会唤醒一次该线程，以后不再唤醒。会一致执行大循环每个循环下发一次控制速度，但是每次循环中的路径可能会由于该线程一直在跑的缘故而发生改变。
接下来看一下MoveBase::planThread的具体执行函数：
1、设置目标点
该函数的第一步首先是设置了一个目标点：
geometry_msgs::PoseStamped temp_goal = planner_goal_;
这里的planner_goal_来自于executeCb中的目标点。通过全局变量传参到这里。然后这里通过一个temp_goal暂存到这里。
2、清空路径存储容器
planner_plan_->clear();
这里的planner_plan_里面存储的是路径中的点，一个容器：
planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
3、全局路径规划
bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
这里调用了makePlan函数，进行全局规划。如果成功，结果将储存进planner_plan_中。但是注意函数实现并不在这个地方实现，makePlan中又调用了：
planner_->makePlan(start, goal, plan)
这个函数是定义在BaseGlobalPlanner基类中的虚函数：
virtual bool makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan) = 0;
在global_planner包中的planner_core.h中继承该基类：
class GlobalPlanner : public nav_core::BaseGlobalPlanner {
......
}
在planner_core.cpp 中添加插件，同时实现函数：
PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)
然后可以在这里得到函数的具体实现流程如下：
3.1、线程上锁
加锁，保证线程安全
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;
}
3.2、清空路径
plan.clear();
对于传入的路径容器进行清空，防止之前的路径点没有清除影响到局部路径规划。但是其实这里在传参过来之前已经清空过一次了，这里可能是为了以防万一再确认一次。
3.3、坐标系判断
在做全局路径规划之前，我们需要判断一下两者的坐标系是否在global frame_id下：
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;
}
因为需要进行全局路径规划，首先起点跟终点是需要在世界坐标系下的，所以这里对输入再次进行了进一步的判断。以确保计算出来的路径是正确的。
3.4、坐标系转换
在确认坐标系没有问题的情况下，在次将起点与终点转换到地图坐标系下：
//判断起始点和目标点是否超出地图范围
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;
//worldToMap函数，其实就是通过坐标和地图的origin原点相减 然后除以分辨率得出：
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);
}
不管是astar还是dijkstra都是基于栅格计算路径的，所以我们需要对起点与终点进行一定的转换，以此保证能找到一条正确的路径。
3.5、处理起点栅格
对于路径规划而言，我们默认起点周围是没有障碍物的，要不然机器人就会与障碍物重叠了，所以会单独对起点做一下处理：
//clearRobotCell函数的定义，主要是把起始点周围的costmap栅格设置为free
clearRobotCell(start, start_x_i, start_y_i);
3.6、计算势场数组
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];
//计算势场数组：用到的是calculatePotentials函数
bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
nx * ny * 2, potential_array_);
这个函数的实现在astar.cpp或者dijkstra.cpp中分别有一个，具体使用哪一个通过use_dijkstra参数判断，判断的地方在
planner_core.cpp中的GlobalPlanner::initialize函数：
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_dijkstra生成两个不同的Expansion函数，同时将planner_指向新生成的Expansion函数。对于DijkstraExpansion与AStarExpansion种都存在一个calculatePotentials函数。当参数use_dijkstra为true时使用dijkstra.cpp，为false时使用astar.cpp中的。calculatePotentials这个函数的作用是计算“一个点”的可行性，也就是这个点是否可以通过。所以其输出的矩阵大小是跟地图大小是一样的。注意calculatePotentials这个函数并不会直接返回路径，而是一系列点的代价值。类似于下面这种情况：
dijkstra：
astar：
图中灰色的点是在calculatePotentials中被更新过的代价点，关于这些代价点的计算后面单独展开看一下。下一步这些点会被用来计算一条路径。
3.7、提取全局路径
//Traceback* path_maker_;
path_maker_->getPath(potential_array_, start_x, start_y, goal_x, goal_y, path)
提取全局路径规划函数getPath是在Traceback类里面的虚函数：
class Traceback {
public:
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;
该函数由GridPath实例化
class GridPath : public Traceback {
public:
bool getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path);
};
这个函数的作用是使用计算好的potential值生成一条全局规划路径。算法首先将goal所在的点的(x,y)塞到path，然后搜索当前的点的四周的四个临近点，选取这四个临近点的potential的值最小的点min，然后把这点塞到path，然后再将当前点更新为min这点，由于start 点的potential的值是0，所以这是个梯度下降的方法。
以astar举例，算法会从终点开始，沿着3.6图二中的灰色点向起点搜索，将所有代价值最小点连接起来，形成一条路径：
对于dijkstra的结果：
4、将路径转换到世界坐标系下
之前规划出来的每个点是在栅格地图中的坐标，在路径规划完成后这个路径点会被放到世界坐标系下。另外需要注意的是这里的点是不带方向信息的，默认的四元数都是0001。方向信息需要在下一步中给出
// 将path中每个点转换到world下，方向信息还没加入，这里统一设为零，然后依次存储到plan中
for (int i = path.size() -1; i>=0; i--) {
std::pair<float, float> point = path[i];
//convert the plan to world coordinates
// 把map的全局路径转换到world下
double world_x, world_y;
mapToWorld(point.first, point.second, world_x, world_y);
geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = global_frame;
pose.pose.position.x = world_x;
pose.pose.position.y = world_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);
}
这里整个planThread就完成了。可以看到它的作用主要是从map中规划出一条从起点到终点的路径。默认使用的算法包含了dijkstra以及astar两种，具体使用哪一种根据参数use_dijkstra确定。不管是使用哪一个算法，最后的结果都是给出一条路径上连续的栅格点。最后这些点会被转换到全局坐标系下并返回。但是需要注意这里虽然返回了一条路径上的点，但是这些点是没有方向信息的只有坐标信息。
