fix(radar_converter_outputs): add DetectedObjects output (autowarefou…

…ndation#3450)

* fix(radar_tracks_msgs_converter_node): add DetectedObjects output

Signed-off-by: scepter914 <scepter914@gmail.com>

* fix typo

Signed-off-by: scepter914 <scepter914@gmail.com>

* fix(radar_tracks_msgs_converter_node): add DetectedObjects output

Signed-off-by: scepter914 <scepter914@gmail.com>

* fix typo

Signed-off-by: scepter914 <scepter914@gmail.com>

* update launcher

Signed-off-by: scepter914 <scepter914@gmail.com>

---------

Signed-off-by: scepter914 <scepter914@gmail.com>
Signed-off-by: Mingyu Li <mingyu.li@tier4.jp>

perception/radar_tracks_msgs_converter/README.md

@@ -1,6 +1,6 @@
 # radar_tracks_msgs_converter
 
-This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTracks.msg) into [autoware_auto_perception_msgs/msg/TrackedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/TrackedObject.idl).
+This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTracks.msg) into [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) and [autoware_auto_perception_msgs/msg/TrackedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/TrackedObject.idl).
 
 - Calculation cost is O(n).
   - n: The number of radar objects
@@ -13,7 +13,8 @@ This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-pe
   - `~/input/radar_objects` (radar_msgs/msg/RadarTracks.msg): Input radar topic
   - `~/input/odometry` (nav_msgs/msg/Odometry.msg): Ego vehicle odometry topic
 - Output
-  - `~/output/radar_objects` (autoware_auto_perception_msgs/msg/TrackedObject.msg): The topic converted to Autoware's message
+  - `~/output/radar_detected_objects` (autoware_auto_perception_msgs/msg/DetectedObject.idl): The topic converted to Autoware's message
+  - `~/output/radar_tracked_objects` (autoware_auto_perception_msgs/msg/TrackedObject.idl): The topic converted to Autoware's message
 
 ### Parameters
 

perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp

@@ -18,6 +18,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "tier4_autoware_utils/tier4_autoware_utils.hpp"
 
+#include "autoware_auto_perception_msgs/msg/detected_objects.hpp"
 #include "autoware_auto_perception_msgs/msg/object_classification.hpp"
 #include "autoware_auto_perception_msgs/msg/shape.hpp"
 #include "autoware_auto_perception_msgs/msg/tracked_object.hpp"
@@ -33,6 +34,9 @@
 
 namespace radar_tracks_msgs_converter
 {
+using autoware_auto_perception_msgs::msg::DetectedObject;
+using autoware_auto_perception_msgs::msg::DetectedObjectKinematics;
+using autoware_auto_perception_msgs::msg::DetectedObjects;
 using autoware_auto_perception_msgs::msg::ObjectClassification;
 using autoware_auto_perception_msgs::msg::Shape;
 using autoware_auto_perception_msgs::msg::TrackedObject;
@@ -69,7 +73,8 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node
   geometry_msgs::msg::TransformStamped::ConstSharedPtr transform_;
 
   // Publisher
-  rclcpp::Publisher<TrackedObjects>::SharedPtr pub_radar_{};
+  rclcpp::Publisher<TrackedObjects>::SharedPtr pub_tracked_objects_{};
+  rclcpp::Publisher<DetectedObjects>::SharedPtr pub_detected_objects_{};
 
   // Timer
   rclcpp::TimerBase::SharedPtr timer_{};
@@ -86,7 +91,9 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node
   NodeParam node_param_{};
 
   // Core
+  geometry_msgs::msg::PoseWithCovariance convertPoseWithCovariance();
   TrackedObjects convertRadarTrackToTrackedObjects();
+  DetectedObjects convertRadarTrackToDetectedObjects();
   uint8_t convertClassification(const uint16_t classification);
 };
 

perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml

@@ -1,14 +1,16 @@
 <launch>
   <arg name="input/radar_objects" default="input/radar_objects"/>
   <arg name="input/odometry" default="/localization/kinematic_state"/>
-  <arg name="output/radar_objects" default="output/radar_objects"/>
+  <arg name="output/radar_detected_objects" default="output/radar_detected_objects"/>
+  <arg name="output/radar_tracked_objects" default="output/radar_tracked_objects"/>
   <arg name="update_rate_hz" default="20.0"/>
   <arg name="use_twist_compensation" default="false"/>
 
   <node pkg="radar_tracks_msgs_converter" exec="radar_tracks_msgs_converter_node" name="radar_tracks_msgs_converter" output="screen">
     <remap from="~/input/radar_objects" to="$(var input/radar_objects)"/>
     <remap from="~/input/odometry" to="$(var input/odometry)"/>
-    <remap from="~/output/radar_objects" to="$(var output/radar_objects)"/>
+    <remap from="~/output/radar_detected_objects" to="$(var output/radar_detected_objects)"/>
+    <remap from="~/output/radar_tracked_objects" to="$(var output/radar_tracked_objects)"/>
     <param name="update_rate_hz" value="$(var update_rate_hz)"/>
     <param name="use_twist_compensation" value="$(var use_twist_compensation)"/>
   </node>

perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp

@@ -88,7 +88,8 @@ RadarTracksMsgsConverterNode::RadarTracksMsgsConverterNode(const rclcpp::NodeOpt
   transform_listener_ = std::make_shared<tier4_autoware_utils::TransformListener>(this);
 
   // Publisher
-  pub_radar_ = create_publisher<TrackedObjects>("~/output/radar_objects", 1);
+  pub_tracked_objects_ = create_publisher<TrackedObjects>("~/output/radar_tracked_objects", 1);
+  pub_detected_objects_ = create_publisher<DetectedObjects>("~/output/radar_detected_objects", 1);
 
   // Timer
   const auto update_period_ns = rclcpp::Rate(node_param_.update_rate_hz).period();
@@ -152,11 +153,99 @@ void RadarTracksMsgsConverterNode::onTimer()
     node_param_.new_frame_id, header.frame_id, header.stamp, rclcpp::Duration::from_seconds(0.01));
 
   TrackedObjects tracked_objects = convertRadarTrackToTrackedObjects();
+  DetectedObjects detected_objects = convertRadarTrackToDetectedObjects();
   if (!tracked_objects.objects.empty()) {
-    pub_radar_->publish(tracked_objects);
+    pub_tracked_objects_->publish(tracked_objects);
+    pub_detected_objects_->publish(detected_objects);
   }
 }
 
+DetectedObjects RadarTracksMsgsConverterNode::convertRadarTrackToDetectedObjects()
+{
+  DetectedObjects detected_objects;
+  detected_objects.header = radar_data_->header;
+  detected_objects.header.frame_id = node_param_.new_frame_id;
+  using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
+  using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX;
+
+  for (auto & radar_track : radar_data_->tracks) {
+    DetectedObject detected_object;
+
+    detected_object.existence_probability = 1.0;
+
+    detected_object.shape.type = Shape::BOUNDING_BOX;
+    detected_object.shape.dimensions = radar_track.size;
+
+    // kinematics
+    DetectedObjectKinematics kinematics;
+    kinematics.has_twist = true;
+    kinematics.has_twist_covariance = true;
+
+    // convert by tf
+    geometry_msgs::msg::PoseStamped radar_pose_stamped{};
+    radar_pose_stamped.pose.position = radar_track.position;
+    geometry_msgs::msg::PoseStamped transformed_pose_stamped{};
+    tf2::doTransform(radar_pose_stamped, transformed_pose_stamped, *transform_);
+    kinematics.pose_with_covariance.pose = transformed_pose_stamped.pose;
+
+    {
+      auto & pose_cov = kinematics.pose_with_covariance.covariance;
+      auto & radar_position_cov = radar_track.position_covariance;
+      pose_cov[POSE_IDX::X_X] = radar_position_cov[RADAR_IDX::X_X];
+      pose_cov[POSE_IDX::X_Y] = radar_position_cov[RADAR_IDX::X_Y];
+      pose_cov[POSE_IDX::X_Z] = radar_position_cov[RADAR_IDX::X_Z];
+      pose_cov[POSE_IDX::Y_X] = radar_position_cov[RADAR_IDX::X_Y];
+      pose_cov[POSE_IDX::Y_Y] = radar_position_cov[RADAR_IDX::Y_Y];
+      pose_cov[POSE_IDX::Y_Z] = radar_position_cov[RADAR_IDX::Y_Z];
+      pose_cov[POSE_IDX::Z_X] = radar_position_cov[RADAR_IDX::X_Z];
+      pose_cov[POSE_IDX::Z_Y] = radar_position_cov[RADAR_IDX::Y_Z];
+      pose_cov[POSE_IDX::Z_Z] = radar_position_cov[RADAR_IDX::Z_Z];
+    }
+
+    // convert by tf
+    geometry_msgs::msg::Vector3Stamped radar_velocity_stamped{};
+    radar_velocity_stamped.vector = radar_track.velocity;
+    geometry_msgs::msg::Vector3Stamped transformed_vector3_stamped{};
+    tf2::doTransform(radar_velocity_stamped, transformed_vector3_stamped, *transform_);
+    kinematics.twist_with_covariance.twist.linear = transformed_vector3_stamped.vector;
+
+    // twist compensation
+    if (node_param_.use_twist_compensation) {
+      if (odometry_data_) {
+        kinematics.twist_with_covariance.twist.linear.x += odometry_data_->twist.twist.linear.x;
+        kinematics.twist_with_covariance.twist.linear.y += odometry_data_->twist.twist.linear.y;
+        kinematics.twist_with_covariance.twist.linear.z += odometry_data_->twist.twist.linear.z;
+      } else {
+        RCLCPP_INFO(get_logger(), "Odometry data is not coming");
+      }
+    }
+
+    {
+      auto & twist_cov = kinematics.twist_with_covariance.covariance;
+      auto & radar_vel_cov = radar_track.velocity_covariance;
+      twist_cov[POSE_IDX::X_X] = radar_vel_cov[RADAR_IDX::X_X];
+      twist_cov[POSE_IDX::X_Y] = radar_vel_cov[RADAR_IDX::X_Y];
+      twist_cov[POSE_IDX::X_Z] = radar_vel_cov[RADAR_IDX::X_Z];
+      twist_cov[POSE_IDX::Y_X] = radar_vel_cov[RADAR_IDX::X_Y];
+      twist_cov[POSE_IDX::Y_Y] = radar_vel_cov[RADAR_IDX::Y_Y];
+      twist_cov[POSE_IDX::Y_Z] = radar_vel_cov[RADAR_IDX::Y_Z];
+      twist_cov[POSE_IDX::Z_X] = radar_vel_cov[RADAR_IDX::X_Z];
+      twist_cov[POSE_IDX::Z_Y] = radar_vel_cov[RADAR_IDX::Y_Z];
+      twist_cov[POSE_IDX::Z_Z] = radar_vel_cov[RADAR_IDX::Z_Z];
+    }
+    detected_object.kinematics = kinematics;
+
+    // classification
+    ObjectClassification classification;
+    classification.probability = 1.0;
+    classification.label = convertClassification(radar_track.classification);
+    detected_object.classification.emplace_back(classification);
+
+    detected_objects.objects.emplace_back(detected_object);
+  }
+  return detected_objects;
+}
+
 TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects()
 {
   TrackedObjects tracked_objects;