ROS : Navigation - recovery behavior 源码分析

程序员文章站 2022-05-28 23:30:59

...

ROS : Navigation - recovery behavior 源码分析

概述

什么时候会触发 recovery
recovery 时会干什么

源码流程

概述

recovery 机制是当移动机器人认为自己被卡住时，指导机器人进行一系列的恢复行为。

ROS : Navigation - recovery behavior 源码分析

move_base 节点可以在机器人认为自己被卡住时选择性地执行恢复行为。默认情况下， move_base 节点将采取以下操作来清除空间：

首先，用户指定区域以外的障碍物将从机器人的地图上清除。
接下来，如果可能的话，机器人将进行原地旋转以清除空间。
如果这也失败了，机器人将更积极地清除地图，清除矩形区域之外的所有障碍（在这个区域内机器人可以原地旋转）。这之后将进行另一次原地旋转。
如果所有这些都失败了，机器人将认为它的目标是不可行的，并通知用户它已经中止。
可以使用 recovery_behaviour 参数配置这些恢复行为，并使用 recovery_behavior_enabled 参数禁用这些恢复行为。

导航功能包集合中，有 3 个包与恢复机制有关，这 3 个包中分别定义了 3 个类，都继承了 nav_core 中的接口规范。分别为：

clear_costmap_recovery 实现了清除代价地图的恢复行为

遍历所有层，然后如果某层在可清理列表里就清理掉它的 costmap 。默认可清理列表里只有 obstacle layer ，也就是实时扫描建立的 costmap 。
rotate_recovery 实现了旋转的恢复行为， 360 度旋转来尝试清除空间

转一圈看看有没有路。在 runBehavior 里只需要发指令让小车转一圈，有没有路是 local costmap 在转一圈过程中建立发现的。
move_slow_and_clear 实现了缓慢移动的恢复行为

清理 costmap 然后什么都不管，按照前进速度和转角速度走。从代码里可以看到，根据指定的距离，这是通过先清除全局 costmap 跟局部 costmap 一圈的 obstacle layer 的障碍，然后直接发指令实现的。由于只清除了 obstacle layer ，其实 static layer 的障碍还在，而且这里清不清除跟发指令关系不大，该撞上去的还是会撞的，相当于闭着眼睛往前走。

ROS : Navigation - recovery behavior 源码分析

主要实现两个函数，一个负责初始化，另一个负责执行恢复行为。

什么时候会触发 recovery

move_base 中定义了什么时候会触发 recovery ：

enum RecoveryTrigger
{
  PLANNING_R,  // 全局规划失败，即无法算出 plan
  CONTROLLING_R,  // 局部规划失败，即 local planner 无法利用 local costmap 算出一个 local plan ，从而无法得到速度命令
  OSCILLATION_R  // 机器人在不断抖动，长时间被困在一小片区域
};

具体地：

局部规划失败时间大于 controller_patience_ 时会触发恢复机制

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;
}

全局规划失败时间大于 planner_patience_ 或者失败次数（从 0 开始）大于 max_planning_retries_ 时会触发恢复机制

ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);

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;
}

长时间困在一片小区域时间大于 oscillation_timeout_ 时会触发恢复机制

//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;
}

recovery 时会干什么

recovery 时会执行 recovery behavior 。 MoveBase::loadDefaultRecoveryBehaviors 函数会加载一些 default 的 behavior ，基本上两种类型：

clear_costmap_recovery/ClearCostmapRecovery

会清理从机器人所在位置开始，默认指定 reset_distance_=3 米的矩形范围之外的 costmap 中数据，具体清理 costmap 中的哪个 layer 中的数据呢，默认清理 obstacle layer 的数据。看 ClearCostmapRecovery::runBehavior 。清理时，将地图中内容设置为未知。会把 global 和 local costmap 中指定的 layer 都清理掉。
rotate_recovery/RotateRecovery

就是让机器人旋转 360 度。这样就可以更新周围的障碍物数据。

源码流程

调用：恢复机制是在 MoveBase::loadDefaultRecoveryBehaviors 里被调用的，示例如下：
```
//first, we'll load a recovery behavior to clear the costmap
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);
```
- 首先，创建恢复机制实例
- 然后，初始化恢复机制，即 调用恢复机制的 initialize 函数
- 最后，将该恢复机制加入到恢复机制列表中
move_base 中加载的默认恢复机制有两种类型，恢复机制列表*保存 4 个恢复行为：
- 第一次加载清除代价地图的恢复行为
  
  初始化节点名称是 ~/conservative_reset ，即 /move_base/conservative_reset ，这里最重要的参数是 指定距离机器人中心正方形边长多少米区域外的代价地图恢复为静态地图 ， move_base 中设置 ~/conservative_reset/reset_distance 的默认值为 $3.0$
- 第一次加载旋转的恢复行为
  
  初始化节点名称是 ~/rotate_recovery ，即 /move_base/rotate_recovery
- 第二次加载清除代价地图的恢复行为
  
  初始化节点名称是 ~/aggressive_reset ，即 /move_base/aggressive_reset ，同样的，这里最重要的参数是 指定距离机器人中心正方形边长多少米区域外的代价地图恢复为静态地图 ， move_base 中设置 ~/aggressive_reset/reset_distance 的默认值为 $0.46 * 4 = 1.84$
- 第二次加载旋转的恢复行为
  
  和第一次加载旋转的恢复行为是一致的

使用：在 move_base 主程序里，机器人 CLEARING 状态下进行的，这里会 调用恢复机制的 runBehavior 函数

//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;

ROS : Navigation - recovery behavior 源码分析