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 9feb7433422c2..5631714b946cd 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 @@ -52,6 +52,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; + vel_vis_position.y = object.kinematics.pose_with_covariance.pose.position.y - 1.0; + vel_vis_position.z = object.kinematics.pose_with_covariance.pose.position.z - 1.0; + 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); + } } }