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();

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

/*
获取了costmap的各个地图层，在for循环中不断寻找clearable_layers_中是否有各地图层的名字，
若有，则对其调用clearMap、进行清理。
*/
void ClearCostmapRecovery::clear(costmap_2d::Costmap2DROS *costmap) {
  std::vector<boost::shared_ptr<costmap_2d::Layer>> *plugins =
      costmap->getLayeredCostmap()->getPlugins();

  geometry_msgs::PoseStamped pose;

  // 获得机器人全局位姿
  if (!costmap->getRobotPose(pose)) {
    ROS_ERROR("Cannot clear map because pose cannot be retrieved");
    return;
  }

  double x = pose.pose.position.x;
  double y = pose.pose.position.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('/');
    // 如果找到匹配的
    // npos是一个常数，用来表示不存在的位置,string::npos代表字符串到头了结束了
    if (slash != std::string::npos) {
      name = name.substr(slash + 1);
    }

    if (clearable_layers_.count(name) != 0) {

      // 检查该layer是否来自costmap_2d::CostmapLayer
      if (!dynamic_cast<costmap_2d::CostmapLayer *>(plugin.get())) {
        ROS_ERROR_STREAM("Layer "
                         << name
                         << " is not derived from costmap_2d::CostmapLayer");
        continue;
      }

      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()));

  // 以当前位置为中心　reset_distance_为边长的正方形区域
  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); // meter --> pixel
  costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);
  // 将特定栅格更新为未知信息
  costmap->clearArea(start_x, start_y, end_x, end_y, invert_area_to_clear_);

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

}; // namespace clear_costmap_recovery

2.rotate_recovery

旋转360度，更新周围的障碍物信息；

该模块代码注释说明：

#include <algorithm>
#include <angles/angles.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Twist.h>
#include <nav_core/parameter_magic.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <rotate_recovery/rotate_recovery.h>
#include <string>
#include <tf2/utils.h>

// register this planner as a RecoveryBehavior plugin
PLUGINLIB_EXPORT_CLASS(rotate_recovery::RotateRecovery,
                       nav_core::RecoveryBehavior)

namespace rotate_recovery {
RotateRecovery::RotateRecovery()
    : local_costmap_(NULL), initialized_(false), world_model_(NULL) {}

void RotateRecovery::initialize(std::string name, tf2_ros::Buffer *,
                                costmap_2d::Costmap2DROS *,
                                costmap_2d::Costmap2DROS *local_costmap) {
  if (!initialized_) {
    local_costmap_ = local_costmap;

    // get some parameters from the parameter server
    ros::NodeHandle private_nh("~/" + name);
    ros::NodeHandle blp_nh("~/TrajectoryPlannerROS");

    // 默认会模拟仿真每一度(degree)旋转的情况
    private_nh.param("sim_granularity", sim_granularity_, 0.017);
    private_nh.param("frequency", frequency_, 20.0);

    acc_lim_th_ = nav_core::loadParameterWithDeprecation(
        blp_nh, "acc_lim_theta", "acc_lim_th", 3.2);
    max_rotational_vel_ = nav_core::loadParameterWithDeprecation(
        blp_nh, "max_vel_theta", "max_rotational_vel", 1.0);
    min_rotational_vel_ = nav_core::loadParameterWithDeprecation(
        blp_nh, "min_in_place_vel_theta", "min_in_place_rotational_vel", 0.4);
    blp_nh.param("yaw_goal_tolerance", tolerance_, 0.10);

    world_model_ =
        new base_local_planner::CostmapModel(*local_costmap_->getCostmap());

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

RotateRecovery::~RotateRecovery() { delete world_model_; }

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

  if (local_costmap_ == NULL) {
    ROS_ERROR("The costmap 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);

  //　获取当前位姿　朝向
  geometry_msgs::PoseStamped global_pose;
  local_costmap_->getRobotPose(global_pose);

  double current_angle = tf2::getYaw(global_pose.pose.orientation);
  // step 1 : 恢复前的位姿朝向
  double start_angle = current_angle;

  bool got_180 = false;

  while (n.ok() &&
         (!got_180 || std::fabs(angles::shortest_angular_distance(
                          current_angle, start_angle)) > tolerance_)) {
    // shortest_angular_distance调用了angles包，获取两角度的最小角度差
    // step 2 : while循环中更新当前角度
    local_costmap_->getRobotPose(global_pose);
    current_angle = tf2::getYaw(global_pose.pose.orientation);

    double dist_left; // 剩余向左旋转的距离

    // step 3: 更新dist_left
    if (!got_180) // case 1 : 如果没有旋转到半圈
    {
      // 如果没有转到180度，还要旋转 distance_to_180度 + 半圈
      double distance_to_180 = std::fabs(
          angles::shortest_angular_distance(current_angle, start_angle + M_PI));
      dist_left = M_PI + distance_to_180;
      // 到了误差允许范围
      if (distance_to_180 < tolerance_) {
        got_180 = true;
      }
    } else // case 2:  旋转有半圈了
    {
      dist_left = std::fabs(
          angles::shortest_angular_distance(current_angle, start_angle));
    }

    double x = global_pose.pose.position.x, y = global_pose.pose.position.y;

    // step 4: 检查该速度下的推算位姿是否有碰撞可能
    double sim_angle = 0.0;
    while (sim_angle < dist_left) {
      double theta = current_angle + sim_angle;

      // 确保这个位姿没有碰撞风险，不然会停止
      // 获取该位姿下的代价值，若<0　则会碰撞
      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_;
    }

    // step 5: 计算下发速度，让机器人可以在到达终点时停止
    double vel = sqrt(2 * acc_lim_th_ * dist_left);

    // step 6: 确保下发旋转速度满足速度限制要求
    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);

    r.sleep();
  }
}
}; // namespace rotate_recovery

3.move_slow_and_clean

缓慢移动一段距离

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

#include <costmap_2d/obstacle_layer.h>
#include <move_slow_and_clear/move_slow_and_clear.h>
#include <pluginlib/class_list_macros.h>

PLUGINLIB_EXPORT_CLASS(move_slow_and_clear::MoveSlowAndClear,
                       nav_core::RecoveryBehavior)

namespace move_slow_and_clear {
MoveSlowAndClear::MoveSlowAndClear()
    : global_costmap_(NULL), local_costmap_(NULL), initialized_(false),
      remove_limit_thread_(NULL), limit_set_(false) {}

MoveSlowAndClear::~MoveSlowAndClear() { delete remove_limit_thread_; }

void MoveSlowAndClear::initialize(std::string n, tf2_ros::Buffer *tf,
                                  costmap_2d::Costmap2DROS *global_costmap,
                                  costmap_2d::Costmap2DROS *local_costmap) {
  global_costmap_ = global_costmap;
  local_costmap_ = local_costmap;

  ros::NodeHandle private_nh_("~/" + n);
  // 机器人清除范围
  private_nh_.param("clearing_distance", clearing_distance_, 0.5);
  // 该修复行为时候限定的线速度
  private_nh_.param("limited_trans_speed", limited_trans_speed_, 0.25);
  // 该修复行为时候限定的旋转速度
  private_nh_.param("limited_rot_speed", limited_rot_speed_, 0.45);
  // limited_distance_*limited_distance_ 允许机器人运动的最大距离
  private_nh_.param("limited_distance", limited_distance_, 0.3);

  private_nh_.param("max_trans_param_name", max_trans_param_name_,
                    std::string("max_trans_vel"));
  private_nh_.param("max_rot_param_name", max_rot_param_name_,
                    std::string("max_rot_vel"));

  std::string planner_namespace;
  private_nh_.param("planner_namespace", planner_namespace,
                    std::string("DWAPlannerROS"));
  planner_nh_ = ros::NodeHandle("~/" + planner_namespace);
  planner_dynamic_reconfigure_service_ =
      planner_nh_.serviceClient<dynamic_reconfigure::Reconfigure>(
          "set_parameters", true);
  initialized_ = true;
}

void MoveSlowAndClear::runBehavior() {
  if (!initialized_) {
    ROS_ERROR(
        "This recovery behavior has not been initialized, doing nothing.");
    return;
  }
  ROS_WARN("Move slow and clear recovery behavior started.");
  // 获取当前位姿
  geometry_msgs::PoseStamped global_pose, local_pose;
  global_costmap_->getRobotPose(global_pose);
  local_costmap_->getRobotPose(local_pose);

  std::vector<geometry_msgs::Point> global_poly, local_poly;
  geometry_msgs::Point pt;
  // 确定要清除的区域大小　正方形
  for (int i = -1; i <= 1; i += 2) {
    pt.x = global_pose.pose.position.x + i * clearing_distance_;
    pt.y = global_pose.pose.position.y + i * clearing_distance_;
    global_poly.push_back(pt);

    pt.x = global_pose.pose.position.x + i * clearing_distance_;
    pt.y = global_pose.pose.position.y + -1.0 * i * clearing_distance_;
    global_poly.push_back(pt);

    pt.x = local_pose.pose.position.x + i * clearing_distance_;
    pt.y = local_pose.pose.position.y + i * clearing_distance_;
    local_poly.push_back(pt);

    pt.x = local_pose.pose.position.x + i * clearing_distance_;
    pt.y = local_pose.pose.position.y + -1.0 * i * clearing_distance_;
    local_poly.push_back(pt);
  }

  // 清除代价地图中特定区域
  // 获取每一层的插件
  std::vector<boost::shared_ptr<costmap_2d::Layer>> *plugins =
      global_costmap_->getLayeredCostmap()->getPlugins();
  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;
    if (plugin->getName().find("obstacles") != std::string::npos) {
      boost::shared_ptr<costmap_2d::ObstacleLayer> costmap;
      costmap = boost::static_pointer_cast<costmap_2d::ObstacleLayer>(plugin);
      //将global_map的指定区域设置为free
      costmap->setConvexPolygonCost(global_poly, costmap_2d::FREE_SPACE);
    }
  }

  plugins = local_costmap_->getLayeredCostmap()->getPlugins();
  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;
    if (plugin->getName().find("obstacles") != std::string::npos) {
      boost::shared_ptr<costmap_2d::ObstacleLayer> costmap;
      costmap = boost::static_pointer_cast<costmap_2d::ObstacleLayer>(plugin);
      //将local_costmap的指定区域设置为free
      costmap->setConvexPolygonCost(local_poly, costmap_2d::FREE_SPACE);
    }
  }

  // 加锁保护
  boost::mutex::scoped_lock l(mutex_);

  // 获得上个阶段的最大速度
  if (!limit_set_) {
    // 如果之前没有限制速度
    if (!planner_nh_.getParam(max_trans_param_name_, old_trans_speed_)) {
      ROS_ERROR("The planner %s, does not have the parameter %s",
                planner_nh_.getNamespace().c_str(),
                max_trans_param_name_.c_str());
    }

    if (!planner_nh_.getParam(max_rot_param_name_, old_rot_speed_)) {
      ROS_ERROR("The planner %s, does not have the parameter %s",
                planner_nh_.getNamespace().c_str(),
                max_rot_param_name_.c_str());
    }
  }

  // we also want to save our current position so that we can remove the speed
  // limit we impose later on 存储当前位置
  speed_limit_pose_ = global_pose;

  // limit the speed of the robot until it moves a certain distance
  setRobotSpeed(limited_trans_speed_, limited_rot_speed_);
  limit_set_ = true;
  distance_check_timer_ = private_nh_.createTimer(
      ros::Duration(0.1), &MoveSlowAndClear::distanceCheck, this);
}

// 计算当前已经移动的距离
double MoveSlowAndClear::getSqDistance() {
  geometry_msgs::PoseStamped global_pose;
  global_costmap_->getRobotPose(global_pose);
  double x1 = global_pose.pose.position.x;
  double y1 = global_pose.pose.position.y;

  double x2 = speed_limit_pose_.pose.position.x;
  double y2 = speed_limit_pose_.pose.position.y;

  return (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
}

void MoveSlowAndClear::distanceCheck(const ros::TimerEvent &e) {
  // 已行驶的距离是否超出设定的距离
  if (limited_distance_ * limited_distance_ <= getSqDistance()) {
    ROS_INFO("Moved far enough, removing speed limit.");
    // have to do this because a system call within a timer cb does not seem to
    // play nice
    if (remove_limit_thread_) {
      remove_limit_thread_->join();
      delete remove_limit_thread_;
    }
    remove_limit_thread_ = new boost::thread(
        boost::bind(&MoveSlowAndClear::removeSpeedLimit, this));

    distance_check_timer_.stop();
  }
}

void MoveSlowAndClear::removeSpeedLimit() {
  boost::mutex::scoped_lock l(mutex_);
  // 恢复之前的速度
  setRobotSpeed(old_trans_speed_, old_rot_speed_);
  limit_set_ = false;
}

void MoveSlowAndClear::setRobotSpeed(double trans_speed, double rot_speed) {
  {
    dynamic_reconfigure::Reconfigure vel_reconfigure;
    dynamic_reconfigure::DoubleParameter new_trans;
    new_trans.name = max_trans_param_name_;
    new_trans.value = trans_speed;
    vel_reconfigure.request.config.doubles.push_back(new_trans);
    try {
      planner_dynamic_reconfigure_service_.call(vel_reconfigure);
      ROS_INFO_STREAM("Recovery setting trans vel: " << trans_speed);
    } catch (...) {
      ROS_ERROR(
          "Something went wrong in the service call to dynamic_reconfigure");
    }
  }
  {
    dynamic_reconfigure::Reconfigure rot_reconfigure;
    dynamic_reconfigure::DoubleParameter new_rot;
    new_rot.name = max_rot_param_name_;
    new_rot.value = rot_speed;
    rot_reconfigure.request.config.doubles.push_back(new_rot);
    try {
      planner_dynamic_reconfigure_service_.call(rot_reconfigure);
      ROS_INFO_STREAM("Recovery setting rot vel: " << rot_speed);
    } catch (...) {
      ROS_ERROR(
          "Something went wrong in the service call to dynamic_reconfigure");
    }
  }
}
}; // namespace move_slow_and_clear

１. reference源码分析
2.reference