【ROS-Navigation】Recovery Behavior恢复行为源码解读

记录学习阅读ROS Navigation源码的理解，本文为RecoveryBehavior恢复行为源码学习记录，以文字总结、绘制结构图说明、代码注释为主。仍在学习过程中，有错误欢迎指正，共同进步。

RecoveryBehavior用来应对导航过程中各模块的故障，当全局规划故障、局部规划故障、震荡时都会进入到恢复行为中，它先清理周围一定范围以外的costmap（障碍层），接下来重新执行规划，若不奏效，则旋转180度，再执行规划。交替两次后，已转过360度，若还是没能排除故障，则恢复行为失败，关闭Movebase规划器。

【示意图】

【相关文件】

• clear_costmap_recovery/src/clear_costmap_recovery.cpp
• rotate_recovery/src/rotate_recovery.cpp

本篇记录对ClearCostmapRecovery类和RotateRecovery类的阅读和理解。

【代码分析】

1. Movebase中的恢复行为逻辑

在Movebase的全局规划和局部规划模块中，当规划失败时都会进入到恢复行为中，这里来关注一下恢复行为在Movebase中的调用逻辑。

整体概括可以看上面的示意图。

<1> 载入恢复行为

在Movebase的构造函数中，可以从参数服务器上加载恢复行为列表，当参数服务器上不存在时，调用loadDefaultRecoveryBehaviors函数加载默认的恢复行为列表，这里来看一下这个函数。

    if(!loadRecoveryBehaviors(private_nh)){
      loadDefaultRecoveryBehaviors();
    }

在loadDefaultRecoveryBehaviors函数中，按顺序加载了①“cons_clear”、②“rotate”、③“ags_clear”、④“rotate”。

实际上，①/③都是ClearCostmapRecovery类，负责清理机器人一定范围外的costmap上的数据，区别在于③保留的范围更小，清理的更多。恢复行为是按照列表顺序调用的，当①、②无效后，才会执行③，清理更多区域。

②/④都是RotateRecovery类，使机器人原地旋转。

  void MoveBase::loadDefaultRecoveryBehaviors(){
    recovery_behaviors_.clear();
    try{
      //we need to set some parameters based on what's been passed in to us to maintain backwards compatibility
      ros::NodeHandle n("~");
      n.setParam("conservative_reset/reset_distance", conservative_reset_dist_);
      n.setParam("aggressive_reset/reset_distance", circumscribed_radius_ * 4);

      //加载清理代价地图的恢复行为
      boost::shared_ptr<nav_core::RecoveryBehavior> cons_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
      cons_clear->initialize("conservative_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
      recovery_behaviors_.push_back(cons_clear);

      //加载原地自转的恢复行为
      boost::shared_ptr<nav_core::RecoveryBehavior> rotate(recovery_loader_.createInstance("rotate_recovery/RotateRecovery"));
      if(clearing_rotation_allowed_){
        rotate->initialize("rotate_recovery", &tf_, planner_costmap_ros_, controller_costmap_ros_);
        recovery_behaviors_.push_back(rotate);
      }

      //next, we'll load a recovery behavior that will do an aggressive reset of the costmap
      boost::shared_ptr<nav_core::RecoveryBehavior> ags_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
      ags_clear->initialize("aggressive_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
      recovery_behaviors_.push_back(ags_clear);

      //we'll rotate in-place one more time
      if(clearing_rotation_allowed_)
        recovery_behaviors_.push_back(rotate);
    }
    catch(pluginlib::PluginlibException& ex){
      ROS_FATAL("Failed to load a plugin. This should not happen on default recovery behaviors. Error: %s", ex.what());
    }

    return;
  }

<2> 恢复行为的使用场景

Movebase有三个状态state_：

• PLANNING：全局规划中，尚未得到全局路径
• CONTROLLING：全局规划完成，进入局部规划
• CLEARING：恢复行为

当①全局规划失败、②机器人震荡、③局部规划失败时会进入到恢复行为。

① 全局规划失败

这里截取全局规划线程的一部分代码分析。

全局规划线程有一个外层的循环，当wait_for_wake为false、runPlanner_为真时才会越过内层第一个循环，进行全局规划，当全局规划成功，state_设置为CONTROLLING。
当全局规划失败时，重新进行循环，尝试规划，当失败次数太多/超时，设置state_为CLEARING，并发布0速，runPlanner_置为假，全局规划线程将进入第一个内层循环，不向下进行。

简言之，当全局规划失败到达上限，进入恢复行为，否则线程内一直循环全局规划。

	/********略过********/
    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;
      }
	/********略过********/
      bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
      if(gotPlan){
		/********略过********/
        if(runPlanner_) 
          state_ = CONTROLLING;
		/********略过********/
      }
      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();
      }

② 机器人震荡

这里截取executeCycle函数的一部分。

当进行到局部规划时，如果state_还是PLANNING即全局规划还没得出结果，继续在全局规划线程进行规划。

若state_为CONTROLLING，即全局规划已完成，准备进入到局部规划生成速度控制，这时候，检查上一次记录震荡的时刻到现在的时间是否超过限制，若超过，则发布0速，设置state_为CLEARING，进入恢复行为。

那么上一次记录震荡的时刻是什么时候呢？实际上从开始规划即记录这个时刻，当运动离开其一定范围外，更新时刻。见下面第一个if结构。

	/********略过********/
    if(distance(current_position, oscillation_pose_) >= oscillation_distance_)
    {
      //把最新的振荡重置设置为当前时间
      last_oscillation_reset_ = ros::Time::now();

      //振荡位姿设为当前姿态
      oscillation_pose_ = current_position;

      //如果我们上一次的恢复行为是OSCILLATION_R由振荡引起，我们就重新设置恢复行为的索引
      if(recovery_trigger_ == OSCILLATION_R)
        recovery_index_ = 0;
    }

	/********略过********/
    switch(state_){
      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;

      case CONTROLLING:
        ROS_DEBUG_NAMED("move_base","In controlling state.");
        //查询是否到达终点，如果到达终点，结束
        if(tc_->isGoalReached()){
		/********略过********/
          return true;
        }
        //如果未到达终点，检查是否处于振荡状态
        if(oscillation_timeout_ > 0.0 &&
            last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
        {
          //如果振荡状态超时了，发布0速度
          publishZeroVelocity();
          //MoveBase状态置为恢复行为
          state_ = CLEARING;
          //恢复行为触发器置为，长时间困在一片小区域
          recovery_trigger_ = OSCILLATION_R;
        }

③ 局部规划失败

继续向下看executeCycle函数。当state_为CONTROLLING，而调用computeVelocityCommands函数进行局部规划失败时，停下，若用时未超限，再次回到全局规划；若超限，设置state_为CLEARING，进入恢复行为。

        {
         boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_ros_->getCostmap()->getMutex()));
        //局部规划器实例tc_被传入了全局规划后，调用computeVelocityCommands函数计算速度存储在cmd_vel中
        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();
          //向底盘发送速度控制消息，一个循环只发一次速度命令
          vel_pub_.publish(cmd_vel);
          //如果恢复行为触发器值是局部规划失败，把索引置0
          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_);

          //若局部规划用时超过限制
          if(ros::Time::now() > attempt_end){
            //发布0速度，进入恢复行为，触发器置为局部规划失败
            publishZeroVelocity();
            state_ = CLEARING;
            recovery_trigger_ = CONTROLLING_R;
          }
          //若局部规划用时没超过限制
          else{
            //发布0速度，在机器人当前位置再次回到全局规划
            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();
          }
        }
        }

        break;

<3> 恢复行为做什么？

当state_为CLEARING，进入恢复行为时，它将调用列表中的恢复行为（runBehavior函数），进行地图清理或旋转，并将state_设置为PLANNING，下个循环将回到全局规划，重新进行。
下次若再进入恢复行为，则调用列表中的下一个，以此类推。

      //如果全局规划失败，进入了恢复行为状态，我们尝试去用用户提供的恢复行为去清除空间
      case CLEARING:
        ROS_DEBUG_NAMED("move_base","In clearing/recovery state");
        //如果允许使用恢复行为，且恢复行为索引值小于恢复行为数组的大小
        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());
          //开始恢复行为，在executeCycle的循环中一次次迭代恢复行为
          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_++;
        }

<4> 索引的恢复

我们可以发现，当从全局规划失败/震荡/局部规划失败三个入口进入到恢复行为时，recovery_trigger_会记录PLANNING_R、OSCILLATION_R、CONTROLLING_R，作为“故障原因”。

发生一个故障时，恢复行为会通过索引的自增，不断尝试用清理、自转、清理、自转来排除故障。当这个故障解决时，索引就恢复，以免下一次出现故障时由于索引未清零无法从头开始排除故障。

这里的策略是，在“全局规划成功”/“离开震荡位置一段距离”/“局部规划成功”的位置检查recovery_trigger_，若正好对应PLANNING_R/OSCILLATION_R/CONTROLLING_R，则说明对应的上一次的故障被解决，索引清零，以等待下一次故障时再排除故障。

当然，在一些特殊情况发生时，比如规划过程中收到新目标等，索引同样将被恢复。

2. ClearCostmapRecovery类

这里我们就主要从Movebase中调用的runBehavior函数开始说起，它全局和局部的costmap调用了clear函数。

void ClearCostmapRecovery::runBehavior(){
  if(!initialized_){
    ROS_ERROR("This object must be initialized before runBehavior is called");
    return;
  }

  if(global_costmap_ == NULL || local_costmap_ == NULL){
    ROS_ERROR("The costmaps passed to the ClearCostmapRecovery object cannot be NULL. Doing nothing.");
    return;
  }
  ROS_WARN("Clearing costmap to unstuck robot (%fm).", reset_distance_);
  clear(global_costmap_);
  clear(local_costmap_);
}

来看clear函数，这里获取了costmap的各个地图层，在for循环中不断寻找clearable_layers_中是否有各地图层的名字，若有，则对其调用clearMap、进行清理。

实际上，clearable_layers_在initialize函数中默认设置为ObstacleLayer，所以默认是只清理障碍层的。

void ClearCostmapRecovery::clear(costmap_2d::Costmap2DROS* costmap){
  std::vector<boost::shared_ptr<costmap_2d::Layer> >* plugins = costmap->getLayeredCostmap()->getPlugins();

  tf::Stamped<tf::Pose> pose;

  if(!costmap->getRobotPose(pose)){
    ROS_ERROR("Cannot clear map because pose cannot be retrieved");
    return;
  }

  double x = pose.getOrigin().x();
  double y = pose.getOrigin().y();

  for (std::vector<boost::shared_ptr<costmap_2d::Layer> >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp) 
  {
    boost::shared_ptr<costmap_2d::Layer> plugin = *pluginp;
    std::string name = plugin->getName();
    int slash = name.rfind('/');
    if( slash != std::string::npos ){
        name = name.substr(slash+1);
    }

    if(clearable_layers_.count(name)!=0){
      boost::shared_ptr<costmap_2d::CostmapLayer> costmap;
      costmap = boost::static_pointer_cast<costmap_2d::CostmapLayer>(plugin);
      clearMap(costmap, x, y);
    }
  }
}

clearMap函数就执行真正的清理工作了，它保留机器人周围以reset_distance_为边长的矩形区域，将其余区域的cell都设置为未知。

void ClearCostmapRecovery::clearMap(boost::shared_ptr<costmap_2d::CostmapLayer> costmap, 
                                        double pose_x, double pose_y){
  boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
 
  double start_point_x = pose_x - reset_distance_ / 2;
  double start_point_y = pose_y - reset_distance_ / 2;
  double end_point_x = start_point_x + reset_distance_;
  double end_point_y = start_point_y + reset_distance_;

  int start_x, start_y, end_x, end_y;
  costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
  costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);

  unsigned char* grid = costmap->getCharMap();
  for(int x=0; x<(int)costmap->getSizeInCellsX(); x++){
    bool xrange = x>start_x && x<end_x;
                   
    for(int y=0; y<(int)costmap->getSizeInCellsY(); y++){
      if(xrange && y>start_y && y<end_y)
        continue;
      int index = costmap->getIndex(x,y);
      if(grid[index]!=NO_INFORMATION){
        grid[index] = NO_INFORMATION;
      }
    }
  }

  double ox = costmap->getOriginX(), oy = costmap->getOriginY();
  double width = costmap->getSizeInMetersX(), height = costmap->getSizeInMetersY();
  costmap->addExtraBounds(ox, oy, ox + width, oy + height);
  return;
}

3. RotateRecovery类

这个函数使机器人在原地自转180度。

void RotateRecovery::runBehavior(){
  if(!initialized_){
    ROS_ERROR("This object must be initialized before runBehavior is called");
    return;
  }

  if(global_costmap_ == NULL || local_costmap_ == NULL){
    ROS_ERROR("The costmaps passed to the RotateRecovery object cannot be NULL. Doing nothing.");
    return;
  }
  ROS_WARN("Rotate recovery behavior started.");

  ros::Rate r(frequency_);
  ros::NodeHandle n;
  ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);

  tf::Stamped<tf::Pose> global_pose;
  local_costmap_->getRobotPose(global_pose);

  double current_angle = -1.0 * M_PI;

略过前面，这里的目标是转过180度，将当前角度与目标180的偏差记录在dist_left中，并通过这个偏差产生角速度的控制量，具体为vel = sqrt(2 * acc_lim_th_ * dist_left)，并且要确保这个速度在角速度阈值之内，发布这个角速度，并循环检测偏差是否满足tolerance_范围，当符合时，退出。

  bool got_180 = false;

  double start_offset = 0 - angles::normalize_angle(tf::getYaw(global_pose.getRotation()));
  while(n.ok()){
    local_costmap_->getRobotPose(global_pose);

    double norm_angle = angles::normalize_angle(tf::getYaw(global_pose.getRotation()));
    current_angle = angles::normalize_angle(norm_angle + start_offset);

    //compute the distance left to rotate
    double dist_left = M_PI - current_angle;

    double x = global_pose.getOrigin().x(), y = global_pose.getOrigin().y();

    //check if that velocity is legal by forward simulating
    double sim_angle = 0.0;
    while(sim_angle < dist_left){
      double theta = tf::getYaw(global_pose.getRotation()) + sim_angle;

      //make sure that the point is legal, if it isn't... we'll abort
      double footprint_cost = world_model_->footprintCost(x, y, theta, local_costmap_->getRobotFootprint(), 0.0, 0.0);
      if(footprint_cost < 0.0){
        ROS_ERROR("Rotate recovery can't rotate in place because there is a potential collision. Cost: %.2f", footprint_cost);
        return;
      }

      sim_angle += sim_granularity_;
    }

    //compute the velocity that will let us stop by the time we reach the goal
    double vel = sqrt(2 * acc_lim_th_ * dist_left);

    //make sure that this velocity falls within the specified limits
    vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_);

    geometry_msgs::Twist cmd_vel;
    cmd_vel.linear.x = 0.0;
    cmd_vel.linear.y = 0.0;
    cmd_vel.angular.z = vel;

    vel_pub.publish(cmd_vel);

    //makes sure that we won't decide we're done right after we start
    if(current_angle < 0.0)
      got_180 = true;

    //if we're done with our in-place rotation... then return
    if(got_180 && current_angle >= (0.0 - tolerance_))
      return;

    r.sleep();
  }
}

中间对sim_angle进行判断的if结构，实际上是在模拟机器人自转过程中足迹的变化，一旦发现旋转过程中足迹遇障，立刻停止。