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

最新推荐文章于 2024-06-25 17:41:17 发布

wallEVA96

最新推荐文章于 2024-06-25 17:41:17 发布

阅读量9.6k

点赞数 7

分类专栏： ros

本文链接：https://blog.csdn.net/walleva96/article/details/79079902

版权

ros 专栏收录该内容

24 篇文章 7 订阅

订阅专栏

前言：

使用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论坛。

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

解决：

所以，怎么解决呢；

1. 首先，就要修改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 代码文件

void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& raw_message,  
                                              const boost::shared_ptr<ObservationBuffer>& buffer)  
{  
<span style="color:#FF0000;">  // 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 == 0.0))  
    {  
      message.ranges[ i ] = message.range_max - epsilon;  
  
    }  
  
  }</span>  
  
  // 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();  
}

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 命令设置系统时间。