[collision_monitor] Range source spam warning when out of max range

[BriceRenaudeau]

Bug report

When receiving out-of-range data, the collision_monitor is spamming warnings in the terminal.

Required Info:

• Operating System:
  • Ubuntu 22.04
• ROS2 Version:
  • Iron
• Version or commit hash:
  • main
• DDS implementation:
  • Cyclone

Steps to reproduce issue

Create a collision_monitor that subscribes to a range sensor that publishes data gretter than max range.

ros2 topic pub /range_sensor sensor_msgs/msg/Range "header:
  stamp:
    sec: 0
    nanosec: 0
  frame_id: ''range_sensor"
radiation_type: 0
field_of_view: 1.04
min_range: 0.0
max_range: 1.5
range: 1.6" 

Actual behavior

The points out of range are correctly not added to the data list but the node is spamming in the terminal.

[collision_monitor-21] [WARN] [1687507456.693448211] [collision_monitor]: [us_sensor_]: Data range 1.600000m is out of {0.000000..1.500000} sensor span. Ignoring...
[collision_monitor-21] [WARN] [1687507456.693455238] [collision_monitor]: [us_sensor_]: Data range 1.600000m is out of {0.000000..1.500000} sensor span. Ignoring...

Expected behavior

The points out of range are correctly not added to the data list without spam.

There are several solutions:

• The node may output only once when the source got out-of-span data.
• The node may output DEBUG logging. (what we did locally)

Additional information

The check is done in the range.cpp where the source is defined.
https://github.com/ros-planning/navigation2/blob/d82c0a47f465b32a128b08afb635db5c68a18e95/nav2_collision_monitor/src/range.cpp#L80-L87

This check is required. We don't want to process out-of-range points. Strangely, this check is done only for Range sources and not Scans and PointClouds.

The ultrasonic range sensors are often noisy sensors very sensitive to reflections and interferences. To get a smoother navigation (approach field no trigger for no reason) we caped the range measurement at less than their real capacity. The behavior is good but the node spams the terminal.

If we send range = range_max, the data is taken into account and the velocity is reduced by the collision_monitor. In ROS1, filtering out wrong measurements by sending range = range_max+1 was common.

[SteveMacenski]

The node may output DEBUG logging. (what we did locally)

That sounds reasonable to me, if you open a PR I'll merge that pretty quickly.

Strangely, this check is done only for Range sources and not Scans and PointClouds.

That is strange, but I can see it making sense if you have sonars with low ranges. Any measurement in that sense would be probably in your collision stop zone, so you want to remove anything at all that's not in range. More or less explaining what you mention

[BriceRenaudeau]

Thanks Steve for your quick reply. That was also my thought, that's why I propose modification only in the range source.

I will make the PR immediately.