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

2023-10-14 07:30
文章标签 解决 激光雷达 layer 2d 清除 干净 obstacle 障碍物 costmap

本文主要是介绍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。

相关文章

如何解决线上平台抽佣高 线下门店客流少的痛点！

如何解决线上平台抽佣高 线下门店客流少的痛点！

目前，许多传统零售店铺正遭遇客源下降的难题。尽管广告推广能带来一定的客流，但其费用昂贵。鉴于此，众多零售商纷纷选择加入像美团、饿了么和抖音这样的大型在线平台，但这些平台的高佣金率导致了利润的大幅缩水。在这样的市场环境下，商家之间的合作网络逐渐成为一种有效的解决方案，通过资源和客户基础的共享，实现共同的利益增长。 以最近在上海兴起的一个跨行业合作平台为例，该平台融合了环保消费积分系统，在短
阅读更多...
基于UE5和ROS2的激光雷达+深度RGBD相机小车的仿真指南(五)：Blender锥桶建模

基于UE5和ROS2的激光雷达+深度RGBD相机小车的仿真指南(五)：Blender锥桶建模

前言 本系列教程旨在使用UE5配置一个具备激光雷达+深度摄像机的仿真小车，并使用通过跨平台的方式进行ROS2和UE5仿真的通讯，达到小车自主导航的目的。本教程默认有ROS2导航及其gazebo仿真相关方面基础，Nav2相关的学习教程可以参考本人的其他博客Nav2代价地图实现和原理–Nav2源码解读之CostMap2D(上)-CSDN博客往期教程： 第一期：基于UE5和ROS2的激光雷达+深度RG
阅读更多...
pip-tools：打造可重复、可控的 Python 开发环境，解决依赖关系，让代码更稳定

pip-tools：打造可重复、可控的 Python 开发环境，解决依赖关系，让代码更稳定

在 Python 开发中，管理依赖关系是一项繁琐且容易出错的任务。手动更新依赖版本、处理冲突、确保一致性等等，都可能让开发者感到头疼。而 pip-tools 为开发者提供了一套稳定可靠的解决方案。 什么是 pip-tools？ pip-tools 是一组命令行工具，旨在简化 Python 依赖关系的管理，确保项目环境的稳定性和可重复性。它主要包含两个核心工具：pip-compile 和 pip
阅读更多...
【VUE】跨域问题的概念，以及解决方法。

【VUE】跨域问题的概念，以及解决方法。

目录 1.跨域概念 2.解决方法 2.1 配置网络请求代理 2.2 使用@CrossOrigin 注解 2.3 通过配置文件实现跨域 2.4 添加 CorsWebFilter 来解决跨域问题 1.跨域概念 跨域问题是由于浏览器实施了同源策略，该策略要求请求的域名、协议和端口必须与提供资源的服务相同。如果不相同，则需要服务器显式地允许这种跨域请求。一般在springbo
阅读更多...
速盾高防cdn是怎么解决网站攻击的？

速盾高防cdn是怎么解决网站攻击的？

速盾高防CDN是一种基于云计算技术的网络安全解决方案，可以有效地保护网站免受各种网络攻击的威胁。它通过在全球多个节点部署服务器，将网站内容缓存到这些服务器上，并通过智能路由技术将用户的请求引导到最近的服务器上，以提供更快的访问速度和更好的网络性能。 速盾高防CDN主要采用以下几种方式来解决网站攻击： 分布式拒绝服务攻击(DDoS)防护：DDoS攻击是一种常见的网络攻击手段，攻击者通过向目标网
阅读更多...
Jenkins 插件 地址证书报错问题解决思路

Jenkins 插件 地址证书报错问题解决思路

问题提示摘要： SunCertPathBuilderException: unable to find valid certification path to requested target...... 网上很多的解决方式是更新站点的地址，我这里修改了一个日本的地址（清华镜像也好），其实发现是解决不了上述的报错问题的，其实，最终拉去插件的时候，会提示证书的问题，几经周折找到了其中一遍博文
阅读更多...
Redis中使用布隆过滤器解决缓存穿透问题

Redis中使用布隆过滤器解决缓存穿透问题

一、缓存穿透(失效)问题 缓存穿透是指查询一个一定不存在的数据，由于缓存中没有命中，会去数据库中查询，而数据库中也没有该数据，并且每次查询都不会命中缓存，从而每次请求都直接打到了数据库上，这会给数据库带来巨大压力。 二、布隆过滤器原理 布隆过滤器（Bloom Filter）是一种空间效率很高的随机数据结构，它利用多个不同的哈希函数将一个元素映射到一个位数组中的多个位置，并将这些位置的值置
阅读更多...
linux 下Time_wait过多问题解决

linux 下Time_wait过多问题解决

转自：http://blog.csdn.net/jaylong35/article/details/6605077 问题起因： 自己开发了一个服务器和客户端，通过短连接的方式来进行通讯，由于过于频繁的创建连接，导致系统连接数量被占用，不能及时释放。看了一下18888，当时吓到了。 现象： 1、外部机器不能正常连接SSH 2、内向外不能够正常的ping通过，域名也不能正常解析。
阅读更多...
proxy代理解决vue中跨域问题

proxy代理解决vue中跨域问题

vue.config.js module.exports = {...// webpack-dev-server 相关配置devServer: {host: '0.0.0.0',port: port,open: true,proxy: {'/api': {target: `https://vfadmin.insistence.tech/prod-api`,changeOrigin: true,p
阅读更多...
解决Office Word不能切换中文输入

解决Office Word不能切换中文输入

我们在使用WORD的时可能会经常碰到WORD中无法输入中文的情况。因为，虽然我们安装了搜狗输入法，但是到我们在WORD中使用搜狗的输入法的切换中英文的按键的时候会发现根本没有效果，无法将输入法切换成中文的。下面我就介绍一下如何在WORD中把搜狗输入法切换到中文。
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2