Adds support for nvblox uneven terrain and crater detection by taking ESDF (DistanceMapSlices) from adjacent heights, one above ground, and one significantly below ground. The adjacent ESDF slices are combined by computing the difference in origins and aligning the 2 slices using this difference. Once aligned, the below-ground cost map is inverted, and superimposed onto the above-ground cost map, keeping the more dangerous values for each cost map (the lower value in the cost map data array).
This combined costmap is then published to a ROS topic, for use with Nav2 or other navigation plugins. Point clouds are also combined, using identical logic; aligning the layers, and then publishing a merged point cloud using the minimum (most 'dangerous') values from each cloud. All of this logic is fully optional via nvblox config file, and without specification, will still default to base nvblox behavior, with no costmap combination or uneven terrain support.
Here are 2 screenshots of a combined point cloud and cost map running in RViz, using a .svo file prerecorded on a Zed2i. The visible crater in the recorded terrain is highlighted as dangerous in the point cloud, and Nav2 (a Ros2 navigation plugin), using the combined cost map, goes out of its way to avoid the crater/ uneven terrain obstacles (the plotted nav2 path is seen by the black line from the robot to the red arrow).
Adds support for nvblox uneven terrain and crater detection by taking ESDF (DistanceMapSlices) from adjacent heights, one above ground, and one significantly below ground. The adjacent ESDF slices are combined by computing the difference in origins and aligning the 2 slices using this difference. Once aligned, the below-ground cost map is inverted, and superimposed onto the above-ground cost map, keeping the more dangerous values for each cost map (the lower value in the cost map data array). This combined costmap is then published to a ROS topic, for use with Nav2 or other navigation plugins. Point clouds are also combined, using identical logic; aligning the layers, and then publishing a merged point cloud using the minimum (most 'dangerous') values from each cloud. All of this logic is fully optional via nvblox config file, and without specification, will still default to base nvblox behavior, with no costmap combination or uneven terrain support.
Here are 2 screenshots of a combined point cloud and cost map running in RViz, using a .svo file prerecorded on a Zed2i. The visible crater in the recorded terrain is highlighted as dangerous in the point cloud, and Nav2 (a Ros2 navigation plugin), using the combined cost map, goes out of its way to avoid the crater/ uneven terrain obstacles (the plotted nav2 path is seen by the black line from the robot to the red arrow).
Signed-off-by: Alex Berg