feat(autoware_perception_rviz_plugin): add area visualization for det…

autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp

@@ -111,6 +111,11 @@ get_existence_probability_marker_ptr(
   const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
   const float existence_probability, const std_msgs::msg::ColorRGBA & color_rgba);
 
+AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
+get_area_marker_ptr(
+  const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
+  const float area, const std_msgs::msg::ColorRGBA & color_rgba);
+
 AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
 get_uuid_marker_ptr(
   const std::string & uuid, const geometry_msgs::msg::Point & centroid,

autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp

@@ -248,7 +248,18 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
       return std::nullopt;
     }
   }
-
+  template <typename ClassificationContainerT>
+  std::optional<Marker::SharedPtr> get_area_marker_ptr(
+    const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
+    const float area, const ClassificationContainerT & labels) const
+  {
+    if (m_display_area_property.getBool()) {
+      const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels);
+      return detail::get_area_marker_ptr(centroid, orientation, area, color_rgba);
+    } else {
+      return std::nullopt;
+    }
+  }
   template <typename ClassificationContainerT>
   std::optional<Marker::SharedPtr> get_uuid_marker_ptr(
     const unique_identifier_msgs::msg::UUID & uuid, const geometry_msgs::msg::Point & centroid,
@@ -565,6 +576,9 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
 
   rviz_common::properties::BoolProperty m_display_existence_probability_property;
 
+  rviz_common::properties::BoolProperty m_display_area_property{
+    "Display Area", false, "Enable/disable area visualization", this};
+
   // Property to decide line width of object shape
   rviz_common::properties::FloatProperty m_line_width_property;
   // Default topic name to be visualized

autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp

@@ -27,7 +27,26 @@ namespace object_detection
 DetectedObjectsDisplay::DetectedObjectsDisplay() : ObjectPolygonDisplayBase("detected_objects")
 {
 }
-
+double getArea(const autoware_perception_msgs::msg::Shape & shape)
+{
+  switch (shape.type) {
+    case autoware_perception_msgs::msg::Shape::BOUNDING_BOX:
+      return shape.dimensions.x * shape.dimensions.y;
+    case autoware_perception_msgs::msg::Shape::CYLINDER:
+      return shape.dimensions.x * shape.dimensions.x * M_PI * 0.25;
+    case autoware_perception_msgs::msg::Shape::POLYGON: {
+      double area = 0.0;
+      for (size_t i = 0; i < shape.footprint.points.size(); ++i) {
+        size_t j = (i + 1) % shape.footprint.points.size();
+        area += 0.5 * (shape.footprint.points.at(i).x * shape.footprint.points.at(j).y -
+                       shape.footprint.points.at(j).x * shape.footprint.points.at(i).y);
+      }
+      return area;
+    }
+    default:
+      return 0.0;
+  }
+}
 void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg)
 {
   clear_markers();
@@ -79,6 +98,20 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg)
       marker_ptr->id = id++;
       add_marker(marker_ptr);
     }
+    // Get marker for area
+    geometry_msgs::msg::Point area_position;
+    area_position.x = object.kinematics.pose_with_covariance.pose.position.x + 0.5;
+    area_position.y = object.kinematics.pose_with_covariance.pose.position.y;
+    area_position.z = object.kinematics.pose_with_covariance.pose.position.z - 1.0;
+    auto area_marker = get_area_marker_ptr(
+      area_position, object.kinematics.pose_with_covariance.pose.orientation, getArea(object.shape),
+      object.classification);
+    if (area_marker) {
+      auto area_marker_ptr = area_marker.value();
+      area_marker_ptr->header = msg->header;
+      area_marker_ptr->id = id++;
+      add_marker(area_marker_ptr);
+    }
 
     // Get marker for existence probability
     geometry_msgs::msg::Point existence_probability_position;

autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp

@@ -491,6 +491,22 @@ visualization_msgs::msg::Marker::SharedPtr get_existence_probability_marker_ptr(
   return marker_ptr;
 }
 
+visualization_msgs::msg::Marker::SharedPtr get_area_marker_ptr(
+  const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
+  const float area, const std_msgs::msg::ColorRGBA & color_rgba)
+{
+  auto marker_ptr = std::make_shared<Marker>();
+  marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+  marker_ptr->ns = std::string("area");
+  marker_ptr->scale.x = 0.5;
+  marker_ptr->scale.z = 0.5;
+  marker_ptr->text = std::to_string(area);
+  marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
+  marker_ptr->pose = marker_ptr->pose = to_pose(centroid, orientation);
+  marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
+  marker_ptr->color = color_rgba;
+  return marker_ptr;
+}
 visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr(
   const autoware_perception_msgs::msg::Shape & shape_msg,
   const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,