feat(autoware_auto_perception_rviz_plugin): improve rviz performance

common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp

@@ -84,6 +84,12 @@ get_shape_marker_ptr(
   const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
   const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width);
 
+AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
+get_2d_shape_marker_ptr(
+  const autoware_auto_perception_msgs::msg::Shape & shape_msg,
+  const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
+  const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width);
+
 /// \brief Convert the given polygon into a marker representing the shape in 3d
 /// \param centroid Centroid position of the shape in Object.header.frame_id frame
 /// \return Marker ptr. Id and header will have to be set by the caller
@@ -131,10 +137,18 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list(
   const autoware_auto_perception_msgs::msg::Shape & shape,
   std::vector<geometry_msgs::msg::Point> & points);
 
+AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list(
+  const autoware_auto_perception_msgs::msg::Shape & shape,
+  std::vector<geometry_msgs::msg::Point> & points);
+
 AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list(
   const autoware_auto_perception_msgs::msg::Shape & shape,
   std::vector<geometry_msgs::msg::Point> & points);
 
+AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_cylinder_bottom_line_list(
+  const autoware_auto_perception_msgs::msg::Shape & shape,
+  std::vector<geometry_msgs::msg::Point> & points);
+
 AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_circle_line_list(
   const geometry_msgs::msg::Point center, const double radius,
   std::vector<geometry_msgs::msg::Point> & points, const int n);
@@ -143,6 +157,10 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_polygon_line_list(
   const autoware_auto_perception_msgs::msg::Shape & shape,
   std::vector<geometry_msgs::msg::Point> & points);
 
+AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_polygon_bottom_line_list(
+  const autoware_auto_perception_msgs::msg::Shape & shape,
+  std::vector<geometry_msgs::msg::Point> & points);
+
 AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_path_line_list(
   const autoware_auto_perception_msgs::msg::PredictedPath & paths,
   std::vector<geometry_msgs::msg::Point> & points);

common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp

@@ -14,6 +14,8 @@
 #ifndef OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_
 #define OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_
 
+#include "rviz_common/properties/enum_property.hpp"
+
 #include <common/color_alpha_property.hpp>
 #include <object_detection/object_polygon_detail.hpp>
 #include <rviz_common/display.hpp>
@@ -59,8 +61,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
 
   explicit ObjectPolygonDisplayBase(const std::string & default_topic)
   : m_marker_common(this),
-    m_display_3d_property{
-      "Display 3d polygon", true, "Enable/disable height visualization of the polygon", this},
+    // m_display_type_property{"Polygon Type", "3d", "Type of the polygon to display object", 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{
@@ -79,6 +80,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     m_line_width_property{"Line Width", 0.03, "Line width of object-shape", this},
     m_default_topic{default_topic}
   {
+    m_display_type_property = new rviz_common::properties::EnumProperty(
+      "Polygon Type", "3d", "Type of the polygon to display object.", this, SLOT(updatePalette()));
+    // Option values here must correspond to indices in palette_textures_ array in onInitialize()
+    // below.
+    m_display_type_property->addOption("3d", 0);
+    m_display_type_property->addOption("2d", 1);
+    m_display_type_property->addOption("Disable", 2);
     // 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;
@@ -150,14 +158,22 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     const ClassificationContainerT & labels, const double & line_width) const
   {
     const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels);
-
-    if (m_display_3d_property.getBool()) {
+    if (m_display_type_property->getOptionInt() == 0) {
       return detail::get_shape_marker_ptr(shape_msg, centroid, orientation, color_rgba, line_width);
+    } else if (m_display_type_property->getOptionInt() == 1) {
+      return detail::get_2d_shape_marker_ptr(
+        shape_msg, centroid, orientation, color_rgba, line_width);
     } else {
       return std::nullopt;
     }
   }
 
+  template <typename ClassificationContainerT>
+  visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr(
+    const autoware_auto_perception_msgs::msg::Shape & shape_msg,
+    const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
+    const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width);
+
   /// \brief Convert given shape msg into a Marker to visualize label name
   /// \tparam ClassificationContainerT List type with ObjectClassificationMsg
   /// \param centroid Centroid position of the shape in Object.header.frame_id frame
@@ -366,8 +382,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
   std::list<rviz_common::properties::Property> m_class_group_properties;
   // Map to store class labels and its corresponding properties
   PolygonPropertyMap m_polygon_properties;
-  // Property to enable/disable height visualization of the polygon
-  rviz_common::properties::BoolProperty m_display_3d_property;
+  // Property to choose type of visualization polygon
+  rviz_common::properties::EnumProperty * m_display_type_property;
   // Property to enable/disable label visualization
   rviz_common::properties::BoolProperty m_display_label_property;
   // Property to enable/disable uuid visualization

common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp

@@ -276,6 +276,38 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr(
   return marker_ptr;
 }
 
+visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr(
+  const autoware_auto_perception_msgs::msg::Shape & shape_msg,
+  const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
+  const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width)
+{
+  auto marker_ptr = std::make_shared<Marker>();
+  marker_ptr->ns = std::string("shape");
+
+  using autoware_auto_perception_msgs::msg::Shape;
+  if (shape_msg.type == Shape::BOUNDING_BOX) {
+    marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
+    calc_2d_bounding_box_bottom_line_list(shape_msg, marker_ptr->points);
+  } else if (shape_msg.type == Shape::CYLINDER) {
+    marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
+    calc_2d_cylinder_bottom_line_list(shape_msg, marker_ptr->points);
+  } else if (shape_msg.type == Shape::POLYGON) {
+    marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
+    calc_2d_polygon_bottom_line_list(shape_msg, marker_ptr->points);
+  } else {
+    marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
+    calc_2d_polygon_bottom_line_list(shape_msg, marker_ptr->points);
+  }
+
+  marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
+  marker_ptr->pose = to_pose(centroid, orientation);
+  marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2);
+  marker_ptr->scale.x = line_width;
+  marker_ptr->color = color_rgba;
+
+  return marker_ptr;
+}
+
 void calc_bounding_box_line_list(
   const autoware_auto_perception_msgs::msg::Shape & shape,
   std::vector<geometry_msgs::msg::Point> & points)
@@ -392,6 +424,49 @@ void calc_bounding_box_line_list(
   points.push_back(point);
 }
 
+void calc_2d_bounding_box_bottom_line_list(
+  const autoware_auto_perception_msgs::msg::Shape & shape,
+  std::vector<geometry_msgs::msg::Point> & points)
+{
+  geometry_msgs::msg::Point point;
+  // down surface
+  point.x = shape.dimensions.x / 2.0;
+  point.y = shape.dimensions.y / 2.0;
+  point.z = -shape.dimensions.z / 2.0;
+  points.push_back(point);
+  point.x = -shape.dimensions.x / 2.0;
+  point.y = shape.dimensions.y / 2.0;
+  point.z = -shape.dimensions.z / 2.0;
+  points.push_back(point);
+
+  point.x = shape.dimensions.x / 2.0;
+  point.y = shape.dimensions.y / 2.0;
+  point.z = -shape.dimensions.z / 2.0;
+  points.push_back(point);
+  point.x = shape.dimensions.x / 2.0;
+  point.y = -shape.dimensions.y / 2.0;
+  point.z = -shape.dimensions.z / 2.0;
+  points.push_back(point);
+
+  point.x = -shape.dimensions.x / 2.0;
+  point.y = shape.dimensions.y / 2.0;
+  point.z = -shape.dimensions.z / 2.0;
+  points.push_back(point);
+  point.x = -shape.dimensions.x / 2.0;
+  point.y = -shape.dimensions.y / 2.0;
+  point.z = -shape.dimensions.z / 2.0;
+  points.push_back(point);
+
+  point.x = shape.dimensions.x / 2.0;
+  point.y = -shape.dimensions.y / 2.0;
+  point.z = -shape.dimensions.z / 2.0;
+  points.push_back(point);
+  point.x = -shape.dimensions.x / 2.0;
+  point.y = -shape.dimensions.y / 2.0;
+  point.z = -shape.dimensions.z / 2.0;
+  points.push_back(point);
+}
+
 void calc_cylinder_line_list(
   const autoware_auto_perception_msgs::msg::Shape & shape,
   std::vector<geometry_msgs::msg::Point> & points)
@@ -435,6 +510,21 @@ void calc_cylinder_line_list(
   }
 }
 
+void calc_2d_cylinder_bottom_line_list(
+  const autoware_auto_perception_msgs::msg::Shape & shape,
+  std::vector<geometry_msgs::msg::Point> & points)
+{
+  const double radius = shape.dimensions.x * 0.5;
+  {
+    constexpr int n = 20;
+    geometry_msgs::msg::Point center;
+    center.x = 0.0;
+    center.y = 0.0;
+    center.z = -shape.dimensions.z * 0.5;
+    calc_circle_line_list(center, radius, points, n);
+  }
+}
+
 void calc_circle_line_list(
   const geometry_msgs::msg::Point center, const double radius,
   std::vector<geometry_msgs::msg::Point> & points, const int n)
@@ -521,6 +611,33 @@ void calc_polygon_line_list(
   }
 }
 
+void calc_2d_polygon_bottom_line_list(
+  const autoware_auto_perception_msgs::msg::Shape & shape,
+  std::vector<geometry_msgs::msg::Point> & points)
+{
+  if (shape.footprint.points.size() < 2) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("ObjectPolygonDisplayBase"),
+      "there are no enough footprint to visualize polygon");
+    return;
+  }
+  for (size_t i = 0; i < shape.footprint.points.size(); ++i) {
+    geometry_msgs::msg::Point point;
+    point.x = shape.footprint.points.at(i).x;
+    point.y = shape.footprint.points.at(i).y;
+    point.z = -shape.dimensions.z / 2.0;
+    points.push_back(point);
+    point.x = shape.footprint.points
+                .at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
+                .x;
+    point.y = shape.footprint.points
+                .at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
+                .y;
+    point.z = -shape.dimensions.z / 2.0;
+    points.push_back(point);
+  }
+}
+
 void calc_path_line_list(
   const autoware_auto_perception_msgs::msg::PredictedPath & paths,
   std::vector<geometry_msgs::msg::Point> & points)