From 40d8b14e4d460e8e1c03e5f2d672b4f621fd34bd Mon Sep 17 00:00:00 2001 From: dan-du-car <62157949+dan-du-car@users.noreply.github.com> Date: Thu, 7 Dec 2023 15:02:12 -0500 Subject: [PATCH 1/2] Telematic bridge: Fix available topic return null value in topics field when there is no topic available (#569) # PR Details ## Description Telematic bridge sends null topics when there is no topic available. The idea value for this topics is empty array value [] rather than a null value. This PR is to initialize the topics value with empty array ## Related Issue https://github.com/usdot-fhwa-OPS/V2X-Hub/issues/570 ## Motivation and Context telematic testing ## How Has This Been Tested? Integration test ## Types of changes - [x] Defect fix (non-breaking change that fixes an issue) - [ ] New feature (non-breaking change that adds functionality) - [ ] Breaking change (fix or feature that cause existing functionality to change) ## Checklist: - [ ] I have added any new packages to the sonar-scanner.properties file - [ ] My change requires a change to the documentation. - [ ] I have updated the documentation accordingly. - [x] I have read the **CONTRIBUTING** document. [V2XHUB Contributing Guide](https://github.com/usdot-fhwa-OPS/V2X-Hub/blob/develop/Contributing.md) - [ ] I have added tests to cover my changes. - [ ] All new and existing tests passed. --- src/v2i-hub/TelematicBridgePlugin/src/TelematicUnit.cpp | 2 +- src/v2i-hub/TelematicBridgePlugin/test/test_TelematicUnit.cpp | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/v2i-hub/TelematicBridgePlugin/src/TelematicUnit.cpp b/src/v2i-hub/TelematicBridgePlugin/src/TelematicUnit.cpp index e99029b53..948b9f017 100644 --- a/src/v2i-hub/TelematicBridgePlugin/src/TelematicUnit.cpp +++ b/src/v2i-hub/TelematicBridgePlugin/src/TelematicUnit.cpp @@ -219,7 +219,7 @@ namespace TelematicBridge message[TESTING_TYPE_KEY] = testingType; message[EVENT_NAME_KEY] = eventName; message[TIMESTAMP_KEY] = duration_cast(system_clock::now().time_since_epoch()).count(); - Json::Value topics; + Json::Value topics = Json::arrayValue; for (const auto &topic : availableTopicList) { if (!boost::icontains(excludedTopics, topic)) diff --git a/src/v2i-hub/TelematicBridgePlugin/test/test_TelematicUnit.cpp b/src/v2i-hub/TelematicBridgePlugin/test/test_TelematicUnit.cpp index 9a81427b4..c1fbdae50 100644 --- a/src/v2i-hub/TelematicBridgePlugin/test/test_TelematicUnit.cpp +++ b/src/v2i-hub/TelematicBridgePlugin/test/test_TelematicUnit.cpp @@ -43,6 +43,10 @@ namespace TelematicBridge auto reply = TelematicUnit::constructAvailableTopicsReplyString(unit, eventLocation, testingType, eventName, topics, excluded_topic); auto json = TelematicUnit::parseJson(reply); ASSERT_EQ("test_topic", json["topics"][0]["name"].asString()); + + reply = TelematicUnit::constructAvailableTopicsReplyString(unit, eventLocation, testingType, eventName, {}, excluded_topic); + json = TelematicUnit::parseJson(reply); + ASSERT_EQ(1, json["topics"].isArray()); } TEST_F(test_TelematicUnit, constructPublishedDataString) From ada57ac1c5e7d2000d0224cc1954b5ac50714eed Mon Sep 17 00:00:00 2001 From: paulbourelly999 <77466294+paulbourelly999@users.noreply.github.com> Date: Mon, 18 Dec 2023 11:19:11 -0500 Subject: [PATCH 2/2] Fix SDSM encoding in CARMA Streets Plugin (#571) # PR Details ## Description Meant to fix encoding issues faced when attempting to encoded SDSM JSON messages sent from the SDSM service. Previous encoding logic was attempt to reduce the amount of raw pointers used in creating the SDSM message from the JSON payload by creating smart pointers instead. This logic was able to create seemly valid C Structs, inspected using asn print functions but encoding these would return errors similar to the one displayed below To get functional encoding logic, this PR revert to using raw pointers and memory allocation ``` [2023-12-07 15:29:47.603] src/CARMAStreetsPlugin.cpp (691) - ERROR : Failed to encoded SDSM message : {"msg_cnt":3,"source_id":"","equipment_type":1,"sdsm_time_stamp":{"second":641000,"minute":29,"hour":20,"day":7,"month":12,"year":2023,"offset":0},"ref_pos":{"long":0,"lat":0},"ref_pos_xy_conf":{"semi_major":0,"semi_minor":0,"orientation":0},"objects":[{"detected_object_data":{"detected_object_common_data":{"obj_type":0,"object_id":1,"obj_type_cfd":70,"measurement_time":0,"time_confidence":0,"pos":{"offset_x":-11,"offset_y":-20,"offset_z":-32},"pos_confidence":{"pos":32734,"elevation":1300573664},"speed":70,"speed_confidence":0,"speed_z":50,"heading":0,"heading_conf":0},"detected_object_optional_data":{"detected_obstacle_data":{"obst_size":{"width":5,"length":20,"height":10},"obst_size_confidence":{"width_confidence":0,"length_confidence":0}}}}}]} Exception encountered: Unable to encode MessageFrame to bytes. [2023-12-07 15:29:47.603] Utils/src/PluginClient.cpp (405) - ERROR : CARMAStreetsPlugin terminating from unhandled exception: Segmentation fault backtrace: (Hint: Use addr2line -C -e 0x#######) to find line number) CARMAStreetsPlugin(+0x3e359) [0x55c14e2e0359] /usr/local/lib/libasn_j2735_r63.so(OCTET_STRING_free+0xb5) [0x7f4e2ca3a1ba] /lib/x86_64-linux-gnu/libc.so.6(+0x42520) [0x7f4e2c278520] /usr/local/lib/libasn_j2735_r63.so(OCTET_STRING_free+0xb5) [0x7f4e2ca3a1ba] /usr/local/lib/libasn_j2735_r63.so(SEQUENCE_free+0x100) [0x7f4e2ca5964a] CARMAStreetsPlugin(+0x37015) [0x55c14e2d9015] CARMAStreetsPlugin(+0x1137c1) [0x55c14e3b57c1] CARMAStreetsPlugin(+0xf67cb) [0x55c14e3987cb] CARMAStreetsPlugin(+0xe6e68) [0x55c14e388e68] CARMAStreetsPlugin(+0xdbe22) [0x55c14e37de22] /lib/x86_64-linux-gnu/libboost_thread.so.1.74.0(+0x150cb) [0x7f4e2c9ac0cb] /lib/x86_64-linux-gnu/libc.so.6(+0x94ac3) [0x7f4e2c2caac3] /lib/x86_64-linux-gnu/libc.so.6(clone+0x44) [0x7f4e2c35bbf4] diagnostic info: /home/V2X-Hub/src/tmx/TmxUtils/src/PluginExec.cpp(220): Throw in function HandleSignal Dynamic exception type: tmx::utils::SignalException std::exception::what: Segmentation fault ``` ## Related Issue [CDAR-585 ](https://usdot-carma.atlassian.net/browse/CDAR-585) ## Motivation and Context Get working SDSM encoding logic ## How Has This Been Tested? Unit testing ## Types of changes - [x] Defect fix (non-breaking change that fixes an issue) - [ ] New feature (non-breaking change that adds functionality) - [ ] Breaking change (fix or feature that cause existing functionality to change) ## Checklist: - [ ] I have added any new packages to the sonar-scanner.properties file - [ ] My change requires a change to the documentation. - [ ] I have updated the documentation accordingly. - [x] I have read the **CONTRIBUTING** document. [V2XHUB Contributing Guide](https://github.com/usdot-fhwa-OPS/V2X-Hub/blob/develop/Contributing.md) - [x] I have added tests to cover my changes. - [x] All new and existing tests passed. --- .../src/CARMAStreetsPlugin.cpp | 12 +- .../src/JsonToJ3224SDSMConverter.cpp | 616 ++++++++------- .../src/JsonToJ3224SDSMConverter.h | 9 +- .../test/test_JsonToJ3224SDSMConverter.cpp | 722 +++++++++++++++--- 4 files changed, 931 insertions(+), 428 deletions(-) diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index 23a21d6bf..9ad219e1f 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -685,23 +685,19 @@ void CARMAStreetsPlugin::SubscribeSDSMKafkaTopic(){ { sdsm_convertor.encodeSDSM(sdsm_ptr, sdsmEncodedMsg); } - catch (TmxException &ex) + catch( std::exception const & x ) { - // Skip messages that fail to encode. - PLOG(logERROR) << "Failed to encoded SDSM message : \n" << payload_str << std::endl << "Exception encountered: " - << ex.what() << std::endl; - ASN_STRUCT_FREE_CONTENTS_ONLY(asn_DEF_SensorDataSharingMessage, sdsm_ptr.get()); // may be unnecessary + PLOG(logERROR) << "Failed to encoded SDSM message : " << payload_str << std::endl << boost::diagnostic_information( x ) << std::endl; SetStatus(Key_SDSMMessageSkipped, ++_sdsmMessageSkipped); continue; } - ASN_STRUCT_FREE_CONTENTS_ONLY(asn_DEF_SensorDataSharingMessage, sdsm_ptr.get()); // same as above PLOG(logDEBUG) << "sdsmEncodedMsg: " << sdsmEncodedMsg; - //Broadcast the encoded SDSM message sdsmEncodedMsg.set_flags(IvpMsgFlags_RouteDSRC); sdsmEncodedMsg.addDsrcMetadata(0x8002); - BroadcastMessage(static_cast(sdsmEncodedMsg)); + BroadcastMessage(static_cast(sdsmEncodedMsg)); + } } } diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.cpp index 55e4cd8bf..7b81db987 100644 --- a/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.cpp @@ -35,334 +35,328 @@ namespace CARMAStreetsPlugin } return parseResult; } - - - void JsonToJ3224SDSMConverter::convertJsonToSDSM(const Json::Value &sdsm_json, std::shared_ptr sdsm) const - { - std::vector> shared_ptrs; - + void JsonToJ3224SDSMConverter::convertJsonToSDSM(const Json::Value &sdsm_json, const std::shared_ptr &sdsm) const { + ASN_STRUCT_FREE_CONTENTS_ONLY(asn_DEF_SensorDataSharingMessage, sdsm.get()); + // Message Count sdsm->msgCnt = sdsm_json["msg_cnt"].asInt64(); - - // TODO: confirm input sourceID from JSON to C struct constructs octet appropriately - // sourceID - TemporaryID_t tempID; - - std::string id_data = sdsm_json["source_id"].asString(); - std::vector id_vector(id_data.begin(), id_data.end()); - uint8_t *id_ptr = &id_vector[0]; - tempID.buf = id_ptr; - tempID.size = sizeof(id_ptr); - sdsm->sourceID = tempID; - + // Source ID (Expecting format "rsu_<4-digit-number>") + std::string id_data = sdsm_json["source_id"].asString().substr(4); + auto *tempID = (TemporaryID_t *)calloc(1, sizeof(TemporaryID_t)); + OCTET_STRING_fromString(tempID, id_data.c_str()); + sdsm->sourceID = *tempID; + free(tempID); + + // Equipment Type sdsm->equipmentType = sdsm_json["equipment_type"].asInt64(); - - // sDSMTimeStamp - auto sdsm_time_stamp_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - - auto year_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *year_ptr = sdsm_json["sdsm_time_stamp"]["year"].asInt64(); - sdsm_time_stamp_ptr->year = year_ptr; - - auto month_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *month_ptr = sdsm_json["sdsm_time_stamp"]["month"].asInt64(); - sdsm_time_stamp_ptr->month = month_ptr; - - auto day_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *day_ptr = sdsm_json["sdsm_time_stamp"]["day"].asInt64(); - sdsm_time_stamp_ptr->day = day_ptr; - - auto hour_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *hour_ptr = sdsm_json["sdsm_time_stamp"]["hour"].asInt64(); - sdsm_time_stamp_ptr->hour = hour_ptr; - - auto minute_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *minute_ptr = sdsm_json["sdsm_time_stamp"]["minute"].asInt64(); - sdsm_time_stamp_ptr->minute = minute_ptr; - - auto second_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *second_ptr = sdsm_json["sdsm_time_stamp"]["second"].asInt64(); - sdsm_time_stamp_ptr->second = second_ptr; - - auto offset_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *offset_ptr = sdsm_json["sdsm_time_stamp"]["offset"].asInt64(); - sdsm_time_stamp_ptr->offset = offset_ptr; - - sdsm->sDSMTimeStamp = *sdsm_time_stamp_ptr; - - // refPos - auto ref_pos_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - ref_pos_ptr->lat = sdsm_json["ref_pos"]["lat"].asInt64(); - ref_pos_ptr->Long = sdsm_json["ref_pos"]["long"].asInt64(); - auto elevation_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *elevation_ptr = sdsm_json["ref_pos"]["elevation"].asInt64(); - ref_pos_ptr->elevation = elevation_ptr; - - sdsm->refPos = *ref_pos_ptr; - - // refPosXYConf - PositionalAccuracy_t ref_pos_xy_conf; - ref_pos_xy_conf.semiMajor = sdsm_json["ref_pos_xy_conf"]["semi_major"].asInt64(); - ref_pos_xy_conf.semiMinor = sdsm_json["ref_pos_xy_conf"]["semi_minor"].asInt64(); - ref_pos_xy_conf.orientation = sdsm_json["ref_pos_xy_conf"]["orientation"].asInt64(); - - sdsm->refPosXYConf = ref_pos_xy_conf; - - // refPosElConf - auto ref_pos_el_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *ref_pos_el_conf_ptr = sdsm_json["ref_pos_el_conf"].asInt64(); - sdsm->refPosElConf = ref_pos_el_conf_ptr; - - // Creat initial pointers for detected object list and contained objects - auto object_list = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - auto object_data = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - - - if(sdsm_json.isMember("objects") && sdsm_json["objects"].isArray()){ + // SDSM DateTime timestamp + DDateTime_t sDSMTimeStamp; + // Optional Year + if ( sdsm_json["sdsm_time_stamp"].isMember("year") ) { + auto year = (DYear_t*) calloc(1, sizeof(DYear_t)); + *year = sdsm_json["sdsm_time_stamp"]["year"].asInt64(); + sDSMTimeStamp.year = year; + } + // Optional Month + if ( sdsm_json["sdsm_time_stamp"].isMember("month") ) { + auto month = (DMonth_t*) calloc(1, sizeof(DMonth_t)); + *month = sdsm_json["sdsm_time_stamp"]["month"].asInt64(); + sDSMTimeStamp.month = month; + } + // Optional Day + if ( sdsm_json["sdsm_time_stamp"].isMember("day") ) { + auto day = (DDay_t*) calloc(1, sizeof(DDay_t)); + *day = sdsm_json["sdsm_time_stamp"]["day"].asInt64(); + sDSMTimeStamp.day = day; + } + // Optional Hour + if ( sdsm_json["sdsm_time_stamp"].isMember("hour") ) { + auto hour = (DHour_t*) calloc(1, sizeof(DHour_t)); + *hour = sdsm_json["sdsm_time_stamp"]["hour"].asInt64(); + sDSMTimeStamp.hour = hour; + } + // Optional Minute + if ( sdsm_json["sdsm_time_stamp"].isMember("minute") ) { + auto minute = (DMinute_t*) calloc(1, sizeof(DMinute_t)); + *minute = sdsm_json["sdsm_time_stamp"]["minute"].asInt64(); + sDSMTimeStamp.minute = minute; + } + // Optional Second + if ( sdsm_json["sdsm_time_stamp"].isMember("second") ) { + auto second = (DSecond_t*) calloc(1, sizeof(DSecond_t)); + *second = sdsm_json["sdsm_time_stamp"]["second"].asInt64(); + sDSMTimeStamp.second = second; + } + // Optional Offset + if ( sdsm_json["sdsm_time_stamp"].isMember("offset") ) { + auto offset = (DOffset_t*) calloc( 1, sizeof(DOffset_t)); + *offset = sdsm_json["sdsm_time_stamp"]["offset"].asInt64(); + sDSMTimeStamp.offset = offset; + } + sdsm->sDSMTimeStamp = sDSMTimeStamp; + // Reference Position + sdsm->refPos.lat = sdsm_json["ref_pos"]["lat"].asInt64(); + sdsm->refPos.Long = sdsm_json["ref_pos"]["long"].asInt64(); + // Optional elevation + if (sdsm_json["ref_pos"].isMember("elevation") ) { + auto elevation = (DSRC_Elevation_t*) calloc(1, sizeof(DSRC_Elevation_t)); + *elevation = sdsm_json["ref_pos"]["elevation"].asInt64(); + sdsm->refPos.elevation = elevation; + } + // Positional accuracy + sdsm->refPosXYConf.semiMajor = sdsm_json["ref_pos_xy_conf"]["semi_major"].asInt64(); + sdsm->refPosXYConf.semiMinor = sdsm_json["ref_pos_xy_conf"]["semi_minor"].asInt64(); + sdsm->refPosXYConf.orientation = sdsm_json["ref_pos_xy_conf"]["orientation"].asInt64(); + if (sdsm_json.isMember("ref_pos_el_conf")) { + auto elevation_confidence = (ElevationConfidence_t*) calloc(1, sizeof(ElevationConfidence_t)); + *elevation_confidence = sdsm_json["ref_pos_el_conf"].asInt64(); + sdsm->refPosElConf = elevation_confidence; + } + if (sdsm_json.isMember("objects") && sdsm_json["objects"].isArray() ) { + auto objects = (DetectedObjectList_t*) calloc(1, sizeof(DetectedObjectList_t)); Json::Value objectsJsonArr = sdsm_json["objects"]; for(auto itr = objectsJsonArr.begin(); itr != objectsJsonArr.end(); itr++){ - - auto common_data = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - - // Propegate common data - common_data->objType = (*itr)["detected_object_data"]["detected_object_common_data"]["obj_type"].asInt64();; - common_data->objTypeCfd = (*itr)["detected_object_data"]["detected_object_common_data"]["obj_type_cfd"].asInt64(); - common_data->objectID = (*itr)["detected_object_data"]["detected_object_common_data"]["object_id"].asInt64(); - common_data->measurementTime = (*itr)["detected_object_data"]["detected_object_common_data"]["measurement_time"].asInt64(); - common_data->timeConfidence = (*itr)["detected_object_data"]["detected_object_common_data"]["time_confidence"].asInt64(); - - // pos (posOffsetXYZ) - common_data->pos.offsetX = (*itr)["detected_object_data"]["detected_object_common_data"]["pos"]["offset_x"].asInt64(); - common_data->pos.offsetY = (*itr)["detected_object_data"]["detected_object_common_data"]["pos"]["offset_y"].asInt64(); - auto offset_z_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *offset_z_ptr = (*itr)["detected_object_data"]["detected_object_common_data"]["pos"]["offset_z"].asInt64(); - common_data->pos.offsetZ = offset_z_ptr; - - // posConfidence - common_data->posConfidence.pos = (*itr)["detected_object_data"]["detected_object_common_data"]["pos_confidence"]["pos"].asInt64(); - common_data->posConfidence.elevation = (*itr)["detected_object_data"]["detected_object_common_data"]["pos_confidence"]["elevation"].asInt64(); - - // speed/speedConfidence - common_data->speed = (*itr)["detected_object_data"]["detected_object_common_data"]["speed"].asInt64(); - common_data->speedConfidence = (*itr)["detected_object_data"]["detected_object_common_data"]["speed_confidence"].asInt64(); - - // speedZ/speedConfidenceZ - auto speed_z_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *speed_z_ptr = (*itr)["detected_object_data"]["detected_object_common_data"]["speed_z"].asInt64(); - common_data->speedZ = speed_z_ptr; - auto speed_conf_z_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *speed_conf_z_ptr = (*itr)["detected_object_data"]["detected_object_common_data"]["speed_confidence_z"].asInt64(); - common_data->speedConfidenceZ = speed_conf_z_ptr; - - // heading/headingConf - common_data->heading = (*itr)["detected_object_data"]["detected_object_common_data"]["heading"].asInt64(); - common_data->headingConf = (*itr)["detected_object_data"]["detected_object_common_data"]["heading_conf"].asInt64(); - - // accel4way - auto accel_4_way_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - accel_4_way_ptr->Long = (*itr)["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["long"].asInt64(); - accel_4_way_ptr->lat = (*itr)["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["lat"].asInt64(); - accel_4_way_ptr->vert = (*itr)["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["vert"].asInt64(); - accel_4_way_ptr->yaw = (*itr)["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["yaw"].asInt64(); - common_data->accel4way = accel_4_way_ptr; - - // accCfd(X/Y/Z/Yaw) - auto acc_cfd_x_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *acc_cfd_x_ptr = (*itr)["detected_object_data"]["detected_object_common_data"]["acc_cfd_x"].asInt64(); - common_data->accCfdX = acc_cfd_x_ptr; - - auto acc_cfd_y_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *acc_cfd_y_ptr = (*itr)["detected_object_data"]["detected_object_common_data"]["acc_cfd_y"].asInt64(); - common_data->accCfdY = acc_cfd_y_ptr; - - auto acc_cfd_z_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *acc_cfd_z_ptr = (*itr)["detected_object_data"]["detected_object_common_data"]["acc_cfd_z"].asInt64(); - common_data->accCfdZ = acc_cfd_z_ptr; - - auto acc_cfd_yaw_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *acc_cfd_yaw_ptr = (*itr)["detected_object_data"]["detected_object_common_data"]["acc_cfd_yaw"].asInt64(); - common_data->accCfdYaw = acc_cfd_yaw_ptr; - - - // Add common data to object data - object_data->detObjCommon = *common_data; - - - - // Propegate optional data fields - - // detectedObjectOptionalData - auto optional_data_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - - // determine which optional data choice is being used if any - // detVeh - if((*itr)["detected_object_data"]["detected_object_optional_data"].isMember("detected_vehicle_data")){ - - // set presence val to veh - optional_data_ptr->present = DetectedObjectOptionalData_PR_detVeh; - - // TODO: find a better way to convert/test lights val without calloc - // // lights - // auto lights_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - // auto lights = static_cast((*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["lights"].asInt()); - // lights_ptr->buf = (uint8_t *)calloc(2, sizeof(uint8_t)); // TODO: find calloc alternative if possible, causes a memory leak - // lights_ptr->size = 2 * sizeof(uint8_t); - // lights_ptr->bits_unused = 0; - // lights_ptr->buf[1] = static_cast(lights); - // lights_ptr->buf[0] = (lights >> 8); - - // optional_data_ptr->choice.detVeh.lights = lights_ptr; - - - // vehAttitude - auto attitude_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - attitude_ptr->pitch = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude"]["pitch"].asInt64(); - attitude_ptr->roll = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude"]["roll"].asInt64(); - attitude_ptr->yaw = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude"]["yaw"].asInt64(); - optional_data_ptr->choice.detVeh.vehAttitude = attitude_ptr; - - // vehAttitudeConfidence - auto attitude_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - attitude_conf_ptr->pitchConfidence = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude_confidence"]["pitch_confidence"].asInt64(); - attitude_conf_ptr->rollConfidence = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude_confidence"]["roll_confidence"].asInt64(); - attitude_conf_ptr->yawConfidence = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude_confidence"]["yaw_confidence"].asInt64(); - optional_data_ptr->choice.detVeh.vehAttitudeConfidence = attitude_conf_ptr; - - // vehAngVel - auto ang_vel_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - ang_vel_ptr->pitchRate = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_ang_vel"]["pitch_rate"].asInt64(); - ang_vel_ptr->rollRate = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_ang_vel"]["roll_rate"].asInt64(); - optional_data_ptr->choice.detVeh.vehAngVel = ang_vel_ptr; - - // vehAngVelConfidence - auto ang_vel_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - auto pitch_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *pitch_conf_ptr = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_ang_vel_confidence"]["pitch_rate_confidence"].asInt64(); - ang_vel_conf_ptr->pitchRateConfidence = pitch_conf_ptr; - auto roll_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *roll_conf_ptr = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_ang_vel_confidence"]["roll_rate_confidence"].asInt64(); - ang_vel_conf_ptr->rollRateConfidence = roll_conf_ptr; - optional_data_ptr->choice.detVeh.vehAngVelConfidence = ang_vel_conf_ptr; - - // size (VehicleSize) - auto size_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - size_ptr->width = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["size"]["width"].asInt64(); - size_ptr->length = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["size"]["length"].asInt64(); - optional_data_ptr->choice.detVeh.size = size_ptr; - - // height - auto height_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *height_ptr = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["height"].asInt64(); - optional_data_ptr->choice.detVeh.height = height_ptr; - - // vehcleSizeConfidence - auto size_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - size_conf_ptr->vehicleWidthConfidence = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["vehicle_size_confidence"]["vehicle_width_confidence"].asInt64(); - size_conf_ptr->vehicleLengthConfidence = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["vehicle_size_confidence"]["vehicle_length_confidence"].asInt64(); - auto height_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *height_conf_ptr = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["vehicle_size_confidence"]["vehicle_height_confidence"].asInt64(); - size_conf_ptr->vehicleHeightConfidence = height_conf_ptr; - optional_data_ptr->choice.detVeh.vehicleSizeConfidence = size_conf_ptr; - - // vehicleClass - auto veh_class_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *veh_class_ptr = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["vehicle_class"].asInt64(); - optional_data_ptr->choice.detVeh.vehicleClass = veh_class_ptr; - - // vehClassConf - auto veh_class_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *veh_class_conf_ptr = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["vehicle_class_conf"].asInt64(); - optional_data_ptr->choice.detVeh.classConf = veh_class_conf_ptr; + auto objectData = (DetectedObjectData_t*) calloc(1, sizeof(DetectedObjectData_t)); + // Object Common Required Properties + // Object Type + objectData->detObjCommon.objType = (*itr)["detected_object_data"]["detected_object_common_data"]["obj_type"].asInt64(); + // Object Type Classification confidence + objectData->detObjCommon.objTypeCfd = (*itr)["detected_object_data"]["detected_object_common_data"]["obj_type_cfd"].asInt64(); + // Object ID + objectData->detObjCommon.objectID = (*itr)["detected_object_data"]["detected_object_common_data"]["object_id"].asInt64(); + // Time offset from SDSM timestamp + objectData->detObjCommon.measurementTime = (*itr)["detected_object_data"]["detected_object_common_data"]["measurement_time"].asInt64(); + // Time offset confidence + objectData->detObjCommon.timeConfidence = (*itr)["detected_object_data"]["detected_object_common_data"]["time_confidence"].asInt64(); + // Position offset from reference position + objectData->detObjCommon.pos.offsetX = (*itr)["detected_object_data"]["detected_object_common_data"]["pos"]["offset_x"].asInt64(); + objectData->detObjCommon.pos.offsetY = (*itr)["detected_object_data"]["detected_object_common_data"]["pos"]["offset_y"].asInt64(); + // Optional Z offset + if ( (*itr)["detected_object_data"]["detected_object_common_data"]["pos"].isMember("offset_z") ) { + auto offset_z = (ObjectDistance_t*) calloc(1, sizeof(ObjectDistance_t)); + *offset_z = (*itr)["detected_object_data"]["detected_object_common_data"]["pos"]["offset_z"].asInt64(); + objectData->detObjCommon.pos.offsetZ = offset_z; } - - // detVRU - else if((*itr)["detected_object_data"]["detected_object_optional_data"].isMember("detected_vru_data")){ - - optional_data_ptr->present = DetectedObjectOptionalData_PR_detVRU; - - // basicType - auto basic_type_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *basic_type_ptr = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vru_data"]["basic_type"].asInt64(); - optional_data_ptr->choice.detVRU.basicType = basic_type_ptr; - - // propulsion choice struct (check to see if propulsion exists) - auto propulsion_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - - if((*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vru_data"]["propulsion"].isMember("human")){ - propulsion_ptr->present = PropelledInformation_PR_human; - propulsion_ptr->choice.human = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vru_data"]["propulsion"]["human"].asInt64(); - } - else if((*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vru_data"]["propulsion"].isMember("animal")){ - propulsion_ptr->present = PropelledInformation_PR_animal; - propulsion_ptr->choice.animal = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vru_data"]["propulsion"]["animal"].asInt64(); - } - else if((*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vru_data"]["propulsion"].isMember("motor")){ - propulsion_ptr->present = PropelledInformation_PR_motor; - propulsion_ptr->choice.motor = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vru_data"]["propulsion"]["motor"].asInt64(); - } - else{ - propulsion_ptr->present = PropelledInformation_PR_NOTHING; - std::cout << "Value was nothing" << std::endl; - } - optional_data_ptr->choice.detVRU.propulsion = propulsion_ptr; - - // attachement - auto attachment_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *attachment_ptr = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vru_data"]["attachment"].asInt64(); - optional_data_ptr->choice.detVRU.attachment = attachment_ptr; - - // radius - auto radius_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *radius_ptr = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_vru_data"]["radius"].asInt64(); - optional_data_ptr->choice.detVRU.radius = radius_ptr; + // Position Confidence + objectData->detObjCommon.posConfidence.pos = (*itr)["detected_object_data"]["detected_object_common_data"]["pos_confidence"]["pos"].asInt64(); + // Elevation Confidence + objectData->detObjCommon.posConfidence.elevation = (*itr)["detected_object_data"]["detected_object_common_data"]["pos_confidence"]["elevation"].asInt64(); + // Horizontal Speed + objectData->detObjCommon.speed = (*itr)["detected_object_data"]["detected_object_common_data"]["speed"].asInt64(); + // Horizontal Speed confidence + objectData->detObjCommon.speedConfidence = (*itr)["detected_object_data"]["detected_object_common_data"]["speed_confidence"].asInt64(); + // Optional Vertical Speed + if ( (*itr)["detected_object_data"]["detected_object_common_data"].isMember("speed_z") ) { + auto speed_z = (Speed_t*) calloc(1, sizeof(Speed_t)); + *speed_z = (*itr)["detected_object_data"]["detected_object_common_data"]["speed_z"].asInt64(); + objectData->detObjCommon.speedZ = speed_z; } - - - // detObst - else if((*itr)["detected_object_data"]["detected_object_optional_data"].isMember("detected_obstacle_data")){ - optional_data_ptr->present = DetectedObjectOptionalData_PR_detObst; - - ObstacleSize obst_size; - obst_size.width = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_obstacle_data"]["obst_size"]["width"].asInt64(); - obst_size.length = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_obstacle_data"]["obst_size"]["length"].asInt64(); - auto obst_height_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *obst_height_ptr = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_obstacle_data"]["obst_size"]["height"].asInt64(); - obst_size.height = obst_height_ptr; - optional_data_ptr->choice.detObst.obstSize = obst_size; - - ObstacleSizeConfidence obst_size_conf; - obst_size_conf.widthConfidence = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_obstacle_data"]["obst_size_confidence"]["width_confidence"].asInt64(); - obst_size_conf.lengthConfidence = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_obstacle_data"]["obst_size_confidence"]["length_confidence"].asInt64(); - auto obst_height_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); - *obst_height_conf_ptr = (*itr)["detected_object_data"]["detected_object_optional_data"]["detected_obstacle_data"]["obst_size_confidence"]["height_confidence"].asInt64(); - obst_size_conf.heightConfidence = obst_height_conf_ptr; - optional_data_ptr->choice.detObst.obstSizeConfidence = obst_size_conf; + // Optional Vertical Speed confidence + if ( (*itr)["detected_object_data"]["detected_object_common_data"].isMember("speed_confidence_z")) { + auto speed_confidence_z = (SpeedConfidence_t*) calloc(1, sizeof(SpeedConfidence_t)); + *speed_confidence_z = (*itr)["detected_object_data"]["detected_object_common_data"]["speed_confidence_z"].asInt64(); + objectData->detObjCommon.speedConfidenceZ = speed_confidence_z; } - - - // Add optional data to object data - object_data->detObjOptData = optional_data_ptr; - - + // Heading + objectData->detObjCommon.heading = (*itr)["detected_object_data"]["detected_object_common_data"]["heading"].asInt64(); + // Heading Confidence + objectData->detObjCommon.headingConf = (*itr)["detected_object_data"]["detected_object_common_data"]["heading_conf"].asInt64(); + // Optional 4 way acceleration + if ( (*itr)["detected_object_data"]["detected_object_common_data"].isMember("accel_4_way") ){ + auto accel_4way = (AccelerationSet4Way_t*) calloc(1, sizeof(AccelerationSet4Way_t)); + accel_4way->Long = (*itr)["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["long"].asInt64(); + accel_4way->lat = (*itr)["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["lat"].asInt64(); + accel_4way->vert = (*itr)["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["vert"].asInt64(); + accel_4way->yaw = (*itr)["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["yaw"].asInt64(); + objectData->detObjCommon.accel4way = accel_4way; + } + // Optional acceleration confidence X + if( (*itr)["detected_object_data"]["detected_object_common_data"].isMember("acc_cfd_x") ) { + auto acc_cfd_x = (AccelerationConfidence_t*)calloc(1, sizeof(AccelerationConfidence_t)); + *acc_cfd_x = (*itr)["detected_object_data"]["detected_object_common_data"]["acc_cfd_x"].asInt64(); + objectData->detObjCommon.accCfdX = acc_cfd_x; + } + // Optional acceleration confidence Y + if( (*itr)["detected_object_data"]["detected_object_common_data"].isMember("acc_cfd_y") ) { + auto acc_cfd_y = (AccelerationConfidence_t*)calloc(1, sizeof(AccelerationConfidence_t)); + *acc_cfd_y = (*itr)["detected_object_data"]["detected_object_common_data"]["acc_cfd_y"].asInt64(); + objectData->detObjCommon.accCfdY = acc_cfd_y; + } + // Optional acceleration confidence Z + if( (*itr)["detected_object_data"]["detected_object_common_data"].isMember("acc_cfd_z") ) { + auto acc_cfd_z = (AccelerationConfidence_t*)calloc(1, sizeof(AccelerationConfidence_t)); + *acc_cfd_z = (*itr)["detected_object_data"]["detected_object_common_data"]["acc_cfd_z"].asInt64(); + objectData->detObjCommon.accCfdZ = acc_cfd_z; + } + // Optional acceleration confidence Yaw + if( (*itr)["detected_object_data"]["detected_object_common_data"].isMember("acc_cfd_yaw") ) { + auto acc_cfd_yaw = (AccelerationConfidence_t*)calloc(1, sizeof(YawRateConfidence_t)); + *acc_cfd_yaw = (*itr)["detected_object_data"]["detected_object_common_data"]["acc_cfd_yaw"].asInt64(); + objectData->detObjCommon.accCfdYaw = acc_cfd_yaw; + } + // Object Optional Data + if ((*itr)["detected_object_data"].isMember("detected_object_optional_data") ){ + auto optional_data = (DetectedObjectOptionalData_t*)calloc(1, sizeof(DetectedObjectOptionalData_t)); + populateOptionalData((*itr)["detected_object_data"]["detected_object_optional_data"], optional_data); + objectData->detObjOptData = optional_data; + } + ASN_SEQUENCE_ADD(&objects->list.array, objectData); } - } - - // Append the object to the detected objects list - asn_sequence_add(&object_list->list.array, object_data); - - // Set the data to the ASN.1 C struct - sdsm->objects = *object_list; - + sdsm->objects = *objects; + free(objects); + } + asn_fprint(stdout, &asn_DEF_SensorDataSharingMessage, sdsm.get()); } - void JsonToJ3224SDSMConverter::encodeSDSM(const std::shared_ptr &sdsmPtr, tmx::messages::SdsmEncodedMessage &encodedSDSM) const + void JsonToJ3224SDSMConverter::encodeSDSM(const std::shared_ptr &sdsmPtr, tmx::messages::SdsmEncodedMessage &encodedSDSM) const { - tmx::messages::MessageFrameMessage frame(sdsmPtr); + auto _sdsmMessage = new tmx::messages::SdsmMessage(sdsmPtr); + tmx::messages::MessageFrameMessage frame(_sdsmMessage->get_j2735_data()); encodedSDSM.set_data(tmx::messages::TmxJ2735EncodedMessage::encode_j2735_message>(frame)); + asn_fprint(stdout, &asn_DEF_MessageFrame, frame.get_j2735_data().get()); free(frame.get_j2735_data().get()); + delete(_sdsmMessage); } + void JsonToJ3224SDSMConverter::populateOptionalData(const Json::Value &optional_data_json, DetectedObjectOptionalData_t *optional_data) const { + if ( optional_data_json.isMember("detected_vehicle_data") ) { + optional_data->present = DetectedObjectOptionalData_PR_detVeh; + // Optional Vehicle Attitude + if (optional_data_json["detected_vehicle_data"].isMember("veh_attitude")) { + auto attitude = (Attitude_t*) calloc( 1, sizeof(Attitude_t)); + attitude->pitch = optional_data_json["detected_vehicle_data"]["veh_attitude"]["pitch"].asInt64(); + attitude->roll = optional_data_json["detected_vehicle_data"]["veh_attitude"]["roll"].asInt64(); + attitude->yaw = optional_data_json["detected_vehicle_data"]["veh_attitude"]["yaw"].asInt64(); + optional_data->choice.detVeh.vehAttitude = attitude; + } + // Optional Vehicle Attitude Confidence + if (optional_data_json["detected_vehicle_data"].isMember("veh_attitude_confidence")) { + auto attitude_confidence = (AttitudeConfidence_t*) calloc( 1, sizeof(AttitudeConfidence_t)); + attitude_confidence->pitchConfidence = optional_data_json["detected_vehicle_data"]["veh_attitude_confidence"]["pitch_confidence"].asInt64(); + attitude_confidence->rollConfidence = optional_data_json["detected_vehicle_data"]["veh_attitude_confidence"]["roll_confidence"].asInt64(); + attitude_confidence->yawConfidence = optional_data_json["detected_vehicle_data"]["veh_attitude_confidence"]["yaw_confidence"].asInt64(); + optional_data->choice.detVeh.vehAttitudeConfidence = attitude_confidence; + } + // Optional Vehicle Angular Velocity + if (optional_data_json["detected_vehicle_data"].isMember("veh_ang_vel")) { + auto angular_velocity = (AngularVelocity_t*) calloc( 1, sizeof(AngularVelocity_t)); + angular_velocity->pitchRate = optional_data_json["detected_vehicle_data"]["veh_ang_vel"]["pitch_rate"].asInt64(); + 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; + } + // Optional Vehicle Size + if (optional_data_json["detected_vehicle_data"].isMember("size")) { + auto veh_size = (VehicleSize_t*) calloc( 1, sizeof(VehicleSize_t)); + veh_size->length = optional_data_json["detected_vehicle_data"]["size"]["length"].asInt64(); + veh_size->width = optional_data_json["detected_vehicle_data"]["size"]["width"].asInt64(); + optional_data->choice.detVeh.size = veh_size; + } + // Optional Vehicle Height + if (optional_data_json["detected_vehicle_data"].isMember("height")) { + auto veh_height = (VehicleHeight_t*)calloc(1, sizeof(VehicleHeight_t)); + *veh_height = optional_data_json["detected_vehicle_data"]["height"].asInt64(); + optional_data->choice.detVeh.height = veh_height; + } + // Optional Vehicle Size Confidence + if (optional_data_json["detected_vehicle_data"].isMember("vehicle_size_confidence")) { + auto veh_size_confidence = (VehicleSizeConfidence_t*)calloc(1, sizeof(VehicleSizeConfidence_t)); + veh_size_confidence->vehicleLengthConfidence = optional_data_json["detected_vehicle_data"]["vehicle_size_confidence"]["vehicle_length_confidence"].asInt64(); + veh_size_confidence->vehicleWidthConfidence = optional_data_json["detected_vehicle_data"]["vehicle_size_confidence"]["vehicle_width_confidence"].asInt64(); + if (optional_data_json["detected_vehicle_data"]["vehicle_size_confidence"].isMember("vehicle_height_confidence")) { + auto veh_height_confidence = (SizeValueConfidence_t*)calloc(1, sizeof(SizeValueConfidence_t)); + *veh_height_confidence = optional_data_json["detected_vehicle_data"]["vehicle_size_confidence"]["vehicle_height_confidence"].asInt64(); + veh_size_confidence->vehicleHeightConfidence = veh_height_confidence; + } + optional_data->choice.detVeh.vehicleSizeConfidence = veh_size_confidence; + } + // Optional Vehicle Class + if (optional_data_json["detected_vehicle_data"].isMember("vehicle_class")) { + auto vehicle_class = (BasicVehicleClass_t*)calloc(1, sizeof(BasicVehicleClass_t)); + *vehicle_class = optional_data_json["detected_vehicle_data"]["vehicle_class"].asInt64(); + optional_data->choice.detVeh.vehicleClass = vehicle_class; + } + if (optional_data_json["detected_vehicle_data"].isMember("vehicle_class_conf")) { + auto vehicle_class_conf = (ClassificationConfidence_t*)calloc(1, sizeof(ClassificationConfidence_t)); + *vehicle_class_conf = optional_data_json["detected_vehicle_data"]["vehicle_class_conf"].asInt64(); + optional_data->choice.detVeh.classConf = vehicle_class_conf; + } + + } + else if (optional_data_json.isMember("detected_vru_data") ) { + optional_data->present = DetectedObjectOptionalData_PR_detVRU; + // Optional VRU Basic Type + if ( optional_data_json["detected_vru_data"].isMember("basic_type")) { + auto basic_type = (PersonalDeviceUserType_t*) calloc(1, sizeof(PersonalDeviceUserType_t)); + *basic_type = optional_data_json["detected_vru_data"]["basic_type"].asInt64(); + optional_data->choice.detVRU.basicType = basic_type; + } + // Optional propulsion information + if (optional_data_json["detected_vru_data"].isMember("propulsion")) { + auto propelled_info = (PropelledInformation_t*) calloc(1, sizeof(PropelledInformation_t)); + if (optional_data_json["detected_vru_data"]["propulsion"].isMember("human") ) { + propelled_info->present = PropelledInformation_PR_human; + propelled_info->choice.human = optional_data_json["detected_vru_data"]["propulsion"]["human"].asInt64(); + } + else if (optional_data_json["detected_vru_data"]["propulsion"].isMember("animal") ) { + propelled_info->present = PropelledInformation_PR_animal; + propelled_info->choice.animal = optional_data_json["detected_vru_data"]["propulsion"]["animal"].asInt64(); + } + else if (optional_data_json["detected_vru_data"]["propulsion"].isMember("motor") ) { + propelled_info->present = PropelledInformation_PR_motor; + propelled_info->choice.motor = optional_data_json["detected_vru_data"]["propulsion"]["motor"].asInt64(); + } + optional_data->choice.detVRU.propulsion = propelled_info; + } + // Optional VRU Attachment + if ( optional_data_json["detected_vru_data"].isMember("attachment")) { + auto attachment = (Attachment_t*) calloc(1, sizeof(Attachment_t)); + *attachment = optional_data_json["detected_vru_data"]["attachment"].asInt64(); + optional_data->choice.detVRU.attachment = attachment; + } + // Optional VRU Attachment Radius + if ( optional_data_json["detected_vru_data"].isMember("radius")) { + auto radius = (AttachmentRadius_t*) calloc(1, sizeof(AttachmentRadius_t)); + *radius = optional_data_json["detected_vru_data"]["radius"].asInt64(); + optional_data->choice.detVRU.radius = radius; + } + + + }else if (optional_data_json.isMember("detected_obstacle_data")) { + optional_data->present = DetectedObjectOptionalData_PR_detObst; + optional_data->choice.detObst.obstSize.length = optional_data_json["detected_obstacle_data"]["obst_size"]["length"].asInt64(); + optional_data->choice.detObst.obstSize.width = optional_data_json["detected_obstacle_data"]["obst_size"]["width"].asInt64(); + // Optional Obstacle Height + if (optional_data_json["detected_obstacle_data"]["obst_size"].isMember("height")) { + auto obst_height = (SizeValue_t*)calloc(1, sizeof(SizeValue_t)); + *obst_height = optional_data_json["detected_obstacle_data"]["obst_size"]["height"].asInt64(); + optional_data->choice.detObst.obstSize.height = obst_height; + } + optional_data->choice.detObst.obstSizeConfidence.lengthConfidence = optional_data_json["detected_obstacle_data"]["obst_size_confidence"]["length_confidence"].asInt64(); + optional_data->choice.detObst.obstSizeConfidence.widthConfidence = optional_data_json["detected_obstacle_data"]["obst_size_confidence"]["width_confidence"].asInt64(); + // Optional Obstalce Height Confidence + if (optional_data_json["detected_obstacle_data"]["obst_size_confidence"].isMember("height_confidence")) { + auto obst_height_confidence = (SizeValueConfidence_t*)calloc(1, sizeof(SizeValueConfidence_t)); + *obst_height_confidence = optional_data_json["detected_obstacle_data"]["obst_size_confidence"]["height_confidence"].asInt64(); + optional_data->choice.detObst.obstSizeConfidence.heightConfidence = obst_height_confidence; + } + } + } } \ No newline at end of file diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.h b/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.h index ea13123f2..f5b783677 100644 --- a/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.h +++ b/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.h @@ -31,16 +31,19 @@ namespace CARMAStreetsPlugin * @param json Incoming Json value with sdsm information that is consumed from a Kafka topic. * @param sdsm Outgoing J3224 sdsm object that is populated by the json value. */ - void convertJsonToSDSM(const Json::Value &sdsm_json, std::shared_ptr sdsm) const; + + void convertJsonToSDSM(const Json::Value &sdsm_json, const std::shared_ptr &sdsm) const; /*** * @brief Encode J3224 SDSM * @param Pointer to J3224 SDSM object * @param Encoded J3224 SDSM */ - void encodeSDSM(const std::shared_ptr &sdsmPtr, tmx::messages::SdsmEncodedMessage &encodedSDSM) const; - + void encodeSDSM(const std::shared_ptr &sdsmPtr, tmx::messages::SdsmEncodedMessage &encodedSDSM) const; + ~JsonToJ3224SDSMConverter() = default; + private: + void populateOptionalData(const Json::Value &optional_data_json, DetectedObjectOptionalData_t *optional_data) const; }; } \ No newline at end of file diff --git a/src/v2i-hub/CARMAStreetsPlugin/test/test_JsonToJ3224SDSMConverter.cpp b/src/v2i-hub/CARMAStreetsPlugin/test/test_JsonToJ3224SDSMConverter.cpp index 5d38d4716..2ca5ba7ff 100644 --- a/src/v2i-hub/CARMAStreetsPlugin/test/test_JsonToJ3224SDSMConverter.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/test/test_JsonToJ3224SDSMConverter.cpp @@ -20,169 +20,679 @@ namespace CARMAStreetsPlugin std::string valid_sdsm_json = "{\"equipment_type\":1,\"msg_cnt\":1,\"ref_pos\":{\"long\":600000000,\"elevation\":30,\"lat\":400000000},\"ref_pos_el_conf\":10,\"ref_pos_xy_conf\":{\"orientation\":25000,\"semi_major\":235,\"semi_minor\":200},\"sdsm_time_stamp\":{\"day\":4,\"hour\":19,\"minute\":15,\"month\":7,\"offset\":400,\"second\":5000,\"year\":2007},\"source_id\":\"01020304\",\"objects\":[{\"detected_object_data\":{\"detected_object_common_data\":{\"acc_cfd_x\":4,\"acc_cfd_y\":5,\"acc_cfd_yaw\":3,\"acc_cfd_z\":6,\"accel_4_way\":{\"lat\":-500,\"long\":200,\"vert\":1,\"yaw\":400},\"heading\":16000,\"heading_conf\":4,\"measurement_time\":-1100,\"object_id\":12200,\"obj_type\":1,\"obj_type_cfd\":65,\"pos\":{\"offset_x\":4000,\"offset_y\":-720,\"offset_z\":20},\"pos_confidence\":{\"elevation\":5,\"pos\":2},\"speed\":2100,\"speed_confidence\":3,\"speed_confidence_z\":4,\"speed_z\":1000,\"time_confidence\":2}}}]}"; Json::Value root; bool result = converter.parseJsonString(valid_sdsm_json, root); - ASSERT_TRUE(result); + EXPECT_TRUE(result); std::string invalid_json = "invalid"; result = converter.parseJsonString(invalid_json, root); ASSERT_FALSE(result); } - // Test for SDSM header data - TEST_F(test_JsonToJ3224SDSMConverter, convertJsonToSDSM_header) + // // Test for SDSM common data + TEST_F(test_JsonToJ3224SDSMConverter, convertToSdsm) { JsonToJ3224SDSMConverter converter; - std::string valid_json_str = "{\"equipment_type\":1,\"msg_cnt\":1,\"ref_pos\":{\"long\":600000000,\"elevation\":30,\"lat\":400000000},\"ref_pos_el_conf\":10,\"ref_pos_xy_conf\":{\"orientation\":25000,\"semi_major\":235,\"semi_minor\":200},\"sdsm_time_stamp\":{\"day\":4,\"hour\":19,\"minute\":15,\"month\":7,\"offset\":400,\"second\":5000,\"year\":2007},\"source_id\":\"00001234\",\"objects\":[{\"detected_object_data\":{\"detected_object_common_data\":{\"acc_cfd_x\":4,\"acc_cfd_y\":5,\"acc_cfd_yaw\":3,\"acc_cfd_z\":6,\"accel_4_way\":{\"lat\":-500,\"long\":200,\"vert\":1,\"yaw\":400},\"heading\":16000,\"heading_conf\":4,\"measurement_time\":-1100,\"object_id\":12200,\"obj_type\":1,\"obj_type_cfd\":65,\"pos\":{\"offset_x\":4000,\"offset_y\":-720,\"offset_z\":20},\"pos_confidence\":{\"elevation\":5,\"pos\":2},\"speed\":2100,\"speed_confidence\":3,\"speed_confidence_z\":4,\"speed_z\":1000,\"time_confidence\":2}}}]}"; + std::string valid_json_str = R"( + { + "equipment_type":1, + "msg_cnt":1, + "ref_pos":{ + "long":600000000, + "elevation":30, + "lat":400000000 + }, + "ref_pos_el_conf":10, + "ref_pos_xy_conf":{ + "orientation":25000, + "semi_major":235, + "semi_minor":200 + }, + "sdsm_time_stamp":{ + "day":4, + "hour":19, + "minute":15, + "month":7, + "offset":400, + "second":5000, + "year":2007 + }, + "source_id":"rsu_1234", + "objects":[ + { + "detected_object_data":{ + "detected_object_common_data":{ + "acc_cfd_x":4, + "acc_cfd_y":5, + "acc_cfd_yaw":3, + "acc_cfd_z":6, + "accel_4_way":{ + "lat":-500, + "long":200, + "vert":1, + "yaw":400 + }, + "heading":16000, + "heading_conf":4, + "measurement_time":-1100, + "object_id":12200, + "obj_type":1, + "obj_type_cfd":65, + "pos":{ + "offset_x":4000, + "offset_y":-720, + "offset_z":20 + }, + "pos_confidence":{ + "elevation":5, + "pos":2 + }, + "speed":2100, + "speed_confidence":3, + "speed_confidence_z":4, + "speed_z":1000, + "time_confidence":2 + } + } + } + ] + })"; Json::Value root; bool result = converter.parseJsonString(valid_json_str, root); - ASSERT_TRUE(result); - auto sdsmPtr = std::make_shared(); + EXPECT_TRUE(result); + auto sdsmPtr = std::make_shared(); converter.convertJsonToSDSM(root, sdsmPtr); - ASSERT_EQ(1, sdsmPtr->msgCnt); - - // TODO: find a way to generate test octet strings for unit test comparisons - size_t test_size = 8; - ASSERT_EQ(test_size, sdsmPtr->sourceID.size); // this only compares size since size is an int - - ASSERT_EQ(1, sdsmPtr->equipmentType); - ASSERT_EQ(2007, *sdsmPtr->sDSMTimeStamp.year); - ASSERT_EQ(7, *sdsmPtr->sDSMTimeStamp.month); - ASSERT_EQ(4, *sdsmPtr->sDSMTimeStamp.day); - ASSERT_EQ(19, *sdsmPtr->sDSMTimeStamp.hour); - ASSERT_EQ(15, *sdsmPtr->sDSMTimeStamp.minute); - ASSERT_EQ(5000, *sdsmPtr->sDSMTimeStamp.second); - ASSERT_EQ(400, *sdsmPtr->sDSMTimeStamp.offset); - - ASSERT_EQ(400000000, sdsmPtr->refPos.lat); - ASSERT_EQ(600000000, sdsmPtr->refPos.Long); - ASSERT_EQ(30, *sdsmPtr->refPos.elevation); - - ASSERT_EQ(235, sdsmPtr->refPosXYConf.semiMajor); - ASSERT_EQ(200, sdsmPtr->refPosXYConf.semiMinor); - ASSERT_EQ(25000, sdsmPtr->refPosXYConf.orientation); - - ASSERT_EQ(10, *sdsmPtr->refPosElConf); + EXPECT_EQ(1, sdsmPtr->objects.list.array[0]->detObjCommon.objType); + EXPECT_EQ(65, sdsmPtr->objects.list.array[0]->detObjCommon.objTypeCfd); + EXPECT_EQ(12200, sdsmPtr->objects.list.array[0]->detObjCommon.objectID); + EXPECT_EQ(-1100, sdsmPtr->objects.list.array[0]->detObjCommon.measurementTime); + EXPECT_EQ(2, sdsmPtr->objects.list.array[0]->detObjCommon.timeConfidence); + + EXPECT_EQ(4000, sdsmPtr->objects.list.array[0]->detObjCommon.pos.offsetX); + EXPECT_EQ(-720, sdsmPtr->objects.list.array[0]->detObjCommon.pos.offsetY); + EXPECT_EQ(20, *sdsmPtr->objects.list.array[0]->detObjCommon.pos.offsetZ); + + EXPECT_EQ(2, sdsmPtr->objects.list.array[0]->detObjCommon.posConfidence.pos); + EXPECT_EQ(5, sdsmPtr->objects.list.array[0]->detObjCommon.posConfidence.elevation); + + EXPECT_EQ(2100, sdsmPtr->objects.list.array[0]->detObjCommon.speed); + EXPECT_EQ(3, sdsmPtr->objects.list.array[0]->detObjCommon.speedConfidence); + EXPECT_EQ(1000, *sdsmPtr->objects.list.array[0]->detObjCommon.speedZ); + EXPECT_EQ(4, *sdsmPtr->objects.list.array[0]->detObjCommon.speedConfidenceZ); + + EXPECT_EQ(16000, sdsmPtr->objects.list.array[0]->detObjCommon.heading); + EXPECT_EQ(4, sdsmPtr->objects.list.array[0]->detObjCommon.headingConf); + + EXPECT_EQ(200, sdsmPtr->objects.list.array[0]->detObjCommon.accel4way->Long); + EXPECT_EQ(-500, sdsmPtr->objects.list.array[0]->detObjCommon.accel4way->lat); + EXPECT_EQ(1, sdsmPtr->objects.list.array[0]->detObjCommon.accel4way->vert); + EXPECT_EQ(400, sdsmPtr->objects.list.array[0]->detObjCommon.accel4way->yaw); + EXPECT_EQ(4, *sdsmPtr->objects.list.array[0]->detObjCommon.accCfdX); + EXPECT_EQ(5, *sdsmPtr->objects.list.array[0]->detObjCommon.accCfdY); + EXPECT_EQ(6, *sdsmPtr->objects.list.array[0]->detObjCommon.accCfdZ); + EXPECT_EQ(3, *sdsmPtr->objects.list.array[0]->detObjCommon.accCfdYaw); + + tmx::messages::SdsmEncodedMessage encodedSdsm; + converter.encodeSDSM(sdsmPtr, encodedSdsm); + EXPECT_EQ(41, encodedSdsm.get_msgId()); + + std::string expectedSDSMEncHex = "00293981313233343fdf5dc933c4e226c29af8da011e1a2ffe203dd790c3514007f304bea06402c7cfbe97c00992a0d18fa23e809130bb901031f2e6"; + EXPECT_EQ(expectedSDSMEncHex, encodedSdsm.get_payload_str()); } - // // Test for SDSM common data - TEST_F(test_JsonToJ3224SDSMConverter, convertJsonToSDSM_common) + + // Test for SDSM optional data - vehicle data + TEST_F(test_JsonToJ3224SDSMConverter, convertJsonToSDSM_veh) { JsonToJ3224SDSMConverter converter; - std::string valid_json_str = "{\"equipment_type\":1,\"msg_cnt\":1,\"ref_pos\":{\"long\":600000000,\"elevation\":30,\"lat\":400000000},\"ref_pos_el_conf\":10,\"ref_pos_xy_conf\":{\"orientation\":25000,\"semi_major\":235,\"semi_minor\":200},\"sdsm_time_stamp\":{\"day\":4,\"hour\":19,\"minute\":15,\"month\":7,\"offset\":400,\"second\":5000,\"year\":2007},\"source_id\":\"01020304\",\"objects\":[{\"detected_object_data\":{\"detected_object_common_data\":{\"acc_cfd_x\":4,\"acc_cfd_y\":5,\"acc_cfd_yaw\":3,\"acc_cfd_z\":6,\"accel_4_way\":{\"lat\":-500,\"long\":200,\"vert\":1,\"yaw\":400},\"heading\":16000,\"heading_conf\":4,\"measurement_time\":-1100,\"object_id\":12200,\"obj_type\":1,\"obj_type_cfd\":65,\"pos\":{\"offset_x\":4000,\"offset_y\":-720,\"offset_z\":20},\"pos_confidence\":{\"elevation\":5,\"pos\":2},\"speed\":2100,\"speed_confidence\":3,\"speed_confidence_z\":4,\"speed_z\":1000,\"time_confidence\":2}}}]}"; + std::string valid_json_str = R"( + { + "equipment_type":1, + "msg_cnt":1, + "objects":[ + { + "detected_object_data":{ + "detected_object_common_data":{ + "acc_cfd_x":4, + "acc_cfd_y":5, + "acc_cfd_yaw":3, + "acc_cfd_z":6, + "accel_4_way":{ + "lat":-500," + long":200, + "vert":1," + yaw":400 + }, + "heading":16000, + "heading_conf":4, + "measurement_time":-1100, + "object_id":12200, + "obj_type":1, + "obj_type_cfd":65, + "pos":{"offset_x":4000,"offset_y":-720,"offset_z":20}, + "pos_confidence":{"elevation":5,"pos":2}, + "speed":2100, + "speed_confidence":3, + "speed_confidence_z":4, + "speed_z":1000, + "time_confidence":2 + }, + "detected_object_optional_data":{ + "detected_vehicle_data":{ + "height":70, + "lights":8, + "size":{ + "length":700, + "width":300 + }, + "veh_ang_vel":{ + "pitch_rate":600, + "roll_rate":-800 + }, + "veh_ang_vel_confidence":{ + "pitch_rate_confidence":3, + "roll_rate_confidence":4 + }, + "veh_attitude":{ + "pitch":2400, + "roll":-12000, + "yaw":400 + }, + "veh_attitude_confidence":{ + "pitch_confidence":2, + "roll_confidence":3, + "yaw_confidence":4 + }, + "vehicle_class":11, + "vehicle_class_conf":75, + "vehicle_size_confidence":{ + "vehicle_height_confidence":5, + "vehicle_length_confidence":6, + "vehicle_width_confidence":7 + } + } + } + } + } + ], + "ref_pos":{ + "long":600000000, + "elevation":30, + "lat":400000000 + }, + "ref_pos_el_conf":10, + "ref_pos_xy_conf":{"orientation":25000,"semi_major":235,"semi_minor":200}, + "sdsm_time_stamp":{"day":4,"hour":19,"minute":15,"month":7,"offset":400,"second":5000,"year":2007}, + "source_id":"01020304" + })"; Json::Value root; bool result = converter.parseJsonString(valid_json_str, root); - ASSERT_TRUE(result); + EXPECT_TRUE(result); auto sdsmPtr = std::make_shared(); converter.convertJsonToSDSM(root, sdsmPtr); - ASSERT_EQ(1, sdsmPtr->objects.list.array[0]->detObjCommon.objType); - ASSERT_EQ(65, sdsmPtr->objects.list.array[0]->detObjCommon.objTypeCfd); - ASSERT_EQ(12200, sdsmPtr->objects.list.array[0]->detObjCommon.objectID); - ASSERT_EQ(-1100, sdsmPtr->objects.list.array[0]->detObjCommon.measurementTime); - ASSERT_EQ(2, sdsmPtr->objects.list.array[0]->detObjCommon.timeConfidence); - ASSERT_EQ(4000, sdsmPtr->objects.list.array[0]->detObjCommon.pos.offsetX); - ASSERT_EQ(-720, sdsmPtr->objects.list.array[0]->detObjCommon.pos.offsetY); - ASSERT_EQ(20, *sdsmPtr->objects.list.array[0]->detObjCommon.pos.offsetZ); + EXPECT_EQ(2400, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitude->pitch); + EXPECT_EQ(-12000, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitude->roll); + EXPECT_EQ(400, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitude->yaw); - ASSERT_EQ(2, sdsmPtr->objects.list.array[0]->detObjCommon.posConfidence.pos); - ASSERT_EQ(5, sdsmPtr->objects.list.array[0]->detObjCommon.posConfidence.elevation); + EXPECT_EQ(2, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitudeConfidence->pitchConfidence); + EXPECT_EQ(3, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitudeConfidence->rollConfidence); + EXPECT_EQ(4, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitudeConfidence->yawConfidence); - ASSERT_EQ(2100, sdsmPtr->objects.list.array[0]->detObjCommon.speed); - ASSERT_EQ(3, sdsmPtr->objects.list.array[0]->detObjCommon.speedConfidence); - ASSERT_EQ(1000, *sdsmPtr->objects.list.array[0]->detObjCommon.speedZ); - ASSERT_EQ(4, *sdsmPtr->objects.list.array[0]->detObjCommon.speedConfidenceZ); + EXPECT_EQ(600, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAngVel->pitchRate); + EXPECT_EQ(-800, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAngVel->rollRate); - ASSERT_EQ(16000, sdsmPtr->objects.list.array[0]->detObjCommon.heading); - ASSERT_EQ(4, sdsmPtr->objects.list.array[0]->detObjCommon.headingConf); + EXPECT_EQ(3, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAngVelConfidence->pitchRateConfidence); + EXPECT_EQ(4, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAngVelConfidence->rollRateConfidence); - ASSERT_EQ(200, sdsmPtr->objects.list.array[0]->detObjCommon.accel4way->Long); - ASSERT_EQ(-500, sdsmPtr->objects.list.array[0]->detObjCommon.accel4way->lat); - ASSERT_EQ(1, sdsmPtr->objects.list.array[0]->detObjCommon.accel4way->vert); - ASSERT_EQ(400, sdsmPtr->objects.list.array[0]->detObjCommon.accel4way->yaw); + EXPECT_EQ(300, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.size->width); + EXPECT_EQ(700, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.size->length); + EXPECT_EQ(70, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.height); + EXPECT_EQ(7, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehicleSizeConfidence->vehicleWidthConfidence); + EXPECT_EQ(6, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehicleSizeConfidence->vehicleLengthConfidence); + EXPECT_EQ(5, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehicleSizeConfidence->vehicleHeightConfidence); + + EXPECT_EQ(11, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehicleClass); + EXPECT_EQ(75, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.classConf); + tmx::messages::SdsmEncodedMessage encodedSdsm; + converter.encodeSDSM(sdsmPtr, encodedSdsm); + EXPECT_EQ(41, encodedSdsm.get_msgId()); + + std::string expectedSDSMEncHex = "00294e81303330343fdf5dc933c4e226c29af8da011e1a2ffe203dd790c3514017f304bea06402c7cfbe97c00992a0d18fa23e808fa0bb900ffff2e61ff96004b039d04e412bbe6fee2585791aeca172c0"; + EXPECT_EQ(expectedSDSMEncHex, encodedSdsm.get_payload_str()); } - // Test for SDSM optional data - vehicle data - TEST_F(test_JsonToJ3224SDSMConverter, convertJsonToSDSM_veh) + // Test for SDSM optional data - VRU data + TEST_F(test_JsonToJ3224SDSMConverter, convertJsonToSDSM_vru) { JsonToJ3224SDSMConverter converter; - std::string valid_json_str = "{\"equipment_type\":1,\"msg_cnt\":1,\"objects\":[{\"detected_object_data\":{\"detected_object_common_data\":{\"acc_cfd_x\":4,\"acc_cfd_y\":5,\"acc_cfd_yaw\":3,\"acc_cfd_z\":6,\"accel_4_way\":{\"lat\":-500,\"long\":200,\"vert\":1,\"yaw\":400},\"heading\":16000,\"heading_conf\":4,\"measurement_time\":-1100,\"object_id\":12200,\"obj_type\":1,\"obj_type_cfd\":65,\"pos\":{\"offset_x\":4000,\"offset_y\":-720,\"offset_z\":20},\"pos_confidence\":{\"elevation\":5,\"pos\":2},\"speed\":2100,\"speed_confidence\":3,\"speed_confidence_z\":4,\"speed_z\":1000,\"time_confidence\":2},\"detected_object_optional_data\":{\"detected_vehicle_data\":{\"height\":70,\"lights\":8,\"size\":{\"length\":700,\"width\":300},\"veh_ang_vel\":{\"pitch_rate\":600,\"roll_rate\":-800},\"veh_ang_vel_confidence\":{\"pitch_rate_confidence\":3,\"roll_rate_confidence\":4},\"veh_attitude\":{\"pitch\":2400,\"roll\":-12000,\"yaw\":400},\"veh_attitude_confidence\":{\"pitch_confidence\":2,\"roll_confidence\":3,\"yaw_confidence\":4},\"vehicle_class\":11,\"vehicle_class_conf\":75,\"vehicle_size_confidence\":{\"vehicle_height_confidence\":5,\"vehicle_length_confidence\":6,\"vehicle_width_confidence\":7}}}}}],\"ref_pos\":{\"long\":600000000,\"elevation\":30,\"lat\":400000000},\"ref_pos_el_conf\":10,\"ref_pos_xy_conf\":{\"orientation\":25000,\"semi_major\":235,\"semi_minor\":200},\"sdsm_time_stamp\":{\"day\":4,\"hour\":19,\"minute\":15,\"month\":7,\"offset\":400,\"second\":5000,\"year\":2007},\"source_id\":\"01020304\"}"; + std::string valid_json_str = R"( + { + "equipment_type": 1, + "msg_cnt": 1, + "objects": [ + { + "detected_object_data": { + "detected_object_common_data": { + "acc_cfd_x": 4, + "acc_cfd_y": 5, + "acc_cfd_yaw": 3, + "acc_cfd_z": 6, + "accel_4_way": { + "lat": -500, + "long": 200, + "vert": 1, + "yaw": 400 + }, + "heading": 16000, + "heading_conf": 4, + "measurement_time": -1100, + "object_id": 12200, + "obj_type": 1, + "obj_type_cfd": 65, + "pos": { + "offset_x": 4000, + "offset_y": -720, + "offset_z": 20 + }, + "pos_confidence": { + "elevation": 5, + "pos": 2 + }, + "speed": 2100, + "speed_confidence": 3, + "speed_confidence_z": 4, + "speed_z": 1000, + "time_confidence": 2 + }, + "detected_object_optional_data": { + "detected_vru_data": { + "attachment": 3, + "basic_type": 1, + "propulsion": { + "human": 2 + }, + "radius": 30 + } + } + } + } + ], + "ref_pos": { + "long": 600000000, + "elevation": 30, + "lat": 400000000 + }, + "ref_pos_el_conf": 10, + "ref_pos_xy_conf": { + "orientation": 25000, + "semi_major": 235, + "semi_minor": 200 + }, + "sdsm_time_stamp": { + "day": 4, + "hour": 19, + "minute": 15, + "month": 7, + "offset": 400, + "second": 5000, + "year": 2007 + }, + "source_id": "01020304" + })"; Json::Value root; bool result = converter.parseJsonString(valid_json_str, root); - ASSERT_TRUE(result); + EXPECT_TRUE(result); auto sdsmPtr = std::make_shared(); converter.convertJsonToSDSM(root, sdsmPtr); - // TODO: Find a better way to test light data following an updated implementation - // // Similar to sourceID, need a better way to compare retrieved ASN.1 values (in this case bit strings) to verify conversion - // size_t test_size = 2; - // ASSERT_EQ(test_size, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.lights->size); - - ASSERT_EQ(2400, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitude->pitch); - ASSERT_EQ(-12000, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitude->roll); - ASSERT_EQ(400, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitude->yaw); - - ASSERT_EQ(2, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitudeConfidence->pitchConfidence); - ASSERT_EQ(3, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitudeConfidence->rollConfidence); - ASSERT_EQ(4, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitudeConfidence->yawConfidence); - - 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(300, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.size->width); - ASSERT_EQ(700, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.size->length); - ASSERT_EQ(70, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.height); - - ASSERT_EQ(7, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehicleSizeConfidence->vehicleWidthConfidence); - ASSERT_EQ(6, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehicleSizeConfidence->vehicleLengthConfidence); - ASSERT_EQ(5, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehicleSizeConfidence->vehicleHeightConfidence); - - ASSERT_EQ(11, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehicleClass); - ASSERT_EQ(75, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.classConf); + EXPECT_EQ(1, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVRU.basicType); + EXPECT_EQ(2, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVRU.propulsion->choice.human); + EXPECT_EQ(3, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVRU.attachment); + EXPECT_EQ(30, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVRU.radius); + tmx::messages::SdsmEncodedMessage encodedSdsm; + converter.encodeSDSM(sdsmPtr, encodedSdsm); + EXPECT_EQ(41, encodedSdsm.get_msgId()); + + std::string expectedSDSMEncHex = "00293d81303330343fdf5dc933c4e226c29af8da011e1a2ffe203dd790c3514017f304bea06402c7cfbe97c00992a0d18fa23e809130bb901031f2e6f88231e0"; + EXPECT_EQ(expectedSDSMEncHex, encodedSdsm.get_payload_str()); } - // Test for SDSM optional data - VRU data - TEST_F(test_JsonToJ3224SDSMConverter, convertJsonToSDSM_vru) + // Test for SDSM optional data - obstacle data + TEST_F(test_JsonToJ3224SDSMConverter, convertJsonToSDSM_obst) { JsonToJ3224SDSMConverter converter; - std::string valid_json_str = "{\"equipment_type\":1,\"msg_cnt\":1,\"objects\":[{\"detected_object_data\":{\"detected_object_common_data\":{\"acc_cfd_x\":4,\"acc_cfd_y\":5,\"acc_cfd_yaw\":3,\"acc_cfd_z\":6,\"accel_4_way\":{\"lat\":-500,\"long\":200,\"vert\":1,\"yaw\":400},\"heading\":16000,\"heading_conf\":4,\"measurement_time\":-1100,\"object_id\":12200,\"obj_type\":1,\"obj_type_cfd\":65,\"pos\":{\"offset_x\":4000,\"offset_y\":-720,\"offset_z\":20},\"pos_confidence\":{\"elevation\":5,\"pos\":2},\"speed\":2100,\"speed_confidence\":3,\"speed_confidence_z\":4,\"speed_z\":1000,\"time_confidence\":2},\"detected_object_optional_data\":{\"detected_vru_data\":{\"attachment\":3,\"basic_type\":1,\"propulsion\":{\"human\":2},\"radius\":30}}}}],\"ref_pos\":{\"long\":600000000,\"elevation\":30,\"lat\":400000000},\"ref_pos_el_conf\":10,\"ref_pos_xy_conf\":{\"orientation\":25000,\"semi_major\":235,\"semi_minor\":200},\"sdsm_time_stamp\":{\"day\":4,\"hour\":19,\"minute\":15,\"month\":7,\"offset\":400,\"second\":5000,\"year\":2007},\"source_id\":\"01020304\"}"; + std::string valid_json_str = R"( + { + "equipment_type": 1, + "msg_cnt": 1, + "objects": [ + { + "detected_object_data": { + "detected_object_common_data": { + "acc_cfd_x": 4, + "acc_cfd_y": 5, + "acc_cfd_yaw": 3, + "acc_cfd_z": 6, + "accel_4_way": { + "lat": -500, + "long": 200, + "vert": 1, + "yaw": 400 + }, + "heading": 16000, + "heading_conf": 4, + "measurement_time": -1100, + "object_id": 12200, + "obj_type": 1, + "obj_type_cfd": 65, + "pos": { + "offset_x": 4000, + "offset_y": -720, + "offset_z": 20 + }, + "pos_confidence": { + "elevation": 5, + "pos": 2 + }, + "speed": 2100, + "speed_confidence": 3, + "speed_confidence_z": 4, + "speed_z": 1000, + "time_confidence": 2 + }, + "detected_object_optional_data": { + "detected_obstacle_data": { + "obst_size": { + "height": 100, + "length": 300, + "width": 400 + }, + "obst_size_confidence": { + "height_confidence": 8, + "length_confidence": 7, + "width_confidence": 6 + } + } + } + } + } + ], + "ref_pos": { + "long": 600000000, + "elevation": 30, + "lat": 400000000 + }, + "ref_pos_el_conf": 10, + "ref_pos_xy_conf": { + "orientation": 25000, + "semi_major": 235, + "semi_minor": 200 + }, + "sdsm_time_stamp": { + "day": 4, + "hour": 19, + "minute": 15, + "month": 7, + "offset": 400, + "second": 5000, + "year": 2007 + }, + "source_id": "01020304" + } + )"; Json::Value root; bool result = converter.parseJsonString(valid_json_str, root); - ASSERT_TRUE(result); + EXPECT_TRUE(result); auto sdsmPtr = std::make_shared(); converter.convertJsonToSDSM(root, sdsmPtr); - ASSERT_EQ(1, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVRU.basicType); - ASSERT_EQ(2, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVRU.propulsion->choice.human); - ASSERT_EQ(3, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVRU.attachment); - ASSERT_EQ(30, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVRU.radius); - + EXPECT_EQ(400, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detObst.obstSize.width); + EXPECT_EQ(300, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detObst.obstSize.length); + EXPECT_EQ(100, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detObst.obstSize.height); + + EXPECT_EQ(6, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detObst.obstSizeConfidence.widthConfidence); + EXPECT_EQ(7, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detObst.obstSizeConfidence.lengthConfidence); + EXPECT_EQ(8, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detObst.obstSizeConfidence.heightConfidence); + tmx::messages::SdsmEncodedMessage encodedSdsm; + converter.encodeSDSM(sdsmPtr, encodedSdsm); + EXPECT_EQ(41, encodedSdsm.get_msgId()); + + std::string expectedSDSMEncHex = "00293f81303330343fdf5dc933c4e226c29af8da011e1a2ffe203dd790c3514017f304bea06402c7cfbe97c00992a0d18fa23e809130bb901031f2e75904b064b3c0"; + EXPECT_EQ(expectedSDSMEncHex, encodedSdsm.get_payload_str()); } // Test for SDSM optional data - obstacle data - TEST_F(test_JsonToJ3224SDSMConverter, convertJsonToSDSM_obst) + TEST_F(test_JsonToJ3224SDSMConverter, convertJsonToSDSM_sdsm_service_generated) { JsonToJ3224SDSMConverter converter; - std::string valid_json_str = "{\"equipment_type\":1,\"msg_cnt\":1,\"objects\":[{\"detected_object_data\":{\"detected_object_common_data\":{\"acc_cfd_x\":4,\"acc_cfd_y\":5,\"acc_cfd_yaw\":3,\"acc_cfd_z\":6,\"accel_4_way\":{\"lat\":-500,\"long\":200,\"vert\":1,\"yaw\":400},\"heading\":16000,\"heading_conf\":4,\"measurement_time\":-1100,\"object_id\":12200,\"obj_type\":1,\"obj_type_cfd\":65,\"pos\":{\"offset_x\":4000,\"offset_y\":-720,\"offset_z\":20},\"pos_confidence\":{\"elevation\":5,\"pos\":2},\"speed\":2100,\"speed_confidence\":3,\"speed_confidence_z\":4,\"speed_z\":1000,\"time_confidence\":2},\"detected_object_optional_data\":{\"detected_obstacle_data\":{\"obst_size\":{\"height\":100,\"length\":300,\"width\":400},\"obst_size_confidence\":{\"height_confidence\":8,\"length_confidence\":7,\"width_confidence\":6}}}}}],\"ref_pos\":{\"long\":600000000,\"elevation\":30,\"lat\":400000000},\"ref_pos_el_conf\":10,\"ref_pos_xy_conf\":{\"orientation\":25000,\"semi_major\":235,\"semi_minor\":200},\"sdsm_time_stamp\":{\"day\":4,\"hour\":19,\"minute\":15,\"month\":7,\"offset\":400,\"second\":5000,\"year\":2007},\"source_id\":\"01020304\"}"; + std::string valid_json_str = R"( + { + "msg_cnt": 0, + "source_id": "rsu_1234", + "equipment_type": 1, + "sdsm_time_stamp": { + "second": 30275, + "minute": 3, + "hour": 19, + "day": 17, + "month": 12, + "year": 2023, + "offset": 0 + }, + "ref_pos": { + "long": 0, + "lat": 0 + }, + "ref_pos_xy_conf": { + "semi_major": 0, + "semi_minor": 0, + "orientation": 0 + }, + "objects": [ + { + "detected_object_data": { + "detected_object_common_data": { + "obj_type": 0, + "object_id": 1, + "obj_type_cfd": 70, + "measurement_time": 0, + "time_confidence": 0, + "pos": { + "offset_x": -11, + "offset_y": -20, + "offset_z": -32 + }, + "pos_confidence": { + "pos": 0, + "elevation": 0 + }, + "speed": 70, + "speed_confidence": 0, + "speed_z": 50, + "heading": 0, + "heading_conf": 0 + }, + "detected_object_optional_data": { + "detected_obstacle_data": { + "obst_size": { + "width": 5, + "length": 20, + "height": 10 + }, + "obst_size_confidence": { + "width_confidence": 0, + "length_confidence": 0 + } + } + } + } + } + ] + } + )"; Json::Value root; bool result = converter.parseJsonString(valid_json_str, root); - ASSERT_TRUE(result); + EXPECT_TRUE(result); auto sdsmPtr = std::make_shared(); converter.convertJsonToSDSM(root, sdsmPtr); - ASSERT_EQ(400, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detObst.obstSize.width); - ASSERT_EQ(300, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detObst.obstSize.length); - ASSERT_EQ(100, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detObst.obstSize.height); + tmx::messages::SdsmEncodedMessage encodedSdsm; + converter.encodeSDSM(sdsmPtr, encodedSdsm); + EXPECT_EQ(41, encodedSdsm.get_msgId()); + + std::string expectedSDSMEncHex = "00293400313233343fdf9f2330dd90da406b49d200d693a3fe00000000014011800057700bffa3ff5bfef80011800c80000a0282805000"; + EXPECT_EQ(expectedSDSMEncHex, encodedSdsm.get_payload_str()); + } - ASSERT_EQ(6, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detObst.obstSizeConfidence.widthConfidence); - ASSERT_EQ(7, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detObst.obstSizeConfidence.lengthConfidence); - ASSERT_EQ(8, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detObst.obstSizeConfidence.heightConfidence); - } +// Test for SDSM optional data - obstacle data + TEST_F(test_JsonToJ3224SDSMConverter, convertJsonToSDSM_multiple_objects) + { + JsonToJ3224SDSMConverter converter; + std::string valid_json_str = R"( + { + "msg_cnt": 127, + "source_id": "rsu_1234", + "equipment_type": 1, + "sdsm_time_stamp": { + "second": 1781, + "minute": 0, + "hour": 20, + "day": 17, + "month": 12, + "year": 2023, + "offset": 0 + }, + "ref_pos": { + "long": 0, + "lat": 0 + }, + "ref_pos_xy_conf": { + "semi_major": 0, + "semi_minor": 0, + "orientation": 0 + }, + "objects": [ + { + "detected_object_data": { + "detected_object_common_data": { + "obj_type": 0, + "object_id": 1, + "obj_type_cfd": 70, + "measurement_time": 0, + "time_confidence": 0, + "pos": { + "offset_x": -11, + "offset_y": -20, + "offset_z": -32 + }, + "pos_confidence": { + "pos": 0, + "elevation": 0 + }, + "speed": 70, + "speed_confidence": 0, + "speed_z": 50, + "heading": 0, + "heading_conf": 0 + }, + "detected_object_optional_data": { + "detected_obstacle_data": { + "obst_size": { + "width": 5, + "length": 20, + "height": 10 + }, + "obst_size_confidence": { + "width_confidence": 0, + "length_confidence": 0 + } + } + } + } + }, + { + "detected_object_data": { + "detected_object_common_data": { + "obj_type": 2, + "object_id": 2, + "obj_type_cfd": 70, + "measurement_time": 0, + "time_confidence": 0, + "pos": { + "offset_x": -11, + "offset_y": -20, + "offset_z": -32 + }, + "pos_confidence": { + "pos": 0, + "elevation": 0 + }, + "speed": 26, + "speed_confidence": 0, + "speed_z": 0, + "heading": 0, + "heading_conf": 0 + }, + "detected_object_optional_data": { + "detected_vru_data": {} + } + } + }, + { + "detected_object_data": { + "detected_object_common_data": { + "obj_type": 1, + "object_id": 3, + "obj_type_cfd": 70, + "measurement_time": 0, + "time_confidence": 0, + "pos": { + "offset_x": -11, + "offset_y": -20, + "offset_z": -32 + }, + "pos_confidence": { + "pos": 0, + "elevation": 0 + }, + "speed": 26, + "speed_confidence": 0, + "speed_z": 0, + "heading": 0, + "heading_conf": 0 + }, + "detected_object_optional_data": { + "detected_vehicle_data": { + "size": { + "width": 50, + "length": 50 + }, + "height": 40 + } + } + } + } + ] + } + )"; + Json::Value root; + bool result = converter.parseJsonString(valid_json_str, root); + EXPECT_TRUE(result); + auto sdsmPtr = std::make_shared(); + converter.convertJsonToSDSM(root, sdsmPtr); + EXPECT_EQ(3, sdsmPtr->objects.list.count); + // Encode message + tmx::messages::SdsmEncodedMessage encodedSdsm; + converter.encodeSDSM(sdsmPtr, encodedSdsm); + EXPECT_EQ(41, encodedSdsm.get_msgId()); + + std::string expectedSDSMEncHex = "0029617f313233343fdf9f234001bd5a406b49d200d693a3fe00000000054011800057700bffa3ff5bfef80011800c80000a028280500280a300012ee017ff47feb7fdf0000d0000000008500c600035dc02ffe8ffd6ffbe0001a0000000000301901928"; + EXPECT_EQ(expectedSDSMEncHex, encodedSdsm.get_payload_str()); + } + } \ No newline at end of file