Relative Variants of Voxel and Passthrough Filters and Footprint Projection for Slope Navigation

[JakubWarkocki]

[Feature] Added Relative Variants of Voxel and Passthrough Filters and Footprint Projection for Slope Navigation

Summary

This PR introduces new "relative" variants of the voxel and passthrough filters, which allow for more accurate obstacle detection on non-horizontal surfaces. By making the obstacle detection bounds relative to a customizable z_reference_frame (rather than the world frame), robots can now navigate uneven terrain without encountering the previous issues observed in Gazebo with the robot_localization and Nav2 stacks. The feature for clearing the robot footprint was also updated to provide an option to use 3D IMU orientation to project the footprint on the costmap.

These updates ensure improved behavior when broadcasting the robot's state with both 3D IMU orientation and z position on slopes, without causing unexpected navigation issues. The changes are implemented to ensure full backward compatibility with projects running the current version of SVTL.

Problem Description

• Problems with using the robot's z position: Since obstacle bounds are relative to the world frame, any change in the robot's z position altered the effective obstacle heights for the robot.
• Incorrect Obstacle Detection on Slopes: When using full 3D IMU orientation data, robots navigating slopes experienced improper obstacle detection, which could be dangerous, especially on downhill slopes. Obstacles' z coordinates in the world frame were not registered correctly since their global z coordinate was smaller than their height relative to the surface. On uphill slopes, the terrain itself could be mistakenly registered as an obstacle.
• Inconsistent Localization on 2D Costmaps: Using fully 2D localization created inconsistencies between GPS-based metrics and LiDAR-based metrics on a 2D costmap. This caused static obstacles to appear to "move" and become distorted as the robot moved on bumpy or sloped surfaces.

Solution

The newly introduced "relative" filter variants enable the robot’s obstacle detection to account for slopes by using the z_reference_frame parameter from the observation source, rather than relying on the world frame. This achieves:

• Slope-Compatible Obstacle Detection: The obstacle bounds adjust dynamically based on the robot’s current frame, allowing it to navigate traversable slopes and detect obstacles on them.
• Enabling SVTL use while tracking the robot's z position: This ensures consistent behavior regardless of altitude relative to the world frame.

Note: With relative filters, obstacles directly in front of the robot may still be inside the footprint transformed without considering the robot's 3D rotation.

This issue was solved by introducing a footprint projection feature, which, if enabled, processes clearing the robot's footprint by transforming it using tf2 instead of the Costmap2D footprint transformation. This provides an accurate projection of the footprint on the XY plane.

Known Limitations

• Drastic Slope Changes: The relative filters may still detect drastic slope changes as obstacles (e.g., transitioning from steep declines to inclines).
• No Absolute Slope Limit Safeguard: The current filters do not provide a mechanism to restrict obstacle detection based on a maximum slope. Further development with more complex point cloud analysis will be required to handle such cases.

Usage

To use the new features, the configuration file needs to be modified:

• Use the "voxel_relative" and "passthrough_relative" values for the filter attribute of the observation source.
• The z_reference_frame parameter, added to the observation source, sets the frame used to evaluate the z of each point of observation to determine if it is an obstacle. It defaults to the world frame of the layered costmap.
• footprint_projection_enabled activates projecting the footprint using tf2 and is strongly recommended if using 3D localization. It defaults to false.
• footprint_frame sets the frame from which the footprint is transformed to the global frame. It is necessary to set this when using footprint projection. The recommended setting is the robot's base frame (manual setting is necessary because the parameter is inaccessible through the layered costmap).

[SteveMacenski]

It seems to me that you're filtering out the data below a minimum height when the robot is non-flat. Why not use then the min_obstacle_height parameter which does this as well in conjunction with setting your robot's base frame to your 6DOF frame to have the RPY enabled as part of the estimate? That would have the same effect without needing to change any software

[JakubWarkocki]

I encountered the issues I described while using the min_obstacle_height and max_obstacle_height parameters in scenarios where the robot is moving on sloped surfaces or across multiple elevations. These parameters work well when the robot's movement is constrained to a flat surface, but issues arise in an operating environment in which robot's base Z-coordinate in the global frame changes with movement.

This happens because the MeasurementBuffer class transforms sensor data points from the sensor_frame to theglobal_frame and filters them based on their Z-coordinate in the global frame. While this works for a robot moving on a flat surface, it causes problems on slopes, stairs, or multi-floor environments, where the robot's base frame and the global frame (which remains fixed) can differ significantly in the Z-axis. This discrepancy results in obstacles being misclassified or ignored depending on whether their Z-coordinate lies within the defined bounds in the global frame.

For robots navigating uneven surfaces, it would be beneficial to temporarily switch to the robot's local frame to classify obstacles accurately, regardless of changes in elevation. This intermediate step would ensure the system remains robust, whether the robot is operating on slopes, or multiple flat surfaces, without relying solely on the global frame's Z-boundaries.

Another issue is clearing the robot's footprint. Default footprint transformation from Costmap2d only takes into account robot's yaw. On a sloped surface this causes the layer to clear obstacles immediately before the robot, as they are within the footprint that was transformed using only robot's yaw. This was solved by introducing an option to use tf2 based footprint projection to clear the costmap.

[SteveMacenski]

I spent some time to philosophically think about this. How I've thought about non-flat terrain was to segment out the ground via height, terrain estimation, AI, or other methods and then once the ground is roughly removed, to feed the remaining data into things like STVL or VL. The idea being that 'what is ground' and how to accurately remove it is very application dependent and its better to push that into a pre-stage than bake every one of N different methods that works on a different type of environment into the pre-processing stages of the costmap layers each.

I don't think after reviewing this that this would effectively deal with remoing the ground for sloped navigation. I don't doubt that it makes it less-bad because you're working with the immediate base frame which removes the Z- related issues, but there can still be alot of local variation around the robot's base roll/pitch orientation (ex: you're on the side of a hill not pointed upward or downward of it, but sideways) that would mark ground that's navigable as obstacles

I'm not against including this change however, but food for thought. I'm going to make some semi-pedantic code changes so we remove unnecessary TF lookups for when they're not required (and make the code more easily understandable for the fast observer).

[SteveMacenski]

Add the new parameters / feature to the readme docs

[SteveMacenski]

Otherwise generally looks good now. I would like you to lint the file though, the formatting is inconsistent in places and its a bit distracting. Use ament_cpplint and ament_uncrustify and I can merge these in.

Please just verify to me that you tested all 4 modes and they continue to work as expected

[SteveMacenski]

Set to false here, most people copy/paste this for use so it should be kept as a good default-centric baseline

[SteveMacenski]

I think somewhere in the readme, we need to have some explanation of your relative filters + how to set it up, so that its clear for later users how to use it and why its useful

[SteveMacenski]

Make it base_link or something reasonable please