MoveBase源码阅读笔记
1 构造函数 MoveBase
构造函数MoveBase::MoveBase()为该节点的入口，初始化了相关变量，加载recovery behaviors，局部和全局的地图、路径规划器等plugin，以及开启服务和线程等。该函数中定义了多个ros::NodeHandle变量，其作用主要是给出开话题的命名空间和区分私有话题（内部线程间通信），具体可参考ros官网的说明和ros::NodeHandle博客,rosrun move_base move_base后可以看到其节点的话题情况。
ros::NodeHandle private_nh("~");
ros::NodeHandle nh;
构造函数中采用了actionlib给出了可中断、及时反馈状态的服务接口, executeCb()为服务回调函数。对外给出了导航目标点话题为/move_base_simple/goal ，在goalCB()回调函数中给将其转化为as_服务器的MoveBaseActionGoal格式目标，并发布到话题/move_base/goal，其实该话题为就是actionlib服务器的目标话题，从而进入executeCb()回调函数（与官方例程中定义客户端发送目标不同，个人认为原理上就是订阅该目标话题进入回调函数）。
as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
ros::NodeHandle simple_nh("move_base_simple");
goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));
开启了规划线程planThread()函数：
//set up the planner's thread
planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
发布两个服务，规划和清除地图;
//advertise a service for getting a plan
make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);
//advertise a service for clearing the costmaps
clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);
2 目标点action服务回调函数 executeCb
代码如下，得到新的目标位置会唤醒planThread规划全局路径，整体状态的操作在executeCycle函数中执行，在循环中一直执行。
movebase状态机有三种：
- PLANNING 全局路径规划状态
- CONTROLLING 局部控制状态
- CLEARING 故障恢复状态：故障恢复触发的方式也有三种 PLANNING_R（全局路径规划）, CONTROLLING_R（局部路径规划）, OSCILLATION_R（振荡摆动）
// 转为全局坐标系位置
geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);
//we have a goal so start the planner 唤醒planThread线程开始规划
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
planner_goal_ = goal;
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();
ros::NodeHandle n;
while(n.ok())
{
if(as_->isPreemptRequested()){
if(as_->isNewGoalAvailable()){
//if we're active and a new goal is available, we'll accept it, but we won't shut anything down
// 如果目标被抢占且收到新目标，继续在该函数中执行上述操作(个人理解executeCb回调函数没有运行完时收到新目标会继续新的规划和没有终止)
move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();
if(!isQuaternionValid(new_goal.target_pose.pose.orientation)){
as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
return;
}
goal = goalToGlobalFrame(new_goal.target_pose);
//we'll make sure that we reset our state for the next execution cycle
recovery_index_ = 0;
state_ = PLANNING;
//we have a new goal so make sure the planner is awake 唤醒规划进程
lock.lock();
planner_goal_ = goal;
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();
}
else {
//if we've been preempted explicitly we need to shut things down
resetState();
//notify the ActionServer that we've successfully preempted
ROS_DEBUG_NAMED("move_base","Move base preempting the current goal");
as_->setPreempted();
//we'll actually return from execute after preempting 终止
return;
}
}
//we also want to check if we've changed global frames because we need to transform our goal pose
// 检查目标是否改变，若没用继续执行上述操作
if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){
goal = goalToGlobalFrame(goal);
//we want to go back to the planning state for the next execution cycle
recovery_index_ = 0;
state_ = PLANNING;
//we have a new goal so make sure the planner is awake
lock.lock();
planner_goal_ = goal;
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();
}
//for timing that gives real time even in simulation
ros::WallTime start = ros::WallTime::now();
//the real work on pursuing a goal is done here 主要工作在executeCycle()函数中
bool done = executeCycle(goal, global_plan);
//if we're done, then we'll return from execute 完成目标终止回调
if(done)
return;
//check if execution of the goal has completed in some way
ros::WallDuration t_diff = ros::WallTime::now() - start;
ROS_DEBUG_NAMED("move_base","Full control cycle time: %.9f\n", t_diff.toSec());
r.sleep();
//make sure to sleep for the remainder of our cycle time
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());
}
//wake up the planner thread so that it can exit cleanly
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;
3 主循环状态控制函数executeCycle
executeCylcle函数会将新生成的全局路径给到局部控制器，根据状态机对整体状态进行切换，CONTROLLING状态时局部规划器中computeVelocityCommands计算速度。
//we need to be able to publish velocity commands
geometry_msgs::Twist cmd_vel;
//update feedback to correspond to our curent position
geometry_msgs::PoseStamped global_pose;
getRobotPose(global_pose, planner_costmap_ros_);
const geometry_msgs::PoseStamped& current_position = global_pose;
//push the feedback out 反馈goal服务状态
move_base_msgs::MoveBaseFeedback feedback;
feedback.base_position = current_position;
as_->publishFeedback(feedback);
//check to see if we've moved far enough to reset our oscillation timeout 判断是否振荡TODO
if(distance(current_position, oscillation_pose_) >= oscillation_distance_)
{
last_oscillation_reset_ = ros::Time::now();
oscillation_pose_ = current_position;
//if our last recovery was caused by oscillation, we want to reset the recovery index
if(recovery_trigger_ == OSCILLATION_R)
recovery_index_ = 0;
}
//check that the observation buffers for the costmap are current, we don't want to drive blind
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;
}
//if we have a new plan then grab it and give it to the controller
// 新的全局路径生成给局部控制器
if(new_global_plan_){
//make sure to set the new plan flag to false
new_global_plan_ = false;
ROS_DEBUG_NAMED("move_base","Got a new plan...swap pointers");
//do a pointer swap under mutex
std::vector<geometry_msgs::PoseStamped>* temp_plan = controller_plan_;
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
controller_plan_ = latest_plan_;
latest_plan_ = temp_plan;
lock.unlock();
ROS_DEBUG_NAMED("move_base","pointers swapped!");
if(!tc_->setPlan(*controller_plan_)){
//ABORT and SHUTDOWN COSTMAPS
ROS_ERROR("Failed to pass global plan to the controller, aborting.");
resetState();
//disable the planner thread
lock.lock();
runPlanner_ = false;
lock.unlock();
as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller.");
return true;
}
//make sure to reset recovery_index_ since we were able to find a valid plan
if(recovery_trigger_ == PLANNING_R)
recovery_index_ = 0;
}
//the move_base state machine, handles the control logic for navigation状态机导航
switch(state_){
//if we are in a planning state, then we'll attempt to make a plan
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;
//if we're controlling, we'll attempt to find valid velocity commands
case CONTROLLING:
ROS_DEBUG_NAMED("move_base","In controlling state.");
//check to see if we've reached our goal
if(tc_->isGoalReached()){
ROS_DEBUG_NAMED("move_base","Goal reached!");
resetState();
//disable the planner thread
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
runPlanner_ = false;
lock.unlock();
as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
return true;
}
//check for an oscillation condition
if(oscillation_timeout_ > 0.0 &&
last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
{
publishZeroVelocity();
state_ = CLEARING;
recovery_trigger_ = OSCILLATION_R;
}
{
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_ros_->getCostmap()->getMutex()));
// 计算速度
if(tc_->computeVelocityCommands(cmd_vel)){
ROS_DEBUG_NAMED( "move_base", "Got a valid command from the local planner: %.3lf, %.3lf, %.3lf",
cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z );
last_valid_control_ = ros::Time::now();
//make sure that we send the velocity command to the base
vel_pub_.publish(cmd_vel);
if(recovery_trigger_ == CONTROLLING_R)
recovery_index_ = 0;
}
else {
ROS_DEBUG_NAMED("move_base", "The local planner could not find a valid plan.");
ros::Time attempt_end = last_valid_control_ + ros::Duration(controller_patience_);
//check if we've tried to find a valid control for longer than our time limit
if(ros::Time::now() > attempt_end){
//we'll move into our obstacle clearing mode
publishZeroVelocity();
state_ = CLEARING;
recovery_trigger_ = CONTROLLING_R;
}
else{
//otherwise, if we can't find a valid control, we'll go back to planning
last_valid_plan_ = ros::Time::now();
planning_retries_ = 0;
state_ = PLANNING;
publishZeroVelocity();
//enable the planner thread in case it isn't running on a clock
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();
}
}
}
break;
//we'll try to clear out space with any user-provided recovery behaviors
case CLEARING:
ROS_DEBUG_NAMED("move_base","In clearing/recovery state");
//we'll invoke whatever recovery behavior we're currently on if they're enabled
if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()){
ROS_DEBUG_NAMED("move_base_recovery","Executing behavior %u of %zu", recovery_index_, recovery_behaviors_.size());
recovery_behaviors_[recovery_index_]->runBehavior();
//we at least want to give the robot some time to stop oscillating after executing the behavior
last_oscillation_reset_ = ros::Time::now();
//we'll check if the recovery behavior actually worked
ROS_DEBUG_NAMED("move_base_recovery","Going back to planning state");
last_valid_plan_ = ros::Time::now();
planning_retries_ = 0;
state_ = PLANNING;
//update the index of the next recovery behavior that we'll try
recovery_index_++;
}
else{
ROS_DEBUG_NAMED("move_base_recovery","All recovery behaviors have failed, locking the planner and disabling it.");
//disable the planner thread
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
runPlanner_ = false;
lock.unlock();
ROS_DEBUG_NAMED("move_base_recovery","Something should abort after this.");
if(recovery_trigger_ == CONTROLLING_R){
ROS_ERROR("Aborting because a valid control could not be found. Even after executing all recovery behaviors");
as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");
}
else if(recovery_trigger_ == PLANNING_R){
ROS_ERROR("Aborting because a valid plan could not be found. Even after executing all recovery behaviors");
as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors.");
}
else if(recovery_trigger_ == OSCILLATION_R){
ROS_ERROR("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors");
as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");
}
resetState();
return true;
}
break;
default:
ROS_ERROR("This case should never be reached, something is wrong, aborting");
resetState();
//disable the planner thread
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
runPlanner_ = false;
lock.unlock();
as_->setAborted(move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it.");
return true;
}
//we aren't done yet
return false;
4 全局规划线程 planThread
代码如下，其中调用 makePlan函数进行全局规划 planner_->makePlan(start, goal, plan)
ros::NodeHandle n;
ros::Timer timer;
bool wait_for_wake = false;
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
while(n.ok()){
//check if we should run the planner (the mutex is locked) 等待唤醒同步
while(wait_for_wake || !runPlanner_){
//if we should not be running the planner then suspend this thread
ROS_DEBUG_NAMED("move_base_plan_thread","Planner thread is suspending");
planner_cond_.wait(lock);
wait_for_wake = false;
}
ros::Time start_time = ros::Time::now();
//time to plan! get a copy of the goal and unlock the mutex 规划开始
geometry_msgs::PoseStamped temp_goal = planner_goal_;
lock.unlock();
ROS_DEBUG_NAMED("move_base_plan_thread","Planning...");
//run planner
planner_plan_->clear();
bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_); // 全局规划函数
if(gotPlan){ //规划成功
ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size());
//pointer swap the plans under mutex (the controller will pull from latest_plan_)
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; // 新的全局规划路径
//make sure we only start the controller if we still haven't reached the goal
if(runPlanner_)
state_ = CONTROLLING;
if(planner_frequency_ <= 0)
runPlanner_ = false;
lock.unlock();
}
//if we didn't get a plan and we are in the planning state (the robot isn't moving)
// 全局路径规划失败
else if(state_==PLANNING){
ROS_DEBUG_NAMED("move_base_plan_thread","No Plan...");
ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);
//check if we've tried to make a plan for over our time limit or our maximum number of retries
//issue #496: we stop planning when one of the conditions is true, but if max_planning_retries_
//is negative (the default), it is just ignored and we have the same behavior as ever
lock.lock();
planning_retries_++;
// 若规划时间超时或次数超限 则进入 CLEARING状态， recovery 恢复模式
if(runPlanner_ &&
(ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_))){
//we'll move into our obstacle clearing mode
state_ = CLEARING;
runPlanner_ = false; // proper solution for issue #523
publishZeroVelocity();
recovery_trigger_ = PLANNING_R;
}
lock.unlock();
}
//take the mutex for the next iteration
lock.lock();
//setup sleep interface if needed
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;
// 小于要求的规划时间，则sleep多余的时间来唤醒该进程， wait_for_wake为True后下一个循环线程将进入等待
// 状态，定时器函数wakePlanner将唤醒本线程，若planner_frequency=0则只在有新目标时候才唤醒本线程
timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this);
}
}
}