Setup to clear the costmap via clearing observation topic

[xaru8145]

I have developed a simple collision checker library in ROS melodic for an application where I use rgbd cameras to mark and clear the costmap using STVL. In this way, I can determine if there are obstacles within the robot footprint.

With fast and aggressive movements of the robot, we can have a situation where some obstacles are marked that cannot be cleared by any of the layers (note that I use a persistent decay model).

In order to deal with this, I have a ROS service to clear the costmap. However, for the period of time between making the service call and receiving a new observation, the robot will have an empty costmap and could report that there are no obstacles while in reality there are, which is a dangerous behavior.

As solution suggested in this ROS issue is to publish a fake message that would clear out observations that are not actively being asserted by the marking observation sources.

plugins:
  - {name: detections_obstacle_layer,   type: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"}
  - {name: inflation,         type: "costmap_2d::InflationLayer"}

detections_obstacle_layer: 
  enabled:               true
  voxel_decay:           20    
  decay_model:           -1      
  voxel_size:            0.02  
  track_unknown_space:   true   
  observation_persistence: 0.0 
  max_obstacle_height:   3.0     
  unknown_threshold:     15   
  mark_threshold:        0     
  update_footprint_enabled: false
  combination_method:    1     
  obstacle_range:        3.5    
  origin_z:              0.0    
  publish_voxel_map:     true   
  transform_tolerance:   0.2    
  mapping_mode:          false  
  map_save_duration:     60     
  observation_sources:   rgbd1_mark rgbd1_clear clear_costmap
  rgbd1_mark:
    data_type: PointCloud2
    topic: /obstacles_cloud
    marking: true
    clearing: false
    min_obstacle_height: 0.2   
    max_obstacle_height: 2.0     
    expected_update_rate: 0.0    
    observation_persistence: 0.0 
    inf_is_valid: false         
    clear_after_reading: true    
    voxel_filter: false          
    voxel_min_points: 0   
  rgbd1_clear:
    enabled: true               
    data_type: PointCloud2
    topic: /front_stereo/stereo/depth
    marking: false
    clearing: true
    min_z: 0.0                   
    max_z: 5.0                 
    vertical_fov_angle: 0.87      
    horizontal_fov_angle: 1.25 
    decay_acceleration: 1.      
    model_type: 0                
  clear_costmap:  
    data_type: LaserScan
    topic: /robot/clear_costmap/scan
    marking: false
    clearing: true
    inf_is_valid: true          
    voxel_filter: true

I have made some tests and the clear_costmap observation source is not clearing anything. I am faking a Laserscan in frame _baselink of 1081 points covering from -PI to PI and using a fix range of 10.0m.

This is the python node I use to publish a fake laserscan to clear the costmap:

#! /usr/bin/env python

import rospy
from sensor_msgs.msg import LaserScan

if __name__ == '__main__':
    rospy.init_node('scan_clear_test')

    pub = rospy.Publisher('/robot/clear_costmap/scan', LaserScan, queue_size=1)

    rate = rospy.Rate(10) 
    data_samples = 1081
    range_value = 10.0
    intensity_value = 0.0

    scan = LaserScan()
    scan.header.frame_id = "base_link"
    scan.angle_min = -3.14159011841
    scan.angle_max = 3.14159011841
    scan.angle_increment = (scan.angle_max - scan.angle_min) / float(data_samples)
    scan.time_increment = 0.0
    scan.scan_time = 0.0
    scan.range_min = 0.10000000149
    scan.range_max = 30.0
    scan.ranges = [range_value] * data_samples
    scan.intensities = [intensity_value] * data_samples

    while not rospy.is_shutdown():
        scan.header.stamp = rospy.Time.now()
        pub.publish(scan)
        rate.sleep()

Is there any issues with mixing PointCloud2 and LaserScan data types? Is it better to fake a PointCloud2 specifying an "infinite" frustrum so that we can clear all space that we are not asserting? Which would be the best way?

Thanks in advance!

[SteveMacenski]

the robot will have an empty costmap and could report that there are no obstacles while in reality there are, which is a dangerous behavior.

That's the key problem in which you need to move to ROS 2 or fix ROS 1 costmap 2D to solve. This is a long-since solved problem in ROS 2. I wouldn't be trying to make hacks on a bug.

[xaru8145]

Thanks a lot @SteveMacenski for such a quick response. I totally agree that ROS2 is the way to go and we have several projects going on in ROS2. However, this particular one is stuck in ROS1 for now and there are no plans to port it soon. In this context, is there any way to set up a laserscan observation source that can clear Pointcloud marking sources from an rgbd camera? Or mixing the two types won't work?

[SteveMacenski]

laserscan observation source that can clear Pointcloud marking sources from an rgbd camera

Of course! A source can both mark and clear, you set all of your layers to have 1 mark and 1 clear. All marking happens after all clearing, though.