Switch twist source for Detections (#2219)

* #2218 Switch twist source for Detections The external object to detection conversion function was using velocity_inst instead of velocity. The former has twist values but not the corresponding covariance matrixes. * #2218 Replace remaining velocity_inst usages --------- Co-authored-by: Misheel Bayartsengel <[email protected]>
usdot-fhwa-stol · Dec 16, 2023 · 4eca5ae · 4eca5ae
1 parent 426a195
commit 4eca5ae
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 21 deletions.
diff --git a/carma_cooperative_perception/src/msg_conversion.cpp b/carma_cooperative_perception/src/msg_conversion.cpp
@@ -314,8 +314,8 @@ auto to_detection_msg(
     detection.pose = object.pose;
   }
 
-  if (object.presence_vector & object.VELOCITY_INST_PRESENCE_VECTOR) {
-    detection.twist = object.velocity_inst;
+  if (object.presence_vector & object.VELOCITY_PRESENCE_VECTOR) {
+    detection.twist = object.velocity;
   }
 
   if (object.presence_vector & object.OBJECT_TYPE_PRESENCE_VECTOR) {
@@ -472,13 +472,13 @@ auto to_detected_object_data_msg(
   }
 
   // speed/speed_z - convert vector velocity to scalar speed val given x/y components
-  if (external_object.presence_vector & external_object.VELOCITY_INST_PRESENCE_VECTOR) {
+  if (external_object.presence_vector & external_object.VELOCITY_PRESENCE_VECTOR) {
     detected_object_common_data.speed.speed = std::hypot(
-      external_object.velocity_inst.twist.linear.x, external_object.velocity_inst.twist.linear.y);
+      external_object.velocity.twist.linear.x, external_object.velocity.twist.linear.y);
 
     detected_object_common_data.presence_vector |=
       carma_v2x_msgs::msg::DetectedObjectCommonData::HAS_SPEED_Z;
-    detected_object_common_data.speed_z.speed = external_object.velocity_inst.twist.linear.z;
+    detected_object_common_data.speed_z.speed = external_object.velocity.twist.linear.z;
 
     // heading - convert ang vel to scale heading
     lanelet::BasicPoint3d external_object_position{

diff --git a/carma_cooperative_perception/test/test_msg_conversion.cpp b/carma_cooperative_perception/test/test_msg_conversion.cpp
@@ -170,16 +170,16 @@ TEST(ToDetectionMsg, FromExternalObject)
   object.pose.pose.orientation.y = 12;
   object.pose.pose.orientation.z = 13;
   object.pose.pose.orientation.w = 14;
-  object.velocity_inst.twist.linear.x = 15;
-  object.velocity_inst.twist.linear.y = 16;
-  object.velocity_inst.twist.linear.z = 17;
-  object.velocity_inst.twist.angular.x = 18;
-  object.velocity_inst.twist.angular.y = 19;
-  object.velocity_inst.twist.angular.z = 20;
+  object.velocity.twist.linear.x = 15;
+  object.velocity.twist.linear.y = 16;
+  object.velocity.twist.linear.z = 17;
+  object.velocity.twist.angular.x = 18;
+  object.velocity.twist.angular.y = 19;
+  object.velocity.twist.angular.z = 20;
   object.object_type = object.SMALL_VEHICLE;
 
   object.presence_vector |= object.BSM_ID_PRESENCE_VECTOR | object.ID_PRESENCE_VECTOR |
-                            object.POSE_PRESENCE_VECTOR | object.VELOCITY_INST_PRESENCE_VECTOR |
+                            object.POSE_PRESENCE_VECTOR | object.VELOCITY_PRESENCE_VECTOR |
                             object.OBJECT_TYPE_PRESENCE_VECTOR;
 
   constexpr carma_cooperative_perception::MotionModelMapping motion_model_mapping{
@@ -195,7 +195,7 @@ TEST(ToDetectionMsg, FromExternalObject)
   EXPECT_EQ(detection.header, object.header);
   EXPECT_EQ(detection.id, "3456-7");
   EXPECT_EQ(detection.pose, object.pose);
-  EXPECT_EQ(detection.twist, object.velocity_inst);
+  EXPECT_EQ(detection.twist, object.velocity);
   EXPECT_EQ(detection.motion_model, detection.MOTION_MODEL_CTRV);
 }
 
@@ -412,20 +412,20 @@ TEST(ToDetectedObjectDataMsg, FromExternalObject)
   object.pose.pose.orientation.y = 12;
   object.pose.pose.orientation.z = 13;
   object.pose.pose.orientation.w = 14;
-  object.velocity_inst.twist.linear.x = 15;
-  object.velocity_inst.twist.linear.y = 16;
-  object.velocity_inst.twist.linear.z = 17;
-  object.velocity_inst.twist.angular.x = 18;
-  object.velocity_inst.twist.angular.y = 19;
-  object.velocity_inst.twist.angular.z = 20;
+  object.velocity.twist.linear.x = 15;
+  object.velocity.twist.linear.y = 16;
+  object.velocity.twist.linear.z = 17;
+  object.velocity.twist.angular.x = 18;
+  object.velocity.twist.angular.y = 19;
+  object.velocity.twist.angular.z = 20;
   object.size.x = 21;
   object.size.y = 22;
   object.size.z = 23;
   object.confidence = 0.9;
   object.object_type = object.SMALL_VEHICLE;
 
   object.presence_vector |= object.BSM_ID_PRESENCE_VECTOR | object.ID_PRESENCE_VECTOR |
-                            object.POSE_PRESENCE_VECTOR | object.VELOCITY_INST_PRESENCE_VECTOR |
+                            object.POSE_PRESENCE_VECTOR | object.VELOCITY_PRESENCE_VECTOR |
                             object.CONFIDENCE_PRESENCE_VECTOR | object.OBJECT_TYPE_PRESENCE_VECTOR |
                             object.SIZE_PRESENCE_VECTOR;
 
@@ -442,7 +442,7 @@ TEST(ToDetectedObjectDataMsg, FromExternalObject)
   EXPECT_EQ(detected_object.detected_object_common_data.detected_id.object_id, 7);
   EXPECT_NEAR(detected_object.detected_object_common_data.speed.speed, std::sqrt(481), 1e-2);
   EXPECT_EQ(
-    detected_object.detected_object_common_data.speed_z.speed, object.velocity_inst.twist.linear.z);
+    detected_object.detected_object_common_data.speed_z.speed, object.velocity.twist.linear.z);
 
   EXPECT_EQ(detected_object.detected_object_optional_data.det_veh.size.vehicle_width, 22);
   EXPECT_EQ(detected_object.detected_object_optional_data.det_veh.size.vehicle_length, 21);