From 938334ef93fd530f411ee0d06c27acadb245b7da Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Wed, 24 Aug 2022 23:41:37 +0900 Subject: [PATCH] feat(autoware_auto_perception_rviz_plugin): add twist information for detected object on rviz (#1456) Signed-off-by: scepter914 --- .../detected_objects_display.cpp | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 63874696b269c..0dd414addf802 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -31,6 +31,7 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) clear_markers(); int id = 0; for (const auto & object : msg->objects) { + // TODO(Satoshi Tanaka): fixing from string label to one string // Get marker for shape auto shape_marker = get_shape_marker_ptr( object.shape, object.kinematics.pose_with_covariance.pose.position, @@ -53,6 +54,28 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) label_marker_ptr->id = id++; add_marker(label_marker_ptr); } + + // Get marker for velocity text + geometry_msgs::msg::Point vel_vis_position; + vel_vis_position.x = object.kinematics.pose_with_covariance.pose.position.x - 0.5; + vel_vis_position.y = object.kinematics.pose_with_covariance.pose.position.y; + vel_vis_position.z = object.kinematics.pose_with_covariance.pose.position.z - 0.5; + auto velocity_text_marker = get_velocity_text_marker_ptr( + object.kinematics.twist_with_covariance.twist, vel_vis_position, object.classification); + if (velocity_text_marker) { + auto velocity_text_marker_ptr = velocity_text_marker.value(); + velocity_text_marker_ptr->header = msg->header; + add_marker(velocity_text_marker_ptr); + } + + // Get marker for twist + auto twist_marker = get_twist_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance); + if (twist_marker) { + auto twist_marker_ptr = twist_marker.value(); + twist_marker_ptr->header = msg->header; + add_marker(twist_marker_ptr); + } } }