Draft: Semantic segmentation plugin for nav2_costmap_2d

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)
@@ -48,6 +49,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
@@ -67,6 +69,7 @@ set(dependencies
   rclcpp_lifecycle
   sensor_msgs
   std_msgs
+  vision_msgs
   std_srvs
   tf2
   tf2_geometry_msgs
@@ -87,6 +90,7 @@ add_library(layers SHARED
   src/observation_buffer.cpp
   plugins/voxel_layer.cpp
   plugins/range_sensor_layer.cpp
+  plugins/semantic_segmentation_layer.cpp
   plugins/denoise_layer.cpp
 )
 ament_target_dependencies(layers

nav2_costmap_2d/costmap_plugins.xml

@@ -15,6 +15,9 @@
     <class type="nav2_costmap_2d::RangeSensorLayer" base_class_type="nav2_costmap_2d::Layer">
       <description>A range-sensor (sonar, IR) based obstacle layer for costmap_2d</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>
     <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>

nav2_costmap_2d/include/nav2_costmap_2d/segmentation.hpp

@@ -0,0 +1,123 @@
+/*
+ * Copyright (c) 2008, 2013, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Authors: Pedro Gonzalez
+ */
+
+#ifndef NAV2_COSTMAP_2D__SEGMENTATION_HPP_
+#define NAV2_COSTMAP_2D__SEGMENTATION_HPP_
+
+#include <geometry_msgs/msg/point.hpp>
+#include <map>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+namespace nav2_costmap_2d
+{
+
+/**
+ * @brief Stores an segmentation in terms of a point cloud and the origin of the source
+ * @note Tried to make members and constructor arguments const but the compiler would not accept the
+ * default assignment operator for vector insertion!
+ */
+class Segmentation
+{
+public:
+  /**
+   * @brief  Creates an empty segmentation
+   */
+  Segmentation()
+  : cloud_(new sensor_msgs::msg::PointCloud2()) {}
+  /**
+   * @brief A destructor
+   */
+  virtual ~Segmentation() {delete cloud_;}
+
+  /**
+   * @brief  Copy assignment operator
+   * @param obs The segmentation to copy
+   */
+  Segmentation & operator=(const Segmentation & obs)
+  {
+    origin_ = obs.origin_;
+    cloud_ = new sensor_msgs::msg::PointCloud2(*(obs.cloud_));
+    class_map_ = obs.class_map_;
+
+    return *this;
+  }
+
+  /**
+   * @brief  Creates an segmentation from an origin point and a point cloud
+   * @param origin The origin point of the segmentation
+   * @param cloud The point cloud of the segmentation
+   * @param obstacle_max_range The range out to which an segmentation should be able to insert
+   * obstacles
+   * @param obstacle_min_range The range from which an segmentation should be able to insert
+   * obstacles
+   * @param raytrace_max_range The range out to which an segmentation should be able to clear via
+   * raytracing
+   * @param raytrace_min_range The range from which an segmentation should be able to clear via
+   * raytracing
+   */
+  Segmentation(
+    geometry_msgs::msg::Point & origin, const sensor_msgs::msg::PointCloud2 & cloud,
+    std::unordered_map<uint16_t, uint8_t> class_map)
+  : origin_(origin), cloud_(new sensor_msgs::msg::PointCloud2(cloud)), class_map_(class_map)
+  {
+  }
+
+  /**
+   * @brief  Copy constructor
+   * @param obs The segmentation to copy
+   */
+  Segmentation(const Segmentation & obs)
+  : origin_(obs.origin_)
+    , cloud_(new sensor_msgs::msg::PointCloud2(*(obs.cloud_)))
+    , class_map_(obs.class_map_)
+  {
+  }
+
+  /**
+   * @brief  Creates an segmentation from a point cloud
+   * @param cloud The point cloud of the segmentation
+   * @param obstacle_max_range The range out to which an segmentation should be able to insert
+   * obstacles
+   * @param obstacle_min_range The range from which an segmentation should be able to insert
+   * obstacles
+   */
+  Segmentation(const sensor_msgs::msg::PointCloud2 & cloud)
+  : cloud_(new sensor_msgs::msg::PointCloud2(cloud))
+  {
+  }
+
+  geometry_msgs::msg::Point origin_;
+  sensor_msgs::msg::PointCloud2 * cloud_;
+  std::unordered_map<uint16_t, uint8_t> class_map_; // contains class_id->cost relation
+};
+
+}  // namespace nav2_costmap_2d
+#endif  // NAV2_COSTMAP_2D__SEGMENTATION_HPP_

nav2_costmap_2d/include/nav2_costmap_2d/segmentation_buffer.hpp

@@ -0,0 +1,185 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Eitan Marder-Eppstein
+ *********************************************************************/
+#ifndef NAV2_COSTMAP_2D__SEGMENTATION_BUFFER_HPP_
+#define NAV2_COSTMAP_2D__SEGMENTATION_BUFFER_HPP_
+
+#include <list>
+#include <string>
+#include <vector>
+
+#include "nav2_costmap_2d/segmentation.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "rclcpp/time.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp"
+#include "vision_msgs/msg/label_info.hpp"
+
+namespace nav2_costmap_2d
+{
+/**
+ * @class SegmentationBuffer
+ * @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them
+ */
+class SegmentationBuffer
+{
+public:
+  /**
+   * @brief  Constructs an segmentation buffer
+   * @param  topic_name The topic of the segmentations, used as an identifier for error and warning
+   * messages
+   * @param  observation_keep_time Defines the persistence of segmentations 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
+   * @param  max_obstacle_height The minimum height of a hitpoint to be considered legal
+   * @param  obstacle_max_range The range to which the sensor should be trusted for inserting
+   * obstacles
+   * @param  obstacle_min_range The range from which the sensor should be trusted for inserting
+   * obstacles
+   * @param  raytrace_max_range The range to which the sensor should be trusted for raytracing to
+   * clear out space
+   * @param  raytrace_min_range The range from which the sensor should be trusted for raytracing to
+   * clear out space
+   * @param  tf2_buffer A reference to a tf2 Buffer
+   * @param  global_frame The frame to transform PointClouds into
+   * @param  sensor_frame The frame of the origin of the sensor, can be left blank to be read from
+   * the messages
+   * @param  tf_tolerance The amount of time to wait for a transform to be available when setting a
+   * new global frame
+   */
+  SegmentationBuffer(
+    const nav2_util::LifecycleNode::WeakPtr & parent, std::string buffer_source,
+    std::vector<std::string> class_types,
+    std::unordered_map<std::string, uint8_t> class_names_cost_map, double observation_keep_time,
+    double expected_update_rate, double max_lookahead_distance, double min_lookahead_distance,
+    tf2_ros::Buffer & tf2_buffer, std::string global_frame, std::string sensor_frame,
+    tf2::Duration tf_tolerance);
+
+  /**
+   * @brief  Destructor... cleans up
+   */
+  ~SegmentationBuffer();
+
+  /**
+   * @brief  Transforms a PointCloud to the global frame and buffers it
+   * <b>Note: The burden is on the user to make sure the transform is available... ie they should
+   * use a MessageNotifier</b>
+   * @param  cloud The cloud to be buffered
+   */
+  void bufferSegmentation(
+    const sensor_msgs::msg::PointCloud2 & cloud, const sensor_msgs::msg::Image & segmentation,
+    const sensor_msgs::msg::Image & confidence);
+
+  /**
+   * @brief  Pushes copies of all current segmentations onto the end of the vector passed in
+   * @param  segmentations The vector to be filled
+   */
+  void getSegmentations(std::vector<Segmentation> & segmentations);
+
+  /**
+   * @brief  gets the class map associated with the segmentations stored in the buffer
+   * @return the class map
+   */
+  std::unordered_map<std::string, uint8_t> getClassMap();
+
+  void createClassIdCostMap(const vision_msgs::msg::LabelInfo & label_info);
+
+  bool isClassIdCostMapEmpty() {return class_ids_cost_map_.empty();}
+
+  /**
+   * @brief  Check if the segmentation buffer is being update at its expected rate
+   * @return True if it is being updated at the expected rate, false otherwise
+   */
+  bool isCurrent() const;
+
+  /**
+   * @brief  Lock the segmentation buffer
+   */
+  inline void lock() {lock_.lock();}
+
+  /**
+   * @brief  Lock the segmentation buffer
+   */
+  inline void unlock() {lock_.unlock();}
+
+  /**
+   * @brief Reset last updated timestamp
+   */
+  void resetLastUpdated();
+
+  /**
+   * @brief Reset last updated timestamp
+   */
+  std::string getBufferSource() {return buffer_source_;}
+  std::vector<std::string> getClassTypes() {return class_types_;}
+
+  void setMinObstacleDistance(double distance) {sq_min_lookahead_distance_ = pow(distance, 2);}
+
+  void setMaxObstacleDistance(double distance) {sq_max_lookahead_distance_ = pow(distance, 2);}
+
+  void updateClassMap(std::string new_class, uint8_t new_cost);
+
+private:
+  /**
+   * @brief  Removes any stale segmentations from the buffer list
+   */
+  void purgeStaleSegmentations();
+
+  rclcpp::Clock::SharedPtr clock_;
+  rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")};
+  tf2_ros::Buffer & tf2_buffer_;
+  std::vector<std::string> class_types_;
+  std::unordered_map<std::string, uint8_t> class_names_cost_map_;
+  std::unordered_map<uint16_t, uint8_t> class_ids_cost_map_;
+  const rclcpp::Duration observation_keep_time_;
+  const rclcpp::Duration expected_update_rate_;
+  rclcpp::Time last_updated_;
+  std::string global_frame_;
+  std::string sensor_frame_;
+  std::list<Segmentation> segmentation_list_;
+  std::string buffer_source_;
+  std::recursive_mutex lock_;    ///< @brief A lock for accessing data in callbacks safely
+  double sq_max_lookahead_distance_;
+  double sq_min_lookahead_distance_;
+  tf2::Duration tf_tolerance_;
+};
+}  // namespace nav2_costmap_2d
+#endif  // NAV2_COSTMAP_2D__SEGMENTATION_BUFFER_HPP_