ros如何及时清除障碍物层或者超声波层的的消息 - maohaihua/ros_study GitHub Wiki

https://blog.csdn.net/u010918541/article/details/78151704

上一篇已经讲过了clear_costmap_recovery 这一篇博客详细将如何清除障碍物信息

首先去github 上下载这个包 clear_costmap_recovery_gao

https://github.com/MOIRobot/MOI_Robot_Winter/tree/master/src/clear_costmap_recovery_gao

包这个是对movebase 里面clear_costmap_recovery 的升级版 可以指定清除哪个层的数据

1,首先将clear_costmap_recovery_gao 包放入工作空间/src目录下 然后catkin_make 确定可以roscd 到这个目录

2,打开move_base.h 里面 添加一个类对象 clear_costmap_recovery_gao::ClearCostmapRecoveryGao mapLayerClearer;

3,在move_base.cpp 里面初始化这个类 在 MoveBase::loadDefaultRecoveryBehaviors() 这个函数里 初始化这个类 //添加一个可用于实时清除地图障碍物层数据或者超声波层数据的恢复的behaviors_ 以下 “超声波清除” mapLayerClearer.initialize("my_clear_costmap_recovery_gao", &tf_, planner_costmap_ros_,controller_costmap_ros_);

4，使用这个类的清除函数 在MoveBase::executeCycle 里面找到如下片段 //if we're controlling, we'll attempt to find valid velocity commands case CONTROLLING: ROS_DEBUG_NAMED("move_base","In controlling state.");

    //check to see if we've reached our goal
    if(tc_->isGoalReached()){
      ROS_DEBUG_NAMED("move_base","Goal reached!");
         
      resetState();


      //disable the planner thread
      boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
      runPlanner_ = false;
      lock.unlock();


      as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
      
      在这里添加这个函数 就可以实现清除了
    //参数 静态层的名字 要清除层的名字 要清除距离机器人中心多远以外的障碍物区域
    mapLayerClearer.clearOnelayer("static_map","sonar",2);
    //清除激光雷达所在层的障碍物
    //mapLayerClearer.clearOnelayer("static_map","obstacle_layer",2);
     
      
     
      return true;

    }

第二种方法，后面直接测试 原来可以直接添加一个名字就好了 不用新建包

更改

//we'll load our default recovery behaviors here void MoveBase::loadDefaultRecoveryBehaviors(){ recovery_behaviors_.clear(); try{ //we need to set some parameters based on what's been passed in to us to maintain backwards compatibility ros::NodeHandle n("~");
n.setParam("conservative_reset/reset_distance", 0.01); n.setParam("aggressive_reset/reset_distance", circumscribed_radius_ * 4);

  std::vector<std::string> clearable_layers;
  clearable_layers.push_back( std::string("obstacle_layer") );//设置默认要被清除的层
  clearable_layers.push_back( std::string("sonar") );
  n.setParam("conservative_reset/layer_names",  clearable_layers);

boost::shared_ptr<nav_core::RecoveryBehavior>cons_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery")); cons_clear->initialize("conservative_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_); recovery_behaviors_.push_back(cons_clear);

  //next, we'll load a recovery behavior to rotate in place
  boost::shared_ptr<nav_core::RecoveryBehavior> rotate(recovery_loader_.createInstance("rotate_recovery/RotateRecovery"));
  if(clearing_rotation_allowed_){
    rotate->initialize("rotate_recovery", &tf_, planner_costmap_ros_, controller_costmap_ros_);
    recovery_behaviors_.push_back(rotate);
  }

  //next, we'll load a recovery behavior that will do an aggressive reset of the costmap
  boost::shared_ptr<nav_core::RecoveryBehavior> ags_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
  ags_clear->initialize("aggressive_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
  recovery_behaviors_.push_back(ags_clear);

  //we'll rotate in-place one more time
  if(clearing_rotation_allowed_)
    recovery_behaviors_.push_back(rotate);
}
catch(pluginlib::PluginlibException& ex){
  ROS_FATAL("Failed to load a plugin. This should not happen on default recovery behaviors. Error: %s", ex.what());
}

return;

}