feat(radar tracks msgs converter): implement dynamic status flag 'is_…

…stationary' of the tracked object message, fix ego motion compensation (autowarefoundation#5823) * fix(radar-tracks-msgs-converter): ground speed compensation, consider yaw rate Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: determine static object fix: remain only the static object flag Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> feat(perception_rviz_plugin): select visualizing object dynamic status fix(perception_rviz_plugin): option only visible in tracked object chore: typo fix Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> chore: change rviz configuration name Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> fix(perception_rviz_plugin): reduce configuration check Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> fix: reduce method complexity Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> fix: object dynamics function, fix azimuth angle Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> remove rviz plugin changes Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fis: set a flag for yaw_rate compensation, default is 'false' Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: remove abbreviation from parameter Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: change definition of is_stationary logic Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: velocity compensation functions to return value Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: define matrix convert functions separately Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: refactoring for clang-tidy Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: refactoring for clang-tidy 2 Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: refactoring for clang-tidy 3 Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: revert method name changes, align variable precisions Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> --------- Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> Co-authored-by: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com>
h-ohta · Dec 22, 2023 · 3a0fd31 · 3a0fd31
1 parent eb9e1e9
commit 3a0fd31
Show file tree

Hide file tree

Showing 4 changed files with 180 additions and 77 deletions.
diff --git a/perception/radar_tracks_msgs_converter/README.md b/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/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).
+This package converts 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
@@ -20,10 +20,14 @@ This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-pe
 
 - `update_rate_hz` (double): The update rate [hz].
   - Default parameter is 20.0
-- `new_frame_id` (string): The header frame of output topic.
+- `new_frame_id` (string): The header frame of the output topic.
   - Default parameter is "base_link"
-- `use_twist_compensation` (bool): If the parameter is true, then the twist of output objects' topic is compensated by ego vehicle motion.
+- `use_twist_compensation` (bool): If the parameter is true, then the twist of the output objects' topic is compensated by ego vehicle motion.
   - Default parameter is "false"
+- `use_twist_yaw_compensation` (bool): If the parameter is true, then the ego motion compensation will also consider yaw motion of the ego vehicle.
+  - Default parameter is "false"
+- `static_object_speed_threshold` (float): Specify the threshold for static object speed which determines the flag `is_stationary` [m/s].
+  - Default parameter is 1.0
 
 ## Note
 
@@ -42,4 +46,5 @@ Label id is defined as below.
 | PEDESTRIAN | 32007      | 7        |
 
 - [radar_msgs/msg/RadarTrack.msg](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTrack.msg): additional vendor-specific classifications are permitted starting from 32000.
+
 - [Autoware objects label](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/ObjectClassification.idl)
diff --git a/...s_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp b/...s_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp
@@ -1,4 +1,4 @@
-// Copyright 2022 TIER IV, Inc.
+// Copyright 2022-2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -43,6 +43,7 @@ using autoware_auto_perception_msgs::msg::TrackedObject;
 using autoware_auto_perception_msgs::msg::TrackedObjectKinematics;
 using autoware_auto_perception_msgs::msg::TrackedObjects;
 using nav_msgs::msg::Odometry;
+using radar_msgs::msg::RadarTrack;
 using radar_msgs::msg::RadarTracks;
 
 class RadarTracksMsgsConverterNode : public rclcpp::Node
@@ -55,6 +56,8 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node
     double update_rate_hz{};
     std::string new_frame_id{};
     bool use_twist_compensation{};
+    bool use_twist_yaw_compensation{};
+    double static_object_speed_threshold{};
   };
 
 private:
@@ -94,6 +97,20 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node
   geometry_msgs::msg::PoseWithCovariance convertPoseWithCovariance();
   TrackedObjects convertRadarTrackToTrackedObjects();
   DetectedObjects convertTrackedObjectsToDetectedObjects(TrackedObjects & objects);
+  geometry_msgs::msg::Vector3 compensateVelocitySensorPosition(
+    const radar_msgs::msg::RadarTrack & radar_track);
+  geometry_msgs::msg::Vector3 compensateVelocityEgoMotion(
+    const geometry_msgs::msg::Vector3 & velocity_in,
+    const geometry_msgs::msg::Point & position_from_veh);
+  bool isStaticObject(
+    const radar_msgs::msg::RadarTrack & radar_track,
+    const geometry_msgs::msg::Vector3 & compensated_velocity);
+  std::array<double, 36> convertPoseCovarianceMatrix(
+    const radar_msgs::msg::RadarTrack & radar_track);
+  std::array<double, 36> convertTwistCovarianceMatrix(
+    const radar_msgs::msg::RadarTrack & radar_track);
+  std::array<double, 36> convertAccelerationCovarianceMatrix(
+    const radar_msgs::msg::RadarTrack & radar_track);
   uint8_t convertClassification(const uint16_t classification);
 };
 

diff --git a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml b/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml
@@ -5,6 +5,8 @@
   <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"/>
+  <arg name="use_twist_yaw_compensation" default="false"/>
+  <arg name="static_object_speed_threshold" default="1.0"/>
 
   <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)"/>
@@ -13,5 +15,7 @@
     <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)"/>
+    <param name="use_twist_yaw_compensation" value="$(var use_twist_yaw_compensation)"/>
+    <param name="static_object_speed_threshold" value="$(var static_object_speed_threshold)"/>
   </node>
 </launch>