Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft: Semantic segmentation plugin for nav2_costmap_2d #3042

Open
wants to merge 16 commits into
base: main
Choose a base branch
from

Conversation

pepisg
Copy link
Contributor

@pepisg pepisg commented Jun 24, 2022


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

@pepisg pepisg changed the title Semantic segmentation plugin Semantic segmentation plugin for nav2_costmap_2d Jun 24, 2022
@mergify
Copy link
Contributor

mergify bot commented Jun 25, 2022

@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).

@pepisg pepisg changed the title Semantic segmentation plugin for nav2_costmap_2d Draft: Semantic segmentation plugin for nav2_costmap_2d Jun 27, 2022
@ros-navigation ros-navigation deleted a comment from mergify bot Jun 28, 2022
@SteveMacenski
Copy link
Member

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

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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


// debug publishers
///< @brief publisher to publish segmentation messages when buffered
std::shared_ptr<rclcpp::Publisher<vision_msgs::msg::SemanticSegmentation>> sgm_debug_pub_;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

@pepisg pepisg Jun 28, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

@pepisg pepisg Jun 29, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

declareParameter("publish_debug_topics", rclcpp::ParameterValue(false));
declareParameter("max_obstacle_distance", rclcpp::ParameterValue(5.0));
declareParameter("min_obstacle_distance", rclcpp::ParameterValue(0.3));
declareParameter("segmentation_topic", rclcpp::ParameterValue(""));
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

declareParameter("pointcloud_topic", rclcpp::ParameterValue(""));
declareParameter("observation_persistence", rclcpp::ParameterValue(0.0));
declareParameter("expected_update_rate", rclcpp::ParameterValue(0.0));
declareParameter("class_types", rclcpp::ParameterValue(std::vector<std::string>({})));
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

As well here

// get the cost for each class
std::vector<std::string> classes_ids;
uint8_t cost;
declareParameter(source + ".classes", rclcpp::ParameterValue(std::vector<std::string>({})));
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

As well here with classes + cost

// For now, confidence of the inference is not used to adjust cost
// sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_confidence(*segmentation.cloud_,
// "confidence");
for (size_t point = 0; point < segmentation.cloud_->height * segmentation.cloud_->width;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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


void SemanticSegmentationLayer::onFootprintChanged()
{
RCLCPP_DEBUG(rclcpp::get_logger("nav2_costmap_2d"),
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is there something this should be doing?

const std::shared_ptr<const sensor_msgs::msg::PointCloud2>& pointcloud)
{
// check if data has the right dimensions
if (segmentation->width * segmentation->height != pointcloud->width * pointcloud->height)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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


rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_sensor_data;

semantic_segmentation_sub_ =
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

sensor_msgs::PointCloud2Iterator<float> iter_z_obs(segmentation_cloud, "z");

// get iterators for the original cloud
sensor_msgs::PointCloud2ConstIterator<float> iter_x_global(global_frame_cloud, "x");
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@pepisg
Copy link
Contributor Author

pepisg commented Jun 28, 2022

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

Just opened the PR on vision messages

@mergify
Copy link
Contributor

mergify bot commented Mar 20, 2023

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

@jmm-slamcore
Copy link

@pepisg Are there any plans to get this merged?

@mergify
Copy link
Contributor

mergify bot commented Oct 11, 2023

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

@pepisg
Copy link
Contributor Author

pepisg commented Dec 16, 2023

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:

    ezgif com-speed

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants