diff --git a/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt b/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt index 53c8ca0ef..f8c11c896 100644 --- a/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt +++ b/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt @@ -9,7 +9,7 @@ TARGET_LINK_LIBRARIES (${PROJECT_NAME} tmxutils rdkafka++ jsoncpp) ############# enable_testing() include_directories(${PROJECT_SOURCE_DIR}/src) -add_library(${PROJECT_NAME}_lib src/J2735MapToJsonConverter.cpp src/JsonToJ2735SSMConverter.cpp src/JsonToJ2735SpatConverter.cpp src/J2735ToSRMJsonConverter.cpp) +add_library(${PROJECT_NAME}_lib src/J2735MapToJsonConverter.cpp src/JsonToJ2735SSMConverter.cpp src/JsonToJ2735SpatConverter.cpp src/J2735ToSRMJsonConverter.cpp src/J3224ToSDSMJsonConverter.cpp) target_link_libraries(${PROJECT_NAME}_lib PUBLIC ${TMXAPI_LIBRARIES} ${ASN_J2735_LIBRARIES} ${MYSQL_LIBRARIES} diff --git a/src/v2i-hub/CARMAStreetsPlugin/manifest.json b/src/v2i-hub/CARMAStreetsPlugin/manifest.json index 5faa1f8e2..22fb4a912 100644 --- a/src/v2i-hub/CARMAStreetsPlugin/manifest.json +++ b/src/v2i-hub/CARMAStreetsPlugin/manifest.json @@ -39,6 +39,11 @@ "type":"Application", "subtype":"SensorDetectedObject", "description": "Sensor detected object for cooperative perception." + }, + { + "type":"J2735", + "subtype": "SensorDataSharingMessage", + "description": "Message to share detected object data." } ], "configuration": [ diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/J3224ToSDSMJsonConverter.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/J3224ToSDSMJsonConverter.cpp new file mode 100644 index 000000000..33fd68e2c --- /dev/null +++ b/src/v2i-hub/CARMAStreetsPlugin/src/J3224ToSDSMJsonConverter.cpp @@ -0,0 +1,177 @@ +#include "J3224ToSDSMJsonConverter.h" + +namespace CARMAStreetsPlugin +{ + + void J3224ToSDSMJsonConverter::convertJ3224ToSDSMJSON(const std::shared_ptr sdsmMsgPtr, Json::Value &SDSMDataJson) const + { + // Define shared pointers for optional data + std::vector> shared_ptrs; + + // SDSM header data + SDSMDataJson["msg_cnt"] = sdsmMsgPtr->msgCnt; + + auto id_len = sdsmMsgPtr->sourceID.size; + unsigned long id_num = 0; + for(auto i = 0; i < id_len; i++) + { + id_num = (id_num << 8) | sdsmMsgPtr->sourceID.buf[i]; + } + std::stringstream id_fill_ss; + id_fill_ss << std::setfill('0') << std::setw(8) <equipmentType; + + Json::Value sDSMTimeStamp; + sDSMTimeStamp["year"] = *sdsmMsgPtr->sDSMTimeStamp.year; + sDSMTimeStamp["month"] = *sdsmMsgPtr->sDSMTimeStamp.month; + sDSMTimeStamp["day"] = *sdsmMsgPtr->sDSMTimeStamp.day; + sDSMTimeStamp["hour"] = *sdsmMsgPtr->sDSMTimeStamp.hour; + sDSMTimeStamp["minute"] = *sdsmMsgPtr->sDSMTimeStamp.minute; + sDSMTimeStamp["second"] = *sdsmMsgPtr->sDSMTimeStamp.second; + sDSMTimeStamp["offset"] = *sdsmMsgPtr->sDSMTimeStamp.offset; + SDSMDataJson["sdsm_time_stamp"] = sDSMTimeStamp; + + Json::Value refPos; + refPos["lat"] = sdsmMsgPtr->refPos.lat; + refPos["Long"] = sdsmMsgPtr->refPos.Long; + refPos["elevation"] = *sdsmMsgPtr->refPos.elevation; + SDSMDataJson["ref_pos"] = refPos; + + Json::Value refPosXYConf; + refPosXYConf["semi_major"] = sdsmMsgPtr->refPosXYConf.semiMajor; + refPosXYConf["semi_minor"] = sdsmMsgPtr->refPosXYConf.semiMinor; + refPosXYConf["orientation"] = sdsmMsgPtr->refPosXYConf.orientation; + SDSMDataJson["ref_pos_xy_conf"] = refPosXYConf; + + SDSMDataJson["ref_pos_el_conf"] = *sdsmMsgPtr->refPosElConf; + + + // Create list to append detected objects to + const DetectedObjectList det_obj_list = sdsmMsgPtr->objects; + + if(det_obj_list.list.array != nullptr){ + + Json::Value objectsJson; + + for(size_t i = 0; i < det_obj_list.list.count; i++){ + + Json::Value detectedObjectJson; + auto det_object = det_obj_list.list.array[i]; + + // Detected object common data + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["object_type"] = det_object->detObjCommon.objType; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["object_type_conf"] = det_object->detObjCommon.objTypeCfd; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["object_id"] = det_object->detObjCommon.objectID; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["measurement_time"] = det_object->detObjCommon.measurementTime; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["time_confidence"] = det_object->detObjCommon.timeConfidence; + + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["pos"]["offset_x"] = det_object->detObjCommon.pos.offsetX; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["pos"]["offset_y"] = det_object->detObjCommon.pos.offsetY; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["pos"]["offset_z"] = *det_object->detObjCommon.pos.offsetZ; + + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["pos_confidence"]["pos"] = det_object->detObjCommon.posConfidence.pos; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["pos_confidence"]["elevation"] = det_object->detObjCommon.posConfidence.elevation; + + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["speed"] = det_object->detObjCommon.speed; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["speed_confidence"] = det_object->detObjCommon.speedConfidence; + + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["heading"] = det_object->detObjCommon.heading; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["heading_conf"] = det_object->detObjCommon.headingConf; + + // Optional common data + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["speed_z"] = *det_object->detObjCommon.speedZ; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["speed_confidence_z"] = *det_object->detObjCommon.speedConfidenceZ; + + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["long"] = det_object->detObjCommon.accel4way->Long; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["lat"] = det_object->detObjCommon.accel4way->lat; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["vert"] = det_object->detObjCommon.accel4way->vert; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["yaw"] = det_object->detObjCommon.accel4way->yaw; + + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["acc_cfd_x"] = *det_object->detObjCommon.accCfdX; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["acc_cfd_y"] = *det_object->detObjCommon.accCfdY; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["acc_cfd_z"] = *det_object->detObjCommon.accCfdZ; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["acc_cfd_yaw"] = *det_object->detObjCommon.accCfdYaw; + + // Detected object optional data for vehicles, VRUs, obstacles + if(det_object->detObjOptData != nullptr){ + Json::Value optionalDataJson; + + switch(det_object->detObjOptData->present) + { + case DetectedObjectOptionalData_PR_NOTHING: + break; + + case DetectedObjectOptionalData_PR_detVeh: + + // TODO: find a better way to convert bit string data (JsonTo2735SpatConverter) + optionalDataJson["detected_vehicle_data"]["lights"] = std::to_string(*det_object->detObjOptData->choice.detVeh.lights->buf); + optionalDataJson["detected_vehicle_data"]["veh_attitude"]["pitch"] = det_object->detObjOptData->choice.detVeh.vehAttitude->pitch; + optionalDataJson["detected_vehicle_data"]["veh_attitude"]["roll"] = det_object->detObjOptData->choice.detVeh.vehAttitude->roll; + optionalDataJson["detected_vehicle_data"]["veh_attitude"]["yaw"] = det_object->detObjOptData->choice.detVeh.vehAttitude->yaw; + + optionalDataJson["detected_vehicle_data"]["veh_attitude_confidence"]["pitch_confidence"] = det_object->detObjOptData->choice.detVeh.vehAttitudeConfidence->pitchConfidence; + optionalDataJson["detected_vehicle_data"]["veh_attitude_confidence"]["roll_confidence"] = det_object->detObjOptData->choice.detVeh.vehAttitudeConfidence->rollConfidence; + optionalDataJson["detected_vehicle_data"]["veh_attitude_confidence"]["yaw_confidence"] = det_object->detObjOptData->choice.detVeh.vehAttitudeConfidence->yawConfidence; + + optionalDataJson["detected_vehicle_data"]["veh_ang_vel"]["pitch_rate"] = det_object->detObjOptData->choice.detVeh.vehAngVel->pitchRate; + optionalDataJson["detected_vehicle_data"]["veh_ang_vel"]["roll_rate"] = det_object->detObjOptData->choice.detVeh.vehAngVel->rollRate; + + optionalDataJson["detected_vehicle_data"]["veh_ang_vel_confidence"]["pitch_rate_confidence"] = *det_object->detObjOptData->choice.detVeh.vehAngVelConfidence->pitchRateConfidence; + optionalDataJson["detected_vehicle_data"]["veh_ang_vel_confidence"]["roll_rate_confidence"] = *det_object->detObjOptData->choice.detVeh.vehAngVelConfidence->rollRateConfidence; + + optionalDataJson["detected_vehicle_data"]["size"]["width"] = det_object->detObjOptData->choice.detVeh.size->width; + optionalDataJson["detected_vehicle_data"]["size"]["length"] = det_object->detObjOptData->choice.detVeh.size->length; + optionalDataJson["detected_vehicle_data"]["height"] = *det_object->detObjOptData->choice.detVeh.height; + + optionalDataJson["detected_vehicle_data"]["vehicle_size_confidence"]["vehicle_width_confidence"] = det_object->detObjOptData->choice.detVeh.vehicleSizeConfidence->vehicleWidthConfidence; + optionalDataJson["detected_vehicle_data"]["vehicle_size_confidence"]["vehicle_length_confidence"] = det_object->detObjOptData->choice.detVeh.vehicleSizeConfidence->vehicleLengthConfidence; + optionalDataJson["detected_vehicle_data"]["vehicle_size_confidence"]["vehicle_height_confidence"] = *det_object->detObjOptData->choice.detVeh.vehicleSizeConfidence->vehicleHeightConfidence; + + optionalDataJson["detected_vehicle_data"]["vehicle_class"] = *det_object->detObjOptData->choice.detVeh.vehicleClass; + optionalDataJson["detected_vehicle_data"]["vehicle_class_conf"] = *det_object->detObjOptData->choice.detVeh.classConf; + break; + + case DetectedObjectOptionalData_PR_detVRU: + optionalDataJson["detected_vru_data"]["basic_type"] = *det_object->detObjOptData->choice.detVRU.basicType; + + switch(det_object->detObjOptData->choice.detVRU.propulsion->present) + { + case PropelledInformation_PR_NOTHING: + break; + case PropelledInformation_PR_human: + optionalDataJson["detected_vru_data"]["propulsion"]["human"] = det_object->detObjOptData->choice.detVRU.propulsion->choice.human; + break; + case PropelledInformation_PR_animal: + optionalDataJson["detected_vru_data"]["propulsion"]["human"] = det_object->detObjOptData->choice.detVRU.propulsion->choice.animal; + break; + case PropelledInformation_PR_motor: + optionalDataJson["detected_vru_data"]["propulsion"]["human"] = det_object->detObjOptData->choice.detVRU.propulsion->choice.motor; + break; + } + + optionalDataJson["detected_vru_data"]["attachment"] = *det_object->detObjOptData->choice.detVRU.attachment; + optionalDataJson["detected_vru_data"]["radius"] = *det_object->detObjOptData->choice.detVRU.radius; + break; + + case DetectedObjectOptionalData_PR_detObst: + optionalDataJson["detected_obst_data"]["obst_size"]["width"] = det_object->detObjOptData->choice.detObst.obstSize.width; + optionalDataJson["detected_obst_data"]["obst_size"]["length"] = det_object->detObjOptData->choice.detObst.obstSize.length; + optionalDataJson["detected_obst_data"]["obst_size"]["height"] = *det_object->detObjOptData->choice.detObst.obstSize.height; // optional + + optionalDataJson["detected_obst_data"]["obst_size"]["width_confidence"] = det_object->detObjOptData->choice.detObst.obstSizeConfidence.widthConfidence; + optionalDataJson["detected_obst_data"]["obst_size"]["length_confidence"] = det_object->detObjOptData->choice.detObst.obstSizeConfidence.lengthConfidence; + optionalDataJson["detected_obst_data"]["obst_size"]["height_confidence"] = *det_object->detObjOptData->choice.detObst.obstSizeConfidence.heightConfidence; + break; + } + detectedObjectJson["detected_object_data"]["detected_object_optional_data"] = optionalDataJson; + } + + // Add the generated object jsons to the detected object list + SDSMDataJson["objects"].append(detectedObjectJson); + + } + } + } +} \ No newline at end of file diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/J3224ToSDSMJsonConverter.h b/src/v2i-hub/CARMAStreetsPlugin/src/J3224ToSDSMJsonConverter.h new file mode 100644 index 000000000..273d7efdd --- /dev/null +++ b/src/v2i-hub/CARMAStreetsPlugin/src/J3224ToSDSMJsonConverter.h @@ -0,0 +1,43 @@ +#include "jsoncpp/json/json.h" +#include + +namespace CARMAStreetsPlugin +{ + + // Template to use when created shared pointer objects for optional data + template + T *create_store_shared(std::vector> &shared_pointers) + { + auto obj_shared = std::make_shared(); + shared_pointers.push_back(obj_shared); + return obj_shared.get(); + } + + // Template for shared pointers with array elements + template + T *create_store_shared_array(std::vector> &shared_pointers, int size) + { + std::shared_ptr array_shared(new T[size]{0}); + shared_pointers.push_back(array_shared); + return array_shared.get(); + } + + + class J3224ToSDSMJsonConverter + { + public: + + J3224ToSDSMJsonConverter() = default; + ~J3224ToSDSMJsonConverter() = default; + + /** + * @brief Convert the J3224 SensorDataSharingMessage into JSON format. + * @param sdsmMsgPtr The input is a constant SensorDataSharingMessage pointer. This prevent any modification to the original SDSM + * @param sdsmJson Pass by reference to allow the method to populate this object with the SensorDataSharingMessage data. + */ + void convertJ3224ToSDSMJSON(const std::shared_ptr sdsmMsgPtr, Json::Value &sdsmJson) const; + + + }; + +} \ No newline at end of file diff --git a/src/v2i-hub/CARMAStreetsPlugin/test/test_J3224ToSDSMJsonConverter.cpp b/src/v2i-hub/CARMAStreetsPlugin/test/test_J3224ToSDSMJsonConverter.cpp new file mode 100644 index 000000000..085e3ba24 --- /dev/null +++ b/src/v2i-hub/CARMAStreetsPlugin/test/test_J3224ToSDSMJsonConverter.cpp @@ -0,0 +1,435 @@ +#include +#include "jsoncpp/json/json.h" +#include +#include + +class test_J3224ToSDSMJsonConverter : public ::testing::Test +{ + public: + test_J3224ToSDSMJsonConverter() = default; + ~test_J3224ToSDSMJsonConverter() = default; +}; + +namespace unit_test +{ + + TEST_F(test_J3224ToSDSMJsonConverter, convertJ3224ToSDSMJSON) + { + std::vector> shared_ptrs; + CARMAStreetsPlugin::J3224ToSDSMJsonConverter sdsmConverter; + + SensorDataSharingMessage *message = (SensorDataSharingMessage_t *)calloc(1, sizeof(SensorDataSharingMessage_t)); + + message->msgCnt = 15; + + uint8_t source_id_bytes[4] = {(uint8_t)1, (uint8_t)2, (uint8_t)3, (uint8_t)4}; + TemporaryID_t source_id; + source_id.buf = source_id_bytes; + source_id.size = sizeof(source_id_bytes); + message->sourceID = source_id; + + message->equipmentType = EquipmentType_rsu; + + auto year_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *year_ptr = 2000; + message->sDSMTimeStamp.year = year_ptr; + auto month_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *month_ptr = 7; + message->sDSMTimeStamp.month = month_ptr; + auto day_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *day_ptr = 4; + message->sDSMTimeStamp.day = day_ptr; + auto hour_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *hour_ptr = 21; + message->sDSMTimeStamp.hour = hour_ptr; + auto minute_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *minute_ptr = 15; + message->sDSMTimeStamp.minute = minute_ptr; + auto second_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *second_ptr = 10000; + message->sDSMTimeStamp.second = second_ptr; + auto offset_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *offset_ptr = 2000; + message->sDSMTimeStamp.offset = offset_ptr; + + message->refPos.lat = 400000000; + message->refPos.Long = 800000000; + auto pos_elevation_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *pos_elevation_ptr = 30; + message->refPos.elevation = pos_elevation_ptr; + + message->refPosXYConf.semiMajor = 250; + message->refPosXYConf.semiMinor = 200; + message->refPosXYConf.orientation = 20000; + + auto elevation_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *elevation_conf_ptr = ElevationConfidence_elev_000_05; + message->refPosElConf = elevation_conf_ptr; + + DetectedObjectList_t detected_obj_list; + + + // Generate an object to test common data and detected vehicle data + DetectedObjectData_t test_obj1; + + test_obj1.detObjCommon.objType = ObjectType_vehicle; + test_obj1.detObjCommon.objTypeCfd = 65; + test_obj1.detObjCommon.objectID = 1200; + test_obj1.detObjCommon.measurementTime = 500; + test_obj1.detObjCommon.timeConfidence = TimeConfidence_time_000_200; + + test_obj1.detObjCommon.pos.offsetX = 1000; + test_obj1.detObjCommon.pos.offsetY = 750; + auto offsetZ_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *offsetZ_ptr = 50; + test_obj1.detObjCommon.pos.offsetZ = offsetZ_ptr; + + test_obj1.detObjCommon.posConfidence.pos = PositionConfidence_a200m; + test_obj1.detObjCommon.posConfidence.elevation = ElevationConfidence_elev_100_00; + + test_obj1.detObjCommon.speed = 2100; + test_obj1.detObjCommon.speedConfidence = SpeedConfidence_prec5ms; + + auto speedZ_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *speedZ_ptr = 1200; + test_obj1.detObjCommon.speedZ = speedZ_ptr; + auto speedZ_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *speedZ_conf_ptr = SpeedConfidence_prec0_1ms; + test_obj1.detObjCommon.speedConfidenceZ = speedZ_conf_ptr; + + test_obj1.detObjCommon.heading = 15000; + test_obj1.detObjCommon.headingConf = HeadingConfidence_prec0_05deg; + + auto accel_4_way_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + accel_4_way_ptr->Long = 200; + accel_4_way_ptr->lat = -500; + accel_4_way_ptr->vert = 1; + accel_4_way_ptr->yaw = 400; + test_obj1.detObjCommon.accel4way = accel_4_way_ptr; + + auto acc_cfd_x_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_x_ptr = AccelerationConfidence_accl_100_00; + test_obj1.detObjCommon.accCfdX = acc_cfd_x_ptr; + auto acc_cfd_y_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_y_ptr = AccelerationConfidence_accl_010_00; + test_obj1.detObjCommon.accCfdY = acc_cfd_y_ptr; + auto acc_cfd_z_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_z_ptr = AccelerationConfidence_accl_005_00; + test_obj1.detObjCommon.accCfdZ = acc_cfd_z_ptr; + + auto acc_cfd_yaw_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_yaw_ptr = YawRateConfidence_degSec_001_00; + test_obj1.detObjCommon.accCfdYaw = acc_cfd_yaw_ptr; + + + auto det_obj_opt_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + + det_obj_opt_ptr->present = DetectedObjectOptionalData_PR_detVeh; + + auto lights_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + // TODO: Make sure lights are being set and converted properly + uint8_t lights_bytes[9] = {(uint8_t)8}; + lights_ptr->buf = lights_bytes; + lights_ptr->size = sizeof(lights_bytes); + det_obj_opt_ptr->choice.detVeh.lights = lights_ptr; + + auto veh_attitude_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + veh_attitude_ptr->pitch = 2400; + veh_attitude_ptr->roll = -12000; + veh_attitude_ptr->yaw = 600; + det_obj_opt_ptr->choice.detVeh.vehAttitude = veh_attitude_ptr; + + auto veh_attitude_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + veh_attitude_conf_ptr->pitchConfidence = HeadingConfidence_prec0_05deg; + veh_attitude_conf_ptr->rollConfidence = HeadingConfidence_prec0_01deg; + veh_attitude_conf_ptr->yawConfidence = HeadingConfidence_prec0_0125deg; + det_obj_opt_ptr->choice.detVeh.vehAttitudeConfidence = veh_attitude_conf_ptr; + + auto veh_ang_vel_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + veh_ang_vel_ptr->pitchRate = 800; + veh_ang_vel_ptr->rollRate = -600; + det_obj_opt_ptr->choice.detVeh.vehAngVel = veh_ang_vel_ptr; + + auto veh_ang_vel_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + auto pitch_rate_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *pitch_rate_conf_ptr = PitchRateConfidence_degSec_001_00; + veh_ang_vel_conf_ptr->pitchRateConfidence = pitch_rate_conf_ptr; + auto roll_rate_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *roll_rate_conf_ptr = RollRateConfidence_degSec_000_10; + veh_ang_vel_conf_ptr->rollRateConfidence = roll_rate_conf_ptr; + det_obj_opt_ptr->choice.detVeh.vehAngVelConfidence = veh_ang_vel_conf_ptr; + + auto veh_size_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + veh_size_ptr->width = 300; + veh_size_ptr->length = 700; + det_obj_opt_ptr->choice.detVeh.size = veh_size_ptr; + + auto veh_height_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *veh_height_ptr = 70; + det_obj_opt_ptr->choice.detVeh.height= veh_height_ptr; + + auto veh_size_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + veh_size_conf_ptr->vehicleWidthConfidence = SizeValueConfidence_size_000_10; + veh_size_conf_ptr->vehicleLengthConfidence = SizeValueConfidence_size_000_05; + auto veh_height_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *veh_height_conf_ptr = SizeValueConfidence_size_000_02; + veh_size_conf_ptr->vehicleHeightConfidence = veh_height_conf_ptr; + det_obj_opt_ptr->choice.detVeh.vehicleSizeConfidence = veh_size_conf_ptr; + + auto veh_class_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *veh_class_ptr = 45; + det_obj_opt_ptr->choice.detVeh.vehicleClass = veh_class_ptr; + + auto veh_class_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *veh_class_conf_ptr = 85; + det_obj_opt_ptr->choice.detVeh.classConf = veh_class_conf_ptr; + + test_obj1.detObjOptData = det_obj_opt_ptr; + + asn_sequence_add(&message->objects.list, &test_obj1); + + + // Generate a second object to test detected VRU data + DetectedObjectData_t test_obj2; + + test_obj2.detObjCommon.objType = ObjectType_vehicle; + test_obj2.detObjCommon.objTypeCfd = 65; + test_obj2.detObjCommon.objectID = 110; + test_obj2.detObjCommon.measurementTime = 500; + test_obj2.detObjCommon.timeConfidence = TimeConfidence_time_000_200; + + test_obj2.detObjCommon.pos.offsetX = 1000; + test_obj2.detObjCommon.pos.offsetY = 750; + auto offsetZ_ptr2 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *offsetZ_ptr2 = 50; + test_obj2.detObjCommon.pos.offsetZ = offsetZ_ptr2; + + test_obj2.detObjCommon.posConfidence.pos = PositionConfidence_a200m; + test_obj2.detObjCommon.posConfidence.elevation = ElevationConfidence_elev_100_00; + + test_obj2.detObjCommon.speed = 2100; + test_obj2.detObjCommon.speedConfidence = SpeedConfidence_prec5ms; + + auto speedZ_ptr2 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *speedZ_ptr2 = 1200; + test_obj2.detObjCommon.speedZ = speedZ_ptr2; + auto speedZ_conf_ptr2 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *speedZ_conf_ptr2 = SpeedConfidence_prec0_1ms; + test_obj2.detObjCommon.speedConfidenceZ = speedZ_conf_ptr2; + + test_obj2.detObjCommon.heading = 15000; + test_obj2.detObjCommon.headingConf = HeadingConfidence_prec0_05deg; + + auto accel_4_way_ptr2 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + accel_4_way_ptr2->Long = 200; + accel_4_way_ptr2->lat = -500; + accel_4_way_ptr2->vert = 1; + accel_4_way_ptr2->yaw = 400; + test_obj2.detObjCommon.accel4way = accel_4_way_ptr2; + + auto acc_cfd_x_ptr2 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_x_ptr2 = AccelerationConfidence_accl_100_00; + test_obj2.detObjCommon.accCfdX = acc_cfd_x_ptr2; + auto acc_cfd_y_ptr2 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_y_ptr2 = AccelerationConfidence_accl_010_00; + test_obj2.detObjCommon.accCfdY = acc_cfd_y_ptr2; + auto acc_cfd_z_ptr2 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_z_ptr2 = AccelerationConfidence_accl_005_00; + test_obj2.detObjCommon.accCfdZ = acc_cfd_z_ptr2; + + auto acc_cfd_yaw_ptr2 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_yaw_ptr2 = YawRateConfidence_degSec_001_00; + test_obj2.detObjCommon.accCfdYaw = acc_cfd_yaw_ptr2; + + auto det_obj_opt_ptr2 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + det_obj_opt_ptr2->present = DetectedObjectOptionalData_PR_detVRU; + + auto basic_type = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *basic_type = PersonalDeviceUserType_aPEDESTRIAN; + det_obj_opt_ptr2->choice.detVRU.basicType = basic_type; + + auto propulsion = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + propulsion->present = PropelledInformation_PR_human; + propulsion->choice.human = HumanPropelledType_onFoot; + det_obj_opt_ptr2->choice.detVRU.propulsion = propulsion; + + auto attachment = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *attachment = Attachment_wheelchair; + det_obj_opt_ptr2->choice.detVRU.attachment = attachment; + + auto radius = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *radius = 30; + det_obj_opt_ptr2->choice.detVRU.radius = radius; + + test_obj2.detObjOptData = det_obj_opt_ptr2; + asn_sequence_add(&message->objects.list, &test_obj2); + + + + // Generate a third object to test detected obstacle data + DetectedObjectData_t test_obj3; + + test_obj3.detObjCommon.objType = ObjectType_vehicle; + test_obj3.detObjCommon.objTypeCfd = 65; + test_obj3.detObjCommon.objectID = 1000; + test_obj3.detObjCommon.measurementTime = 500; + test_obj3.detObjCommon.timeConfidence = TimeConfidence_time_000_200; + + test_obj3.detObjCommon.pos.offsetX = 1000; + test_obj3.detObjCommon.pos.offsetY = 750; + auto offsetZ_ptr3 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *offsetZ_ptr3 = 50; + test_obj3.detObjCommon.pos.offsetZ = offsetZ_ptr3; + + test_obj3.detObjCommon.posConfidence.pos = PositionConfidence_a200m; + test_obj3.detObjCommon.posConfidence.elevation = ElevationConfidence_elev_100_00; + + test_obj3.detObjCommon.speed = 2100; + test_obj3.detObjCommon.speedConfidence = SpeedConfidence_prec5ms; + + auto speedZ_ptr3 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *speedZ_ptr3 = 1200; + test_obj3.detObjCommon.speedZ = speedZ_ptr3; + auto speedZ_conf_ptr3 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *speedZ_conf_ptr3 = SpeedConfidence_prec0_1ms; + test_obj3.detObjCommon.speedConfidenceZ = speedZ_conf_ptr3; + + test_obj3.detObjCommon.heading = 15000; + test_obj3.detObjCommon.headingConf = HeadingConfidence_prec0_05deg; + + auto accel_4_way_ptr3 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + accel_4_way_ptr3->Long = 200; + accel_4_way_ptr3->lat = -500; + accel_4_way_ptr3->vert = 1; + accel_4_way_ptr3->yaw = 400; + test_obj3.detObjCommon.accel4way = accel_4_way_ptr3; + + auto acc_cfd_x_ptr3 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_x_ptr3 = AccelerationConfidence_accl_100_00; + test_obj3.detObjCommon.accCfdX = acc_cfd_x_ptr3; + auto acc_cfd_y_ptr3 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_y_ptr3 = AccelerationConfidence_accl_010_00; + test_obj3.detObjCommon.accCfdY = acc_cfd_y_ptr3; + auto acc_cfd_z_ptr3 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_z_ptr3 = AccelerationConfidence_accl_005_00; + test_obj3.detObjCommon.accCfdZ = acc_cfd_z_ptr3; + + auto acc_cfd_yaw_ptr3 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_yaw_ptr3 = YawRateConfidence_degSec_001_00; + test_obj3.detObjCommon.accCfdYaw = acc_cfd_yaw_ptr3; + + + + auto det_obj_opt_ptr3 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + det_obj_opt_ptr3->present = DetectedObjectOptionalData_PR_detObst; + + det_obj_opt_ptr3->choice.detObst.obstSize.width = 400; + det_obj_opt_ptr3->choice.detObst.obstSize.length = 300; + auto obj_height = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *obj_height = 100; + det_obj_opt_ptr3->choice.detObst.obstSize.height = obj_height; + + det_obj_opt_ptr3->choice.detObst.obstSizeConfidence.widthConfidence = SizeValueConfidence_size_010_00; + det_obj_opt_ptr3->choice.detObst.obstSizeConfidence.lengthConfidence = SizeValueConfidence_size_005_00; + auto obj_height_conf = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *obj_height_conf = SizeValueConfidence_size_002_00; + det_obj_opt_ptr3->choice.detObst.obstSizeConfidence.heightConfidence = obj_height_conf; + + test_obj3.detObjOptData = det_obj_opt_ptr3; + + asn_sequence_add(&message->objects.list, &test_obj3); + + // Create a SensorDataSharingMessage with the test data to load into a Json (sdsmJson) + std::shared_ptr sdsmMsgPtr(message); + Json::Value sdsmJson; + sdsmConverter.convertJ3224ToSDSMJSON(sdsmMsgPtr, sdsmJson); + + // Tests + // Testing SDSM header data + ASSERT_EQ(15, sdsmJson["msg_cnt"].asInt()); + ASSERT_EQ("01020304", sdsmJson["source_id"].asString()); + ASSERT_EQ(1, sdsmJson["equipment_type"].asInt()); + ASSERT_EQ(2000, sdsmJson["sdsm_time_stamp"]["year"].asInt()); + ASSERT_EQ(7, sdsmJson["sdsm_time_stamp"]["month"].asInt()); + ASSERT_EQ(4, sdsmJson["sdsm_time_stamp"]["day"].asInt()); + ASSERT_EQ(21, sdsmJson["sdsm_time_stamp"]["hour"].asInt()); + ASSERT_EQ(15, sdsmJson["sdsm_time_stamp"]["minute"].asInt()); + ASSERT_EQ(10000, sdsmJson["sdsm_time_stamp"]["second"].asInt()); + ASSERT_EQ(2000, sdsmJson["sdsm_time_stamp"]["offset"].asInt()); + ASSERT_EQ(400000000, sdsmJson["ref_pos"]["lat"].asInt()); + ASSERT_EQ(800000000, sdsmJson["ref_pos"]["Long"].asInt()); + ASSERT_EQ(30, sdsmJson["ref_pos"]["elevation"].asInt()); + ASSERT_EQ(250, sdsmJson["ref_pos_xy_conf"]["semi_major"].asInt()); + ASSERT_EQ(200, sdsmJson["ref_pos_xy_conf"]["semi_minor"].asInt()); + ASSERT_EQ(20000, sdsmJson["ref_pos_xy_conf"]["orientation"].asInt()); + ASSERT_EQ(13, sdsmJson["ref_pos_el_conf"].asInt()); + + // Object 1 detected object common data + ASSERT_EQ(1, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["object_type"].asInt()); + ASSERT_EQ(65, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["object_type_conf"].asInt()); + ASSERT_EQ(1200, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["object_id"].asInt()); + ASSERT_EQ(500, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["measurement_time"].asInt()); + ASSERT_EQ(8, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["time_confidence"].asInt()); + ASSERT_EQ(1000, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["pos"]["offset_x"].asInt()); + ASSERT_EQ(750, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["pos"]["offset_y"].asInt()); + ASSERT_EQ(50, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["pos"]["offset_z"].asInt()); + ASSERT_EQ(2, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["pos_confidence"]["pos"].asInt()); + ASSERT_EQ(3, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["pos_confidence"]["elevation"].asInt()); + ASSERT_EQ(2100, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["speed"].asInt()); + ASSERT_EQ(3, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["speed_confidence"].asInt()); + ASSERT_EQ(1200, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["speed_z"].asInt()); + ASSERT_EQ(5, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["speed_confidence_z"].asInt()); + ASSERT_EQ(15000, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["heading"].asInt()); + ASSERT_EQ(5, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["heading_conf"].asInt()); + ASSERT_EQ(200, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["long"].asInt()); + ASSERT_EQ(-500, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["lat"].asInt()); + ASSERT_EQ(1, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["vert"].asInt()); + ASSERT_EQ(400, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["yaw"].asInt()); + ASSERT_EQ(1, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["acc_cfd_x"].asInt()); + ASSERT_EQ(2, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["acc_cfd_y"].asInt()); + ASSERT_EQ(3, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["acc_cfd_z"].asInt()); + ASSERT_EQ(4, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["acc_cfd_yaw"].asInt()); + + // Object 1 detected vehicle data (optional) + // TODO: Better construction/test of ExteriorLights message and resulting bitstring + // ASSERT_EQ(8, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["lights"].asInt()); + ASSERT_EQ(2400, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude"]["pitch"].asInt()); + ASSERT_EQ(-12000, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude"]["roll"].asInt()); + ASSERT_EQ(600, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude"]["yaw"].asInt()); + ASSERT_EQ(2400, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude"]["pitch"].asInt()); + ASSERT_EQ(5, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude_confidence"]["pitch_confidence"].asInt()); + ASSERT_EQ(6, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude_confidence"]["roll_confidence"].asInt()); + ASSERT_EQ(7, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude_confidence"]["yaw_confidence"].asInt()); + ASSERT_EQ(800, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_ang_vel"]["pitch_rate"].asInt()); + ASSERT_EQ(-600, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_ang_vel"]["roll_rate"].asInt()); + ASSERT_EQ(4, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_ang_vel_confidence"]["pitch_rate_confidence"].asInt()); + ASSERT_EQ(5, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_ang_vel_confidence"]["roll_rate_confidence"].asInt()); + ASSERT_EQ(300, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["size"]["width"].asInt()); + ASSERT_EQ(700, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["size"]["length"].asInt()); + ASSERT_EQ(70, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["height"].asInt()); + ASSERT_EQ(10, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["vehicle_size_confidence"]["vehicle_width_confidence"].asInt()); + ASSERT_EQ(11, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["vehicle_size_confidence"]["vehicle_length_confidence"].asInt()); + ASSERT_EQ(12, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["vehicle_size_confidence"]["vehicle_height_confidence"].asInt()); + ASSERT_EQ(45, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["vehicle_class"].asInt()); + ASSERT_EQ(85, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["vehicle_class_conf"].asInt()); + + // Only test the ID and optional VRU data for the 2nd object + ASSERT_EQ(110, sdsmJson["objects"][1]["detected_object_data"]["detected_object_common_data"]["object_id"].asInt()); + ASSERT_EQ(1, sdsmJson["objects"][1]["detected_object_data"]["detected_object_optional_data"]["detected_vru_data"]["basic_type"].asInt()); + ASSERT_EQ(2, sdsmJson["objects"][1]["detected_object_data"]["detected_object_optional_data"]["detected_vru_data"]["propulsion"]["human"].asInt()); + ASSERT_EQ(4, sdsmJson["objects"][1]["detected_object_data"]["detected_object_optional_data"]["detected_vru_data"]["attachment"].asInt()); + ASSERT_EQ(30, sdsmJson["objects"][1]["detected_object_data"]["detected_object_optional_data"]["detected_vru_data"]["radius"].asInt()); + + // Only test the ID and optional obstacle data for the 3rd object + ASSERT_EQ(1000, sdsmJson["objects"][2]["detected_object_data"]["detected_object_common_data"]["object_id"].asInt()); + ASSERT_EQ(400, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obst_data"]["obst_size"]["width"].asInt()); + ASSERT_EQ(300, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obst_data"]["obst_size"]["length"].asInt()); + ASSERT_EQ(100, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obst_data"]["obst_size"]["height"].asInt()); + ASSERT_EQ(4, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obst_data"]["obst_size"]["width_confidence"].asInt()); + ASSERT_EQ(5, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obst_data"]["obst_size"]["length_confidence"].asInt()); + ASSERT_EQ(6, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obst_data"]["obst_size"]["height_confidence"].asInt()); + + } + +}; +