Plugin to accumulate segmentation confidence

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -135,7 +135,7 @@ bool BtActionServer<ActionT>::on_configure()
     node, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link")));
   nav2_util::declare_parameter_if_not_declared(
     node, "transform_tolerance", rclcpp::ParameterValue(0.1));
-  rclcpp::copy_all_parameter_values(node, client_node_);
+  nav2_util::copy_all_parameters(node, client_node_);
 
   // set the timeout in seconds for the action server to discard goal handles if not finished
   double action_server_result_timeout;

nav2_costmap_2d/CMakeLists.txt

@@ -17,6 +17,7 @@ find_package(rclcpp_lifecycle REQUIRED)
 find_package(rmw REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(vision_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2 REQUIRED)
@@ -40,6 +41,7 @@ add_library(nav2_costmap_2d_core SHARED
   src/footprint.cpp
   src/costmap_layer.cpp
   src/observation_buffer.cpp
+  src/segmentation_buffer.cpp
   src/clear_costmap_service.cpp
   src/footprint_collision_checker.cpp
   plugins/costmap_filters/costmap_filter.cpp
@@ -66,6 +68,7 @@ set(dependencies
   rclcpp_lifecycle
   sensor_msgs
   std_msgs
+  vision_msgs
   std_srvs
   tf2
   tf2_geometry_msgs
@@ -88,6 +91,7 @@ add_library(layers SHARED
   plugins/voxel_layer.cpp
   plugins/range_sensor_layer.cpp
   plugins/denoise_layer.cpp
+  plugins/semantic_segmentation_layer.cpp
 )
 add_library(${PROJECT_NAME}::layers ALIAS layers)
 ament_target_dependencies(layers

nav2_costmap_2d/costmap_plugins.xml

@@ -18,6 +18,9 @@
     <class type="nav2_costmap_2d::DenoiseLayer" base_class_type="nav2_costmap_2d::Layer">
       <description>Filters noise-induced freestanding obstacles or small obstacles groups</description>
     </class>
+    <class type="nav2_costmap_2d::SemanticSegmentationLayer" base_class_type="nav2_costmap_2d::Layer">
+      <description>A plugin for semantic segmentation data produced by RGBD cameras</description>
+    </class>
   </library>
 
   <library path="filters">

nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp

@@ -62,6 +62,7 @@ class ObservationBuffer
   /**
    * @brief  Constructs an observation buffer
    * @param  topic_name The topic of the observations, used as an identifier for error and warning messages
+   * @param  source_name The name of the source as declared in the parameters, used to change source specific params
    * @param  observation_keep_time Defines the persistence of observations in seconds, 0 means only keep the latest
    * @param  expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit
    * @param  min_obstacle_height The minimum height of a hitpoint to be considered legal
@@ -78,6 +79,7 @@ class ObservationBuffer
   ObservationBuffer(
     const nav2_util::LifecycleNode::WeakPtr & parent,
     std::string topic_name,
+    std::string source_name,
     double observation_keep_time,
     double expected_update_rate,
     double min_obstacle_height, double max_obstacle_height, double obstacle_max_range,
@@ -132,6 +134,22 @@ class ObservationBuffer
    */
   void resetLastUpdated();
 
+  /**
+   * @brief Set max obstacle distance
+   */
+  void setMaxObstacleDistance(double max_obstacle_distance)
+  {
+    obstacle_max_range_ = max_obstacle_distance;
+  }
+
+  /**
+   * @brief Get source name
+   */
+  std::string getSourceName()
+  {
+    return source_name_;
+  }
+
 private:
   /**
    * @brief  Removes any stale observations from the buffer list
@@ -148,6 +166,7 @@ class ObservationBuffer
   std::string sensor_frame_;
   std::list<Observation> observation_list_;
   std::string topic_name_;
+  std::string source_name_;
   double min_obstacle_height_, max_obstacle_height_;
   std::recursive_mutex lock_;  ///< @brief A lock for accessing data in callbacks safely
   double obstacle_max_range_, obstacle_min_range_, raytrace_max_range_, raytrace_min_range_;

nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp

@@ -253,6 +253,7 @@ class ObstacleLayer : public CostmapLayer
   bool rolling_window_;
   bool was_reset_;
   nav2_costmap_2d::CombinationMethod combination_method_;
+  std::string _topics_string;
 };
 
 }  // namespace nav2_costmap_2d