move_base
在浅析amcl_demo的时候，我们已经了解到move_base是一个用于导航控制的框架。它根据全局和局部的代价地图， 进行路径规划并控制机器人按照规划的路径运动和避障。我们已经分析过它的ROS接口，本文中我们将分析它的源码， 具体了解整个框架的运行机制。
1. 源码结构
$ git clone https://github.com/ros-planning/navigation.git
$cd ./navigation/move_base
$ tree .
.
├── cfg
│ └── MoveBase.cfg
├── CHANGELOG.rst
├── CMakeLists.txt
├── include
│ └── move_base
│ └── move_base.h
├── package.xml
├── planner_test.xml
└── src
├── move_base.cpp
└── move_base_node.cpp
4 directories, 8 files
如右侧代码所示，我们通过Git工具把navigation从github上拉下来，并查看包move_base的目录结构。涉及到的文件和目录并没有多少。
其中子目录cfg中只有一个MoveBase.cfg的文件，实际上它是一个python的脚本，用于动态的修改运行节点的参数。与导航控制的实现无关，这里不再赘述。
子目录include中的move_base.h和src目录下的move_base.cpp一起定义和实现了我们要研究的导航框架类MoveBase。而源文件move_base_node.cpp是ROS系统的节点实现， 它实例化了MoveBase，并开启了ROS的消息循环。
CHANGELOG.rst是更新日志，记录了move_base的历次版本的修改内容。package.xml是ROS系统用于描述包的基本信息的文件，其中记录了包的名称、作者信息、以及依赖关系。
文件planner_test.xml实际上是一个launch文件，是一个以PR2机器人为平台测试规划器的demo，对于我们而言没有什么用处。
CMakeLists.txt是CMake的编译指导文件，描述了如何把源文件编译成实际运行的节点move_base。下面是从中截取的一段代码片段，我们可以看到move_base.cpp被编译成为一个库， 而实际的可执行文件move_base则是由move_base_node.cpp生成的。
add_library(move_base src/move_base.cpp)
target_link_libraries(move_base ${Boost_LIBRARIES} ${catkin_LIBRARIES})
add_dependencies(move_base ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_executable(move_base_node src/move_base_node.cpp)
add_dependencies(move_base_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(move_base_node move_base)
set_target_properties(move_base_node PROPERTIES OUTPUT_NAME move_base)
下面的代码是源文件move_base_node.cpp的内容。其代码逻辑很简单，无非是先初始化ROS系统，然后创建tf2的坐标变换对象。接着以tf2对象为参数实例化MoveBase，最后开启ROS自旋。 需要注意的是，在18年7月份发布的1.16版之后，已经将坐标变换库从tf切换到tf2上了，两者的接口形式多少有些不一样，所以早期的规划器还需要相应的做些调整。
#include <move_base/move_base.h>
#include <tf2_ros/transform_listener.h>
int main(int argc, char** argv){
ros::init(argc, argv, "move_base_node");
tf2_ros::Buffer buffer(ros::Duration(10));
tf2_ros::TransformListener tf(buffer);
move_base::MoveBase move_base( buffer );
ros::spin();
return(0);
}
2. MoveBase的构造函数
以下是MoveBase的构造函数，为了方便本文的写作，我将它的语句顺序做了适当的调整，不影响整个系统的运行。如下面的代码片段所示，在一开始对一系列的成员变量赋予了初值：
- tf_是坐标变换TF2的接口对象;
- as_则是Action服务器;
- planner_costmap_ros_是用于全局规划器的代价地图对象;
- controller_costmap_ros_则是局部规划器所用的代价地图对象;
- bgp_loader_是装载全局规划器插件的工具;
- blp_loader_是装载局部规划器插件的工具;
- planner_plan_, latest_plan_ 和controller_plan_是三个记录规划结果的缓存;
- runPlanner_, setup_, p_freq_change_, c_freq_change_, new_global_plan_则是一些控制和反映MoveBase系统状态的布尔变量。
MoveBase::MoveBase(tf2_ros::Buffer& tf) :
tf_(tf), as_(NULL), planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),
recovery_loader_("nav_core", "nav_core::RecoveryBehavior"),
planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false)
{
在构造函数的一开始，定义了两个ROS的句柄，用于获取节点参数，订阅和发布主题。以下面的第13和14行为例，MoveBase从参数服务器中获取了全局规划器和局部规划器的名称， 如果系统中没有定义这些参数，将以默认值"navfn/NavfnROS"和"base_local_planner/TrajectoryPlannerROS"完成初始化工作。还有很多其它参数需要配置，这里不再一一介绍。
ros::NodeHandle private_nh("~");
ros::NodeHandle nh;
std::string global_planner, local_planner;
private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
// 省略其它加载参数的语句
然后，构建了轨迹规划结果缓存planner_plan_, latest_plan_和controller_plan_。并且创建了一个线程planner_thread_用于轨迹规划。
planner_plan_ = new std::vector();
latest_plan_ = new std::vector();
controller_plan_ = new std::vector();
planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
接着，订阅和发布一些主题。
ros::NodeHandle action_nh("move_base");
ros::NodeHandle simple_nh("move_base_simple");
vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
goal_sub_ = simple_nh.subscribe("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));
current_goal_pub_ = private_nh.advertise("current_goal", 0);
action_goal_pub_ = action_nh.advertise("goal", 1);
下面通过模块costmap_2d构建了全局代价地图对象，并通过其成员函数pause()暂停运行，将在必要的功能模块初始化结束之后通过成员接口start()开启。
// 全局代价地图
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
planner_costmap_ros_->pause();
通过在一开始获取的全局规划器名称global_planner构造全局规划器，并用刚刚构建的全局代价地图完成对其的初始化操作。整个过程在一个try-catch语句块中完成，如果出现异常将退出整个系统。
// 全局规划器
try {
planner_ = bgp_loader_.createInstance(global_planner);
planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
exit(1);
}
以类似的套路，MoveBase还构建了局部代价地图和局部规划器的对象。
// 局部代价地图和局部规划器
controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
controller_costmap_ros_->pause();
try {
tc_ = blp_loader_.createInstance(local_planner);
tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
exit(1);
}
构建了必要的功能模块之后，就可以开启代价地图的计算了。
planner_costmap_ros_->start();
controller_costmap_ros_->start();
开启"make_plan"和"clear_costmaps"的服务。
make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);
clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);
下面这两段代码是MoveBase的一些运行特性，涉及到是否使用代价地图，以及恢复行为。
if(shutdown_costmaps_){
ROS_DEBUG_NAMED("move_base","Stopping costmaps initially");
planner_costmap_ros_->stop();
controller_costmap_ros_->stop();
}
if(!loadRecoveryBehaviors(private_nh)){
loadDefaultRecoveryBehaviors();
}
至此，初始化工作已经基本完成了，下面对系统的状态进行初始化，然后构建Action服务器对象as_并开启服务。构建对象as_的时候提供了一个回调函数executeCb，每当有新的Action请求的时候， 都会调用这一回调函数，进行导航控制。
state_ = PLANNING;
recovery_index_ = 0;
recovery_trigger_ = PLANNING_R;
// Action配置
as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
as_->start();
作为一个松耦合的框架，MoveBase还希望能够动态的修改系统运行参数，所以就有了下面这些动态配置器的初始化过程。
dsrv_ = new dynamic_reconfigure::Server(ros::NodeHandle("~"));
dynamic_reconfigure::Server::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
}
3. 业务逻辑入口——executeCb
在构造函数中，构造Action服务器对象as_的时候，为之绑定了一个回调函数executeCb。每当接收到一个Action目标(goal)的时候，都会调用该函数，它可以看作是整个move_base系统的业务逻辑入口。
如下面的代码片段所示，executeCb以goal消息指针为输入参数。在函数的一开始，首先检查目标点的位姿是否合法。只有当合法之后， 才通过MoveBase的成员函数goalToGlobalFram获取目标点在全局坐标系下的坐标。
void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal) {
if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation)){
as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
return;
}
geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);
成功接收到目标点后，就更新系统的运行状态，规划器的目标。因为MoveBase涉及到多个线程，存在一定的并行特性，所以在更新系统状态的时候，需要对其加锁。
//we have a goal so start the planner
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
planner_goal_ = goal;
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();
然后在主题"/move_base/current_goal"上发布当前目标点，同时构建一个局部变量global_plan用于进行规划。
current_goal_pub_.publish(goal);
std::vector global_plan;
构建一个定时器r用于按照指定的控制频率controller_frequency_进行控制。如果当前代价地图是关闭的，将重新打开之。
ros::Rate r(controller_frequency_);
if(shutdown_costmaps_){
ROS_DEBUG_NAMED("move_base","Starting up costmaps that were shut down previously");
planner_costmap_ros_->start();
controller_costmap_ros_->start();
}
记录下最近一次的控制和规划时间，并重置重规划计数。
//we want to make sure that we reset the last time we had a valid plan and control
last_valid_control_ = ros::Time::now();
last_valid_plan_ = ros::Time::now();
last_oscillation_reset_ = ros::Time::now();
planning_retries_ = 0;
接下来，就进入了导航控制的业务循环中。在循环的一开始先通过变量c_freq_change_检查是否需要修改控制频率，并最终将之转换为ros::Rate的频率r。
ros::NodeHandle n;
while (n.ok()) {
if (c_freq_change_) {
ROS_INFO("Setting controller frequency to %.2f", controller_frequency_);
r = ros::Rate(controller_frequency_);
c_freq_change_ = false;
}
然后，查看Action服务器的状态机。如果触发了抢占，需要相应作出处理，并更新as_的状态机。如果是因为接收到新的目标而触发了抢占行为，我们将接受新目标并开始新的规划和控制。 对于其它抢占行为，我们认为取消了Action任务，将挂起退出。
if (as_->isPreemptRequested()) {
if (as_->isNewGoalAvailable()) {
move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();
// 省略目标点检查
goal = goalToGlobalFrame(new_goal.target_pose);
// 省略接收到新目标后的其它必要操作
// 与刚进入函数的操作类似
} else {
resetState();
ROS_DEBUG_NAMED("move_base","Move base preempting the current goal");
as_->setPreempted();
return;
}
}
MoveBase的框架还允许我们在控制过程中更新地图和全局坐标系，下面的代码片段检查到全局坐标系发生改变之后，重新计算目标点在新坐标系下的位姿，并重置规划状态。
if (goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()) {
goal = goalToGlobalFrame(goal);
// 省略其它重置状态的必要操作，与刚进入函数时的操作类似
}
在完成了各种情况的检测之后，MoveBase才开始执行真正的导航控制业务executeCycle。该函数返回一个布尔变量用于标志是否是否完成了Action任务，如果成功完成了任务将退出业务逻辑。 start和t_diff用于计算executeCycle的运行时间。
ros::WallTime start = ros::WallTime::now();
bool done = executeCycle(goal, global_plan);
if (done) return;
ros::WallDuration t_diff = ros::WallTime::now() - start;
ROS_DEBUG_NAMED("move_base","Full control cycle time: %.9f\n", t_diff.toSec());
在业务循环的最后，通过ros::Rate对象消耗掉一个控制周期中剩余的时间，如果计时发生了偏差，将给出警告。
r.sleep();
if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING)
ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec());
}
在业务循环的while语句中，以ros句柄的ok()接口作为终止业务循环的条件。如果因为该条件而终止了业务，意味着move_base节点被杀死了，需要更新Action服务器对象as_的状态。
lock.lock();
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();
//if the node is killed then we'll abort and return
as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed");
return;
}
4. 业务逻辑主体——executeCycle
如下面的代码片段所示，业务主体executeCycle以目标点goal和轨迹点global_plan为参数。实际上从其源码看来，global_plan没有用到。个人猜测global_plan应该是一个输出参数， 用于记录规划出的轨迹。
为了保证执行业务逻辑的过程中，系统的参数不会发生改变，在业务主体的一开始就定义了一个锁对象ecl。在ecl的构造函数中，完成了对信号量configuration_mutex_的加锁操作。 当该函数退出时，局部变量ecl将被自动释放，在它的析构函数中自动的释放信号量。
bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan) {
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
然后通过MoveBase的成员函数getRobotPose获取当前的机器人位姿。
geometry_msgs::PoseStamped global_pose;
getRobotPose(global_pose, planner_costmap_ros_);
const geometry_msgs::PoseStamped& current_position = global_pose;
并发布反馈消息到Action的客户端。
move_base_msgs::MoveBaseFeedback feedback;
feedback.base_position = current_position;
as_->publishFeedback(feedback);
下面的这些代码的结果用于判定机器人是否发生了移动。只有当机器人运动了足够远，才认为机器人移动了，并用last_oscillation_reset_记录下当前时间。如果在未来的某个时间点， 查看last_oscillation_reset_，发现机器人长时间没有有效移动，将会触发recover的操作，尝试修复这一异常状态。
if (distance(current_position, oscillation_pose_) >= oscillation_distance_) {
last_oscillation_reset_ = ros::Time::now();
oscillation_pose_ = current_position;
if(recovery_trigger_ == OSCILLATION_R)
recovery_index_ = 0;
}
controller_costmap_ros_是MoveBase的局部代价地图，通过其接口isCurrent()，我们可以查看代价地图近期是否得到了更新。如果长时间没有更新，意味着机器人失去了周围的感知能力， 此时移动机器人是危险的，所以我们通过publishZeroVelocity控制机器人停下来并退出业务逻辑。
if(!controller_costmap_ros_->isCurrent()){
ROS_WARN("[%s]:Sensor data is out of date, we're not going to allow commanding of the base for safety",ros::this_node::getName().c_str());
publishZeroVelocity();
return false;
}
当我们接收到一个控制计划，需要将之提供给局部规划器来具体执行计划。首先清楚新规划的标记变量new_global_plan_，然后交换controler_plan_和latest_plan_对象。 并在第30行中更新计划到局部规划器中。
if(new_global_plan_){
new_global_plan_ = false;
// 交换规划指针
std::vector* temp_plan = controller_plan_;
boost::unique_lock lock(planner_mutex_);
controller_plan_ = latest_plan_;
latest_plan_ = temp_plan;
lock.unlock();
// 更新局部规划器
if (!tc_->setPlan(*controller_plan_)) {
// 出错,重置系统状态,退出业务逻辑.略.
as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller.");
return true;
}
if(recovery_trigger_ == PLANNING_R)
recovery_index_ = 0;
}
接下来就是MoveBase的状态机。一共有三种状态:PLANNING, CONTROLLING, CLEARING。在状态PLANNING下，全局规划器根据全局代价地图(global costmap)规划运动轨迹， 此时MoveBase将等待planner给出一条路径后切换状态。CONTROLLING状态，则是局部规划器实际控制机器人沿着轨迹运动的状态，如果机器人到达了目标点，将退出业务逻辑并返回true。 CLEARING则是一种异常处理的状态，它调用用户定义的恢复逻辑尝试清除状态空间。
switch(state_) {
// 省略状态机
}
return false;
}
下面是PLANNING状态下的业务，我们没有什么可以做的，只是等待。而规划是全局规划器在线程planner_thread_中进行的，包括MoveBase的状态机都是在这个线程中更新的。 其中的变量runPlanner_是一个用于控制线程planner_thread_进行规划的标志。在等待的过程中，我们很无聊，就打印一些日志Zzzzz...
case PLANNING:
boost::recursive_mutex::scoped_lock lock(planner_mutex_);
runPlanner_ = true;
planner_cond_.notify_one();
ROS_DEBUG_NAMED("move_base","Waiting for plan, in the planning state.");
break;
CONTROLLING状态下的业务就显得比较多了。首先，我们需要判定是否到达了目标点。如果到达了目标点，就重置系统状态，关闭线程planner_thread_停止规划，并发布Action的Result消息， 告知客户端导航目标已经达成。
case CONTROLLING:
if (tc_->isGoalReached()) {
ROS_DEBUG_NAMED("move_base","Goal reached!");
resetState();
// disable the planner thread
boost::unique_lock lock(planner_mutex_);
runPlanner_ = false;
lock.unlock();
// set action result
as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
return true;
}
有些时候，机器人会长时间在一个地方不怎么移动，此时很可能出现了一些异常的状况。我们需要触发一个修复机制，这里就切换到了CLEANRING状态，并标记修复机制的触发原因是OSCILLATION_R。
if(oscillation_timeout_ > 0.0 && last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now()) {
publishZeroVelocity();
state_ = CLEARING;
recovery_trigger_ = OSCILLATION_R;
}
一般情况下，我们都是通过局部规划器获得对机器人的速度控制指令，发布控制消息。如果局部规划器没有完成控制指令的计算，将回到PLANNING状态重新规划路径。如果多次尝试仍然不能完成， 就进入CLEARING状态。
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_ros_->getCostmap()->getMutex()));
if (tc_->computeVelocityCommands(cmd_vel)) {
last_valid_control_ = ros::Time::now();
vel_pub_.publish(cmd_vel);
if(recovery_trigger_ == CONTROLLING_R)
recovery_index_ = 0;
} else {
ros::Time attempt_end = last_valid_control_ + ros::Duration(controller_patience_);
if(ros::Time::now() > attempt_end){
publishZeroVelocity();
state_ = CLEARING;
recovery_trigger_ = CONTROLLING_R;
} else {
last_valid_plan_ = ros::Time::now();
planning_retries_ = 0;
state_ = PLANNING;
publishZeroVelocity();
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();
}
}
状态CLEARING的存在完全是为了消除一些异常，与路径规划和导航控制没有关系，这里不再赘述。下面我们来分析MoveBase的规划线程planner_thread_。
5. 规划线程——planner_thread_
在构造函数中，我们已经看到MoveBase创建了一个线程planner_thread_。这个线程就是拿来完成路径规划的，它的入口函数是planThread。 下面是该函数的代码片段，一开始先定义了一些局部变量，并对信号量planner_mutex_加锁。
void MoveBase::planThread(){
ros::NodeHandle n;
ros::Timer timer;
bool wait_for_wake = false;
boost::unique_lock lock(planner_mutex_);
下面开启了规划线程的超级循环，通过ROS的句柄n来判定move_base是否还活着。在超级循环的一开始，检查标志wait_for_wake和runPlanner_，判定是否需要进行轨迹规划。 如果不需要，就通过planner_cond_等待唤醒。
进入planner_cond_的wait接口的时候将释放锁，此时planner_thread_将进入一种等待的状态。当我们在其它线程中执行了语句planner_cond_.notify_one()时，将把planner_thread_从等待中唤醒， 并从wait接口中返回，与此同时将再次对信号量加锁。
while (n.ok()) {
while (wait_for_wake || !runPlanner_) {
ROS_DEBUG_NAMED("move_base_plan_thread","Planner thread is suspending");
planner_cond_.wait(lock);
wait_for_wake = false;
}
下面是正常的路径规划逻辑。我们用局部变量start_time记录下开始规划的时间，temp_goal记录当前的目标点后，释放锁。
ros::Time start_time = ros::Time::now();
geometry_msgs::PoseStamped temp_goal = planner_goal_;
lock.unlock();
ROS_DEBUG_NAMED("move_base_plan_thread","Planning...");
然后，清空规划器的规划路径。并调用成员函数makePlan使用全局规划器planner_规划轨迹，规划出来的轨迹将保存在planner_plan_所指的容器中。
planner_plan_->clear();
bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
如果我们成功的计算得到了一个轨迹，就用latest_plan_记录下最新得到的轨迹，并更新系统的状态为CONTROLLING。
if (gotPlan) {
std::vector<geometry_msgs::PoseStamped>* temp_plan = planner_plan_;
lock.lock();
planner_plan_ = latest_plan_;
latest_plan_ = temp_plan;
last_valid_plan_ = ros::Time::now();
planning_retries_ = 0;
new_global_plan_ = true;
if(runPlanner_)
state_ = CONTROLLING;
if(planner_frequency_ <= 0)
runPlanner_ = false;
lock.unlock();
}
否则，意味着我们没有得到一条可行的路径。此时我们检测系统的状态，如果它仍然处于PLANNING的状态，说明出了一些问题，如果多次尝试仍然不能解决，将进入CLEARING的状态。
else if(state_==PLANNING){
ROS_DEBUG_NAMED("move_base_plan_thread","No Plan...");
ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);
lock.lock();
planning_retries_++;
if(runPlanner_ && (ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_))) {
state_ = CLEARING;
runPlanner_ = false;
publishZeroVelocity();
recovery_trigger_ = PLANNING_R;
}
lock.unlock();
}
最后为了下次迭代，我们重新对信号量加锁，并通过一个计时器控制线程进入等待状态来消耗掉规划周期中剩余的时间。
lock.lock();
if (planner_frequency_ > 0) {
ros::Duration sleep_time = (start_time + ros::Duration(1.0/planner_frequency_)) - ros::Time::now();
if (sleep_time > ros::Duration(0.0)){
wait_for_wake = true;
timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this);
}
}
}
6. 完
本文中，我们解读了move_base的源码。发现它只是一个框架，定义了接口来计算全局和局部的代价地图，并进行全局和局部的规划和控制。