costmap_2d: obstacle_layer中關於激光雷達障礙物清除不乾淨的解決

參考：

1. ROS naviagtion analysis: costmap_2d–ObstacleLayer
2. ROS Navigation Stack之costmap_2d源碼分析
3. ROS 中obstacle_layer由於激光雷達測距的侷限性，導致costmap中障礙物不能被及時清除。

總的來說是，使用navigation導航時，會出現由於激光雷達測距的侷限性， 會導致costmap上有行人走過，代價地圖中的障礙物，無法被及時的清除，留下影子一樣的軌跡。

具體細分可以分爲兩點：

1. 激光雷達無法測到距離時（太遠或者激光被折射了，對面是個鏡子等），返回值不是inf(無窮值。在32位的浮點型數據中，當數位都爲1時，表示數值無窮大。);
2. 激光雷達分辨率不夠（或者說是地圖分辨率高於激光雷達分辨率）

1. 修改激光雷達返回值

國內一些比較便宜的激光雷達，在未測到數據時，返回的是0.0 或者激光雷達默認的最大值。
當有行人走過時，機器人的附近會留下一些之前的圖中藍色的障礙點，這將會影響後續的導航和壁障.
解決方案就是修改costmap_2D中對於激光雷達數據的處理,
如果返回值爲0，則設置爲比最大值小一點點。

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)  原始代碼
    if ((!std::isfinite(range) && range > 0) || (range == 0.0))
    {
      message.ranges[ i ] = message.range_max - epsilon;
    }
  }

這裏有一點需要說明：需要在在costmap_common_params.yaml中 配置 inf_is_valid: true,

scan: {data_type: LaserScan, 
        topic: /scan, 
        marking: true, 
        clearing: true,
        inf_is_valid: true,
        observation_keep_time: 0.0, 
        expected_update_rate: 0 }

2. 地圖分辨率高於激光雷達分辨率

在costmap_common_params.yaml中有兩個配置參數

obstacle_range: 2.5  //只有障礙物在這個範圍內纔會被標記
raytrace_range: 3    //這個範圍內不存在的障礙物都會被清除

當raytrace_range = 3， 而激光雷達是360度每一度一個採樣點時，這是距離激光雷達處每個採樣點之間的距離大約是0.052, 也就是說如果地圖的分辨率時0.01的話，如果很靠近激光雷達採樣點如果有一個障礙物點，但是始終在激光雷達兩條採樣線（從激光雷達遠點到採樣點之間的線段）之間的話，也就是始終沒有掃描到的話，那這個點講始終無法被清除掉。

解決方案一：

花點錢每個好點的激光雷達，分辨率高點

解決方案二：

修改cosmap_2D中關於清除點規則

ObstacleLayer

參考文章1,2中有詳細分析ObstacleLayer中的代碼結構與邏輯，不贅述。
需要明確一點的是，這裏使用光線跟蹤 raytraceFreespace清除障礙物

1. raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); 會將所有在(x0,y0)>>(x1,y1)之間的所有cell標記爲FREE_SPACE。
2. 而updateRaytraceBounds 會根據測量的距離，更新擴張（min_x, min_y, max_x, max_y）。
3. updateBounds 在根據測量數據完成clear 操作之後，就開始了mark 操作，對每個測量到的點，標記爲obstacle ：

基本邏輯如下圖所示

所以如果我們想要清除比分辨率比激光雷達高的地圖上的點，一種方式就認爲在檢測範圍內（這裏是3x3)不存在比激光雷達分辨率（0.052）小的障礙物，所以所有激光雷達相鄰掃描線之間的障礙點都應該被清除。簡單點就是在每個激光雷達點周圍（3x3）都生成一個點，再進行光線跟蹤，進行清除。

void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
                                              double* max_x, double* max_y)
{
	...
	
  // for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
  // 清除傳感器原點到檢測到的點之間的障礙物
  for (unsigned int i = 0; i < cloud.points.size(); ++i)
  {
    double wx = cloud.points[i].x;
    double wy = cloud.points[i].y;

    //ROS_INFO("laser scan wx = %.2f, wy = %.2f", wx, wy);
    //在檢測到的點周圍生成6x6的點，
    double inflate_dx = 0.01, inflate_dy = 0.01; //在原來點的位置膨脹的尺度
    std::vector< std::pair<double,double> > inflate_pts;
    inflate_pts.push_back(std::make_pair(wx +    0      , wy +     0     ));
    inflate_pts.push_back(std::make_pair(wx -    0      , wy - inflate_dy));
    inflate_pts.push_back(std::make_pair(wx - inflate_dx, wy -     0     ));
    inflate_pts.push_back(std::make_pair(wx + 0         , wy + inflate_dy));
    inflate_pts.push_back(std::make_pair(wx + inflate_dx, wy +     0      ));
    inflate_pts.push_back(std::make_pair(wx -    0        , wy - 2*inflate_dy));
    inflate_pts.push_back(std::make_pair(wx - 2*inflate_dx, wy -     0     ));
    inflate_pts.push_back(std::make_pair(wx +    0        , wy + 2*inflate_dy));
    inflate_pts.push_back(std::make_pair(wx + 2*inflate_dx, wy +     0      ));
    inflate_pts.push_back(std::make_pair(wx -    0        , wy - 3*inflate_dy));
    inflate_pts.push_back(std::make_pair(wx - 3*inflate_dx, wy -     0     ));
    inflate_pts.push_back(std::make_pair(wx +    0        , wy + 3*inflate_dy));
    inflate_pts.push_back(std::make_pair(wx + 3*inflate_dx, wy +     0      ));  

    std::vector< std::pair<double,double> >::iterator inflate_iter;
    for(inflate_iter = inflate_pts.begin(); inflate_iter != inflate_pts.end(); inflate_iter++){
      wx = (*inflate_iter).first;
      wy = (*inflate_iter).second;

	...
    
    MarkCell marker(costmap_, FREE_SPACE);
    // and finally... we can execute our trace to clear obstacles along that line
    //最終raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); 
    //會將所有在(x0,y0)>>(x1,y1)之間的所有cell標記爲FREE_SPACE。
    raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
    //而updateRaytraceBounds 會根據測量的距離，更新擴張（min_x, min_y, max_x, max_y）。
    updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
  }
  }
}

修改後就不會人總過後有拖影了～～