nav_core::RecoveryBehavior----3 MoveSlowAndClear

ROS

用来清除代价地图中以clearing_distance_为半径内的障碍物，将其所包含的栅格置为FREE_SPACE，并且限制机器人移动速度。

1 void MoveSlowAndClear::initialize (std::string n, tf2_ros::Buffer* tf,
costmap_2d::Costmap2DROS* global_costmap,
costmap_2d::Costmap2DROS* local_costmap)
默认DWAPlannerROS局部规划器使用MoveSlowAndClear机制。

  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);
    private_nh_.param("limited_distance", limited_distance_, 0.3);

std::string planner_namespace;
private_nh_.param("planner_namespace", planner_namespace, std::string("DWAPlannerROS"));//用于重配置参数的规划器命名空间。默认DWAPlannerROS局部规划器使用MoveSlowAndClear机制
planner_nh_ = ros::NodeHandle("~/" + planner_namespace);
planner_dynamic_reconfigure_service_ = planner_nh_.serviceClient<dynamic_reconfigure::Reconfigure>("set_parameters", true); 

2 void MoveSlowAndClear::runBehavior()

//以clearing_distance_为半径内的
 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);
    }


setRobotSpeed(limited_trans_speed_, limited_rot_speed_);

    //将max_vel_trans动态配置成trans_speed，max_vel_theta动态配置成rot_speed
      void MoveSlowAndClear::setRobotSpeed(double trans_speed, double rot_speed)
      {
        {
          dynamic_reconfigure::Reconfigure vel_reconfigure;
          dynamic_reconfigure::DoubleParameter new_trans;
          new_trans.name = "max_vel_trans";
          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_vel_theta";
          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");
          }
        }
      }

恢复到原有的最大线速度和旋转速度参数设置，有一个时间间隔检查距离触发distance_check_timer_ = private_nh_.createTimer(ros::Duration(0.1), &MoveSlowAndClear::distanceCheck, this)，即当if(limited_distance_ * limited_distance_ <= getSqDistance())，则恢复。getSqDistance()与车现在位置和开始MoveSlowAndClear时的车位置有关：

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;//开始MoveSlowAndClear时的车位置
    double y2 = speed_limit_pose_.pose.position.y;

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