ROS障碍层的无法清除干净的处理办法 - maohaihua/ros_study GitHub Wiki

https://blog.csdn.net/bobsweetie/article/details/70194416

使用DWA导航的时候，发现障碍层的地图无法清除干净，在网上找了许久也找不到解决的办法，最后通过查看源码解决了这个问题。查看这个链接的时候：http://answers.ros.org/question/30014/costmap-clearing-of-obstacles/时候大概发现了问题的原因。是因为当激光雷达的数据在达到最大的距离的时候，会出现无法清除障碍物的现象。

于是自己仿真的时候果然出现了这个问题，当激光雷达达到最大的距离的时候，会出现返回的激光的数据变为最大值，这个链接回答后面其中也给出了一些解决方法，但是没有根本上解决。

在Rviz中也看不到等于激光最大检测范围的点，应该是Rviz自动处理了激光距离等于最大范围的数据，不显示。我分析激光雷达的数据是在障碍层进行了处理，于是我查看了costmap_2d软件包中的obstacle_laer.cpp文件，发现下面这段代码：


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();
}

当激光的数据范围大于最大值的时候都会设置为比最大值小一点点。这样就能保证障碍层能够清除了。

然后重新编译costmap_2d软件包

catkin_make
catkin_make install

将工作空间中的instal文件夹下面lib中的costmap_2d文件夹，liblayers.so和libcostmap_2d.so文件覆盖/opt/ros/indigo/lib/下的对应的文件就好了

最后在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