costmap_2d: obstacle_layer中关于激光雷达障碍物清除不干净的解决

本文主要是介绍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 millimetersensor_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);}}
}

修改后就不会人总过后有拖影了~~
在这里插入图片描述

这篇关于costmap_2d: obstacle_layer中关于激光雷达障碍物清除不干净的解决的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/209033

相关文章

使用TomCat,service输出台出现乱码的解决

《使用TomCat,service输出台出现乱码的解决》本文介绍了解决Tomcat服务输出台中文乱码问题的两种方法,第一种方法是修改`logging.properties`文件中的`prefix`和`... 目录使用TomCat,service输出台出现乱码问题1解决方案问题2解决方案总结使用TomCat,

解决Spring运行时报错:Consider defining a bean of type ‘xxx.xxx.xxx.Xxx‘ in your configuration

《解决Spring运行时报错:Considerdefiningabeanoftype‘xxx.xxx.xxx.Xxx‘inyourconfiguration》该文章主要讲述了在使用S... 目录问题分析解决方案总结问题Description:Parameter 0 of constructor in x

解决IDEA使用springBoot创建项目,lombok标注实体类后编译无报错,但是运行时报错问题

《解决IDEA使用springBoot创建项目,lombok标注实体类后编译无报错,但是运行时报错问题》文章详细描述了在使用lombok的@Data注解标注实体类时遇到编译无误但运行时报错的问题,分析... 目录问题分析问题解决方案步骤一步骤二步骤三总结问题使用lombok注解@Data标注实体类,编译时

Java循环创建对象内存溢出的解决方法

《Java循环创建对象内存溢出的解决方法》在Java中,如果在循环中不当地创建大量对象而不及时释放内存,很容易导致内存溢出(OutOfMemoryError),所以本文给大家介绍了Java循环创建对象... 目录问题1. 解决方案2. 示例代码2.1 原始版本(可能导致内存溢出)2.2 修改后的版本问题在

大数据小内存排序问题如何巧妙解决

《大数据小内存排序问题如何巧妙解决》文章介绍了大数据小内存排序的三种方法:数据库排序、分治法和位图法,数据库排序简单但速度慢,对设备要求高;分治法高效但实现复杂;位图法可读性差,但存储空间受限... 目录三种方法:方法概要数据库排序(http://www.chinasem.cn对数据库设备要求较高)分治法(常

Vue项目中Element UI组件未注册的问题原因及解决方法

《Vue项目中ElementUI组件未注册的问题原因及解决方法》在Vue项目中使用ElementUI组件库时,开发者可能会遇到一些常见问题,例如组件未正确注册导致的警告或错误,本文将详细探讨这些问题... 目录引言一、问题背景1.1 错误信息分析1.2 问题原因二、解决方法2.1 全局引入 Element

linux报错INFO:task xxxxxx:634 blocked for more than 120 seconds.三种解决方式

《linux报错INFO:taskxxxxxx:634blockedformorethan120seconds.三种解决方式》文章描述了一个Linux最小系统运行时出现的“hung_ta... 目录1.问题描述2.解决办法2.1 缩小文件系统缓存大小2.2 修改系统IO调度策略2.3 取消120秒时间限制3

C#使用HttpClient进行Post请求出现超时问题的解决及优化

《C#使用HttpClient进行Post请求出现超时问题的解决及优化》最近我的控制台程序发现有时候总是出现请求超时等问题,通常好几分钟最多只有3-4个请求,在使用apipost发现并发10个5分钟也... 目录优化结论单例HttpClient连接池耗尽和并发并发异步最终优化后优化结论我直接上优化结论吧,

解决systemctl reload nginx重启Nginx服务报错:Job for nginx.service invalid问题

《解决systemctlreloadnginx重启Nginx服务报错:Jobfornginx.serviceinvalid问题》文章描述了通过`systemctlstatusnginx.se... 目录systemctl reload nginx重启Nginx服务报错:Job for nginx.javas

Mysql DATETIME 毫秒坑的解决

《MysqlDATETIME毫秒坑的解决》本文主要介绍了MysqlDATETIME毫秒坑的解决,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友们下面随着... 今天写代码突发一个诡异的 bug,代码逻辑大概如下。1. 新增退款单记录boolean save = s