Adding ultrasonic sensors and range sensor layer
Hello, I need some help with a problem I encountered while adding ultrasonic sensors to a robot (loosely based on Linorobot), already equipped with an RPlidar. Hw/Sw: Raspi3B w/ Ubuntu 16.04.6 LTS, ROS kinetic, a Teensy, 2 Nano.
The robot was working fine with just the lidar, but I need to be able to detect correctly glass and some reflective surfaces, so I'm adding the ultrasonic sensors. The hardware and microcontroller (rosserial) parts seem to be working fine, I suspect it's an error from my part, maybe related to namespaces or transform frames... or maybe I'm missing something gargantuan. I checked and re-checked against online tutorials, examples and other questions similar to this one, but I couldn't identify the culprit.
After executing the launch files I get the standard messages (same as before trying to setup the ultrasonic sensors), plus:
[ INFO] [1631195261.554945536]: global_costmap/sonar_layer: ALL as input_sensor_type given
[ INFO] [1631195261.596176257]: RangeSensorLayer: subscribed to topic /ultrasound_front
and I guess that's good.
Unfortunately from that moment onward I get (with increasingly high figures, of course):
[ WARN] [1631195265.533631740]: No range readings received for 4.02 seconds, while expected at least every 2.00 seconds.
here's a sensor message (from "rostopic echo /ultrasound_front"):
----
header:
seq: 1124
stamp:
secs: 1631192726
nsecs: 301432058
frame_id: "sonar_front"
radiation_type: 0
field_of_view: 0.259999990463
min_range: 0.0
max_range: 100.0
range: 52.0
----
So, the topic is published and the massages should be ok...
My costmap_common_params.yaml:
map_type: costmap
transform_tolerance: 1
footprint: [[-0.25, -0.25], [-0.25, 0.25], [0.25, 0.25], [0.25, -0.25]]
inflation_layer:
inflation_radius: 0.28
cost_scaling_factor: 3
obstacle_layer:
obstacle_range: 2.5
raytrace_range: 3.0
observation_sources: scan
observation_persistence: 0.0
scan:
data_type: LaserScan
topic: scan
marking: true
clearing: true
sonar_layer:
frame: sonar_front
topics: ["/ultrasound_front"]
no_readings_timeout: 2.0
clear_on_max_reading: true
clear_threshold: 0.2
mark_threshold: 0.80
My global_costmap_params.yaml:
global_costmap:
global_frame: /map
robot_base_frame: /base_footprint
update_frequency: 1
publish_frequency: 0.5
static_map: true
transform_tolerance: 1
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: sonar_layer, type: "range_sensor_layer::RangeSensorLayer"}
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
My local_costmap_params.yaml:
local_costmap:
global_frame: /odom
robot_base_frame: /base_footprint
update_frequency: 1
publish_frequency: 5.0
static_map: false
rolling_window: true
width: 3
height: 3
resolution: 0.02
transform_tolerance: 1
observation_persistence: 0.0
plugins:
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: sonar_layer, type: "range_sensor_layer::RangeSensorLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
after getting the messages, "rostopic hz /ultrasound_front" gives: (thanks to @miura for the suggestion!)
subscribed to [/ultrasound_front]
average rate: 3.494
min: 0.267s max: 0.305s std dev: 0.01919s window: 3
average rate: 3.384
min: 0.250s max: 0.353s std dev: 0.03533s window: 6
average rate: 3.362
min: 0.250s max: 0.353s std dev: 0.02813s window: 9
average rate: 3.352
min: 0.250s max: 0.353s std dev: 0.02625s window: 13
average rate: 3.349
min: 0.250s max: 0.353s std dev: 0.02447s window: 16
average rate: 3.344
min: 0.250s ...
Have you tried checking the communication cycle with
rostopic hz /ultrasound_front? I think you are getting a warning because the communication cycle is slow.Also, I find it strange that in
rostopic echo /ultrasound_front,rangeis 52.0 whilemax_rangeis 1.0. Normally, therangeshould be smaller than themax_range. If max_range < range persists, I think there is an issue with the sensor.Thank you! Updating the question...
When I refer to the warning displayed in the terminal and the sensor message, the timestamp is way off. If this is the data when they are running at the same time, then the clock may be out of sync. You may need to set one of the clocks.
Also, it may not be the intended behavior, but if you set
no_readings_timeoutto 0.0 in costmap_common_params.yaml, it may work.Thank you once more miura for the help! The timestamp are different because weren't recorded at the same time, I checked the sync and the timestamp
secsin theultrasound_fronttopic seems to correspond to the timestamp from the launch file.I tried setting
no_readings_timeoutto 0.0, as you suggested, I think it's a step forward, or at least could help identify the problem:I got this an error from the launch file, every second or so:
[ERROR] [1631543292.139093839]: Range sensor layer can't transform from odom to sonar_front at 1631543288.530009OR same error, but...from map to sonar_front....I don't get the "no readings" warning anymore, so I'm currently investigating this other error, to see if I can identify the cause (maybe some problem in the tf tree). In any case I will post/update my findings!