nav_core::RecoveryBehavior----2 ClearCostmapRecovery

ROS

Reverts the costmap to the static map outside of a user-specified window and clears unknown space around the robot.
1. void ClearCostmapRecovery::initialize(std::string name, tf2_ros::Buffer* tf,
costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap)

 ros::NodeHandle private_nh("~/" + name_);
    private_nh.param("reset_distance", reset_distance_, 3.0);
    private_nh.param("force_updating", force_updating_, false);
    private_nh.param("affected_maps", affected_maps_, std::string("both"));
    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);

2. void ClearCostmapRecovery::clearMap(boost::shared_ptr costmap,
double pose_x, double pose_y)

//以机器人为中心，半径reset_distance_/2范围的以外的地图栅格如果不是NO_INFORMATION，则置为NO_INFORMATION
      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);
      costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);
      costmap->clearArea(start_x, start_y, end_x, end_y);




void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y)
{
  unsigned char* grid = getCharMap();
  for(int x=0; x<(int)getSizeInCellsX(); x++){
    bool xrange = x>start_x && x<end_x;

    for(int y=0; y<(int)getSizeInCellsY(); y++){
      if(xrange && y>start_y && y<end_y)
        continue;//注意在半径reset_distance_/2范围以内的不清除
      int index = getIndex(x,y);
      if(grid[index]!=NO_INFORMATION){
        grid[index] = NO_INFORMATION;
      }
    }
  }
}