fix(radar_fusion_to_detected_object): make has_twist_covariance enable (

tier4#1692) * add hasTwistCovariance Signed-off-by: scepter914 <scepter914@gmail.com> * enable has_twist_covariance Signed-off-by: scepter914 <scepter914@gmail.com> * fix to magic number Signed-off-by: scepter914 <scepter914@gmail.com> Signed-off-by: scepter914 <scepter914@gmail.com>
boyali · Oct 19, 2022 · 3d7c627 · 3d7c627
1 parent 6e88818
commit 3d7c627
Show file tree

Hide file tree

Showing 2 changed files with 55 additions and 0 deletions.
diff --git a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp
@@ -42,6 +42,45 @@ using geometry_msgs::msg::TwistWithCovariance;
 using tier4_autoware_utils::LinearRing2d;
 using tier4_autoware_utils::Point2d;
 
+enum MSG_COV_IDX {
+  X_X = 0,
+  X_Y = 1,
+  X_Z = 2,
+  X_ROLL = 3,
+  X_PITCH = 4,
+  X_YAW = 5,
+  Y_X = 6,
+  Y_Y = 7,
+  Y_Z = 8,
+  Y_ROLL = 9,
+  Y_PITCH = 10,
+  Y_YAW = 11,
+  Z_X = 12,
+  Z_Y = 13,
+  Z_Z = 14,
+  Z_ROLL = 15,
+  Z_PITCH = 16,
+  Z_YAW = 17,
+  ROLL_X = 18,
+  ROLL_Y = 19,
+  ROLL_Z = 20,
+  ROLL_ROLL = 21,
+  ROLL_PITCH = 22,
+  ROLL_YAW = 23,
+  PITCH_X = 24,
+  PITCH_Y = 25,
+  PITCH_Z = 26,
+  PITCH_ROLL = 27,
+  PITCH_PITCH = 28,
+  PITCH_YAW = 29,
+  YAW_X = 30,
+  YAW_Y = 31,
+  YAW_Z = 32,
+  YAW_ROLL = 33,
+  YAW_PITCH = 34,
+  YAW_YAW = 35
+};
+
 class RadarFusionToDetectedObject
 {
 public:
@@ -107,6 +146,7 @@ class RadarFusionToDetectedObject
   bool isYawCorrect(
     const DetectedObject & object, const TwistWithCovariance & twist_with_covariance,
     const double & yaw_threshold);
+  bool hasTwistCovariance(const TwistWithCovariance & twist_with_covariance);
   Eigen::Vector2d toVector2d(const TwistWithCovariance & twist_with_covariance);
   TwistWithCovariance toTwistWithCovariance(const Eigen::Vector2d & vector2d);
 

diff --git a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp
@@ -108,6 +108,9 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update(
         if (isYawCorrect(split_object, twist_with_covariance, param_.threshold_yaw_diff)) {
           split_object.kinematics.twist_with_covariance = twist_with_covariance;
           split_object.kinematics.has_twist = true;
+          if (hasTwistCovariance(twist_with_covariance)) {
+            split_object.kinematics.has_twist_covariance = true;
+          }
         }
       }
 
@@ -126,6 +129,18 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update(
   return output;
 }
 
+// Judge whether twist covariance is available.
+bool RadarFusionToDetectedObject::hasTwistCovariance(
+  const TwistWithCovariance & twist_with_covariance)
+{
+  auto covariance = twist_with_covariance.covariance;
+  if (covariance[X_X] == 0.0 && covariance[Y_Y] == 0.0 && covariance[Z_Z] == 0.0) {
+    return false;
+  } else {
+    return true;
+  }
+}
+
 // Judge whether object's yaw is same direction with twist's yaw.
 // This function improve multi object tracking with observed speed.
 bool RadarFusionToDetectedObject::isYawCorrect(