diff --git a/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp b/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp index fed20e2..5d50cd8 100644 --- a/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp +++ b/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, diff --git a/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index b05ba5f..ad25e2a 100644 --- a/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +++ b/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 + std::optional 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 std::optional 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 diff --git a/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 9dd0b29..7372eb9 100644 --- a/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/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; diff --git a/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 5742e6b..5fd4a11 100644 --- a/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/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_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,