ros如何及时清除障碍物层或者超声波层的的消息

最新推荐文章于 2025-01-16 21:54:04 发布

csdn_Flying

最新推荐文章于 2025-01-16 21:54:04 发布

阅读量5.7k

点赞数 3

CC 4.0 BY-SA版权

分类专栏： ROS

本文链接：https://blog.youkuaiyun.com/u010918541/article/details/78151704

ROS 专栏收录该内容

25 篇文章

订阅专栏

本文介绍了一种清除路径规划中障碍物信息的方法，通过升级版的clear_costmap_recovery_gao包，可在移动机器人运行过程中实时清除指定层的地图数据，如激光雷达层或超声波层的障碍物。

上一篇已经讲过了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;
}