Unable to clear traces of obstacles on costmap

asked 2019-04-22 07:09:30 -0500

Rayner gravatar image

Hi all,

I am having problems with my RPLidar and Costmap obstacle and inflation layer. When I first launch the file and RViz, the inflation and obstacles seem to inflate well. However, when I start rotating the sensor, there seems to be a trace left behind it and it only starts disappearing at a certain time after. If there is an obstacle moving directly towards the sensor from the front, it seems that the trace behind the obstacle gets longer and longer. This can be seen in the screenshot provided.

image description

(I was rotating my sensor in a clockwise direction, which leaves a trace on the left of the laserscan, the trace is cleared only when i rotate it in the other direction, with the laserscan 'clearing' the traces)

I have tried several solutions online and have also played with the parameters such as raytrace, set inf_is_valid to true, modified the offset in the costmap_2d.h file as mentioned in this link. I have also tried to ensure the sensor range is below the maximum range of my sensor. However, none of them seem to be working.

Are there any other possible solutions to this issue or am I missing out some parameter?

my example_params.yaml file:

global_frame: map
robot_base_frame: base_link

- {name: static_map, type: "costmap_2d::StaticLayer"}
- {name: obstacles,  type: "costmap_2d::ObstacleLayer"}
- {name: inflation,  type: "costmap_2d::InflationLayer"}

update_frequency: 10.0
publish_frequency: 10.0

#set if you want the voxel map published
publish_voxel_map: false

#set to true if you want to initialize the costmap from a static map
static_map: false

#begin - COMMENT these lines if you set static_map to true
rolling_window: true
width: 5.0
height: 5.0
resolution: 0.025
#end - COMMENT these lines if you set static_map to true

transform_tolerance: 0.2
obstacle_range: 2.0
max_obstacle_height: 2.0
raytrace_range: 3.0
footprint: [[-0.1025, -0.1025], [-0.1025, 0.1175], [0.1025, 0.1175], [0.1025, -0.1025]]
#robot_radius: 0.46
footprint_padding: 0.01

  inf_is_valid: true
  observation_sources: scan
  scan: {data_type: LaserScan, sensor_frame: base_link, marking: true, clearing: true, topic: scan, observation_persistance: 10.0}

  inflation_radius: 0.10
  cost_scaling_factor: 10.0
  lethal_cost_threshold: 100

my costmap_2D.launch


  <arg name="map_file" default="$(find launch_files)/map/mapAS.yaml"/>
    <!-- Map server -->
  <node pkg="map_server" name="map_server" type="map_server" output="screen" args="$(arg map_file)"/>

  <param name="/use_sim_time" value="false"/>

  <!-- Run the costmap node -->
  <node name="costmap_node" pkg="costmap_2d" type="costmap_2d_node" >
    <rosparam file="$(find launch_files)/params/example_params.yaml" command="load" ns="costmap" />

  <!-- RVIZ -->

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find launch_files)/rviz/prototype6.rviz" />

  <!-- RPLidar -->

    <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
  <param name="serial_port"         type="string" value="/dev/rplidar"/>
  <param name="serial_baudrate"     type="int"    value="115200"/><!--A1/A2 -->
  <!--param name="serial_baudrate"     type="int"    value="256000"--><!--A3 -->
  <param name="frame_id"            type="string" value="laser"/>
  <param name="inverted"            type="bool"   value="false"/>
  <param name="angle_compensate"    type="bool"   value="true"/>

    <node pkg="tf" type="static_transform_publisher" name="base_laser_broadcaster" args="0 0 0 0 0 0 base_link laser 100"/>

    <node pkg="tf" type ...
edit retag flag offensive close merge delete


How the clearing works is by raycasting. It looks like there are obstacles uncleared but outside the view of the sensor due to readings shorter than those left behind. If there's no ray to go through a space to say "not thing!" it'll keep "thing" there. Another option is to try my Spatio-Temporal voxel layer that was designed essentially to not require raycasting based clearing, but its probably not what you're looking for at the moment.

stevemacenski gravatar image stevemacenski  ( 2019-04-22 13:02:39 -0500 )edit

Yes I understand what you mean about raycasting. I am just puzzled why when I did turtlebot navigation in the past, I did not encounter the same problem as what I am facing now. The dynamic obstacles did not 'grow' in size as the turtlebot moved towards the obstacle, instead, the area behind the sensor data remained grey color as the obstacle got closer. Only the sensor laserscan is inflated.

Rayner gravatar image Rayner  ( 2019-04-23 03:23:38 -0500 )edit