From af979538a889e784111df96c9dd9369a933c7130 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 18 Oct 2022 11:13:38 +0900 Subject: [PATCH] feat(autoware_auto_perception_rviz_plugin): add accel text visualization (#2046) Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- .../object_polygon_detail.hpp | 6 ++++ .../object_polygon_display_base.hpp | 17 +++++++++ .../object_polygon_detail.cpp | 35 +++++++++++++++++++ .../predicted_objects_display.cpp | 15 ++++++++ .../tracked_objects_display.cpp | 15 ++++++++ 5 files changed, 88 insertions(+) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp index 8456b0886cd1d..404ef468a50b3 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -105,6 +106,11 @@ get_velocity_text_marker_ptr( const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, const std_msgs::msg::ColorRGBA & color_rgba); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_acceleration_text_marker_ptr( + const geometry_msgs::msg::Accel & accel, const geometry_msgs::msg::Point & vis_pos, + const std_msgs::msg::ColorRGBA & color_rgba); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 706271541872b..2a5c7e53f484c 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -68,6 +68,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase this}, m_display_velocity_text_property{ "Display Velocity", true, "Enable/disable velocity text visualization", this}, + m_display_acceleration_text_property{ + "Display Acceleration", true, "Enable/disable acceleration text visualization", this}, m_display_twist_property{"Display Twist", true, "Enable/disable twist visualization", this}, m_display_predicted_paths_property{ "Display Predicted Paths", true, "Enable/disable predicted paths visualization", this}, @@ -212,6 +214,19 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } + template + std::optional get_acceleration_text_marker_ptr( + const geometry_msgs::msg::Accel & accel, const geometry_msgs::msg::Point & vis_pos, + const ClassificationContainerT & labels) const + { + if (m_display_acceleration_text_property.getBool()) { + const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); + return detail::get_acceleration_text_marker_ptr(accel, vis_pos, color_rgba); + } else { + return std::nullopt; + } + } + std::optional get_twist_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) const @@ -361,6 +376,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase rviz_common::properties::BoolProperty m_display_pose_with_covariance_property; // Property to enable/disable velocity text visualization rviz_common::properties::BoolProperty m_display_velocity_text_property; + // Property to enable/disable acceleration text visualization + rviz_common::properties::BoolProperty m_display_acceleration_text_property; // Property to enable/disable twist visualization rviz_common::properties::BoolProperty m_display_twist_property; // Property to enable/disable predicted paths visualization diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 937286444ea7b..2ef88880987be 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -20,10 +20,24 @@ #include #include +#include #include +#include #include #include +namespace +{ +// get string of double value rounded after first decimal place +// e.g. roundAfterFirstDecimalPlace(12.345) -> "1.2" +std::string getRoundedDoubleString(const double val) +{ + std::stringstream ss; + ss << std::fixed << std::setprecision(1) << val; + return ss.str(); +} +} // namespace + namespace autoware { namespace rviz_plugins @@ -130,6 +144,27 @@ visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr( return marker_ptr; } +visualization_msgs::msg::Marker::SharedPtr get_acceleration_text_marker_ptr( + const geometry_msgs::msg::Accel & accel, const geometry_msgs::msg::Point & vis_pos, + 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("acceleration"); + marker_ptr->scale.x = 0.5; + marker_ptr->scale.z = 0.5; + + double acc = std::sqrt( + accel.linear.x * accel.linear.x + accel.linear.y * accel.linear.y + + accel.linear.z * accel.linear.z); + marker_ptr->text = getRoundedDoubleString(acc) + std::string("[m/s^2]"); + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose.position = vis_pos; + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->color = color_rgba; + return marker_ptr; +} + visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) { diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index 4edbe26d6284f..870b8f2c77543 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -118,6 +118,21 @@ std::vector PredictedObjectsDisplay: markers.push_back(velocity_text_marker_ptr); } + // Get marker for acceleration text + geometry_msgs::msg::Point acc_vis_position; + acc_vis_position.x = uuid_vis_position.x - 1.0; + acc_vis_position.y = uuid_vis_position.y; + acc_vis_position.z = uuid_vis_position.z - 1.0; + auto acceleration_text_marker = get_acceleration_text_marker_ptr( + object.kinematics.initial_acceleration_with_covariance.accel, acc_vis_position, + object.classification); + if (acceleration_text_marker) { + auto acceleration_text_marker_ptr = acceleration_text_marker.value(); + acceleration_text_marker_ptr->header = msg->header; + acceleration_text_marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(acceleration_text_marker_ptr); + } + // Get marker for twist auto twist_marker = get_twist_marker_ptr( object.kinematics.initial_pose_with_covariance, diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index c84c8adbefa27..57e3ce905d85d 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -94,6 +94,21 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) add_marker(velocity_text_marker_ptr); } + // Get marker for acceleration text + geometry_msgs::msg::Point acc_vis_position; + acc_vis_position.x = uuid_vis_position.x - 1.0; + acc_vis_position.y = uuid_vis_position.y; + acc_vis_position.z = uuid_vis_position.z - 1.0; + auto acceleration_text_marker = get_acceleration_text_marker_ptr( + object.kinematics.acceleration_with_covariance.accel, acc_vis_position, + object.classification); + if (acceleration_text_marker) { + auto acceleration_text_marker_ptr = acceleration_text_marker.value(); + acceleration_text_marker_ptr->header = msg->header; + acceleration_text_marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(acceleration_text_marker_ptr); + } + // Get marker for twist auto twist_marker = get_twist_marker_ptr( object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance);