feat(autoware_auto_perception_rviz_plugin): add perception objects pointcloud publisher

common/autoware_auto_perception_rviz_plugin/CMakeLists.txt

@@ -6,6 +6,8 @@ autoware_package()
 
 find_package(Qt5 REQUIRED COMPONENTS Widgets)
 
+find_package(PCL REQUIRED)
+
 set(OD_PLUGIN_LIB_SRC
   src/object_detection/detected_objects_display.cpp
   src/object_detection/tracked_objects_display.cpp
@@ -46,9 +48,14 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
 target_link_libraries(${PROJECT_NAME}
   rviz_common::rviz_common
   Qt5::Widgets
+  ${PCL_LIBRARIES}
 )
 target_include_directories(${PROJECT_NAME} PRIVATE include)
 
+target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC
+"${PCL_INCLUDE_DIRS}"
+)
+
 # Settings to improve the build as suggested on https://github.com/ros2/rviz/blob/ros2/docs/plugin_development.md
 target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 target_compile_definitions(${PROJECT_NAME} PRIVATE "OBJECT_DETECTION_PLUGINS_BUILDING_LIBRARY")

common/autoware_auto_perception_rviz_plugin/README.md

@@ -19,9 +19,10 @@ Example:
 
 #### Input Types
 
-| Name | Type                                                  | Description            |
-| ---- | ----------------------------------------------------- | ---------------------- |
-|      | `autoware_auto_perception_msgs::msg::DetectedObjects` | detection result array |
+| Name | Type                                                  | Description               |
+| ---- | ----------------------------------------------------- | ------------------------- |
+|      | `autoware_auto_perception_msgs::msg::DetectedObjects` | detection result array    |
+|      | `sensor_msgs::msg::PointCloud2`                       | point cloud for filtering |
 
 #### Visualization Result
 
@@ -31,9 +32,10 @@ Example:
 
 #### Input Types
 
-| Name | Type                                                 | Description           |
-| ---- | ---------------------------------------------------- | --------------------- |
-|      | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking result array |
+| Name | Type                                                 | Description               |
+| ---- | ---------------------------------------------------- | ------------------------- |
+|      | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking result array     |
+|      | `sensor_msgs::msg::PointCloud2`                      | point cloud for filtering |
 
 #### Visualization Result
 
@@ -45,16 +47,23 @@ Overwrite tracking results with detection results.
 
 #### Input Types
 
-| Name | Type                                                   | Description             |
-| ---- | ------------------------------------------------------ | ----------------------- |
-|      | `autoware_auto_perception_msgs::msg::PredictedObjects` | prediction result array |
+| Name | Type                                                   | Description              |
+| ---- | ------------------------------------------------------ | ------------------------ |
+|      | `autoware_auto_perception_msgs::msg::PredictedObjects` | prediction result array  |
+|      | `sensor_msgs::msg::PointCloud2`                        | pointcloud for filtering |
 
 #### Visualization Result
 
 Overwrite prediction results with tracking results.
 
 ![predicted-object-visualization-description](./images/predicted-object-visualization-description.jpg)
 
+### Visualization with active point cloud publishing
+
+Publishing colored point clouds. With colors according to different classes of detected objects, same as polygons.
+
+![visualization-with-pointcloud](./images/3d_pointcloud.png)
+
 ## References/External links
 
 [1] <https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/tree/master/src/tools/visualization/autoware_rviz_plugins>

common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp

@@ -16,7 +16,11 @@
 
 #include <object_detection/object_polygon_display_base.hpp>
 
+#include <autoware_auto_perception_msgs/msg/detected_object.hpp>
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <memory>
 
 namespace autoware
 {
@@ -37,6 +41,15 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay
 
 private:
   void processMessage(DetectedObjects::ConstSharedPtr msg) override;
+  void onInitialize() override;
+  void onObjectsAndObstaclePointCloud(
+    const DetectedObjects::ConstSharedPtr & input_objs_msg,
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg);
+
+  using SyncPolicy =
+    message_filters::sync_policies::ApproximateTime<DetectedObjects, sensor_msgs::msg::PointCloud2>;
+  using Sync = message_filters::Synchronizer<SyncPolicy>;
+  std::shared_ptr<Sync> sync_ptr;
 };
 
 }  // namespace object_detection

common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp

@@ -14,20 +14,51 @@
 #ifndef OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_
 #define OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_
 
+#include "rclcpp/rclcpp.hpp"
 #include "rviz_common/properties/enum_property.hpp"
+#include "rviz_common/properties/ros_topic_property.hpp"
+#include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp"
 
 #include <common/color_alpha_property.hpp>
 #include <object_detection/object_polygon_detail.hpp>
 #include <rviz_common/display.hpp>
+#include <rviz_common/display_context.hpp>
 #include <rviz_common/properties/color_property.hpp>
 #include <rviz_common/properties/float_property.hpp>
 #include <rviz_default_plugins/displays/marker/marker_common.hpp>
 #include <rviz_default_plugins/displays/marker_array/marker_array_display.hpp>
+#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 #include <visibility_control.hpp>
 
 #include <autoware_auto_perception_msgs/msg/object_classification.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
 #include <unique_identifier_msgs/msg/uuid.hpp>
 
+#include <boost/geometry.hpp>
+#include <boost/optional.hpp>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/synchronizer.h>
+#include <pcl/PCLPointCloud2.h>
+#include <pcl/common/common.h>
+#include <pcl/common/transforms.h>
+#include <pcl/conversions.h>
+#include <pcl/filters/crop_box.h>
+#include <pcl/filters/crop_hull.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/search/pcl_search.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <algorithm>
 #include <bitset>
 #include <list>
 #include <memory>
@@ -41,6 +72,32 @@ namespace rviz_plugins
 {
 namespace object_detection
 {
+
+using Polygon2d = tier4_autoware_utils::Polygon2d;
+using Shape = autoware_auto_perception_msgs::msg::Shape;
+
+// struct for creating objects buffer
+struct object_info
+{
+  Shape shape;
+  geometry_msgs::msg::Pose position;
+  std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> classification;
+};
+
+inline pcl::PointXYZRGB toPCL(const double x, const double y, const double z)
+{
+  pcl::PointXYZRGB pcl_point;
+  pcl_point.x = x;
+  pcl_point.y = y;
+  pcl_point.z = z;
+  return pcl_point;
+}
+
+inline pcl::PointXYZRGB toPCL(const geometry_msgs::msg::Point & point)
+{
+  return toPCL(point.x, point.y, point.z);
+}
+
 /// \brief Base rviz plugin class for all object msg types. The class defines common properties
 ///        for the plugin and also defines common helper functions that can be used by its derived
 ///        classes.
@@ -59,9 +116,10 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
   using PolygonPropertyMap =
     std::unordered_map<ObjectClassificationMsg::_label_type, common::ColorAlphaProperty>;
 
-  explicit ObjectPolygonDisplayBase(const std::string & default_topic)
-  : m_marker_common(this),
-    // m_display_type_property{"Polygon Type", "3d", "Type of the polygon to display object", this},
+  explicit ObjectPolygonDisplayBase(
+    const std::string & default_topic, const std::string & default_pointcloud_topic)
+  : point_cloud_common(new rviz_default_plugins::PointCloudCommon(this)),
+    m_marker_common(this),
     m_display_label_property{"Display Label", true, "Enable/disable label visualization", this},
     m_display_uuid_property{"Display UUID", true, "Enable/disable uuid visualization", this},
     m_display_pose_with_covariance_property{
@@ -92,6 +150,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
       SLOT(updatePalette()));
     m_simple_visualize_mode_property->addOption("Normal", 0);
     m_simple_visualize_mode_property->addOption("Simple", 1);
+    m_publish_objs_pointcloud = new rviz_common::properties::BoolProperty(
+      "Publish Objects Pointcloud", true, "Enable/disable objects pointcloud publishing", this);
+    m_default_pointcloud_topic = new rviz_common::properties::RosTopicProperty(
+      "Input pointcloud topic", QString::fromStdString(default_pointcloud_topic), "",
+      "Input for pointcloud visualization of objects detection pipeline.", this,
+      SLOT(updatePalette()));
+    m_default_pointcloud_topic->setReadOnly(true);
     // iterate over default values to create and initialize the properties.
     for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) {
       const auto & class_property_values = map_property_it.second;
@@ -119,6 +184,15 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     this->topic_property_->setMessageType(message_type);
     this->topic_property_->setValue(m_default_topic.c_str());
     this->topic_property_->setDescription("Topic to subscribe to.");
+
+    // get access to rviz node to sub and to pub to topics
+    rclcpp::Node::SharedPtr raw_node =
+      this->context_->getRosNodeAbstraction().lock()->get_raw_node();
+
+    tf_buffer = std::make_unique<tf2_ros::Buffer>(raw_node->get_clock());
+    tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);
+
+    point_cloud_common->initialize(this->context_, this->scene_node_);
   }
 
   void load(const rviz_common::Config & config) override
@@ -127,12 +201,17 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     m_marker_common.load(config);
   }
 
-  void update(float wall_dt, float ros_dt) override { m_marker_common.update(wall_dt, ros_dt); }
+  void update(float wall_dt, float ros_dt) override
+  {
+    m_marker_common.update(wall_dt, ros_dt);
+    point_cloud_common->update(wall_dt, ros_dt);
+  }
 
   void reset() override
   {
     RosTopicDisplay::reset();
     m_marker_common.clearMarkers();
+    point_cloud_common->reset();
   }
 
   void clear_markers() { m_marker_common.clearMarkers(); }
@@ -147,6 +226,62 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     m_marker_common.addMessage(markers_ptr);
   }
 
+  void add_pointcloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud)
+  {
+    point_cloud_common->addMessage(cloud);
+  }
+
+  // transform detected object pose to target frame and return bool result
+  bool transformObjects(
+    const MsgT & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer,
+    MsgT & output_msg)
+  {
+    output_msg = input_msg;
+
+    // transform to world coordinate
+    if (input_msg.header.frame_id != target_frame_id) {
+      output_msg.header.frame_id = target_frame_id;
+      tf2::Transform tf_target2objects_world;
+      tf2::Transform tf_target2objects;
+      tf2::Transform tf_objects_world2objects;
+      {
+        const auto ros_target2objects_world = getTransform(
+          tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp);
+        if (!ros_target2objects_world) {
+          return false;
+        }
+        tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world);
+      }
+      for (auto & object : output_msg.objects) {
+        tf2::fromMsg(object.kinematics.pose_with_covariance.pose, tf_objects_world2objects);
+        tf_target2objects = tf_target2objects_world * tf_objects_world2objects;
+        tf2::toMsg(tf_target2objects, object.kinematics.pose_with_covariance.pose);
+      }
+    }
+    return true;
+  }
+
+  // get transformation from tf2
+  boost::optional<geometry_msgs::msg::Transform> getTransform(
+    const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id,
+    const std::string & target_frame_id, const rclcpp::Time & time)
+  {
+    try {
+      geometry_msgs::msg::TransformStamped self_transform_stamped;
+      self_transform_stamped = tf_buffer.lookupTransform(
+        target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5));
+      return self_transform_stamped.transform;
+    } catch (tf2::TransformException & ex) {
+      RCLCPP_WARN_STREAM(rclcpp::get_logger("autoware_auto_perception_plugin"), ex.what());
+      return boost::none;
+    }
+  }
+
+  // variables for transfer detected objects information between callbacks
+  std::vector<object_info> objs_buffer;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener{nullptr};
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer;
+
 protected:
   /// \brief Convert given shape msg into a Marker
   /// \tparam ClassificationContainerT List type with ObjectClassificationMsg
@@ -382,6 +517,76 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
 
   double get_line_width() { return m_line_width_property.getFloat(); }
 
+  // helper function to get radius for kd-search
+  std::optional<float> getMaxRadius(object_info & object)
+  {
+    if (object.shape.type == Shape::BOUNDING_BOX || object.shape.type == Shape::CYLINDER) {
+      return std::hypot(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
+    } else if (object.shape.type == Shape::POLYGON) {
+      float max_dist = 0.0;
+      for (const auto & point : object.shape.footprint.points) {
+        const float dist = std::hypot(point.x, point.y);
+        max_dist = max_dist < dist ? dist : max_dist;
+      }
+      return max_dist;
+    } else {
+      return std::nullopt;
+    }
+  }
+
+  void filterPolygon(
+    const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr & in_cloud,
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud, const object_info & object)
+  {
+    pcl::PointCloud<pcl::PointXYZRGB> filtered_cloud;
+    std::vector<pcl::Vertices> vertices_array;
+    pcl::Vertices vertices;
+
+    Polygon2d poly2d = tier4_autoware_utils::toPolygon2d(object.position, object.shape);
+    if (boost::geometry::is_empty(poly2d)) {
+      return;
+    }
+
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr hull_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+    for (size_t i = 0; i < poly2d.outer().size(); ++i) {
+      vertices.vertices.emplace_back(i);
+      vertices_array.emplace_back(vertices);
+      hull_cloud->emplace_back(
+        static_cast<float>(poly2d.outer().at(i).x()), static_cast<float>(poly2d.outer().at(i).y()),
+        static_cast<float>(0.0));
+    }
+
+    pcl::CropHull<pcl::PointXYZRGB> crop_hull_filter;
+    crop_hull_filter.setInputCloud(in_cloud);
+    crop_hull_filter.setDim(2);
+    crop_hull_filter.setHullIndices(vertices_array);
+    crop_hull_filter.setHullCloud(hull_cloud);
+    crop_hull_filter.setCropOutside(true);
+
+    crop_hull_filter.filter(filtered_cloud);
+
+    const std_msgs::msg::ColorRGBA color_rgba =
+      get_color_rgba(object.classification);  // need to be converted to int
+
+    for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) {
+      cloud_it->r = std::max(0, std::min(255, static_cast<int>(floor(color_rgba.r * 256.0))));
+      cloud_it->g = std::max(0, std::min(255, static_cast<int>(floor(color_rgba.g * 256.0))));
+      cloud_it->b = std::max(0, std::min(255, static_cast<int>(floor(color_rgba.b * 256.0))));
+    }
+
+    *out_cloud += filtered_cloud;
+  }
+
+  // Default pointcloud topic;
+  rviz_common::properties::RosTopicProperty * m_default_pointcloud_topic;
+  // Property to enable/disable objects pointcloud publishing
+  rviz_common::properties::BoolProperty * m_publish_objs_pointcloud;
+
+  message_filters::Subscriber<MsgT> perception_objects_subscription;
+  message_filters::Subscriber<sensor_msgs::msg::PointCloud2> pointcloud_subscription;
+  // plugin to visualize pointclouds
+  std::unique_ptr<rviz_default_plugins::PointCloudCommon> point_cloud_common;
+
 private:
   // All rviz plugins should have this. Should be initialized with pointer to this class
   MarkerCommon m_marker_common;