diff --git a/carma_cooperative_perception/src/msg_conversion.cpp b/carma_cooperative_perception/src/msg_conversion.cpp index 9cc3a8d369..a6ca846902 100644 --- a/carma_cooperative_perception/src/msg_conversion.cpp +++ b/carma_cooperative_perception/src/msg_conversion.cpp @@ -244,7 +244,23 @@ auto to_detection_list_msg( detection.header.stamp = to_time_msg(detection_time); - detection.id = std::to_string(common_data.detected_id.object_id); + // TemporaryID and octet string terms come from the SAE J2735 message definitions + static constexpr auto to_string = [](const std::vector & temporary_id) { + std::string str; + str.reserve(2 * std::size(temporary_id)); // Two hex characters per octet string + + std::array buffer; + for (const auto & octet_string : temporary_id) { + std::to_chars(std::begin(buffer), std::end(buffer), octet_string, 16); + str.push_back(std::toupper(std::get<0>(buffer))); + str.push_back(std::toupper(std::get<1>(buffer))); + } + + return str; + }; + + detection.id = + to_string(sdsm.source_id.id) + "-" + std::to_string(common_data.detected_id.object_id); const auto pos_offset{PositionOffsetXYZ::from_msg(common_data.pos)}; detection.pose.pose.position = to_position_msg(MapCoordinate{ diff --git a/carma_cooperative_perception/test/test_msg_conversion.cpp b/carma_cooperative_perception/test/test_msg_conversion.cpp index 720b8cf0ca..b2763b40e2 100644 --- a/carma_cooperative_perception/test/test_msg_conversion.cpp +++ b/carma_cooperative_perception/test/test_msg_conversion.cpp @@ -58,6 +58,7 @@ TEST(ToTimeMsg, NulloptSeconds) TEST(ToDetectionMsg, Simple) { carma_v2x_msgs::msg::SensorDataSharingMessage sdsm_msg; + sdsm_msg.source_id.id = {0xBA, 0xDD, 0xCA, 0xFE}; sdsm_msg.sdsm_time_stamp.second.millisecond = 1000; sdsm_msg.sdsm_time_stamp.presence_vector |= sdsm_msg.sdsm_time_stamp.SECOND; sdsm_msg.ref_pos.longitude = -90.703125; // degrees @@ -66,7 +67,7 @@ TEST(ToDetectionMsg, Simple) sdsm_msg.ref_pos.elevation = 300.0; // m carma_v2x_msgs::msg::DetectedObjectData object_data; - object_data.detected_object_common_data.detected_id.object_id = 0xBEEF; + object_data.detected_object_common_data.detected_id.object_id = 1; object_data.detected_object_common_data.measurement_time.measurement_time_offset = -0.1; // s object_data.detected_object_common_data.heading.heading = 34; // true heading; degrees @@ -118,7 +119,7 @@ TEST(ToDetectionMsg, Simple) EXPECT_DOUBLE_EQ(detection.accel.accel.linear.y, 1.0); EXPECT_NEAR(detection.accel.accel.linear.z, 2.4 * 9.80665, 1e-4); - EXPECT_EQ(detection.id, std::to_string(0xBEEF)); + EXPECT_EQ(detection.id, "BADDCAFE-1"); EXPECT_EQ(detection.motion_model, detection.MOTION_MODEL_CTRV); }