slamtoolbox does not clear cells for maxrange lidar scans

[charles-fox]

Required Info:

• Operating System:
  ubuntu 20.04
• Installation type:
  ros2 humble binary release (sudo apt install ros-humble-slam-toolbox)
• ROS Version
  humble
• Laser unit:
  simulated hokuyo, in gazebo

Steps to reproduce issue

We are simulating a robot with a high resolution 2D lidar which operates in environments that are larger than the max range of the lidar. The gazebo simulation creates a laserscan message with 200 ranges. Following REP117, if a lidar angle does not make contact with any object but travels through free space up to its max range, the value +Inf is used as the range for this angle.

Fig 1 shows our simulation. We use a lidar with max range 10 units, and place the robot 11 units away from the wall.

Fig 2 shows the slamtoolbox map that is generated. Only for angles where the wall is within the maxrange are the cells cleared up to the wall (and the wall itself is mapped as occupied). The bug is that for the angles directly in front of the robot, where the range is +Inf, slamtoolbox does not clear the cells.

Expected behavior

Where an angle has +Inf for its range in the scan message, all cells up to rangemax should be cleared in the map. (As defined by REP117).

Actual behavior

Where an angle has +Inf for its range in the scan message, the angle is ignored and no cells along its angle are cleared,

Additional information

Fig 3 shows the /scan message, including the rangemax, angles, and showing the use of +Inf for the clear angles.

The slamtoolbox parameters used are shown below, including max_laser_range=10

# ROS Parameters
odom_frame: odom
map_frame: map
base_frame: base_link
scan_topic: /scan
mode: mapping 

# if you'd like to immediately start continuing a map at a given pose
# or at the dock, but they are mutually exclusive, if pose is given
# will use pose
#map_file_name: /home/computing/ws_gazebo/demo_map_serial
#map_start_pose: [0.0, 0.0, 0.0]
map_start_at_dock: true

debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02 #if 0 never publishes odometry
map_update_interval: 5.0 #2.0 #0.1
resolution: 0.5
max_laser_range: 10.0 #10.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 30.
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
enable_interactive_mode: true

@SteveMacenski

[SteveMacenski]

This is an intentional feature. Freespace is only free one you can raycast a hit. REP117 does not say that max range (ie no detection found) is freespace or is to be interpreted as navigable.

[charles-fox]

Thanks for the quick reply Steve. This seems like a strange feature? Could I suggest changing the behavior so that the space is cleared? There are several usecases where this is important. Once is conventional lidar robots operating in large, empty, outdoor areas, where their scans max out. Another is affordable open source hardware robots which use cheaper depth cams as lidar, but have shorter range. A third is tactile SLAM with whisker sensors, which work like very short range lidars, sweeping out free space or obstacles close to the robot's body. (https://eprints.whiterose.ac.uk/108430/1/fox_icra12.pdf) In all these cases, the lack of any contact in a beam is highly informative, it tells us that the space is free and in particular that we can start moving to explore it. If we don't make use of this information then we might never be able to move at all, in cases where the robot starts off in a location far from any obstacles. Are there any use cases where it is advantageous to NOT clear the freespace like this? (As distinct from a REP117 NaN, which would mean to not clear the space due to lack of any informative data.) Either way -- would you be able to direct us to the relevant part of the source to make the change ourselves? @SteveMacenski

[SteveMacenski]

This package is built for lidars, it has no explicit support for 3d sensors converted into 2d laser scans. That was something done before visual slam and 3d depth mapping methods were more commonplace. At this point, I assume that if you’re using a 2d slam system, it’s because you have 2d lidars in play. If using depth sensors or stereo cameras, I suggest using more data-appropriate methods.

It sounds like, above all else, that’s the real disconnect. This is not a catch-all package.

I do not believe, in general, that a lack of a return means that it’s free space. Also, your navigation system should be able to navigate in unknown space if you have the appropriate costmap & planner settings to allow for it (I’m not sure off hand if those are enabled by default in Nav2).

[charles-fox]

Thanks Steve :-)

I understand that your own interest is focussed entirely on 2D SLAM with 2D lidars. So let’s just consider that case.

Suppose we can afford a 2D (ie, a beam scanning only left to right, not also up and down) SICK scanner with max range 10m. And we want to build a robot to drive around an area of mesopotamian desert looking for and mapping out ancient Sumerian ruins. The ruins are sparse and consist of a few pillars and walls here and there. Most of the area is flat, empty desert.

We drop the robot off in a random nearby location and want it to start exploring. We have dropped it in a place with no objects immediately visible within 10m. The lidar can clearly see that that the 10m circle around the robot is empty. We need to store this very useful information in the map for two reasons:

First, so that we don’t need to come and visit the area again. The objective is to build a map of the area. We here have information about the exact content of the map for a 10m radius. If we throw this information away, then we will have to come back here again later to try again to map it, which is inefficient.

Second, if we don’t update the map, then any exploratory motions we make will now have to be through unmapped space. This is dangerous. Sure, we might be able to configure nav2 to override its sensible safety feature of preventing driving through unmapped areas. But that will expose us to driving into lethal areas, such as lava or pit traps, or perhaps reversing into obstacles in lidar blind spots. But if we don’t override this safety, then the robot will just sit there and can’t explore at all.

In REP117, a maxrange scan, represented as +Inf, is not a lack of return, its a positive piece of information which tells us that all the space in a direction up to maxrange is free. An actual lack of return is represented by NaN. Lack of return -- NaN -- does not indicate free space, so we shouldn't clear the map. But maxrange -- +Inf -- does indicate free space, so we should clear the map.

Can you explain why a robot would ever benefit from /not/ clearing these cells? Maybe there is some good reason but I am struggling to think of it -- being similarly biased by my own research areas -- so it would be interested to know what this is?

I get that this is your own code which you’re developing for your own particular use case. I’m currently looking for a ROS2 stack to teach to students, for a robotics textbook, and for general use in open source hardware robots, so trying to determine if it is also useful for these cases or if we should use something else. I think this small change would help to open up many more usecases including ones outside of your own research, which would be useful if you want to establish slamtoolbox as a general ROS2 community standard. If you’re open to including the change -- at least as a non-default option -- and providing some pointers into the code then I could make it here and do a PR?

@SteveMacenski