ROS障礙層的無法清除乾淨的處理辦法

    使用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