ros机器人在navigation下导航costmap_2d动态层(障碍物层)障碍物无法及时消除的情况解决办法

设备

杉川-3a激光雷达

win10笔记本电脑

ubuntu18.04

ros-melodic

问题

ros机器人在move_base下导航，有静态图层与动态图层，静态图层显示之前已经建立好的地图，而动态层显示现在激光雷达实时扫描到的障碍物。

假设机器人雷达最大范围为8m，在某一时刻，以机器人为原点，在机器人前方5m有一障碍物，在障碍物后方，距离机器人10m远有一堵墙。

在导航中机器人可以识别并标记5m处的障碍物，并在动态层显示障碍物，此时将障碍物平行移动至距离机器人5m处的另一个位置。发现原来位置的动态层处的障碍物位置信息并没有被更新，此时出现了两个障碍物标记，实际只有一个障碍物，但却出现了两个标记。

如果此时人从机器人5m处经过，会留下一排标记且无法更新

原因

move_base导航时，动态图更新时需要可用的激光雷达数据，当障碍物在5m时程序接受到正确的5m的数据，更新了动态层，但当障碍物移动时，原方向上只有10m外的有一堵墙，此时激光雷达无法捕捉到墙的距离信息，因为激光雷达的范围是8m，所以在原方向输出的数据为inf，即无穷大这并不算程序能处理的数据，所以程序认为此方向没有数据，故无法更新该方向上的障碍物标记信息。

解决方法

先查看原激光雷达数据信息

rostopic echo -n 1 /scan

此处雷达信息有多个inf，无穷大

我们打开雷达的功能包，找到输出雷达数据的段落

    for (size_t i = 0; i < node_count; i++)
    {
        size_t current_angle = floor(nodes[i].angle);
        //printf("current_angle = %d\r\n", current_angle);
        if(current_angle >= 360.0)
        {
            //printf("lidar angle over rang\n");
            continue;
        }
        float read_value = (float) nodes[i].distance;
        if (read_value > scan_msg.range_max || read_value < scan_msg.range_min)
        //如果雷达数据比最大值大，比最小值小，即在雷达范围外
            scan_msg.ranges[360 - 1 - current_angle] = std::numeric_limits::infinity();
            //将此方向上的数据输出为无限大，std::numeric_limits::infinity()这句话可以理解为一个无限大的值，赋给/scan节点
        else
            scan_msg.ranges[360 - 1 - current_angle] = read_value;
    }

由于move_base程序无法处理无限大，则修改为

    for (size_t i = 0; i < node_count; i++)
    {
        size_t current_angle = floor(nodes[i].angle);
        //printf("current_angle = %d\r\n", current_angle);
        if(current_angle >= 360.0)
        {
            //printf("lidar angle over rang\n");
            continue;
        }
        float read_value = (float) nodes[i].distance;
        if (read_value > scan_msg.range_max || read_value < scan_msg.range_min)
        //如果雷达数据比最大值大，比最小值小，即在雷达范围外
            //scan_msg.ranges[360 - 1 - current_angle] = std::numeric_limits::infinity();
            scan_msg.ranges[360 - 1 - current_angle] = 0.0;
            //将0.0赋值给/scan节点
        else
            scan_msg.ranges[360 - 1 - current_angle] = read_value;
    }

在catkin_make后在运行激光雷达并监听

确保你的数据是0.0，便可以开始下一步

如果你之前用的是sudo apt install ros-melodic-navigation下载的navigotion请卸载他

sudo apt remove ros-melodic-navigation

然后下载源码版的navigation

新建工作空间，并下载源码

mkdir ~/navigation/src
cd ~/navigation/src
git clone https://github.com/ros-planning/navigation.git 
cd ~/navigation
catkin_make
cd 
gedit .bashrc
将source ~/navigation/devel/setup.bash加入

如果提示报错一般是缺少功能包，运行

sudo apt install ros-melodic-功能包名

注意功能包名的下划线改连字符 _ 号改成 - 号

打开~/navigation/src/navigation/costmap_2d/plugins/obstacle_layer.cpp

找到第272行

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

改成

void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& raw_message,
                                              const boost::shared_ptr& buffer)
{
  // 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))
    // 当雷达数据等于0.0时，认为此时激光雷达为最大范围
    {
      message.ranges[ i ] = message.range_max - epsilon;
    }
  }

回到navigation目录下，catkin_make

打开你的导航配置功能包，找到costmap_common_params.yaml配置文件

robot_radius: 0.2
map_type: costmap

static_layer:
  enabled:              true
  unknown_cost_value: -1
  lethal_cost_threshold: 100
  
obstacle_layer:
  enabled:              true
  max_obstacle_height:  2.0
  origin_z:             0.0
  z_resolution:         0.2
  z_voxels:             10
  unknown_threshold:    10
  mark_threshold:       0
  combination_method:   1
  track_unknown_space:  false  
  publish_voxel_map: false
  observation_sources:  scan 
  scan:
    data_type: LaserScan
    topic: scan
    marking: true
    clearing: true
    min_obstacle_height: -0.1
    max_obstacle_height: 1.5
    obstacle_range: 4.0
    raytrace_range: 4.0
    inf_is_valid: true  #scan的无穷远数据是否有效
 
inflation_layer:
  enabled:              true
  cost_scaling_factor:  10 
  inflation_radius:     0.11  

在obstacle_layer:  /    scan:  /  中添加 inf_is_valid: true使数据有有效

问题解决