ROS 之障碍层的障碍物无法及时清除

冷不防 2022-11-29 13:21 334阅读 0赞

> 使用DWA导航的时候，发现障碍层的地图无法清除干净，在网上找了许久也找不到解决的办法，最后通过查看源码解决了这个问题, 是因为当激光雷达的数据在达到最大的距离的时候，会出现无法清除障碍物的现象。

**源代码如下**

void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& raw_message,
                                                  const boost::shared_ptr<ObservationBuffer>& buffer)
    { 
      // Filter positive infinities ("Inf"s) to max_range.
      float epsilon = 0.0001;  // a tenth of a millimeter
      sensor_msgs::LaserScan message = *raw_message;
      for (size_t i = 0; i < message.ranges.size(); i++)
      { 
        float range = message.ranges[ i ];
        if (!std::isfinite(range) && range > 0)
        { 
          message.ranges[ i ] = message.range_max - epsilon;
        }
      }
     
      // project the laser into a point cloud
      sensor_msgs::PointCloud2 cloud;
      cloud.header = message.header;
     
      // project the scan into a point cloud
      try
      { 
        projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
      }
      catch (tf::TransformException &ex)
      { 
        ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s",
                 global_frame_.c_str(), ex.what());
        projector_.projectLaser(message, cloud);
      }
     
      // buffer the point cloud
      buffer->lock();
      buffer->bufferCloud(cloud);
      buffer->unlock();
    }

上面的代码处理了每一个激光的数据，如果是激光的点是最大的距离，那么将这个点设置为比最大距离小十分之一毫米。我猜想程序设计者也考虑到了这个问题，当激光的距离等于最大的距离的时候会出现障碍物无法清除的现象，因此做这样的处理，使得所有的激光数据的距离小于最大距离。那么既然有了这样的处理为什么还会出现障碍物无法清除的现象呢？于是我查看了/scan数据，我发现激光的/scan话题，这个节点出来的数据是当障碍物的距离大于激光的检测距离时，激光的数据被自动设置为最大距离，而不是inf，因此这段程序并不起作用，因此将程序改为下面的：

**修改后的代码**

void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& raw_message,
                                                  const boost::shared_ptr<ObservationBuffer>& buffer)
    { 
      // Filter positive infinities ("Inf"s) to max_range.
      float epsilon = 0.0001;  // a tenth of a millimeter
      sensor_msgs::LaserScan message = *raw_message;
      for (size_t i = 0; i < message.ranges.size(); i++)
      { 
        float range = message.ranges[ i ];
        if ((!std::isfinite(range) && (range > 0)) || (range >= message.range_max))
        { 
          message.ranges[ i ] = message.range_max - epsilon;
     
        }
     
      }
     
      // project the laser into a point cloud
      sensor_msgs::PointCloud2 cloud;
      cloud.header = message.header;
     
      // project the scan into a point cloud
      try
      { 
        projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
      }
      catch (tf::TransformException &ex)
      { 
        ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s",
                 global_frame_.c_str(), ex.what());
        projector_.projectLaser(message, cloud);
      }
     
      // buffer the point cloud
      buffer->lock();
      buffer->bufferCloud(cloud);
      buffer->unlock();
    }

**0. 重新编译**  
当激光的数据范围大于最大值的时候都会设置为比最大值小一点点。这样就能保证障碍层能够清除了。  
然后重新编译costmap\_2d软件包

catkin_make
    catkin_make install

**1. 覆盖文件**  
将工作空间中的install文件夹下面lib中的costmap\_2d文件夹，liblayers.so和libcostmap\_2d.so文件覆盖/opt/ros/indigo/lib/下的对应的文件

**2. 修改改yaml文件**  
最后在costmap\_common\_params.yaml文件中添加inf\_is\_valid设置为true。

observation_sources: scan
      scan:
        data_type: LaserScan
        topic: /scan
        marking: true
        clearing: true
        inf_is_valid: true
        min_obstacle_height: 0.0
        max_obstacle_height: 1.0