这里我们首先梳理一下,从发布导航目标后到机器人导航到达目标点这个过程中,move_base内部是如何运行的。
进入MoveBase类中,重点关注3个函数MoveBase::planThread
,MoveBase::executeCb
和MoveBase::executeCycle
发布的导航目标首先进入MoveBase::executeCb回调函数中,
第一部分关键代码:
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
planner_goal_ = goal;
runPlanner_ = true;
planner_cond_.notify_one(); // planThread阻塞结束 开始执行规划
lock.unlock();
在这里runPlanner_ 标志置位,同时触发条件变量planner_cond_, 这时把目光转移到MoveBase::planThread线程中,可以看到MoveBase::planThread线程刚开始运行时,是阻塞在下面代码里的planner_cond_.wait中的:
void MoveBase::planThread(){
...
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;
}
...
}
}
planner_cond_触发后,阻塞结束跳出 while(wait_for_wake || !runPlanner_){} 开始继续执行,
接下来拿到在MoveBase::executeCb回调中设置的planner_goal_作为导航目标,然后执行makePlan,这里在这里完成全局路径搜索,搜索结果放置于planner_plan_,它的类型是std::vector<geometry_msgs::PoseStamped>*。
//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_);
接着,如果搜索成功的话,planner_plan_会赋值给latest_plan_,runPlanner_在设置目标点后触发的回调函数中被设置为true,因此状态转为CONTROLLING,意味着开始执行控制,代码如下。
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;
ROS_DEBUG_NAMED("move_base_plan_thread","Generated a plan from the base_global_planner");
//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();
}
接着回到MoveBase::executeCb回调函数中,通过executeCycle
函数对全局路径搜索得到的路径执行跟踪,
bool done = executeCycle(goal, global_plan);
在executeCycle函数中,首先通过tf获取实时位姿:
geometry_msgs::PoseStamped global_pose;
getRobotPose(global_pose, planner_costmap_ros_);
const geometry_msgs::PoseStamped& current_position = global_pose;
接着,new_global_plan_ == true,于是执行:
if(new_global_plan_){
//make sure to set the new plan flag to false
new_global_plan_ = false;
//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();
if(!tc_->setPlan(*controller_plan_)){
...略
}
}
可以看到,全局规划的结果latest_plan_赋值给了controller_plan_,然后,将全局规划的路径传给了局部规划模块tc_->setPlan(*controller_plan_)
那么,局部规划模块便会依据这个全局路径以及反馈的机器人实时位姿,求解出速度,细节见---------------------------------------
接着进入到最核心的一段代码中,重点关注CONTROLLING状态时的操作:
switch(state_){
//if we are in a planning state, then we'll attempt to make a plan
case PLANNING:
...略
break;
//if we're controlling, we'll attempt to find valid velocity commands
case CONTROLLING:
//check to see if we've reached our goal
if(tc_->isGoalReached()){
...略
}
//check for an oscillation condition
if(oscillation_timeout_ > 0.0 &&
last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
{
...略
}
{
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 {
...略
}
}
break;
//we'll try to clear out space with any user-provided recovery behaviors
case CLEARING:
...略
break;
default:
...略
}
这里的核心就是局部规划器求解出速度 tc_->computeVelocityCommands(cmd_vel)
,并将速度发布出去。