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

相关文章

SpringBoot自定义注解如何解决公共字段填充问题

《SpringBoot自定义注解如何解决公共字段填充问题》本文介绍了在系统开发中,如何使用AOP切面编程实现公共字段自动填充的功能,从而简化代码,通过自定义注解和切面类,可以统一处理创建时间和修改时间... 目录1.1 问题分析1.2 实现思路1.3 代码开发1.3.1 步骤一1.3.2 步骤二1.3.3

基于.NET编写工具类解决JSON乱码问题

《基于.NET编写工具类解决JSON乱码问题》在开发过程中,我们经常会遇到JSON数据处理的问题,尤其是在数据传输和解析过程中,很容易出现编码错误导致的乱码问题,下面我们就来编写一个.NET工具类来解... 目录问题背景核心原理工具类实现使用示例总结在开发过程中,我们经常会遇到jsON数据处理的问题,尤其是

Oracle登录时忘记用户名或密码该如何解决

《Oracle登录时忘记用户名或密码该如何解决》:本文主要介绍如何在Oracle12c中忘记用户名和密码时找回或重置用户账户信息,文中通过代码介绍的非常详细,对同样遇到这个问题的同学具有一定的参... 目录一、忘记账户:二、忘记密码:三、详细情况情况 1:1.1. 登录到数据库1.2. 查看当前用户信息1.

springboot3.4和mybatis plus的版本问题的解决

《springboot3.4和mybatisplus的版本问题的解决》本文主要介绍了springboot3.4和mybatisplus的版本问题的解决,主要由于SpringBoot3.4与MyBat... 报错1:spring-boot-starter/3.4.0/spring-boot-starter-

解决java.lang.NullPointerException问题(空指针异常)

《解决java.lang.NullPointerException问题(空指针异常)》本文详细介绍了Java中的NullPointerException异常及其常见原因,包括对象引用为null、数组元... 目录Java.lang.NullPointerException(空指针异常)NullPointer

Android开发中gradle下载缓慢的问题级解决方法

《Android开发中gradle下载缓慢的问题级解决方法》本文介绍了解决Android开发中Gradle下载缓慢问题的几种方法,本文给大家介绍的非常详细,感兴趣的朋友跟随小编一起看看吧... 目录一、网络环境优化二、Gradle版本与配置优化三、其他优化措施针对android开发中Gradle下载缓慢的问

python安装whl包并解决依赖关系的实现

《python安装whl包并解决依赖关系的实现》本文主要介绍了python安装whl包并解决依赖关系的实现,文中通过图文示例介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友们下面... 目录一、什么是whl文件?二、我们为什么需要使用whl文件来安装python库?三、我们应该去哪儿下

MySQL安装时initializing database失败的问题解决

《MySQL安装时initializingdatabase失败的问题解决》本文主要介绍了MySQL安装时initializingdatabase失败的问题解决,文中通过图文介绍的非常详细,对大家的学... 目录问题页面:解决方法:问题页面:解决方法:1.勾选红框中的选项:2.将下图红框中全部改为英

IDEA编译报错“java: 常量字符串过长”的原因及解决方法

《IDEA编译报错“java:常量字符串过长”的原因及解决方法》今天在开发过程中,由于尝试将一个文件的Base64字符串设置为常量,结果导致IDEA编译的时候出现了如下报错java:常量字符串过长,... 目录一、问题描述二、问题原因2.1 理论角度2.2 源码角度三、解决方案解决方案①:StringBui

mybatis和mybatis-plus设置值为null不起作用问题及解决

《mybatis和mybatis-plus设置值为null不起作用问题及解决》Mybatis-Plus的FieldStrategy主要用于控制新增、更新和查询时对空值的处理策略,通过配置不同的策略类型... 目录MyBATis-plusFieldStrategy作用FieldStrategy类型每种策略的作