Draft: Semantic segmentation plugin for nav2_costmap_2d

[pepisg]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	None to my knowledge
Primary OS tested on	Ubuntu
Robotic platform tested on	Custom skid steer drive robot on gazebo and in real life

---

Description of contribution in a few bullet points

• I added a semantic segmentation plugin to the nav2_costmap_2d package for adding segmentation data to costmaps. For now this assumes segmentation comes from and RGBD sensor that can produce aligned pointclouds. The configuration for this plugin looks like this:

    semantic_segmentation_layer:
        plugin: nav2_costmap_2d::SemanticSegmentationLayer
        enabled: True
        segmentation_topic: "/semantic_segmentation/segmentation/image_raw"
        pointcloud_topic: "/camera/depth/color/points"
        max_obstacle_distance: 3.5
        min_obstacle_distance: 0.5
        class_types: ["traversable", "warning", "danger"]
        traversable:
          classes: ["Sidewalk"]
          cost: 0
        warning:
          classes: ["Pads"]
          cost: 90
        danger:
          classes: ["Grass", "Other"]
          cost: 254

A ray tracing approach was tried to make the plugin work with calibrated RGB cameras but was discarded because projections of obstacles over the ground plane caused obstacles to extend beyond their actual location. @SteveMacenski let me know if you think more information about why this approach was not pursued should be added.

The plugin is dependant on new segmentation messages. @SteveMacenski, let me know if you agree with this definition for opening a PR to vision_msgs

Description of documentation updates required from your changes

• Added a new costmap plugin, so a configuration guide needs to be added. The documentation on nav2_costmap_2d also needs updating
• Also it would be good to add a tutorial on navigation2_tutorials. @SteveMacenski I could provide a demo segmentation node for detecting gazebo's ground plane on an image, let me know what you think would be the best way of doing this.

PD: I will start doing this after the first review in which I expect several changes will be requested

Future work that may be required in bullet points

• Extend the plugin to work with any combination of segmentation sources and pointclouds (e.x: 3D lidar + RGB camera)
• Integrate various handling methods for adjusting cost of pixels according to the confidence of the inference

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

NOTE: This PR will not build due to its dependency to vision_msgs's new messages

[mergify]

@pepisg, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[SteveMacenski]

Please open the PR with vision_msgs + tag me and we'll discuss there

[SteveMacenski]

Good first go at it! Mostly just smaller stuff to be honest (beyond missing tests)

[SteveMacenski]

Remove sgm_debug_pub_ orig_pointcloud_pub_ proc_pointcloud_pub_, these are the same as the input topics, there's no reason to repeat them

[pepisg]

I think its worth leaving proc_pointcloud_pub_. This publishes poinclouds with the segmentation class and confidence fields in each point. It may be useful for visualization. What do you think? This is subject to the debug_topics parameter anyway, which is False by default

[SteveMacenski]

These are very heavy topics, I would not like to have them if they're not of very important use -- since they are significantly large and ROS 2 struggles as is with high-bandwidth publication

[pepisg]

Messages are not published through this topic unless the parameter debug_topics is set to true in the plugin configuration. This parameter is false by default, so users should actually define it to get the messages. Do you still think it must be removed? Honestly is useful for our use case for visualization.

[SteveMacenski]

OK, if you leave that last one that's OK

[SteveMacenski]

Declare these without defaults since there is not standard name, as well with the pointcloud_topics below

[SteveMacenski]

As well here

[SteveMacenski]

As well here with classes + cost

[SteveMacenski]

This is not a great use of iterators, see other places where we use the PC2Iterator type, we can have the for loop based on it and in the incrementing condition we can increment all the iterators at once via ++iter_x, ++iter_y, ++iter_class so that we don't need to do any of the *(iter + point) stuff

[SteveMacenski]

Is there something this should be doing?

[SteveMacenski]

Can we not support them if they're not aligned? That would be important to note in documentation that this requires pre-aligned pointclouds.

[SteveMacenski]

It would be nice to support this in the future, many RGBD sensors will give you different resolution choices for RGB and D information.

[pepisg]

I think different resolutions can be supported if they are aligned, the mask could be down/up scaled to match the pointcloud resolution (I can do it if you think is reasonable). For not aligned pointclouds things will get more complicated, extra information should be passed to the plugin since it must do the alignment before filling the costmap. I will leave that in the documentation, maybe an issue can be created when the PR is merged to keep it on track

[SteveMacenski]

Subs should use the provided callback group https://github.com/ros-planning/navigation2/blob/985fd7e61a9701f130c41fe5e95e009847465ad2/nav2_costmap_2d/plugins/obstacle_layer.cpp#L123 so that it runs on another thread

[SteveMacenski]

Same here with use of iterators, see the normal observation buffer as an example

[pepisg]

Please open the PR with vision_msgs + tag me and we'll discuss there

Just opened the PR on vision messages

[mergify]

This pull request is in conflict. Could you fix it @pepisg?

[jmm-slamcore]

@pepisg Are there any plans to get this merged?

[mergify]

This pull request is in conflict. Could you fix it @pepisg?

[pepisg]

Hi!

Sorry for the massive delay. After quite a while I was finally able to pick up this PR. These are the latest changes:

• I removed the custom segmentation message the PR was initially created with. It nows subscribes to sensor_msgs/msg/Image as mask and a latched vision_msgs/msg/LabelInfo for associating class names to class ids in the mask.

• I made a simple node for creating segmentation masks in gazebo called mono_color_segmentation for testing the layer; this node creates a binary mask that separates the ground plane from the other objects in the scene by their color:

  

Here's a video of the layer working in the local costmap:

vokoscreenNG-2023-12-16_14-11-16.mp4

It was made with the testing segmentation node mentioned above and the following paramters:

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.22
      plugins: ["semantic_segmentation_layer", "inflation_layer"]
      semantic_segmentation_layer:
        plugin: nav2_costmap_2d::SemanticSegmentationLayer
        enabled: True
        observation_sources: mono_color_segmentation
        mono_color_segmentation:
          segmentation_topic: "/intel_realsense_r200_depth/mask"
          labels_topic: "/intel_realsense_r200_depth/label_info"
          pointcloud_topic: "/intel_realsense_r200_depth/points"
          max_obstacle_distance: 1.5
          min_obstacle_distance: 0.2
          class_types: ["traversable", "untraversable"]
          traversable:
            classes: ["ground_plane"]
            cost: 0
          untraversable:
            classes: ["others"]
            cost: 254
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      always_send_full_costmap: True

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      robot_radius: 0.22
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "semantic_segmentation_layer", "inflation_layer",]
      semantic_segmentation_layer:
        plugin: nav2_costmap_2d::SemanticSegmentationLayer
        enabled: True
        observation_sources: mono_color_segmentation
        mono_color_segmentation:
          segmentation_topic: "/intel_realsense_r200_depth/mask"
          labels_topic: "/intel_realsense_r200_depth/label_info"
          pointcloud_topic: "/intel_realsense_r200_depth/points"
          max_obstacle_distance: 3.0
          min_obstacle_distance: 0.2
          class_types: ["traversable", "untraversable"]
          traversable:
            classes: ["ground_plane"]
            cost: 0
          untraversable:
            classes: ["others"]
            cost: 254
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      always_send_full_costmap: True

As a comment, I think the turtlebot world may not be the best simulation to showcase the plugin's capabilities, this layer is more for terrain traversability rather than for geometry-based traversability. It may be worth creating another simple world with a sidewalk-like surface for this purpose.

I think this layer should also include:

1. A clearing mechanism, right now the only way to clear the layer is to get a new observation on a previously marked tile that now belongs to a traversable class. Something like stvl's decay would be good IMO.
2. Support for taking in confidence masks and a strategy for including that information. From discussions I've had with @SteveMacenski this may be a simple exclusion rule: "pixels with confidence less than X do not populate the costmap"; or a more fancy accumulation logic like continously "summing" the confidence on the inference on each costmap tile and having a rule that adjusts cost based on accumulated confidence, that way marking a tile with max cost requires a number of consistent observations for the same class on that tile.

Those may be included in separate PRs though, maybe this layer can be merged as an MVP with the current "passthrough" logic (where observations are put directly on the costmap regardless of the confidence and never get cleared), and then these and other improvements can me worked on in ther own PRs.

Let me know what you think @SteveMacenski