ROS 中obstacle_layer由于激光雷达测距的局限性，导致costmap中障碍物不能被及时清除。

使用navigation导航时，会出现由于激光雷达测距的局限性， 会导致costmap，比如说 有行人走过，代价地图中的障碍物，无法被及时的清除。这个问题，主要由于，激光雷达基于三角测距原理，当 测距的距离超过激光雷达的极限，导致无法测到距离，就没有相对应的数据。 

根据不同的激光雷达，在测不到数据时， sick等比较贵的雷达，返回的是inf，无穷值。在32位的浮点型数据中，当数位都为1时，表示数值无穷大。

符号位S：1位	阶码（指数位） ：8位	尾数：————- 23 位
		（尾数是原本的二进制数，因为浮点计数，有点像科学计数法的x.xxxx*(10^y)，头一位一定是1，所以把1舍去了，比如 1.010101*（2^5），存起来就是：0 //5-127//0101010….00 ）

而国内一些比较便宜的激光雷达，在未测到数据时，返回的是0.0 或者激光雷达默认的最大值。

实验的现象主要如下所表现： 由于解决时，未保存图，所以图摘自ros论坛。

当有行人走过时，机器人的附近会留下一些之前的图中蓝色的障碍点，这将会影响后续的导航和壁障，而原因，前面已经描述了。这个问题几乎是机器人只要在大环境下，都肯定会遇到。会比如下图 红色框圈出来的区域。这是由于人走过去留下的，实际中，没有障碍物。

所以，怎么解决呢；  就要修改navigation这个代码包啦。

在catkin_ws的工作区间中，git clone https://github.com/ros-planning/navigation.git 

2 我的ros版本是kinect-devel，所以clone该版本， 当然，你也可以git clone 之后，git checkout kinect的分支。

  然后将其中的costmap_2d的文件夹copy到 工作区间的src文件夹下方。注意这个地方的，如果是要移植到kinect的机器人上  面，就要在kinect的ros版本环境上编译， 笔者之前在indigo版本上编译，把编译得到的库挪到kinect的/opt/ros/kinect/lib下，结果报错

 

error while loading shared libraries: libXXX.so: cannot open shared object file: No such file or directory

 所以注意，如果编译得到的库要移植到kinect上，就要kinect的ros环境下编译。

3 修改代码： 打开 navigation / costmap_2d / plugins /下的obstacles_layer.cpp 代码文件

1. void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& raw_message,  
2.                                               const boost::shared_ptr& buffer)  
3. {  
4. "color:#FF0000;">  // Filter positive infinities ("Inf"s) to max_range.  
5.   float epsilon = 0.0001;  // a tenth of a millimeter  
6.   sensor_msgs::LaserScan message = *raw_message;  
7.   for (size_t i = 0; i < message.ranges.size(); i++)  
8.   {  
9.     float range = message.ranges[ i ]; 
10. // /修改该地方，因为我的雷达没数据时，返回零值，所以判断当距离等于时，作为无穷远点来处理。
11.     if ((!std::isfinite(range) && (range > 0)) || (range == 0.0))  
12.     {  
13.       message.ranges[ i ] = message.range_max - epsilon;  
14.   
15.     }  
16.   
17.   }  
18.   
19.   // project the laser into a point cloud  
20.   sensor_msgs::PointCloud2 cloud;  
21.   cloud.header = message.header;  
22.   
23.   // project the scan into a point cloud  
24.   try  
25.   {  
26.     projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);  
27.   }  
28.   catch (tf::TransformException &ex)  
29.   {  
30.     ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s",  
31.              global_frame_.c_str(), ex.what());  
32.     projector_.projectLaser(message, cloud);  
33.   }  
34.   
35.   // buffer the point cloud  
36.   buffer->lock();  
37.   buffer->bufferCloud(cloud);  
38.   buffer->unlock();  
39. }  

3 接下来就是编译啦。 

catkin_make    // 可以用catkin_make -j2代替，以免树莓派死机

catkin_make install  就可以在install的文件夹下，得到 liblayers.so和libcostmap_2d.so文件。

将以上得到的so文件复制到 /opt/ros/kinect/lib下，然后覆盖到。

4 关于编译的一些问题 ：

    4.1 由于笔者在树莓派上面编译，所以特别吃力，所以使用 catkin_make -j2 使用双线程编译，以免死机，笔者在这地方编译时，卡了二十几分钟才编译好。

    4.2 另外由于在ubuntu上 ，swap空间会比较小，所以 还要设置一下swap空间。以免编译时出现死机，当然这些问题，PC机都不会出现。 参考如下

http://blog.csdn.net/wxz3wxz/article/details/70237302  点击打开链接 ，

   4.3 另外 由于，笔者的树莓派的时间忘记设定，所以 编译时会报错  Warning message: Clock skew detected. Yourbuild may be incomplete .这是由于代码的时间比系统时间来得超前， 所以把时间 往目前时间调，就可以啦。 使用 date -s 命令设置系统时间。