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/5] 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/5] 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 From 70e798b306bf716ad9fc25fa95ef9bf2c5857e7b Mon Sep 17 00:00:00 2001 From: paulbourelly999 <77466294+paulbourelly999@users.noreply.github.com> Date: Wed, 20 Dec 2023 16:08:18 -0500 Subject: [PATCH 3/5] Fix/json serialization detected object (#573) # PR Details ## Description Temporary fix for JSON serialization for TMX messages. See related issue for more detailed description of error behavior. Using simple regex replacement to correct bool, double and integer values ## Related Issue #561 (https://github.com/usdot-fhwa-OPS/V2X-Hub/issues/561) ## Motivation and Context Fix for VRU UC 1 testing of SDSM functionality ## How Has This Been Tested? Integration tested in CDASim deployment ## 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. --- .../CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index 9ad219e1f..eb1ba1597 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -706,8 +706,16 @@ void CARMAStreetsPlugin::SubscribeSDSMKafkaTopic(){ void CARMAStreetsPlugin::HandleSimulatedSensorDetectedMessage(simulation::SensorDetectedObject &msg, routeable_message &routeableMsg) { + // TODO: This is a temporary fix for tmx message container property tree + // serializing all attributes as strings. This issue needs to be fixed but + // is currently out of scope. TMX Messages should be correctly serialize to + // and from json. This temporary fix simply using regex to look for numeric, + // null, and bool values and removes the quotations around them. PLOG(logDEBUG) << "Produce sensor detected message in JSON format: " << msg.to_string() < Date: Thu, 11 Jan 2024 16:43:18 -0500 Subject: [PATCH 4/5] Added SDSM to immediate forward plugin configuration (#574) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # PR Details ## Description Added SDSM to Immediate Forward Plugin Messages to forward and fixed PSID to match j3224 documentation ``` The system shall set the PSID value to the value assigned to “Sensor Sharing Service” (0x90), as listed by IEEE-RA website ``` ## Related Issue [CDAR-300](https://usdot-carma.atlassian.net/browse/CDAR-300) ## Motivation and Context VRU UC1 ## How Has This Been Tested? ## Types of changes CDA Sim deployment tested - [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/tmx/TmxApi/tmx/TmxApiMessages.h | 4 ++-- src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp | 2 +- src/v2i-hub/ImmediateForwardPlugin/manifest.json | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/tmx/TmxApi/tmx/TmxApiMessages.h b/src/tmx/TmxApi/tmx/TmxApiMessages.h index f44406061..ddd10a674 100644 --- a/src/tmx/TmxApi/tmx/TmxApiMessages.h +++ b/src/tmx/TmxApi/tmx/TmxApiMessages.h @@ -196,7 +196,7 @@ enum msgPSID rtcmCorrections_PSID = 0x8000, signalRequestMessage_PSID = 0xE0000016, signalStatusMessage_PSID = 0x8002, - sensorDataSharingMessage_PSID = 0x8002, + sensorDataSharingMessage_PSID = 0x90, travelerInformation_PSID = 0x8003, personalSafetyMessage_PSID = 0x27, testMessage00_PSID = 0xBFEE, @@ -221,7 +221,7 @@ static CONSTEXPR const char *MSGPSID_ROADSIDEALERT_PSID_STRING = "0x8003"; static CONSTEXPR const char *MSGPSID_RTCMCORRECTIONS_PSID_STRING = "0x8000"; static CONSTEXPR const char *MSGPSID_SIGNALREQUESTMESSAGE_PSID_STRING = "0xE0000016"; static CONSTEXPR const char *MSGPSID_SIGNALSTATUSMESSAGE_PSID_STRING = "0x8002"; -static CONSTEXPR const char *MSGPSID_SENSORDATASHARINGMESSAGE_PSID_STRING = "0x8002"; +static CONSTEXPR const char *MSGPSID_SENSORDATASHARINGMESSAGE_PSID_STRING = "0x90"; static CONSTEXPR const char *MSGPSID_TRAVELERINFORMATION_PSID_STRING = "0x8003"; static CONSTEXPR const char *MSGPSID_PERSONALSAFETYMESSAGE_PSID_STRING = "0x27"; static CONSTEXPR const char *MSGPSID_TESTMESSAGE00_PSID_STRING = "0xBFEE"; diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index eb1ba1597..d0d4d27be 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -695,7 +695,7 @@ void CARMAStreetsPlugin::SubscribeSDSMKafkaTopic(){ PLOG(logDEBUG) << "sdsmEncodedMsg: " << sdsmEncodedMsg; //Broadcast the encoded SDSM message sdsmEncodedMsg.set_flags(IvpMsgFlags_RouteDSRC); - sdsmEncodedMsg.addDsrcMetadata(0x8002); + sdsmEncodedMsg.addDsrcMetadata(tmx::messages::api::msgPSID::sensorDataSharingMessage_PSID); BroadcastMessage(static_cast(sdsmEncodedMsg)); } diff --git a/src/v2i-hub/ImmediateForwardPlugin/manifest.json b/src/v2i-hub/ImmediateForwardPlugin/manifest.json index b26756a76..288201cc1 100644 --- a/src/v2i-hub/ImmediateForwardPlugin/manifest.json +++ b/src/v2i-hub/ImmediateForwardPlugin/manifest.json @@ -14,7 +14,7 @@ }, { "key": "Messages_Destination_1", - "default": "{ \"Messages\": [ { \"TmxType\": \"SPAT-P\", \"SendType\": \"SPAT\", \"PSID\": \"0x8002\", \"Channel\": \"183\" }, { \"TmxType\": \"MAP-P\", \"SendType\": \"MAP\", \"PSID\": \"0x8002\", \"Channel\": \"183\" }, { \"TmxType\": \"PSM\", \"SendType\": \"PSM\", \"PSID\": \"0x8002\", \"Channel\": \"183\" } ,{ \"TmxType\": \"TMSG07\", \"SendType\": \"TMSG07\", \"PSID\": \"0x8002\", \"Channel\": \"183\" },{ \"TmxType\": \"TMSG03-P\", \"SendType\": \"TMSG03-P\", \"PSID\": \"0xBFEE\", \"Channel\": \"183\" },{ \"TmxType\": \"TMSG05-P\", \"SendType\": \"TMSG05-P\", \"PSID\": \"0x8003\", \"Channel\": \"183\" }, { \"TmxType\": \"SSM\", \"SendType\": \"SSM\", \"PSID\": \"0x8002\", \"Channel\": \"183\" }] }", + "default": "{ \"Messages\": [ { \"TmxType\": \"SPAT-P\", \"SendType\": \"SPAT\", \"PSID\": \"0x8002\", \"Channel\": \"183\" }, { \"TmxType\": \"MAP-P\", \"SendType\": \"MAP\", \"PSID\": \"0x8002\", \"Channel\": \"183\" }, { \"TmxType\": \"PSM\", \"SendType\": \"PSM\", \"PSID\": \"0x8002\", \"Channel\": \"183\" } ,{ \"TmxType\": \"TMSG07\", \"SendType\": \"TMSG07\", \"PSID\": \"0x8002\", \"Channel\": \"183\" },{ \"TmxType\": \"TMSG03-P\", \"SendType\": \"TMSG03-P\", \"PSID\": \"0xBFEE\", \"Channel\": \"183\" },{ \"TmxType\": \"TMSG05-P\", \"SendType\": \"TMSG05-P\", \"PSID\": \"0x8003\", \"Channel\": \"183\" }, { \"TmxType\": \"SSM\", \"SendType\": \"SSM\", \"PSID\": \"0x8002\", \"Channel\": \"183\" },{ \"TmxType\": \"SDSM\", \"SendType\": \"SensorDataSharingMessage\", \"PSID\": \"0x90\", \"Channel\": \"183\" }] }", "description": "JSON data defining the message types, PSIDs, and channel number for messages forwarded to the V2X radio at destination 1." }, { From 623713b981cf5ce270eecd62b3ae7704841598ba Mon Sep 17 00:00:00 2001 From: paulbourelly999 <77466294+paulbourelly999@users.noreply.github.com> Date: Tue, 6 Feb 2024 14:20:55 -0500 Subject: [PATCH 5/5] CDAR-756: Update UDPSockets to listen at higher frequency (#579) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # PR Details ## Description When running a simulation with CARMA Streets, the position data in the SDSMs appear to lag the ground truth significantly. In the included image, the square inside the blue circle indicates the VRU’s reported position in the SDSM. The blue square is its actual position. They yellow circle shows CARMA’s reported SDSM position with the green rectangle showing its actual position. ![image](https://github.com/usdot-fhwa-OPS/V2X-Hub/assets/77466294/72627660-d8ab-4a4e-8c37-db73c3bd919c) Based on a visualization of carla-sensor-lib’s logs for the pedestrian (actor 221), it appears that the position data does get updated at some point. The image below is a timestamp vs. x position graph. After further investigation, there is a difference in time stamps between the SDSMs and external objects. ![image](https://github.com/usdot-fhwa-OPS/V2X-Hub/assets/77466294/3ddcf1ae-8429-4d3b-82c4-66f2fc646200) It was found that is was largely due to the CDASimAdapter only consuming detection data at 10 hz (wall time). When running at real-time speed, the simulation can generate detection data at significantly higher frequency. This PR updates the consuming frequency to 200 Hz. This fix should be sufficient until the number of simultaneous detection exceeds 10 objects of 100Hz frequency. Improved delay shown below: ![screenshot](https://github.com/usdot-fhwa-OPS/V2X-Hub/assets/77466294/1d03c6c0-bbf2-4318-86ad-583e2b1ae6a6) ## Related Issue ## Motivation and Context Fix detection data delay for SDSM data ## How Has This Been Tested? CDASim Deployment ## 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. --- .github/workflows/sonar-scanner.yml | 2 +- src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp | 16 ++++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/.github/workflows/sonar-scanner.yml b/.github/workflows/sonar-scanner.yml index af844164c..6df6b7812 100644 --- a/.github/workflows/sonar-scanner.yml +++ b/.github/workflows/sonar-scanner.yml @@ -21,7 +21,7 @@ jobs: fetch-depth: 0 submodules: recursive - name: Install sonar-scanner and build-wrapper - uses: sonarsource/sonarcloud-github-c-cpp@v1 + uses: sonarsource/sonarcloud-github-c-cpp@v2 - name: Run install_dependencies.sh script run: | scripts/install_dependencies.sh diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp index c662e6e9c..b9665bcee 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp @@ -126,13 +126,13 @@ namespace CDASimAdapter{ void CDASimAdapter::start_immediate_forward_thread() { if ( !immediate_forward_timer ) { - immediate_forward_timer = std::make_unique(); + immediate_forward_timer = std::make_unique(std::chrono::milliseconds(5)); } immediate_forward_tick_id = immediate_forward_timer->AddPeriodicTick([this]() { this->attempt_message_from_v2xhub(); } // end of lambda expression - , std::chrono::milliseconds(100) ); + , std::chrono::milliseconds(5) ); immediate_forward_timer->Start(); @@ -140,13 +140,13 @@ namespace CDASimAdapter{ void CDASimAdapter::start_message_receiver_thread() { if ( !message_receiver_timer ) { - message_receiver_timer = std::make_unique(); + message_receiver_timer = std::make_unique(std::chrono::milliseconds(5)); } message_receiver_tick_id = message_receiver_timer->AddPeriodicTick([this]() { this->attempt_message_from_simulation(); } // end of lambda expression - , std::chrono::milliseconds(100) ); + , std::chrono::milliseconds(5) ); message_receiver_timer->Start(); } @@ -174,7 +174,7 @@ namespace CDASimAdapter{ { if(!external_object_detection_thread_timer) { - external_object_detection_thread_timer = std::make_unique(); + external_object_detection_thread_timer = std::make_unique(std::chrono::milliseconds(5)); } external_object_detection_thread_timer->AddPeriodicTick([this](){ PLOG(logDEBUG1) << "Listening for Sensor Detected Message from CDASim." << std::endl; @@ -187,7 +187,7 @@ namespace CDASimAdapter{ PLOG(logDEBUG1) << "CDASim connection has not yet received an simulated sensor detected message!" << std::endl; } }//End lambda - , std::chrono::milliseconds(100)); + , std::chrono::milliseconds(5)); external_object_detection_thread_timer->Start(); } catch ( const UdpServerRuntimeError &e ) @@ -216,13 +216,13 @@ namespace CDASimAdapter{ void CDASimAdapter::start_time_sync_thread_timer() { PLOG(logDEBUG) << "Creating Thread Timer for time sync" << std::endl; if ( !time_sync_timer ) { - time_sync_timer = std::make_unique(); + time_sync_timer = std::make_unique(std::chrono::milliseconds(5)); } time_sync_tick_id = time_sync_timer->AddPeriodicTick([this]() { PLOG(logDEBUG1) << "Listening for time sync messages from CDASim." << std::endl; this->attempt_time_sync(); } // end of lambda expression - , std::chrono::milliseconds(100)); + , std::chrono::milliseconds(5)); time_sync_timer->Start(); }