sonars with range_sensor_layer not clearing obstacles
Hi everyone,
I'm working on a turtlebot3 burger and I'm trying to integrate sonars in order to detect low obstacles in front of the robot. Currently, there are two sonars mounted to the robot, and both are publishing Range messages on separate topics (called '/sensor_sonar_left' and '/sensor_sonar_right'. Also separate joints were added in the turtlebot3 burgers urdf file in order to be able to link the distances seen by the sonars to the base frame.
In order to use the sonars for obstacle avoidance, the navigation stack for the turtlebot3 was adapted to include the range_sensor_layer.
Now, when visualizing the costmaps created by the sonar and range sensor layer, we can see that the sonar always seems to add a line of obstacles where the cone formed by the two sonars overlaps. This can mainly be seen going from the second image to the third image, where we inserted an object in front of the sonar in order to add an obstacle to the costmap, and afterwards removed it in order to clear that same space. However, as can be seen on the third image, once the obstacle is added, it is not being removed when the sonars range is further than the previously detected object.
The obstacles that are added cause the robot to get stuck and not want to move forward, although there are no more obstacles there. What am I missing in order to clear obstacles marked by the sonars?
As a reference, I'm using Ros Kinetic on Ubuntu 16.04, and I'm using the following configuration files. As can be seen, infrared sensors were added as well using a separate range_sensor_layer. They are used as fixed distance rangers in order to detect the edge of a table, and are currently working as expected.
costmap_common:
footprint: [[-0.105, -0.105], [-0.105, 0.105], [0.041, 0.105], [0.041, -0.105]]
#robot_radius: 0.105
map_type: costmap
obstacle_layer:
observation_sources: scan
scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}
obstacle_range: 3.0
raytrace_range: 3.5
inflation_layer:
inflation_radius: 0.1
cost_scaling_factor: 3.0
ir_table_layer:
topics: ['/sensor_IR_left_range','/sensor_IR_right_range']
no_readings_timeout: 0.0
clear_threshold: 0.2
mark_threshold: 0.8
clear_on_max_reading: true
sonar_layer:
topics: ['/sensor_sonar_left','/sensor_sonar_right']
no_readings_timeout: 2.0
clear_threshold: 0.4
mark_threshold: 0.9
clear_on_max_reading: true
plugins:
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
- {name: sonar_layer, type: "range_sensor_layer::RangeSensorLayer"}
- {name: ir_table_layer, type: "range_sensor_layer::RangeSensorLayer"}
global_costmap:
global_costmap:
global_frame: odom
robot_base_frame: base_footprint
update_frequency: 10.0
publish_frequency: 10.0
transform_tolerance: 0.5
static_map: false
rolling_window: true
width: 5
height: 5
resolution: 0.05
local_costmap
local_costmap:
global_frame: odom
robot_base_frame: base_footprint
update_frequency: 10.0
publish_frequency: 10.0
transform_tolerance: 0.5
static_map: false
rolling_window: true
width: 3
height: 3
resolution: 0.05
Example of a sonar range message:
---
header:
seq: 2179
stamp:
secs: 1575713861
nsecs: 293962976
frame_id: "sonar_right"
radiation_type: 0
field_of_view: 7.0
min_range: 0.019999999553
max_range: 2.0
range: 1.16586005688
---
Please do not link to images that are hosted on 3rd party services.
I've given you sufficient karma to attach them directly to your question.
Thanks, I edited the question to include the images