feat(autoware_auto_perception_rviz_plugin): add accel text visualizat…

…ion (#2046)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp

@@ -22,6 +22,7 @@
 #include <autoware_auto_perception_msgs/msg/object_classification.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_path.hpp>
 #include <autoware_auto_perception_msgs/msg/shape.hpp>
+#include <geometry_msgs/msg/accel.hpp>
 #include <geometry_msgs/msg/pose_with_covariance.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 #include <geometry_msgs/msg/twist_with_covariance.hpp>
@@ -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,

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 <typename ClassificationContainerT>
+  std::optional<Marker::SharedPtr> 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<Marker::SharedPtr> 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

common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp

@@ -20,10 +20,24 @@
 
 #include <algorithm>
 #include <cmath>
+#include <iomanip>
 #include <memory>
+#include <sstream>
 #include <string>
 #include <vector>
 
+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>();
+  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)
 {

common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp

@@ -118,6 +118,21 @@ std::vector<visualization_msgs::msg::Marker::SharedPtr> 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,

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);