前言:
使用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机都不会出现。 参考如下
4.3
另外 由于,笔者的树莓派的时间忘记设定,所以 编译时会报错
Warning message: Clock skew detected. Yourbuild may be incomplete
.这是由于代码的时间比系统时间来得超前, 所以把时间 往目前时间调,就可以啦。 使用
date -s
命令设置系统时间。