ROS : Navigation - recovery behavior 源碼分析

- 概述
- - 什麼時候會觸發 recovery
  - recovery 時會幹什麼
- 源碼流程

概述

recovery 機制是當移動機器人認為自己被卡住時，指導機器人進行一系列的恢複行為。

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 源碼分析

主要實作兩個函數，一個負責初始化，另一個負責執行恢複行為。

virtual void initialize()
virtual void runBehavior() = 0;

什麼時候會觸發 recovery

move_base 中定義了什麼時候會觸發 recovery ：

enum RecoveryTrigger
{
  PLANNING_R,  // 全局規劃失敗，即無法算出 plan
  CONTROLLING_R,  // 局部規劃失敗，即 local planner 無法利用 local costmap 算出一個 local plan ，進而無法得到速度指令
  OSCILLATION_R  // 機器人在不斷抖動，長時間被困在一小片區域
};

MoveBase::planThread 裡，如果發現無法算出最新的 plan ，就會把 move_base 的狀态設定為 CLEARING
在 MoveBase::executeCycle 的循環中，在 CONTROLLING 狀态時，即還未到達目的地并且已經獲得 Plan 的狀态下。如果長時間沒有成功拿到 local planner 算出的速度指令，比如 local planner 無法算出速度（可能是 local costmap 裡有障礙物），就會設定 move_base 狀态為 CLEARING
在震動時，如果沒有恢複，也會設定狀态成 CLEARING

具體地：

局部規劃失敗時間大于 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 3.0 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 0.46 * 4 = 1.84 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 源碼分析