Updates

usdot-fhwa-OPS · Dec 17, 2023 · 15fa63c · 15fa63c
1 parent b93fab0
commit 15fa63c
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 2 deletions.
diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.cpp
@@ -246,6 +246,21 @@ namespace CARMAStreetsPlugin
                 angular_velocity->rollRate = optional_data_json["detected_vehicle_data"]["veh_ang_vel"]["roll_rate"].asInt64();
                 optional_data->choice.detVeh.vehAngVel = angular_velocity;
             }
+            // Optional Vehicle Angular Velocity
+            if (optional_data_json["detected_vehicle_data"].isMember("veh_ang_vel_confidence")) {
+                auto angular_velocity_confidence = (AngularVelocityConfidence_t*) calloc( 1, sizeof(AngularVelocityConfidence_t));
+                if (optional_data_json["detected_vehicle_data"]["veh_ang_vel_confidence"].isMember("pitch_rate_confidence")) {
+                    auto pitch_rate_confidence = (PitchRateConfidence_t*) calloc(1, sizeof(PitchRateConfidence_t));
+                    *pitch_rate_confidence = optional_data_json["detected_vehicle_data"]["veh_ang_vel_confidence"]["pitch_rate_confidence"].asInt64();
+                    angular_velocity_confidence->pitchRateConfidence = pitch_rate_confidence;
+                }
+                if (optional_data_json["detected_vehicle_data"]["veh_ang_vel_confidence"].isMember("roll_rate_confidence")) {
+                    auto roll_rate_confidence = (RollRateConfidence_t*) calloc(1, sizeof(RollRateConfidence_t));
+                    *roll_rate_confidence = optional_data_json["detected_vehicle_data"]["veh_ang_vel_confidence"]["roll_rate_confidence"].asInt64();
+                    angular_velocity_confidence->rollRateConfidence = roll_rate_confidence;
+                }
+                optional_data->choice.detVeh.vehAngVelConfidence = angular_velocity_confidence;
+            }
         }
     }
 

diff --git a/src/v2i-hub/CARMAStreetsPlugin/test/test_JsonToJ3224SDSMConverter.cpp b/src/v2i-hub/CARMAStreetsPlugin/test/test_JsonToJ3224SDSMConverter.cpp
@@ -242,8 +242,8 @@ namespace CARMAStreetsPlugin
         ASSERT_EQ(600, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAngVel->pitchRate);
         ASSERT_EQ(-800, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAngVel->rollRate);
 
-        // ASSERT_EQ(3, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAngVelConfidence->pitchRateConfidence);
-        // ASSERT_EQ(4, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAngVelConfidence->rollRateConfidence);
+        ASSERT_EQ(3, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAngVelConfidence->pitchRateConfidence);
+        ASSERT_EQ(4, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAngVelConfidence->rollRateConfidence);
 
         // ASSERT_EQ(300, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.size->width);
         // ASSERT_EQ(700, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.size->length);