ros-navigation recovery_beheviors机制

一.总览

当移动机器人认为自己被卡住时，指导机器人进行一系列的恢复行为。
navigation包中的恢复行为主要包括三种：

• clean_costmap_recovery:先清理周围一定范围以外的costmap
• rotate_recovery:旋转360度
• move_slow_and_clean:缓慢移动

二.recovery的触发条件

都是在move_base.cpp里进行的主流程的控制
三种触发条件，如下：

  enum RecoveryTrigger
  {
   
   
    PLANNING_R,  // 全局规划失败
    CONTROLLING_R, // 局部轨迹规划失败
    OSCILLATION_R // 长时间在小区域运动（机器人震荡）
  };

三.恢复行为

1.clean_costmap_recovery

先清理周围一定范围以外的costmap

会清理从机器人所在位置开始，指定 reset_distance_米的矩形范围之外的 costmap 中数据， 默认清理 obstacle layer 的数据 （取决于layer_names配置）。
清理时，将地图中内容设置为未知。
清除指定的 layer 层，affected_maps参数设置是否清除global/local/all。

该模块代码注释说明：

#include <boost/pointer_cast.hpp>
#include <clear_costmap_recovery/clear_costmap_recovery.h>
#include <pluginlib/class_list_macros.h>
#include <vector>

// register this planner as a RecoveryBehavior plugin
// 该插件将costmap中给定半径（reset_distance_默认值3.0）范围之内的区域进行清理，即将栅格状态更新为未知信息
PLUGINLIB_EXPORT_CLASS(clear_costmap_recovery::ClearCostmapRecovery,
                       nav_core::RecoveryBehavior)

using costmap_2d::NO_INFORMATION;

namespace clear_costmap_recovery {
   
   
ClearCostmapRecovery::ClearCostmapRecovery()
    : global_costmap_(NULL), local_costmap_(NULL), tf_(NULL),
      initialized_(false) {
   
   }

void ClearCostmapRecovery::initialize(std::string name, tf2_ros::Buffer *tf,
                                      costmap_2d::Costmap2DROS *global_costmap,
                                      costmap_2d::Costmap2DROS *local_costmap) {
   
   
  if (!initialized_) {
   
   
    name_ = name;
    tf_ = tf;
    global_costmap_ = global_costmap;
    local_costmap_ = local_costmap;

    // 参数服务器中获得参数
    ros::NodeHandle private_nh("~/" + name_);
    // 代价地图清除的距离　以该范围为边长　画一个正方形
    //从地图上清除用户该区域以外的障碍物
    private_nh.param("reset_distance", reset_distance_, 3.0);
    private_nh.param("invert_area_to_clear", invert_area_to_clear_, false);
    private_nh.param("force_updating", force_updating_, false);
    private_nh.param("affected_maps", affected_maps_, std::string("both"));
    // 地图清理的范围　local global 或者两者都包括
    if (affected_maps_ != "local" && affected_maps_ != "global" &&
        affected_maps_ != "both") {
   
   
      ROS_WARN("Wrong value for affected_maps parameter: '%s'; valid values "
               "are 'local', 'global' or 'both'; "
               "defaulting to 'both'",
               affected_maps_.c_str());
      affected_maps_ = "both";
    }
    　 // 清理的地图层　默认障碍物层
        std::vector<std::string>
            clearable_layers_default, clearable_layers;
    clearable_layers_default.push_back(std::string("obstacles"));
    private_nh.param("layer_names", clearable_layers, clearable_layers_default);

    for (unsigned i = 0; i < clearable_layers.size(); i++) {
   
   
      ROS_INFO("Recovery behavior will clear layer '%s'",
               clearable_layers[i].c_str());
      clearable_layers_.insert(clearable_layers[i]);
    }

    initialized_ = true;
  } else {
   
   
    ROS_ERROR(
        "You should not call initialize twice on this object, doing nothing");
  }
}

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

  if (!invert_area_to_clear_) {
   
   
    ROS_WARN("Clearing %s costmap%s outside a square (%.2fm) large centered on "
             "the robot.",
             affected_maps_.c_str(), affected_maps_ == "both" ? "s" : "",
             reset_distance_);
  } else {
   
   
    ROS_WARN("Clearing %s costmap%s inside a square (%.2fm) large centered on "
             "the robot.",
             affected_maps_.c_str(), affected_maps_ == "both" ? "s" : "",
             reset_distance_);
  }

  ros::WallTime t0 = ros::WallTime::now();
  if (affected_maps_ == "global" || affected_maps_ == "both") {
   
   
    clear(global_costmap_);

    if (force_updating_)
      global_costmap_->updateMap();

    ROS_DEBUG("Global costmap cleared in %fs",
              (ros::WallTime::now() - t0).toSec());
  }

  t0 = ros::WallTime::now();
  if (affected_maps_ == "local" || affected_maps_ == "both") {
   
   
    clear(local_costmap_);

    if (force_updating_)
      local_costmap_->updateMap(