diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.cpp index 7c9c099aa..a2de1e2a3 100644 --- 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 index 4e818a23d..cdbdacbc5 100644 --- 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);