How should I integrate my semantic segmentation with costmap_2d?
I currently have a working drivable-space semantic segmentation (and might add more classes in the future, such as pedestrians), and I’m using message_filters.TimeSynchronizer and depth_image_proc/point_cloud_xyz to overlay this with the corresponding depth image and create (decimated) point cloud data for either class.
Is there a recommended way to combine this with the standard navigation stack? I’m using the TEB local planner, and a grid map from RTAB-map, but I would like to somehow add the non-drivable space to the resulting cost map as not lethal obstacles, but as perhaps intermediate-cost obstacles with some inflation.
I would prefer not to add these as hard obstacles, because there will occasionally be transient holes in the segmentation, and I don’t want these to cause erratic driving. I can do some morphological filtering to cut down on these holes, but the idea of using intermediate cost values to code these segmentations is appealing, because it leaves open the possibility of driving, for instance, on grass that RTABmap still deems drivable.
What is the recommended way to go about integrating such data? (For that matter, how are multiple costmap layers actually combined—with addition or max? I guess the costmap is an 8-bit unsigned image, so addition would need to be capped.)
I see that an intern at Nvidia created a custom layer for doing something similar to this, but they assume a particular fixed perspective transform, and then they also only use LETHAL_OBSTACLE, NO_INFORMATION, and FREE_SPACE to fill in the costmap. In contrast, I have corresponding depth data per pixel, so it's easy to use point_cloud_xyz to produce a PointCloud2 topic. So, an equivalent to the NVidia code would be to use a regular obstacle layer to project this pointcloud into the costmap, but this would also only admit lethal obstacles.
Should I give up on the idea of having intermediate-valued costs?
