ROS Navigation 学习之Clear_Costmap_Recovery

clear_costmap_recovery和rotate_recovery是ROS导航包中的一种恢复机制:

接下来主要分析clear_costmap_recovery包：

调用这个包，会将global_costmap和local_costmap中给定半径（reset_distance默认值3.0）范围之外的区域进行清理，即将栅格占有或者非占有状态清除为未知。

namespace clear_costmap_recovery{  /**   * @class ClearCostmapRecovery   * @brief A recovery behavior that reverts the navigation stack's costmaps to the static map outside of a user-specified region.   */  class ClearCostmapRecovery : public nav_core::RecoveryBehavior {    public:      /**       * @brief  Constructor, make sure to call initialize in addition to actually initialize the object       * @param         * @return        */      ClearCostmapRecovery();      /**       * @brief  Initialization function for the ClearCostmapRecovery recovery behavior       * @param tf A pointer to a transform listener       * @param global_costmap A pointer to the global_costmap used by the navigation stack        * @param local_costmap A pointer to the local_costmap used by the navigation stack        */      void initialize(std::string name, tf::TransformListener* tf,           costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap);      /**       * @brief  Run the ClearCostmapRecovery recovery behavior. Reverts the       * costmap to the static map outside of a user-specified window and       * clears unknown space around the robot.       */      void runBehavior();    private:      void clear(costmap_2d::Costmap2DROS* costmap);            void clearMap(boost::shared_ptr<costmap_2d::CostmapLayer> costmap, double pose_x, double pose_y);      costmap_2d::Costmap2DROS* global_costmap_, *local_costmap_;      std::string name_;      tf::TransformListener* tf_;      bool initialized_;      double reset_distance_;      std::set<std::string> clearable_layers_; ///< Layer names which will be cleared.  };};#endif 

通过头文件可以看出来主要有这么几个函数：

初始化函数：

void initialize(std::string name, tf::TransformListener* tf,           costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap);

清除函数：

void ClearCostmapRecovery::clear(costmap_2d::Costmap2DROS* costmap){  std::vector<boost::shared_ptr<costmap_2d::Layer> >* plugins = costmap->getLayeredCostmap()->getPlugins();  tf::Stamped<tf::Pose> pose;  if(!costmap->getRobotPose(pose)){    ROS_ERROR("Cannot clear map because pose cannot be retrieved");    return;  }  double x = pose.getOrigin().x();  //机器人原始位置  double y = pose.getOrigin().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('/');  //返回该字符在字符串中的下标    if( slash != std::string::npos ){  //如果查找到匹配        name = name.substr(slash+1);    }    if(clearable_layers_.count(name)!=0){      boost::shared_ptr<costmap_2d::CostmapLayer> costmap;      costmap = boost::static_pointer_cast<costmap_2d::CostmapLayer>(plugin);      clearMap(costmap, x, y);    }  }}

最后是最关键的函数：

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()));   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);  unsigned char* grid = costmap->getCharMap();  for(int x=0; x<(int)costmap->getSizeInCellsX(); x++){    bool xrange = x>start_x && x<end_x;                       for(int y=0; y<(int)costmap->getSizeInCellsY(); y++){      if(xrange && y>start_y && y<end_y)        continue;      int index = costmap->getIndex(x,y);      if(grid[index]!=NO_INFORMATION){  //将栅格更新为没有信息        grid[index] = NO_INFORMATION;      }    }  }  double ox = costmap->getOriginX(), oy = costmap->getOriginY();  double width = costmap->getSizeInMetersX(), height = costmap->getSizeInMetersY();  costmap->addExtraBounds(ox, oy, ox + width, oy + height);  ROS_INFO("haha: Clearing costmap over!");  return;}