From 842aa5b2689a54f69cb0face169f4b357d6fbdf1 Mon Sep 17 00:00:00 2001 From: dan-du-car <62157949+dan-du-car@users.noreply.github.com> Date: Wed, 24 May 2023 13:41:22 -0400 Subject: [PATCH 01/28] V2XHub / CARMA Streets to process and broadcast SSM (#533) # PR Details ## Description For the POC TIM/TSP use case, priority-eligible vehicles will broadcast J2375 Signal Request Message (SRM) to the MMITSS Roadside Processor (MRP), which in turn will send J2375 Signal Status Message (SSM) in acknowledgement. Since MRP is being integrated with CARMA Streets, CARMA Streets / V2XHub needs to #1) consume the json SSM (used by the MMITSS MRP internally, see [here](https://github.com/mmitss/mmitss-az/blob/master/src/mrp/priority-request-server/Readme.md) and [here](https://github.com/mmitss/mmitss-az/blob/master/src/mrp/priority-request-server/SSM.json)) sent on the CARMA Streets Kafka broker, #2) encode it according to the J2375 [ASN.1 ](https://leidoscorpus.sharepoint.us/:f:/s/STR/EpU-cLOWhUtGvsddHUSf8I4Bf7-Ot8oMIY4yf2m7x2-Uag?e=0Rvpug)schema, and #3) broadcast J2375 SSMs through RSU. For #1) the V2XHub carma-streets-plugin needs to pull the json SSM from CARMA Steets Kafa broker. For #3) the V2XHub immediate-forward-plugin configuration needs to to be updated. ## Related Issue https://github.com/usdot-fhwa-OPS/V2X-Hub/issues/534 ## Motivation and Context TM/TSP ## How Has This Been Tested? Unit test ## Types of changes - [ ] Defect fix (non-breaking change that fixes an issue) - [x] 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/CARMAStreetsPlugin/CMakeLists.txt | 2 +- src/v2i-hub/CARMAStreetsPlugin/manifest.json | 10 ++ .../src/CARMAStreetsPlugin.cpp | 89 +++++++++- .../src/CARMAStreetsPlugin.h | 23 ++- .../src/JsonToJ2735SSMConverter.cpp | 156 ++++++++++++++++++ .../src/JsonToJ2735SSMConverter.h | 49 ++++++ .../test/test_JsonToJ2735SSMConverter.cpp | 102 ++++++++++++ .../ImmediateForwardPlugin/manifest.json | 2 +- 8 files changed, 428 insertions(+), 5 deletions(-) create mode 100644 src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ2735SSMConverter.cpp create mode 100644 src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ2735SSMConverter.h create mode 100644 src/v2i-hub/CARMAStreetsPlugin/test/test_JsonToJ2735SSMConverter.cpp diff --git a/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt b/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt index f904ecac6..f98aebcae 100644 --- a/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt +++ b/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt @@ -9,7 +9,7 @@ TARGET_LINK_LIBRARIES (${PROJECT_NAME} tmxutils rdkafka++ jsoncpp) ############# enable_testing() include_directories(${PROJECT_SOURCE_DIR}/src) -add_library(${PROJECT_NAME}_lib src/J2735MapToJsonConverter.cpp src/JsonToJ2735SpatConverter.cpp src/J2735ToSRMJsonConverter.cpp) +add_library(${PROJECT_NAME}_lib src/J2735MapToJsonConverter.cpp src/JsonToJ2735SSMConverter.cpp src/JsonToJ2735SpatConverter.cpp src/J2735ToSRMJsonConverter.cpp) target_link_libraries(${PROJECT_NAME}_lib PUBLIC ${TMXAPI_LIBRARIES} ${ASN_J2735_LIBRARIES} ${MYSQL_LIBRARIES} diff --git a/src/v2i-hub/CARMAStreetsPlugin/manifest.json b/src/v2i-hub/CARMAStreetsPlugin/manifest.json index c4fb269cd..790ef1af9 100644 --- a/src/v2i-hub/CARMAStreetsPlugin/manifest.json +++ b/src/v2i-hub/CARMAStreetsPlugin/manifest.json @@ -102,6 +102,16 @@ "default": "modified_spat", "description": "Apache Kafka topic plugin will transmit message to." }, + { + "key": "SsmTopic", + "default": "v2xhub_ssm_sub", + "description": "Apache Kafka topic plugin will transmit message to." + }, + { + "key": "SsmConsumerGroupId", + "default": "v2xhub_ssm", + "description": "Apache Kafka consumer group ID for spat consumer." + }, { "key": "SpatConsumerGroupId", "default": "v2xhub_spat", diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index 2e2fdf499..08bc96d8a 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -46,7 +46,9 @@ void CARMAStreetsPlugin::UpdateConfigSettings() { GetConfigValue("SchedulingPlanTopic", _subscribeToSchedulingPlanTopic); GetConfigValue("SchedulingPlanConsumerGroupId", _subscribeToSchedulingPlanConsumerGroupId); GetConfigValue("SpatTopic", _subscribeToSpatTopic); + GetConfigValue("SsmTopic", _subscribeToSsmTopic); GetConfigValue("SpatConsumerGroupId", _subscribeToSpatConsumerGroupId); + GetConfigValue("SsmConsumerGroupId", _subscribeToSSMConsumerGroupId); GetConfigValue("BsmTopic", _transmitBSMTopic); GetConfigValue("MobilityOperationTopic", _transmitMobilityOperationTopic); GetConfigValue("MobilityPathTopic", _transmitMobilityPathTopic); @@ -68,6 +70,8 @@ void CARMAStreetsPlugin::UpdateConfigSettings() { kafka_conf = RdKafka::Conf::create(RdKafka::Conf::CONF_GLOBAL); kafka_conf_sp_consumer = RdKafka::Conf::create(RdKafka::Conf::CONF_GLOBAL); kafka_conf_spat_consumer = RdKafka::Conf::create(RdKafka::Conf::CONF_GLOBAL); + kafka_conf_ssm_consumer = RdKafka::Conf::create(RdKafka::Conf::CONF_GLOBAL); + PLOG(logDEBUG) <<"Attempting to connect to " << kafkaConnectString; if ((kafka_conf->set("bootstrap.servers", kafkaConnectString, error_string) != RdKafka::Conf::CONF_OK)) { @@ -87,7 +91,10 @@ void CARMAStreetsPlugin::UpdateConfigSettings() { if (kafka_conf_sp_consumer->set("bootstrap.servers", kafkaConnectString, error_string) != RdKafka::Conf::CONF_OK || (kafka_conf_sp_consumer->set("group.id", _subscribeToSchedulingPlanConsumerGroupId, error_string) != RdKafka::Conf::CONF_OK) || (kafka_conf_spat_consumer->set("bootstrap.servers", kafkaConnectString, error_string) != RdKafka::Conf::CONF_OK) - || (kafka_conf_spat_consumer->set("group.id", _subscribeToSpatConsumerGroupId, error_string) != RdKafka::Conf::CONF_OK)) { + || (kafka_conf_spat_consumer->set("group.id", _subscribeToSpatConsumerGroupId, error_string) != RdKafka::Conf::CONF_OK) + || (kafka_conf_ssm_consumer->set("bootstrap.servers", kafkaConnectString, error_string) != RdKafka::Conf::CONF_OK) + || (kafka_conf_ssm_consumer->set("group.id", _subscribeToSSMConsumerGroupId, error_string) != RdKafka::Conf::CONF_OK) + ) { PLOG(logERROR) <<"Setting kafka config group.id options failed with error:" << error_string << "\n" <<"Exiting with exit code 1"; exit(1); } else { @@ -95,20 +102,24 @@ void CARMAStreetsPlugin::UpdateConfigSettings() { } kafka_conf_sp_consumer->set("enable.partition.eof", "true", error_string); kafka_conf_spat_consumer->set("enable.partition.eof", "true", error_string); + kafka_conf_ssm_consumer->set("enable.partition.eof", "true", error_string); _scheduing_plan_kafka_consumer = RdKafka::KafkaConsumer::create(kafka_conf_sp_consumer, error_string); _spat_kafka_consumer = RdKafka::KafkaConsumer::create(kafka_conf_spat_consumer, error_string); + _ssm_kafka_consumer = RdKafka::KafkaConsumer::create(kafka_conf_ssm_consumer, error_string); - if ( !_scheduing_plan_kafka_consumer || !_spat_kafka_consumer) { + if ( !_scheduing_plan_kafka_consumer || !_spat_kafka_consumer || !_ssm_kafka_consumer) { PLOG(logERROR) << "Failed to create Kafka consumers: " << error_string << std::endl; exit(1); } PLOG(logDEBUG) << "Created consumer " << _scheduing_plan_kafka_consumer->name() << std::endl; PLOG(logDEBUG) << "Created consumer " << _spat_kafka_consumer->name() << std::endl; + PLOG(logDEBUG) << "Created consumer " << _ssm_kafka_consumer->name() << std::endl; //create kafka topics RdKafka::Conf *tconf_spat = RdKafka::Conf::create(RdKafka::Conf::CONF_TOPIC); RdKafka::Conf *tconf_sp = RdKafka::Conf::create(RdKafka::Conf::CONF_TOPIC); + RdKafka::Conf *tconf_ssm = RdKafka::Conf::create(RdKafka::Conf::CONF_TOPIC); if(!tconf_spat && !tconf_sp) { PLOG(logERROR) << "RDKafka create topic conf failed "; @@ -129,11 +140,20 @@ void CARMAStreetsPlugin::UpdateConfigSettings() { return ; } + _ssm_topic = RdKafka::Topic::create(_ssm_kafka_consumer,_subscribeToSsmTopic,tconf_ssm,error_string); + if(!_ssm_topic) + { + PLOG(logERROR) << "RDKafka create SSM topic failed:" << error_string; + return ; + } + delete tconf_sp; delete tconf_spat; + delete tconf_ssm; boost::thread thread_schpl(&CARMAStreetsPlugin::SubscribeSchedulingPlanKafkaTopic, this); boost::thread thread_spat(&CARMAStreetsPlugin::SubscribeSpatKafkaTopic, this); + boost::thread thread_ssm(&CARMAStreetsPlugin::SubscribeSSMKafkaTopic, this); } void CARMAStreetsPlugin::OnConfigChanged(const char *key, const char *value) { @@ -659,6 +679,71 @@ void CARMAStreetsPlugin::SubscribeSpatKafkaTopic(){ } } +void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){ + + if(_subscribeToSsmTopic.length() > 0) + { + PLOG(logDEBUG) << "SubscribeSSMKafkaTopics:" <<_subscribeToSsmTopic << std::endl; + std::vector topics; + topics.emplace_back(_subscribeToSsmTopic); + + RdKafka::ErrorCode err = _ssm_kafka_consumer->subscribe(topics); + if (err) + { + PLOG(logERROR) << "Failed to subscribe to " << topics.size() << " topics: " << RdKafka::err2str(err) << std::endl; + return; + } + //Initialize Json to J2735 SSM convertor + JsonToJ2735SSMConverter ssm_convertor; + while (true) + { + auto msg = _ssm_kafka_consumer->consume( 500 ); + if( msg->err() == RdKafka::ERR_NO_ERROR ) + { + auto payload_str = static_cast( msg->payload() ); + if(msg->len() > 0) + { + PLOG(logDEBUG) << "consumed message payload: " << payload_str <(Key_SSMMessageSkipped, ++_ssmMessageSkipped); + continue; + } + //Convert the SSM JSON string into J2735 SSM message and encode it. + auto ssm_ptr = std::make_shared(); + ssm_convertor.toJ2735SSM(ssmDoc, ssm_ptr); + tmx::messages::SsmEncodedMessage ssmEncodedMsg; + try + { + ssm_convertor.encodeSSM(ssm_ptr, ssmEncodedMsg); + } + catch (TmxException &ex) + { + // Skip messages that fail to encode. + PLOG(logERROR) << "Failed to encoded SSM message : \n" << payload_str << std::endl << "Exception encountered: " + << ex.what() << std::endl; + ASN_STRUCT_FREE_CONTENTS_ONLY(asn_DEF_SignalStatusMessage, ssm_ptr.get()); + SetStatus(Key_SSMMessageSkipped, ++_ssmMessageSkipped); + continue; + } + + ASN_STRUCT_FREE_CONTENTS_ONLY(asn_DEF_SignalStatusMessage, ssm_ptr.get()); + PLOG(logDEBUG) << "ssmEncodedMsg: " << ssmEncodedMsg; + + //Broadcast the encoded SSM message + ssmEncodedMsg.set_flags(IvpMsgFlags_RouteDSRC); + ssmEncodedMsg.addDsrcMetadata(0x8002); + BroadcastMessage(static_cast(ssmEncodedMsg)); + } + } + delete msg; + } + } + +} bool CARMAStreetsPlugin::getEncodedtsm3( tsm3EncodedMessage *tsm3EncodedMsg, Json::Value metadata, Json::Value payload_json ) { try diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h index a9111e945..566aaf35c 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h @@ -17,6 +17,7 @@ #include "J2735MapToJsonConverter.h" #include "JsonToJ2735SpatConverter.h" #include "J2735ToSRMJsonConverter.h" +#include "JsonToJ2735SSMConverter.h" @@ -62,6 +63,10 @@ class CARMAStreetsPlugin: public PluginClient { * @brief Subcribe to SPAT Kafka topic created by carma-streets */ void SubscribeSpatKafkaTopic(); + /** + * @brief Subcribe to SSM Kafka topic created by carma-streets + */ + void SubscribeSSMKafkaTopic(); bool getEncodedtsm3(tsm3EncodedMessage *tsm3EncodedMsg, Json::Value metadata, Json::Value payload_json); /** @@ -79,7 +84,9 @@ class CARMAStreetsPlugin: public PluginClient { std::string _subscribeToSchedulingPlanTopic; std::string _subscribeToSchedulingPlanConsumerGroupId; std::string _subscribeToSpatTopic; + std::string _subscribeToSsmTopic; std::string _subscribeToSpatConsumerGroupId; + std::string _subscribeToSSMConsumerGroupId; std::string _transmitMobilityPathTopic; std::string _transmitBSMTopic; std::string _transmitMAPTopic; @@ -89,11 +96,14 @@ class CARMAStreetsPlugin: public PluginClient { RdKafka::Conf *kafka_conf; RdKafka::Conf *kafka_conf_spat_consumer; RdKafka::Conf *kafka_conf_sp_consumer; + RdKafka::Conf *kafka_conf_ssm_consumer; RdKafka::Producer *kafka_producer; RdKafka::KafkaConsumer *_scheduing_plan_kafka_consumer; RdKafka::KafkaConsumer *_spat_kafka_consumer; + RdKafka::KafkaConsumer *_ssm_kafka_consumer; RdKafka::Topic *_scheduing_plan_topic; RdKafka::Topic *_spat_topic; + RdKafka::Topic *_ssm_topic; std::vector _strategies; tmx::messages::tsm3Message *_tsm3Message{NULL}; std::mutex data_lock; @@ -130,6 +140,7 @@ class CARMAStreetsPlugin: public PluginClient { /** * @brief Status label for Mobility Operation messages skipped due to errors. */ + const char* Key_MobilityOperationMessageSkipped = "Mobility Operation messages skipped due to errors."; /** @@ -156,7 +167,17 @@ class CARMAStreetsPlugin: public PluginClient { * @brief Count for BSM messages skipped due to errors. */ uint _bsmMessageSkipped = 0; - + + /** + * @brief Status label for SSM messages skipped due to errors. + */ + const char* Key_SSMMessageSkipped = "SSM messages skipped due to errors."; + + /** + * @brief Count for SSM messages skipped due to errors. + */ + uint _ssmMessageSkipped = 0; + /** * @brief Intersection Id for intersection */ diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ2735SSMConverter.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ2735SSMConverter.cpp new file mode 100644 index 000000000..10e59d87d --- /dev/null +++ b/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ2735SSMConverter.cpp @@ -0,0 +1,156 @@ +#include "JsonToJ2735SSMConverter.h" + +namespace CARMAStreetsPlugin +{ + + bool JsonToJ2735SSMConverter::parseJsonString(const string &consumedMsg, Json::Value &ssmDoc) const + { + const auto jsonLen = static_cast(consumedMsg.length()); + Json::CharReaderBuilder builder; + JSONCPP_STRING err; + const std::unique_ptr reader(builder.newCharReader()); + bool parseResult = reader->parse(consumedMsg.c_str(), consumedMsg.c_str() + jsonLen, &ssmDoc, &err); + if (!parseResult) + { + PLOG(logERROR) << "Parse error: " << err << endl; + } + return parseResult; + } + + void JsonToJ2735SSMConverter::toJ2735SSM(const Json::Value &ssmDoc, std::shared_ptr ssmPtr) const + { + try + { + ASN_STRUCT_FREE_CONTENTS_ONLY(asn_DEF_SignalStatusMessage, ssmPtr.get()); + if (!ssmDoc.isMember("SignalStatus")) + { + PLOG(logERROR) << "No SignalStatus present in JSON." << std::endl; + return; + } + + // populate SignalStatusMessage::second + if (ssmDoc["SignalStatus"].isMember("msOfMinute") && ssmDoc["SignalStatus"]["msOfMinute"].isNumeric()) + { + ssmPtr->second = ssmDoc["SignalStatus"]["msOfMinute"].asInt64(); + } + + // populate SignalStatusMessage::timstamp + if (ssmDoc["SignalStatus"].isMember("minuteOfYear") && ssmDoc["SignalStatus"]["minuteOfYear"].isNumeric()) + { + MinuteOfTheYear_t *timeStamp = (MinuteOfTheYear_t *)calloc(1, sizeof(MinuteOfTheYear_t)); + *timeStamp = ssmDoc["SignalStatus"]["minuteOfYear"].asInt64(); + ssmPtr->timeStamp = timeStamp; + } + + SignalStatusList_t *statusPtr = (SignalStatusList_t *)calloc(1, sizeof(SignalStatusList_t)); + SignalStatus *signalStatus = (SignalStatus *)calloc(1, sizeof(SignalStatus)); + + // populate SignalStatusMessage::status::id + if (ssmDoc["SignalStatus"].isMember("intersectionID") && ssmDoc["SignalStatus"]["intersectionID"].isNumeric()) + { + signalStatus->id.id = ssmDoc["SignalStatus"]["intersectionID"].asInt64(); + } + + // populate SignalStatusMessage::status::sequenceNumber + if (ssmDoc["SignalStatus"].isMember("sequenceNumber") && ssmDoc["SignalStatus"]["sequenceNumber"].isNumeric()) + { + signalStatus->sequenceNumber = ssmDoc["SignalStatus"]["sequenceNumber"].asInt64(); + } + + // populate SignalStatusMessage::status::sigStatus + if (ssmDoc["SignalStatus"].isMember("requestorInfo") && ssmDoc["SignalStatus"]["requestorInfo"].isArray()) + { + Json::Value requesterJsonArr = ssmDoc["SignalStatus"]["requestorInfo"]; + for (auto itr = requesterJsonArr.begin(); itr != requesterJsonArr.end(); itr++) + { + SignalStatusPackage *signalStatusPackage = (SignalStatusPackage *)calloc(1, sizeof(SignalStatusPackage)); + populateSigStatusPackage(signalStatusPackage, itr); + asn_sequence_add(&signalStatus->sigStatus.list.array, signalStatusPackage); + } // Populate signal status package + } + + asn_sequence_add(&statusPtr->list.array, signalStatus); + ssmPtr->status = *statusPtr; + } + catch(exception &ex) + { + PLOG(logERROR) << "Cannot read JSON file." << std::endl; + } + } + + void JsonToJ2735SSMConverter::populateSigStatusPackage(SignalStatusPackage *signalStatusPackage, Json::Value::iterator itr) const + { + signalStatusPackage->requester = (SignalRequesterInfo *)calloc(1, sizeof(SignalRequesterInfo)); + + // populate SignalStatusMessage::status::sigStatus::requester::request + if (itr->isMember("requestID") && (*itr)["requestID"].isNumeric()) + { + signalStatusPackage->requester->request = (*itr)["requestID"].asInt64(); + } + + // populate SignalStatusMessage::status::sigStatus::requester::id + if (itr->isMember("vehicleID") && (*itr)["vehicleID"].isNumeric()) + { + signalStatusPackage->requester->id.choice.stationID = (*itr)["vehicleID"].asInt64(); + signalStatusPackage->requester->id.present = VehicleID_PR_stationID; + } + + // populate SignalStatusMessage::status::sigStatus::requester::sequenceNumber + if (itr->isMember("msgCount") && (*itr)["msgCount"].isNumeric()) + { + signalStatusPackage->requester->sequenceNumber = (*itr)["msgCount"].asInt64(); + } + + // populate SignalStatusMessage::status::sigStatus::requester::role + if (itr->isMember("basicVehicleRole") && (*itr)["basicVehicleRole"].isNumeric()) + { + signalStatusPackage->requester->role = (BasicVehicleRole_t *)calloc(1, sizeof(BasicVehicleRole_t)); + *signalStatusPackage->requester->role = (*itr)["basicVehicleRole"].asInt64(); + } + + // populate SignalStatusMessage::status::sigStatus::inboundOn + if (itr->isMember("inBoundLaneID") && (*itr)["inBoundLaneID"].isNumeric()) + { + signalStatusPackage->inboundOn.present = IntersectionAccessPoint_PR_lane; + signalStatusPackage->inboundOn.choice.lane = (*itr)["inBoundLaneID"].asInt64(); + } + else if (itr->isMember("inBoundApproachID") && (*itr)["inBoundApproachID"].isNumeric()) + { + signalStatusPackage->inboundOn.present = IntersectionAccessPoint_PR_approach; + signalStatusPackage->inboundOn.choice.approach = (*itr)["inBoundApproachID"].asInt64(); + } + + // populate SignalStatusMessage::status::sigStatus::status + if (itr->isMember("priorityRequestStatus") && (*itr)["priorityRequestStatus"].isNumeric()) + { + signalStatusPackage->status = (*itr)["priorityRequestStatus"].asInt64(); + } + + // populate SignalStatusMessage::status::sigStatus::duration + if (itr->isMember("ETA_Duration") && (*itr)["ETA_Duration"].isNumeric()) + { + signalStatusPackage->duration = (DSecond_t *)calloc(1, sizeof(DSecond_t)); + *signalStatusPackage->duration = (*itr)["ETA_Duration"].asInt64(); + } + + // populate SignalStatusMessage::status::sigStatus::minute + if (itr->isMember("ETA_Minute") && (*itr)["ETA_Minute"].isNumeric()) + { + signalStatusPackage->minute = (DSecond_t *)calloc(1, sizeof(DSecond_t)); + *signalStatusPackage->minute = (*itr)["ETA_Minute"].asInt64(); + } + + // populate SignalStatusMessage::status::sigStatus::second + if (itr->isMember("ETA_Second") && (*itr)["ETA_Second"].isNumeric()) + { + signalStatusPackage->minute = (DSecond_t *)calloc(1, sizeof(DSecond_t)); + *signalStatusPackage->minute = (*itr)["ETA_Second"].asInt64(); + } + } + void JsonToJ2735SSMConverter::encodeSSM(const std::shared_ptr &ssmPtr, tmx::messages::SsmEncodedMessage &encodedSSM) const + { + tmx::messages::MessageFrameMessage frame(ssmPtr); + encodedSSM.set_data(tmx::messages::TmxJ2735EncodedMessage::encode_j2735_message>(frame)); + free(frame.get_j2735_data().get()); + } +} diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ2735SSMConverter.h b/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ2735SSMConverter.h new file mode 100644 index 000000000..64d90c816 --- /dev/null +++ b/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ2735SSMConverter.h @@ -0,0 +1,49 @@ +#ifndef JSONTOJ2735SSMCONVERTER_H_ +#define JSONTOJ2735SSMCONVERTER_H_ +#include "jsoncpp/json/json.h" +#include +#include +#include + +using namespace std; +using namespace tmx::utils; +namespace CARMAStreetsPlugin +{ + class JsonToJ2735SSMConverter + { + public: + // Constructor to initialize object + JsonToJ2735SSMConverter() = default; + /*** + * @brief Parse Json string into Json object + * @param jsonstring that is consumed from a kafka topic + * @param JsonObject that is populated by the input json string + * @return boolean. True if parse successfully, false if parse with errors + */ + bool parseJsonString(const string &consumed_msg, Json::Value &ssmDoc) const; + /*** + * @brief Read SSM Json document and populate J2735 SSM message + * @param ssmDoc Json object that contains the SSM information in Json format + * @param ssmPtr a pointer to J2735 message object. This object will be populated with the values in the Json object + */ + void toJ2735SSM(const Json::Value &ssmDoc, std::shared_ptr ssmPtr) const; + /*** + * @brief Encode J2735 SSM + * @param Pointer to J2735 SSM object + * @param Encoded J2735 SSM + */ + void encodeSSM(const std::shared_ptr &ssmPtr, tmx::messages::SsmEncodedMessage &encodedSSM) const; + + /*** + * @brief Populate J2735 SignalStatusPackage object with Json Value + * @param Pointer J2735 SignalStatusPackage object + * @param Json::Value SignalstatusPackage Json string iterator + */ + void populateSigStatusPackage(SignalStatusPackage *signalStatusPackage, Json::Value::iterator itr) const; + + ~JsonToJ2735SSMConverter() = default; + }; + +} + +#endif \ No newline at end of file diff --git a/src/v2i-hub/CARMAStreetsPlugin/test/test_JsonToJ2735SSMConverter.cpp b/src/v2i-hub/CARMAStreetsPlugin/test/test_JsonToJ2735SSMConverter.cpp new file mode 100644 index 000000000..2bd9fb405 --- /dev/null +++ b/src/v2i-hub/CARMAStreetsPlugin/test/test_JsonToJ2735SSMConverter.cpp @@ -0,0 +1,102 @@ +#include +#include +#include +#include +#include "jsoncpp/json/json.h" +#include "JsonToJ2735SSMConverter.h" +using namespace std; + +namespace CARMAStreetsPlugin +{ + class test_JsonToJ2735SSMConverter : public ::testing::Test + { + }; + + TEST_F(test_JsonToJ2735SSMConverter, parseJsonString) + { + JsonToJ2735SSMConverter converter; + // Json string refer to: https://github.com/mmitss/mmitss-az/blob/master/src/mrp/priority-request-server/SSM.json + string valid_json_str = "{\"MessageType\":\"SSM\",\"SignalStatus\":{\"intersectionID\":1003,\"minuteOfYear\":345239,\"msOfMinute\":54000,\"regionalID\":0,\"requestorInfo\":[{\"ETA_Duration\":2000,\"ETA_Minute\":1,\"ETA_Second\":20.001000000000005,\"basicVehicleRole\":16,\"inBoundLaneID\":8,\"msgCount\":2,\"priorityRequestStatus\":4,\"requestID\":5,\"vehicleID\":601},{\"ETA_Duration\":2000,\"ETA_Minute\":1,\"ETA_Second\":22.001000000000005,\"basicVehicleRole\":16,\"inBoundLaneID\":12,\"msgCount\":6,\"priorityRequestStatus\":4,\"requestID\":5,\"vehicleID\":605},{\"ETA_Duration\":2000,\"ETA_Minute\":1,\"ETA_Second\":22.001000000000005,\"basicVehicleRole\":9,\"inBoundLaneID\":12,\"msgCount\":6,\"priorityRequestStatus\":0,\"requestID\":5,\"vehicleID\":610}],\"sequenceNumber\":4,\"updateCount\":4},\"noOfRequest\":3}"; + Json::Value root; + bool result = converter.parseJsonString(valid_json_str, root); + ASSERT_TRUE(result); + string invalid_json = "invalid"; + result = converter.parseJsonString(invalid_json, root); + ASSERT_FALSE(result); + } + + TEST_F(test_JsonToJ2735SSMConverter, toJ2735SSM) + { + JsonToJ2735SSMConverter converter; + // Json string refer to: https://github.com/mmitss/mmitss-az/blob/master/src/mrp/priority-request-server/SSM.json + // Json has three requestors + string valid_json_str = "{\"MessageType\":\"SSM\",\"SignalStatus\":{\"intersectionID\":1003,\"minuteOfYear\":345239,\"msOfMinute\":54000,\"regionalID\":0,\"requestorInfo\":[{\"ETA_Duration\":2000,\"ETA_Minute\":1,\"ETA_Second\":20.001000000000005,\"basicVehicleRole\":16,\"inBoundApproachID\":8,\"msgCount\":2,\"priorityRequestStatus\":4,\"requestID\":5,\"vehicleID\":601},{\"ETA_Duration\":2000,\"ETA_Minute\":1,\"ETA_Second\":22.001000000000005,\"basicVehicleRole\":16,\"inBoundLaneID\":12,\"msgCount\":6,\"priorityRequestStatus\":4,\"requestID\":5,\"vehicleID\":605},{\"ETA_Duration\":2000,\"ETA_Minute\":1,\"ETA_Second\":22.001000000000005,\"basicVehicleRole\":9,\"inBoundLaneID\":12,\"msgCount\":6,\"priorityRequestStatus\":0,\"requestID\":5,\"vehicleID\":610}],\"sequenceNumber\":4,\"updateCount\":4},\"noOfRequest\":3}"; + Json::Value root; + bool result = converter.parseJsonString(valid_json_str, root); + ASSERT_TRUE(result); + auto ssmPtr = std::make_shared(); + converter.toJ2735SSM(root, ssmPtr); + ASSERT_EQ(1, ssmPtr->status.list.count); + ASSERT_EQ(1003, ssmPtr->status.list.array[0]->id.id); + ASSERT_EQ(54000, ssmPtr->second); + ASSERT_EQ(345239, *ssmPtr->timeStamp); + ASSERT_EQ(3, ssmPtr->status.list.array[0]->sigStatus.list.count); + ASN_STRUCT_FREE_CONTENTS_ONLY(asn_DEF_SignalStatusMessage, ssmPtr.get()); + } + + TEST_F(test_JsonToJ2735SSMConverter, toJ2735SSM_2) + { + JsonToJ2735SSMConverter converter; + // Json has one requestor + string valid_json_str = "{\"MessageType\":\"SSM\",\"SignalStatus\":{\"intersectionID\":1004,\"minuteOfYear\":345240,\"msOfMinute\":54001,\"regionalID\":0,\"requestorInfo\":[{\"ETA_Duration\":2000,\"ETA_Minute\":1,\"ETA_Second\":20.001000000000005,\"basicVehicleRole\":16,\"inBoundLaneID\":8,\"msgCount\":2,\"priorityRequestStatus\":4,\"requestID\":5,\"vehicleID\":601}],\"sequenceNumber\":4,\"updateCount\":4},\"noOfRequest\":3}"; + Json::Value root; + bool result = converter.parseJsonString(valid_json_str, root); + ASSERT_TRUE(result); + auto ssmPtr = std::make_shared(); + converter.toJ2735SSM(root, ssmPtr); + ASSERT_EQ(1, ssmPtr->status.list.count); + ASSERT_EQ(1004, ssmPtr->status.list.array[0]->id.id); + ASSERT_EQ(54001, ssmPtr->second); + ASSERT_EQ(345240, *ssmPtr->timeStamp); + ASSERT_EQ(1, ssmPtr->status.list.array[0]->sigStatus.list.count); + ASN_STRUCT_FREE_CONTENTS_ONLY(asn_DEF_SignalStatusMessage, ssmPtr.get()); + } + + TEST_F(test_JsonToJ2735SSMConverter, toJ2735SSM_3) + { + JsonToJ2735SSMConverter converter; + // Json has NO requestor + string valid_json_str = "{\"MessageType\":\"SSM\",\"SignalStatus\":{\"intersectionID\":1004,\"minuteOfYear\":345240,\"msOfMinute\":54001,\"regionalID\":0,\"sequenceNumber\":4,\"updateCount\":4},\"noOfRequest\":3}"; + Json::Value root; + bool result = converter.parseJsonString(valid_json_str, root); + ASSERT_TRUE(result); + auto ssmPtr = std::make_shared(); + converter.toJ2735SSM(root, ssmPtr); + ASSERT_EQ(0, ssmPtr->status.list.array[0]->sigStatus.list.count); + ASN_STRUCT_FREE_CONTENTS_ONLY(asn_DEF_SignalStatusMessage, ssmPtr.get()); + } + + TEST_F(test_JsonToJ2735SSMConverter, encodeSSM) + { + JsonToJ2735SSMConverter converter; + // Json has one requestor + string valid_json_str = "{\"MessageType\":\"SSM\",\"SignalStatus\":{\"intersectionID\":1004,\"minuteOfYear\":345240,\"msOfMinute\":54001,\"regionalID\":0,\"requestorInfo\":[{\"ETA_Duration\":2000,\"ETA_Minute\":1,\"ETA_Second\":20.001000000000005,\"basicVehicleRole\":16,\"inBoundLaneID\":8,\"msgCount\":2,\"priorityRequestStatus\":4,\"requestID\":5,\"vehicleID\":601}],\"sequenceNumber\":4,\"updateCount\":4},\"noOfRequest\":3}"; + Json::Value root; + converter.parseJsonString(valid_json_str, root); + auto ssmPtr = std::make_shared(); + converter.toJ2735SSM(root, ssmPtr); + tmx::messages::SsmEncodedMessage encodedSSM; + converter.encodeSSM(ssmPtr, encodedSSM); + string expected_payload_hex = "001e18454498d2f1001007d8054a000004b20a090010000280fa08"; + ASSERT_EQ(expected_payload_hex, encodedSSM.get_payload_str()); + + // Json has three requestors + valid_json_str = "{\"MessageType\":\"SSM\",\"SignalStatus\":{\"intersectionID\":1003,\"minuteOfYear\":345239,\"msOfMinute\":54000,\"regionalID\":0,\"requestorInfo\":[{\"ETA_Duration\":2000,\"ETA_Minute\":1,\"ETA_Second\":20.001000000000005,\"basicVehicleRole\":16,\"inBoundLaneID\":8,\"msgCount\":2,\"priorityRequestStatus\":4,\"requestID\":5,\"vehicleID\":601},{\"ETA_Duration\":2000,\"ETA_Minute\":1,\"ETA_Second\":22.001000000000005,\"basicVehicleRole\":16,\"inBoundLaneID\":12,\"msgCount\":6,\"priorityRequestStatus\":4,\"requestID\":5,\"vehicleID\":605},{\"ETA_Duration\":2000,\"ETA_Minute\":1,\"ETA_Second\":22.001000000000005,\"basicVehicleRole\":9,\"inBoundLaneID\":12,\"msgCount\":6,\"priorityRequestStatus\":0,\"requestID\":5,\"vehicleID\":610}],\"sequenceNumber\":4,\"updateCount\":4},\"noOfRequest\":3}"; + converter.parseJsonString(valid_json_str, root); + converter.toJ2735SSM(root, ssmPtr); + converter.encodeSSM(ssmPtr, encodedSSM); + expected_payload_hex = "001e35454497d2f0001007d6254a000004b20a090010000280fa08a940000097414320030000581f4115280000131028624060000b03e800"; + ASSERT_EQ(expected_payload_hex, encodedSSM.get_payload_str()); + ASN_STRUCT_FREE_CONTENTS_ONLY(asn_DEF_SignalStatusMessage, ssmPtr.get()); + } +} \ No newline at end of file diff --git a/src/v2i-hub/ImmediateForwardPlugin/manifest.json b/src/v2i-hub/ImmediateForwardPlugin/manifest.json index 5b142b2b1..43e6603c4 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\" }] }", + "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\" },] }", "description": "JSON data defining the message types, PSIDs, and channel number for messages forwarded to the V2X radio at destination 1." }, { From 4528b26194e76de39bade494e85cea8d6c47cf1b Mon Sep 17 00:00:00 2001 From: Will Martin Date: Fri, 16 Jun 2023 09:33:40 -0400 Subject: [PATCH 02/28] modified: src/v2i-hub/ImmediateForwardPlugin/manifest.json (#541) # PR Details ## Description Removing unexpected comma in manifest.json ## Related Issue ## Motivation and Context ## How Has This Been Tested? ## Types of changes - [ ] 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. - [ ] 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/ImmediateForwardPlugin/manifest.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/v2i-hub/ImmediateForwardPlugin/manifest.json b/src/v2i-hub/ImmediateForwardPlugin/manifest.json index 43e6603c4..b26756a76 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\" }] }", "description": "JSON data defining the message types, PSIDs, and channel number for messages forwarded to the V2X radio at destination 1." }, { From 37eeb9ba7f043548f9743c1e961c61adee0bbfc6 Mon Sep 17 00:00:00 2001 From: Will Martin Date: Wed, 5 Jul 2023 11:23:14 -0400 Subject: [PATCH 03/28] Routable metadata (#545) # PR Details ## Description Adds PSID metadata to BSMs and SRMs that are sent to the Message Receiver plugin. If "RouteMessage" is enabled, the messages will be available for the Immediate Forward plugin to use. Additional changes were made to build process to speed up process. ## Related Issue ## Motivation and Context ## How Has This Been Tested? Sent BSMs and SRMs to Message Receiver plugin. Messages were transmitted by Immediate Forward plugin when "RouteMessage" was enabled and not routed when disabled. Previously, messages were skipped or not routed, respectively. ## Types of changes - [ ] Defect fix (non-breaking change that fixes an issue) - [X] 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. --- ext/build.sh | 15 +++--- scripts/install_dependencies.sh | 3 ++ src/build.sh | 5 +- src/tmx/TmxApi/tmx/IvpMessage.h | 4 +- src/tmx/TmxApi/tmx/TmxApiMessages.h | 51 ++++++++++++++++++- .../src/ImmediateForwardPlugin.cpp | 2 +- .../MessageReceiverPlugin/manifest.json | 4 +- .../src/MessageReceiverPlugin.cpp | 9 ++-- 8 files changed, 76 insertions(+), 17 deletions(-) diff --git a/ext/build.sh b/ext/build.sh index 74a9d578c..03d2ec4fb 100755 --- a/ext/build.sh +++ b/ext/build.sh @@ -3,6 +3,9 @@ # exit on errors set -e +# Find number of cores available +numCPU=$(nproc) + # An OPENAPI based Qt webservice is needed by the plugins for http requests processing. A custom generated code using OPENAPI framework is located in GitHub. pushd /tmp QHTTPENGINE_VERSION=1.0.1 @@ -10,7 +13,7 @@ wget -O qhttpengine-${QHTTPENGINE_VERSION}.tar.gz https://github.com/nitroshare/ tar xvf qhttpengine-${QHTTPENGINE_VERSION}.tar.gz cd qhttpengine-${QHTTPENGINE_VERSION}/ cmake . -make +make -j${numCPU} make install popd @@ -20,7 +23,7 @@ wget -O date-${DATELIB_VERSION}.tar.gz https://github.com/HowardHinnant/date/arc tar xvf date-${DATELIB_VERSION}.tar.gz cd date-${DATELIB_VERSION}/ cmake . -make +make -j${numCPU} make install popd @@ -29,19 +32,19 @@ ldconfig # Server for the Qt webservice pushd server cmake . -make +make -j${numCPU} make install popd pushd ccserver cmake . -make +make -j${numCPU} make install popd pushd pdclient cmake . -make +make -j${numCPU} make install popd @@ -50,6 +53,6 @@ pushd /tmp git clone https://github.com/ckgt/NemaTode.git cd NemaTode cmake . -make +make -j${numCPU} make install popd \ No newline at end of file diff --git a/scripts/install_dependencies.sh b/scripts/install_dependencies.sh index 9ddc86c72..2fa104c86 100755 --- a/scripts/install_dependencies.sh +++ b/scripts/install_dependencies.sh @@ -41,9 +41,12 @@ LIBRARY_DEPENDENCIES=" \ # install all things needed for deployment, always done apt-get install -y $DEPENDENCIES ${LIBRARY_DEPENDENCIES} +numCPU=$(nproc) + # install gtest cd /usr/src/googletest/ mkdir -p build/ cd build cmake .. +make -j${numCPU} make install diff --git a/src/build.sh b/src/build.sh index 48fa43381..9bb3d4c4f 100755 --- a/src/build.sh +++ b/src/build.sh @@ -18,6 +18,8 @@ set -e # script executes all tmx and v2i build and coverage steps so that they can be singularly # wrapped by the sonarcloud build-wrapper +numCPU=$(nproc) + RELEASE_BUILD=0 if [ "$1" = "release" ]; then RELEASE_BUILD=1 @@ -36,7 +38,7 @@ fi pushd tmx cmake -Bbuild -DCMAKE_PREFIX_PATH=\"/usr/local/share/tmx\;\/opt/carma/cmake\;\" -DCMAKE_CXX_FLAGS="${COVERAGE_FLAGS}" -DCMAKE_C_FLAGS="${COVERAGE_FLAGS}" -DCMAKE_BUILD_TYPE="${BUILD_TYPE}" . pushd build -make +make -j${numCPU} make install popd popd @@ -44,6 +46,7 @@ popd pushd v2i-hub cmake -Bbuild -DCMAKE_PREFIX_PATH=\"/usr/local/share/tmx\;\/opt/carma/cmake\;\" -DqserverPedestrian_DIR=/usr/local/share/qserverPedestrian/cmake -Dv2xhubWebAPI_DIR=/usr/local/share/v2xhubWebAPI/cmake/ -DCMAKE_CXX_FLAGS="${COVERAGE_FLAGS}" -DCMAKE_C_FLAGS="${COVERAGE_FLAGS}" -DCMAKE_BUILD_TYPE="${BUILD_TYPE}" . pushd build +make -j${numCPU} make install popd popd diff --git a/src/tmx/TmxApi/tmx/IvpMessage.h b/src/tmx/TmxApi/tmx/IvpMessage.h index 7bd74b3ac..698f66cf0 100644 --- a/src/tmx/TmxApi/tmx/IvpMessage.h +++ b/src/tmx/TmxApi/tmx/IvpMessage.h @@ -20,8 +20,8 @@ typedef unsigned int IvpMsgFlags; #define IvpMsgFlags_RouteDSRC 0x01 typedef struct IvpDsrcMetadata { - int channel; int psid; + int channel; } IvpDsrcMetadata; typedef struct IvpMessage { @@ -64,7 +64,7 @@ typedef enum { */ IvpMessage *ivpMsg_create(const char *type, const char *subtype, const char *encoding, IvpMsgFlags flags, cJSON *payload); -IvpMessage *ivpMsg_addDsrcMetadata(IvpMessage *msg, int channel, int psid); +IvpMessage *ivpMsg_addDsrcMetadata(IvpMessage *msg, int psid, int channel); /*! * Creates a new IvpMessage from a json string. diff --git a/src/tmx/TmxApi/tmx/TmxApiMessages.h b/src/tmx/TmxApi/tmx/TmxApiMessages.h index b4327e50f..b680329ff 100644 --- a/src/tmx/TmxApi/tmx/TmxApiMessages.h +++ b/src/tmx/TmxApi/tmx/TmxApiMessages.h @@ -177,7 +177,56 @@ static CONSTEXPR const char *STATUS_STARTED_STRING = "Started, waiting for conne static CONSTEXPR const char *STATUS_RUNNING_STRING = "Running"; static CONSTEXPR const char *STATUS_STALE_STRING = "Connection going stale"; static CONSTEXPR const char *STATUS_STOPPED_STRING = "Stopped / Disconnected"; - + +enum msgPSID +{ + None_PSID = 0x00, + mapData_PSID = 0x8002, + signalPhaseAndTimingMessage_PSID = 0x8002, + basicSafetyMessage_PSID = 0x20, + commonSafetyRequest_PSID = 0x20, + emergencyVehicleAlert_PSID = 0x8005, + intersectionCollision_PSID = 0x8002, + nmeaCorrections_PSID = 0x8000, + probeDataManagement_PSID = 0x8004, + probeVehicleData_PSID = 0x8004, + roadSideAlert_PSID = 0x8003, + rtcmCorrections_PSID = 0x8000, + signalRequestMessage_PSID = 0xE0000016, + signalStatusMessage_PSID = 0x8002, + travelerInformation_PSID = 0x8003, + personalSafetyMessage_PSID = 0x27, + testMessage00_PSID = 0xBFEE, + testMessage01_PSID = 0xBFEE, + testMessage02_PSID = 0xBFEE, + testMessage03_PSID = 0xBFEE, + testMessage04_PSID = 0x8003, + testMessage05_PSID = 0x8003 +}; + +static CONSTEXPR const char *MSGPSID_NONE_PSID_STRING = "None"; +static CONSTEXPR const char *MSGPSID_MAPDATA_PSID_STRING = "0x8002"; +static CONSTEXPR const char *MSGPSID_SIGNALPHASEANDTIMINGMESSAGE_PSID_STRING = "0x8002"; +static CONSTEXPR const char *MSGPSID_BASICSAFETYMESSAGE_PSID_STRING = "0x20"; +static CONSTEXPR const char *MSGPSID_COMMONSAFETYREQUEST_PSID_STRING = "0x20"; +static CONSTEXPR const char *MSGPSID_EMERGENCYVEHICLEALERT_PSID_STRING = "0x8005"; +static CONSTEXPR const char *MSGPSID_INTERSECTIONCOLLISION_PSID_STRING = "0x8002"; +static CONSTEXPR const char *MSGPSID_NMEACORRECTIONS_PSID_STRING = "0x8000"; +static CONSTEXPR const char *MSGPSID_PROBEDATAMANAGEMENT_PSID_STRING = "0x8004"; +static CONSTEXPR const char *MSGPSID_PROBEVEHICLEDATA_PSID_STRING = "0x8004"; +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_TRAVELERINFORMATION_PSID_STRING = "0x8003"; +static CONSTEXPR const char *MSGPSID_PERSONALSAFETYMESSAGE_PSID_STRING = "0x27"; +static CONSTEXPR const char *MSGPSID_TESTMESSAGE00_PSID_STRING = "0xBFEE"; +static CONSTEXPR const char *MSGPSID_TESTMESSAGE01_PSID_STRING = "0xBFEE"; +static CONSTEXPR const char *MSGPSID_TESTMESSAGE02_PSID_STRING = "0xBFEE"; +static CONSTEXPR const char *MSGPSID_TESTMESSAGE03_PSID_STRING = "0xBFEE"; +static CONSTEXPR const char *MSGPSID_TESTMESSAGE04_PSID_STRING = "0x8003"; +static CONSTEXPR const char *MSGPSID_TESTMESSAGE05_PSID_STRING = "0x8003"; + } /* End namespace api */ } /* End namespace messages */ diff --git a/src/v2i-hub/ImmediateForwardPlugin/src/ImmediateForwardPlugin.cpp b/src/v2i-hub/ImmediateForwardPlugin/src/ImmediateForwardPlugin.cpp index 7e957c80c..e907ce667 100644 --- a/src/v2i-hub/ImmediateForwardPlugin/src/ImmediateForwardPlugin.cpp +++ b/src/v2i-hub/ImmediateForwardPlugin/src/ImmediateForwardPlugin.cpp @@ -428,7 +428,7 @@ void ImmediateForwardPlugin::SendMessageToRadio(IvpMessage *msg) if (!foundMessageType) { SetStatus(Key_SkippedNoMessageRoute, ++_skippedNoMessageRoute); - PLOG(logWARNING)<<" WARNINNG TMX Subtype not found in configuration. Message Ignored: " << + PLOG(logWARNING)<<" WARNING TMX Subtype not found in configuration. Message Ignored: " << "Type: " << msg->type << ", Subtype: " << msg->subtype; return; } diff --git a/src/v2i-hub/MessageReceiverPlugin/manifest.json b/src/v2i-hub/MessageReceiverPlugin/manifest.json index 0ca9ed689..7be05050d 100644 --- a/src/v2i-hub/MessageReceiverPlugin/manifest.json +++ b/src/v2i-hub/MessageReceiverPlugin/manifest.json @@ -29,9 +29,9 @@ "description":"Port for the incoming message network connection." }, { - "key":"RouteDSRC", + "key":"RouteJ2735", "default":"false", - "description":"Set the flag to route a received J2735 message." + "description":"Set the flag to route/broadcast a received J2735 message to TMX Core." }, { "key":"EnableSimulatedBSM", diff --git a/src/v2i-hub/MessageReceiverPlugin/src/MessageReceiverPlugin.cpp b/src/v2i-hub/MessageReceiverPlugin/src/MessageReceiverPlugin.cpp index dee2bfa7e..0da347c74 100644 --- a/src/v2i-hub/MessageReceiverPlugin/src/MessageReceiverPlugin.cpp +++ b/src/v2i-hub/MessageReceiverPlugin/src/MessageReceiverPlugin.cpp @@ -166,7 +166,7 @@ void MessageReceiverPlugin::OnMessageReceived(routeable_message &msg) BsmEncodedMessage encodedBsm; SrmEncodedMessage encodedSrm; - + int msgPSID = api::msgPSID::None_PSID; if (msg.get_type() == "Unknown" && msg.get_subtype() == "Unknown") { @@ -263,6 +263,7 @@ void MessageReceiverPlugin::OnMessageReceived(routeable_message &msg) } sendMsg = encode(encodedBsm, bsm); + msgPSID = api::msgPSID::basicSafetyMessage_PSID; if (!simBSM) return; } break; @@ -280,7 +281,7 @@ void MessageReceiverPlugin::OnMessageReceived(routeable_message &msg) ntohl(*((uint32_t*)&(bytes.data()[24]))), ntohl(*((uint32_t*)&(bytes.data()[28])))); sendMsg = encode(encodedSrm, srm); - + msgPSID = api::msgPSID::signalRequestMessage_PSID; } break; default: @@ -330,6 +331,7 @@ void MessageReceiverPlugin::OnMessageReceived(routeable_message &msg) if (routeDsrc) { sendMsg->set_flags(IvpMsgFlags_RouteDSRC); + sendMsg->addDsrcMetadata(msgPSID); } else { @@ -345,7 +347,7 @@ void MessageReceiverPlugin::UpdateConfigSettings() lock_guard lock(syncLock); // Atomic flags - GetConfigValue("RouteDSRC", routeDsrc); + GetConfigValue("RouteJ2735", routeDsrc); GetConfigValue("EnableSimulatedBSM", simBSM); GetConfigValue("EnableSimulatedSRM", simSRM); GetConfigValue("EnableSimulatedLocation", simLoc); @@ -465,7 +467,6 @@ int MessageReceiverPlugin::Main() SetStatus(Key_SkippedSignVerifyError, ++_skippedSignVerifyErrorResponse); PLOG(logERROR) << "Error parsing Messages: " << ex.what(); continue; -; } PLOG(logDEBUG1) << "SCMS Contain response = " << result << std::endl; cJSON *root = cJSON_Parse(result.c_str()); From b708d325f68cbda0888333fd464746f515abf163 Mon Sep 17 00:00:00 2001 From: Andy <109987630+gainesaw@users.noreply.github.com> Date: Wed, 5 Jul 2023 15:13:38 -0400 Subject: [PATCH 04/28] updated initialization script for amd64 and arm64. (#546) # PR Details ## Description script now installs necessary dependencies, creates secret files, prompts user to input mysql passwords, and create v2xhub user ## Related Issue open JIRA task to update initialization script for amd64 and arm64 ## Motivation and Context requested by project leads, automates v2xhub setup/initialization ## How Has This Been Tested? code has been used to setup v2xhub from scratch ## Types of changes - [ ] Defect fix (non-breaking change that fixes an issue) - [ x] 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 - [x ] My change requires a change to the documentation. - ###potential change to documentation as running this script automates setup. user only needs to clone the repo, run the script, and input passwords when prompted, then navigate to https://127.0.0.1:19760 to accept security certs then v2xhub is ready to use. - [ ] 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. --- configuration/amd64/initialization.sh | 37 +++++++++++++++++++++++++-- configuration/arm64/initialization.sh | 36 ++++++++++++++++++++++++-- 2 files changed, 69 insertions(+), 4 deletions(-) diff --git a/configuration/amd64/initialization.sh b/configuration/amd64/initialization.sh index 9a9f68e18..03660e9d5 100755 --- a/configuration/amd64/initialization.sh +++ b/configuration/amd64/initialization.sh @@ -1,12 +1,45 @@ #!/bin/bash + +# update and upgrade commands to update linux OS +sudo apt update -y && sudo apt upgrade -y + +#installing necessary and useful apps +sudo apt-get install chromium-browser -y #Chrome required for CARMA platform/V2X Hub UI(?) +sudo apt install curl -y #Curl for downloading files over internet + +#install docker +curl -L https://raw.githubusercontent.com/usdot-fhwa-stol/carma-platform/develop/engineering_tools/install-docker.sh | bash + +#make passwords for mysql +mkdir secrets && cd secrets + +#creates password files where user inputs password +read -p "enter password for the mysql_root_password: " sql_root_pass +echo "$sql_root_pass" > sql_root_pass.txt + +read -p "enter password for mysql_password: " sql_pass +echo "$sql_pass" > sql_pass.txt + +#remove endline characters from password files +tr -d '\n' mysql_root_password.txt && tr -d '\n' mysql_password.txt +rm sql_root_pass.txt && rm sql_pass.txt + +#AMD64 initialzation +cd .. sudo apt-get -y remove docker docker-engine docker.io containerd runc -sudo apt-get update sudo apt-get -y install apt-transport-https ca-certificates curl gnupg-agent software-properties-common curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo apt-key add - sudo add-apt-repository "deb [arch=amd64] https://download.docker.com/linux/ubuntu $(lsb_release -cs) stable" -sudo apt-get update sudo apt-get -y install docker-ce docker-ce-cli containerd.io sudo apt -y install python3-pip sudo pip3 install docker-compose sudo docker-compose pull +sudo apt update -y && sudo apt upgrade -y sudo docker-compose up -d + +#create v2xhub user +cd mysql +./add_v2xhub_user.bash + +chromium-browser "https://127.0.0.1" > /dev/null 2>&1 & +chromium-browser "https://127.0.0.1:19760" > /dev/null 2>&1 & diff --git a/configuration/arm64/initialization.sh b/configuration/arm64/initialization.sh index f8a4dc088..2430a1641 100755 --- a/configuration/arm64/initialization.sh +++ b/configuration/arm64/initialization.sh @@ -1,13 +1,45 @@ #!/bin/bash +# update and upgrade commands to update linux OS +sudo apt update -y && sudo apt upgrade -y + +#installing necessary and useful apps +sudo apt-get install chromium-browser -y #Chrome required for CARMA platform/V2X Hub UI(?) +sudo apt install curl -y #Curl for downloading files over internet + +#install docker +curl -L https://raw.githubusercontent.com/usdot-fhwa-stol/carma-platform/develop/engineering_tools/install-docker.sh | bash + +#make passwords for mysql +mkdir secrets && cd secrets + +#creates password files where user inputs password +read -p "enter password for the mysql_root_password: " sql_root_pass +echo "$sql_root_pass" > sql_root_pass.txt + +read -p "enter password for mysql_password: " sql_pass +echo "$sql_pass" > sql_pass.txt + +#remove endline characters from password files +tr -d '\n' mysql_root_password.txt && tr -d '\n' mysql_password.txt +rm sql_root_pass.txt && rm sql_pass.txt + +#ARM initialization +cd .. sudo apt-get -y remove docker docker-engine docker.io containerd runc -sudo apt-get update sudo apt-get -y install apt-transport-https ca-certificates curl gnupg-agent software-properties-common OS=$(lsb_release -i | awk 'FS=":" {print $3;}' | awk '{print tolower($0)}') arch=$(dpkg --print-architecture) curl -fsSL https://download.docker.com/linux/$OS/gpg | sudo apt-key add - sudo add-apt-repository "deb [arch=$arch] https://download.docker.com/linux/$OS $(lsb_release -cs) stable" -sudo apt-get update sudo apt-get -y install docker-ce docker-ce-cli containerd.io docker-compose-plugin sudo apt -y install python3-pip sudo pip3 install docker-compose +sudo apt update -y && sudo apt upgrade -y sudo docker-compose up -d + +#create v2xhub user +cd mysql +./add_v2xhub_user.bash + +chromium-browser "https://127.0.0.1" > /dev/null 2>&1 & +chromium-browser "https://127.0.0.1:19760" > /dev/null 2>&1 & From 1c5a862ce4ffabca662159859a6d6f95300b78cb Mon Sep 17 00:00:00 2001 From: dan-du-car <62157949+dan-du-car@users.noreply.github.com> Date: Fri, 7 Jul 2023 10:53:31 -0400 Subject: [PATCH 05/28] CARMA Streets Plugin Kafka Consumers (#547) # PR Details ## Description - Remove the kafka consumer/producer initialization during config parameter update. - Replace consumer creation with v2xhub kafka_client library. - Add producer creation in kafka_client library ## Related Issue https://github.com/usdot-fhwa-OPS/V2X-Hub/issues/543 ## Motivation and Context NA ## How Has This Been Tested? Local integration 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) - [ ] I have added tests to cover my changes. - [ ] All new and existing tests passed. --- .devcontainer/docker-compose-vscode.yml | 2 +- src/tmx/TmxUtils/src/kafka/kafka_client.cpp | 14 + src/tmx/TmxUtils/src/kafka/kafka_client.h | 1 + .../src/kafka/kafka_consumer_worker.cpp | 10 +- .../src/kafka/kafka_consumer_worker.h | 1 + .../src/kafka/kafka_producer_worker.cpp | 52 ++- .../src/kafka/kafka_producer_worker.h | 27 +- .../test/test_kafka_producer_worker.cpp | 15 + .../src/CARMAStreetsPlugin.cpp | 397 ++++++------------ .../src/CARMAStreetsPlugin.h | 26 +- 10 files changed, 269 insertions(+), 276 deletions(-) diff --git a/.devcontainer/docker-compose-vscode.yml b/.devcontainer/docker-compose-vscode.yml index c3e19d9a8..a34dd25c9 100755 --- a/.devcontainer/docker-compose-vscode.yml +++ b/.devcontainer/docker-compose-vscode.yml @@ -72,7 +72,7 @@ services: DOCKER_HOST_IP: 127.0.0.1 KAFKA_ADVERTISED_HOST_NAME: 127.0.0.1 KAFKA_ZOOKEEPER_CONNECT: zookeeper:2181 - KAFKA_CREATE_TOPICS: "time_sync:1:1" + KAFKA_CREATE_TOPICS: "time_sync:1:1,modified_spat:1:1,v2xhub_scheduling_plan_sub:1:1,v2xhub_ssm_sub:1:1" KAFKA_LOG_DIRS: "/kafka/kafka-logs" volumes: - /var/run/docker.sock:/var/run/docker.sock diff --git a/src/tmx/TmxUtils/src/kafka/kafka_client.cpp b/src/tmx/TmxUtils/src/kafka/kafka_client.cpp index df5346f4f..925b35aa4 100644 --- a/src/tmx/TmxUtils/src/kafka/kafka_client.cpp +++ b/src/tmx/TmxUtils/src/kafka/kafka_client.cpp @@ -34,5 +34,19 @@ namespace tmx::utils } } + std::shared_ptr kafka_client::create_producer(const std::string &bootstrap_server) const + { + try + { + auto producer_ptr = std::make_shared(bootstrap_server); + return producer_ptr; + } + catch (const std::runtime_error &e) + { + FILE_LOG(logERROR) << "Create producer failure: " << e.what() << std::endl; + exit(1); + } + } + } \ No newline at end of file diff --git a/src/tmx/TmxUtils/src/kafka/kafka_client.h b/src/tmx/TmxUtils/src/kafka/kafka_client.h index b4da5e059..778a17026 100644 --- a/src/tmx/TmxUtils/src/kafka/kafka_client.h +++ b/src/tmx/TmxUtils/src/kafka/kafka_client.h @@ -15,6 +15,7 @@ namespace tmx::utils std::shared_ptr create_consumer(const std::string &broker_str, const std::string &topic_str, const std::string &group_id_str) const; std::shared_ptr create_producer(const std::string &broker_str, const std::string &topic_str) const; + std::shared_ptr create_producer(const std::string &bootstrap_server) const; }; } diff --git a/src/tmx/TmxUtils/src/kafka/kafka_consumer_worker.cpp b/src/tmx/TmxUtils/src/kafka/kafka_consumer_worker.cpp index 74c4e2735..a0a3d2c8f 100644 --- a/src/tmx/TmxUtils/src/kafka/kafka_consumer_worker.cpp +++ b/src/tmx/TmxUtils/src/kafka/kafka_consumer_worker.cpp @@ -42,6 +42,12 @@ namespace tmx::utils return false; } + if (conf->set(ENABLE_AUTO_COMMIT, "true", errstr) != RdKafka::Conf::CONF_OK) + { + FILE_LOG(logWARNING) << "RDKafka conf set enable auto commit failed: " << errstr.c_str() << std::endl; + return false; + } + // set consumer group if (conf->set(GROUP_ID, _group_id_str, errstr) != RdKafka::Conf::CONF_OK) { @@ -88,6 +94,8 @@ namespace tmx::utils void kafka_consumer_worker::stop() { _run = false; + //Close and shutdown the consumer. + _consumer->close(); /*Destroy kafka instance*/ // Wait for RdKafka to decommission. RdKafka::wait_destroyed(5000); } @@ -134,7 +142,7 @@ namespace tmx::utils switch (message->err()) { case RdKafka::ERR__TIMED_OUT: - FILE_LOG(logWARNING) << _consumer->name() << " consume failed: " << message->errstr() << std::endl; + FILE_LOG(logDEBUG4) << _consumer->name() << " consume failed: " << message->errstr() << std::endl; break; case RdKafka::ERR_NO_ERROR: FILE_LOG(logDEBUG1) << _consumer->name() << " read message at offset " << message->offset() << std::endl; diff --git a/src/tmx/TmxUtils/src/kafka/kafka_consumer_worker.h b/src/tmx/TmxUtils/src/kafka/kafka_consumer_worker.h index a3e00c00b..95a9c96a8 100644 --- a/src/tmx/TmxUtils/src/kafka/kafka_consumer_worker.h +++ b/src/tmx/TmxUtils/src/kafka/kafka_consumer_worker.h @@ -40,6 +40,7 @@ namespace tmx::utils { const std::string GROUP_ID="group.id"; const std::string MAX_PARTITION_FETCH_SIZE="max.partition.fetch.bytes"; const std::string ENABLE_PARTITION_END_OF="enable.partition.eof"; + const std::string ENABLE_AUTO_COMMIT="enable.auto.commit"; //maximum size for pulling message from a single partition at a time std::string STR_FETCH_NUM = "10240000"; diff --git a/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.cpp b/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.cpp index 46cbc55e3..e81364c19 100644 --- a/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.cpp +++ b/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.cpp @@ -33,13 +33,26 @@ namespace tmx::utils { } + kafka_producer_worker::kafka_producer_worker(const std::string &brokers) + : _broker_str(brokers), _run(true) + { + } + bool kafka_producer_worker::init() + { + if(init_producer()) + { + return init_topic(); + } + return false; + } + + bool kafka_producer_worker::init_producer() { std::string errstr = ""; // Create configuration objects RdKafka::Conf *conf = RdKafka::Conf::create(RdKafka::Conf::CONF_GLOBAL); - RdKafka::Conf *tconf = RdKafka::Conf::create(RdKafka::Conf::CONF_TOPIC); /*** * Set Configuration properties @@ -75,7 +88,12 @@ namespace tmx::utils delete conf; FILE_LOG(logINFO) << "Created producer: " << _producer->name() << std::endl; + } + bool kafka_producer_worker::init_topic() + { + RdKafka::Conf *tconf = RdKafka::Conf::create(RdKafka::Conf::CONF_TOPIC); + std::string errstr = ""; // Create topic handle _topic = RdKafka::Topic::create(_producer, _topics_str, tconf, errstr); if (!_topic) @@ -156,6 +174,38 @@ namespace tmx::utils _producer->poll(0); } + void kafka_producer_worker::send(const std::string& message, const std::string& topic_name ) const + { + bool retry = true; + while (retry) + { + RdKafka::ErrorCode produce_error = _producer->produce(topic_name, + RdKafka::Topic::PARTITION_UA, + RdKafka::Producer::RK_MSG_COPY, + const_cast(message.c_str()), + message.size(), + nullptr, 0, 0, nullptr); + + if (produce_error == RdKafka::ERR_NO_ERROR) { + PLOG(logDEBUG) <<"Queued message:" << message; + retry = false; + } + else + { + PLOG(logERROR) <<"Failed to queue message:" << message <<" with error:" << RdKafka::err2str(produce_error); + if (produce_error == RdKafka::ERR__QUEUE_FULL) { + PLOG(logERROR) <<"Message queue full...retrying..."; + _producer->poll(500); /* ms */ + retry = true; + } + else { + PLOG(logERROR) <<"Unhandled error in queue_kafka_message:" << RdKafka::err2str(produce_error); + retry = false; + } + } + } + } + void kafka_producer_worker::stop() { /* Wait for final messages to be delivered or fail. diff --git a/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.h b/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.h index a06a5587c..7dde427d1 100644 --- a/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.h +++ b/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.h @@ -53,7 +53,13 @@ namespace tmx::utils * @param topic_str topic producer should produce to. * @param n_partition partition producer should be assigned to. */ - kafka_producer_worker(const std::string &brokers, const std::string &topics, int n_partition = 0); + explicit kafka_producer_worker(const std::string &brokers, const std::string &topics, int n_partition = 0); + /** + * @brief Construct a new kafka producer worker object + * + * @param broker_str network address of kafka broker. + */ + explicit kafka_producer_worker(const std::string &brokers); /** * @brief Initialize kafka_producer_worker. This method must be called before send! * @@ -61,12 +67,29 @@ namespace tmx::utils * @return false if unsuccessful. */ virtual bool init(); + /** + * @brief Initialize kafka topic. + * @return true if successful. + * @return false if unsuccessful. + */ + virtual bool init_topic(); + /** + * @brief Initialize kafka producer. + * @return true if successful. + * @return false if unsuccessful. + */ + virtual bool init_producer(); /** * @brief Produce to topic. Will result in segmentation fault if init() is not called on producer first. - * * @param msg message to produce. */ virtual void send(const std::string &msg); + /** + * @brief Produce to a specific topic. Will result in segmentation fault if init() is not called on producer first. + * @param msg message to produce. + * @param topic_name topic to send the message to. + */ + virtual void send(const std::string& message, const std::string& topic_name ) const; /** * @brief Is kafka_producer_worker still running? * diff --git a/src/tmx/TmxUtils/test/test_kafka_producer_worker.cpp b/src/tmx/TmxUtils/test/test_kafka_producer_worker.cpp index 28aaf6159..ca04a1e19 100644 --- a/src/tmx/TmxUtils/test/test_kafka_producer_worker.cpp +++ b/src/tmx/TmxUtils/test/test_kafka_producer_worker.cpp @@ -15,3 +15,18 @@ TEST(test_kafka_producer_worker, create_producer) worker->send(msg); worker->stop(); } + +TEST(test_kafka_producer_worker, create_producer_no_topic) +{ + std::string broker_str = "localhost:9092"; + std::string topic = "test"; + auto client = std::make_shared(); + std::shared_ptr worker; + worker = client->create_producer(broker_str); + worker->init(); + worker->printCurrConf(); + std::string msg = "test message"; + // // Run this unit test without launching kafka broker will throw connection refused error + worker->send(msg, topic); + worker->stop(); +} \ No newline at end of file diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index 08bc96d8a..71703bd59 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -20,7 +20,6 @@ namespace CARMAStreetsPlugin { */ CARMAStreetsPlugin::CARMAStreetsPlugin(string name) : PluginClient(name) { - AddMessageFilter < BsmMessage > (this, &CARMAStreetsPlugin::HandleBasicSafetyMessage); AddMessageFilter < tsm3Message > (this, &CARMAStreetsPlugin::HandleMobilityOperationMessage); AddMessageFilter < tsm2Message > (this, &CARMAStreetsPlugin::HandleMobilityPathMessage); @@ -32,6 +31,10 @@ CARMAStreetsPlugin::CARMAStreetsPlugin(string name) : } CARMAStreetsPlugin::~CARMAStreetsPlugin() { + //Todo: It does not seem the desctructor is called. + _spat_kafka_consumer_ptr->stop(); + _scheduing_plan_kafka_consumer_ptr->stop(); + _ssm_kafka_consumer_ptr->stop(); } void CARMAStreetsPlugin::UpdateConfigSettings() { @@ -64,96 +67,41 @@ void CARMAStreetsPlugin::UpdateConfigSettings() { getline( ss, substring, ','); _strategies.push_back( substring); } +} + +void CARMAStreetsPlugin::InitKafkaConsumerProducers() +{ std::string kafkaConnectString = _kafkaBrokerIp + ':' + _kafkaBrokerPort; std::string error_string; kafkaConnectString = _kafkaBrokerIp + ':' + _kafkaBrokerPort; - kafka_conf = RdKafka::Conf::create(RdKafka::Conf::CONF_GLOBAL); - kafka_conf_sp_consumer = RdKafka::Conf::create(RdKafka::Conf::CONF_GLOBAL); - kafka_conf_spat_consumer = RdKafka::Conf::create(RdKafka::Conf::CONF_GLOBAL); - kafka_conf_ssm_consumer = RdKafka::Conf::create(RdKafka::Conf::CONF_GLOBAL); - - - PLOG(logDEBUG) <<"Attempting to connect to " << kafkaConnectString; - if ((kafka_conf->set("bootstrap.servers", kafkaConnectString, error_string) != RdKafka::Conf::CONF_OK)) { - PLOG(logERROR) <<"Setting kafka config options failed with error:" << error_string << "\n" <<"Exiting with exit code 1"; - exit(1); - } else { - PLOG(logDEBUG) <<"Kafka config options set successfully"; - } - - kafka_producer = RdKafka::Producer::create(kafka_conf, error_string); - if (!kafka_producer) { - PLOG(logERROR) <<"Creating kafka producer failed with error:" << error_string << "\n" <<"Exiting with exit code 1"; - exit(1); - } - PLOG(logDEBUG) <<"Kafka producer created"; - - if (kafka_conf_sp_consumer->set("bootstrap.servers", kafkaConnectString, error_string) != RdKafka::Conf::CONF_OK - || (kafka_conf_sp_consumer->set("group.id", _subscribeToSchedulingPlanConsumerGroupId, error_string) != RdKafka::Conf::CONF_OK) - || (kafka_conf_spat_consumer->set("bootstrap.servers", kafkaConnectString, error_string) != RdKafka::Conf::CONF_OK) - || (kafka_conf_spat_consumer->set("group.id", _subscribeToSpatConsumerGroupId, error_string) != RdKafka::Conf::CONF_OK) - || (kafka_conf_ssm_consumer->set("bootstrap.servers", kafkaConnectString, error_string) != RdKafka::Conf::CONF_OK) - || (kafka_conf_ssm_consumer->set("group.id", _subscribeToSSMConsumerGroupId, error_string) != RdKafka::Conf::CONF_OK) - ) { - PLOG(logERROR) <<"Setting kafka config group.id options failed with error:" << error_string << "\n" <<"Exiting with exit code 1"; - exit(1); - } else { - PLOG(logDEBUG) <<"Kafka config group.id options set successfully"; - } - kafka_conf_sp_consumer->set("enable.partition.eof", "true", error_string); - kafka_conf_spat_consumer->set("enable.partition.eof", "true", error_string); - kafka_conf_ssm_consumer->set("enable.partition.eof", "true", error_string); - - _scheduing_plan_kafka_consumer = RdKafka::KafkaConsumer::create(kafka_conf_sp_consumer, error_string); - _spat_kafka_consumer = RdKafka::KafkaConsumer::create(kafka_conf_spat_consumer, error_string); - _ssm_kafka_consumer = RdKafka::KafkaConsumer::create(kafka_conf_ssm_consumer, error_string); + kafka_client client; - if ( !_scheduing_plan_kafka_consumer || !_spat_kafka_consumer || !_ssm_kafka_consumer) { - PLOG(logERROR) << "Failed to create Kafka consumers: " << error_string << std::endl; - exit(1); - } - PLOG(logDEBUG) << "Created consumer " << _scheduing_plan_kafka_consumer->name() << std::endl; - PLOG(logDEBUG) << "Created consumer " << _spat_kafka_consumer->name() << std::endl; - PLOG(logDEBUG) << "Created consumer " << _ssm_kafka_consumer->name() << std::endl; - - //create kafka topics - RdKafka::Conf *tconf_spat = RdKafka::Conf::create(RdKafka::Conf::CONF_TOPIC); - RdKafka::Conf *tconf_sp = RdKafka::Conf::create(RdKafka::Conf::CONF_TOPIC); - RdKafka::Conf *tconf_ssm = RdKafka::Conf::create(RdKafka::Conf::CONF_TOPIC); - if(!tconf_spat && !tconf_sp) + //Producer + _kafka_producer_ptr = client.create_producer(kafkaConnectString); + if(!_kafka_producer_ptr->init_producer()) { - PLOG(logERROR) << "RDKafka create topic conf failed "; return; - } - - _scheduing_plan_topic = RdKafka::Topic::create(_scheduing_plan_kafka_consumer,_subscribeToSchedulingPlanTopic,tconf_sp,error_string); - if(!_scheduing_plan_topic) - { - PLOG(logERROR) << "RDKafka create scheduing plan topic failed:" << error_string; - return ; } - _spat_topic = RdKafka::Topic::create(_spat_kafka_consumer,_subscribeToSpatTopic,tconf_spat,error_string); - if(!_spat_topic) + //Consumers + _spat_kafka_consumer_ptr = client.create_consumer(kafkaConnectString, _subscribeToSpatTopic,_subscribeToSpatConsumerGroupId); + _scheduing_plan_kafka_consumer_ptr = client.create_consumer(kafkaConnectString, _subscribeToSchedulingPlanTopic,_subscribeToSchedulingPlanConsumerGroupId); + _ssm_kafka_consumer_ptr = client.create_consumer(kafkaConnectString, _subscribeToSsmTopic,_subscribeToSSMConsumerGroupId); + if(!_scheduing_plan_kafka_consumer_ptr || !_spat_kafka_consumer_ptr || !_ssm_kafka_consumer_ptr) { - PLOG(logERROR) << "RDKafka create SPAT topic failed:" << error_string; - return ; + PLOG(logERROR) <<"Failed to create Kafka consumers."; + return; } - - _ssm_topic = RdKafka::Topic::create(_ssm_kafka_consumer,_subscribeToSsmTopic,tconf_ssm,error_string); - if(!_ssm_topic) + PLOG(logDEBUG) <<"Kafka consumers created"; + if(!_spat_kafka_consumer_ptr->init() || !_scheduing_plan_kafka_consumer_ptr->init() || !_ssm_kafka_consumer_ptr->init()) { - PLOG(logERROR) << "RDKafka create SSM topic failed:" << error_string; - return ; + PLOG(logERROR) <<"Kafka consumers init() failed!"; + return; } - delete tconf_sp; - delete tconf_spat; - delete tconf_ssm; - boost::thread thread_schpl(&CARMAStreetsPlugin::SubscribeSchedulingPlanKafkaTopic, this); boost::thread thread_spat(&CARMAStreetsPlugin::SubscribeSpatKafkaTopic, this); - boost::thread thread_ssm(&CARMAStreetsPlugin::SubscribeSSMKafkaTopic, this); + boost::thread thread_ssm(&CARMAStreetsPlugin::SubscribeSSMKafkaTopic, this); } void CARMAStreetsPlugin::OnConfigChanged(const char *key, const char *value) { @@ -167,8 +115,7 @@ void CARMAStreetsPlugin::HandleMobilityOperationMessage(tsm3Message &msg, routea auto mobilityOperation = msg.get_j2735_data(); PLOG(logDEBUG) << "Body OperationParams : " << mobilityOperation->body.operationParams.buf << "\n" << "Body Strategy : " << mobilityOperation->body.strategy.buf<< "\n" - <<"Queueing kafka message:topic:" << _transmitMobilityOperationTopic << " " - << kafka_producer->outq_len() <<"messages already in queue"; + <<"Queueing kafka message:topic:" << _transmitMobilityOperationTopic; std::stringstream strat; std::stringstream payload; @@ -504,34 +451,7 @@ void CARMAStreetsPlugin::HandleMapMessage(MapDataMessage &msg, routeable_message void CARMAStreetsPlugin::produce_kafka_msg(const string& message, const string& topic_name) const { - bool retry = true; - while (retry) - { - RdKafka::ErrorCode produce_error = kafka_producer->produce(topic_name, - RdKafka::Topic::PARTITION_UA, - RdKafka::Producer::RK_MSG_COPY, - const_cast(message.c_str()), - message.size(), - nullptr, 0, 0, nullptr); - - if (produce_error == RdKafka::ERR_NO_ERROR) { - PLOG(logDEBUG) <<"Queued message:" << message; - retry = false; - } - else - { - PLOG(logERROR) <<"Failed to queue message:" << message <<" with error:" << RdKafka::err2str(produce_error); - if (produce_error == RdKafka::ERR__QUEUE_FULL) { - PLOG(logERROR) <<"Message queue full...retrying..."; - kafka_producer->poll(500); /* ms */ - retry = true; - } - else { - PLOG(logERROR) <<"Unhandled error in queue_kafka_message:" << RdKafka::err2str(produce_error); - retry = false; - } - } - } + _kafka_producer_ptr->send(message, topic_name); } void CARMAStreetsPlugin::OnStateChange(IvpPluginState state) { @@ -539,6 +459,7 @@ void CARMAStreetsPlugin::OnStateChange(IvpPluginState state) { if (state == IvpPluginState_registered) { UpdateConfigSettings(); + InitKafkaConsumerProducers(); } } @@ -547,67 +468,54 @@ void CARMAStreetsPlugin::SubscribeSchedulingPlanKafkaTopic() if(_subscribeToSchedulingPlanTopic.length() > 0) { PLOG(logDEBUG) << "SubscribeSchedulingPlanKafkaTopics:" <<_subscribeToSchedulingPlanTopic << std::endl; - std::vector topics; - topics.emplace_back(_subscribeToSchedulingPlanTopic); - - RdKafka::ErrorCode err = _scheduing_plan_kafka_consumer->subscribe(topics); - if (err) - { - PLOG(logERROR) << "Failed to subscribe to " << topics.size() << " topics: " << RdKafka::err2str(err) << std::endl; - return; - } + _scheduing_plan_kafka_consumer_ptr->subscribe(); - while (true) + while (_scheduing_plan_kafka_consumer_ptr->is_running()) { - auto msg = _scheduing_plan_kafka_consumer->consume( 500 ); - if( msg->err() == RdKafka::ERR_NO_ERROR ) + std::string payload_str = _scheduing_plan_kafka_consumer_ptr->consume(500); + if(payload_str.length() > 0) { - auto payload_str = static_cast( msg->payload() ); - if(msg->len() > 0) - { - PLOG(logDEBUG) << "consumed message payload: " << payload_str <(Key_ScheduleMessageSkipped, ++_scheduleMessageSkipped); - continue; - } + PLOG(logDEBUG) << "consumed message payload: " << payload_str <(Key_ScheduleMessageSkipped, ++_scheduleMessageSkipped); + continue; + } - Json::Value metadata = payload_root["metadata"]; - Json::Value payload_json_array = payload_root["payload"]; - - for ( int index = 0; index < payload_json_array.size(); ++index ) + Json::Value metadata = payload_root["metadata"]; + Json::Value payload_json_array = payload_root["payload"]; + + for ( int index = 0; index < payload_json_array.size(); ++index ) + { + PLOG(logDEBUG) << payload_json_array[index] << std::endl; + Json::Value payload_json = payload_json_array[index]; + tsm3EncodedMessage tsm3EncodedMsgs; + if( getEncodedtsm3 (&tsm3EncodedMsgs, metadata, payload_json) ) { - PLOG(logDEBUG) << payload_json_array[index] << std::endl; - Json::Value payload_json = payload_json_array[index]; - tsm3EncodedMessage tsm3EncodedMsgs; - if( getEncodedtsm3 (&tsm3EncodedMsgs, metadata, payload_json) ) - { - tsm3EncodedMsgs.set_flags( IvpMsgFlags_RouteDSRC ); - tsm3EncodedMsgs.addDsrcMetadata(0xBFEE ); - PLOG(logDEBUG) << "tsm3EncodedMsgs: " << tsm3EncodedMsgs; - BroadcastMessage(static_cast( tsm3EncodedMsgs )); - } + tsm3EncodedMsgs.set_flags( IvpMsgFlags_RouteDSRC ); + tsm3EncodedMsgs.addDsrcMetadata(0xBFEE ); + PLOG(logDEBUG) << "tsm3EncodedMsgs: " << tsm3EncodedMsgs; + BroadcastMessage(static_cast( tsm3EncodedMsgs )); } - //Empty payload - if(payload_json_array.empty()) - { - Json::Value payload_json = {}; - tsm3EncodedMessage tsm3EncodedMsgs; - if( getEncodedtsm3 (&tsm3EncodedMsgs, metadata, payload_json) ) - { - tsm3EncodedMsgs.set_flags( IvpMsgFlags_RouteDSRC ); - tsm3EncodedMsgs.addDsrcMetadata(0xBFEE); - PLOG(logDEBUG) << "tsm3EncodedMsgs: " << tsm3EncodedMsgs; - BroadcastMessage(static_cast( tsm3EncodedMsgs )); - } - } } + //Empty payload + if(payload_json_array.empty()) + { + Json::Value payload_json = {}; + tsm3EncodedMessage tsm3EncodedMsgs; + if( getEncodedtsm3 (&tsm3EncodedMsgs, metadata, payload_json) ) + { + tsm3EncodedMsgs.set_flags( IvpMsgFlags_RouteDSRC ); + tsm3EncodedMsgs.addDsrcMetadata(0xBFEE); + PLOG(logDEBUG) << "tsm3EncodedMsgs: " << tsm3EncodedMsgs; + BroadcastMessage(static_cast( tsm3EncodedMsgs )); + } + } } - delete msg; } } @@ -617,64 +525,51 @@ void CARMAStreetsPlugin::SubscribeSpatKafkaTopic(){ if(_subscribeToSpatTopic.length() > 0) { PLOG(logDEBUG) << "SubscribeSpatKafkaTopics:" <<_subscribeToSpatTopic << std::endl; - std::vector topics; - topics.emplace_back(_subscribeToSpatTopic); - - RdKafka::ErrorCode err = _spat_kafka_consumer->subscribe(topics); - if (err) - { - PLOG(logERROR) << "Failed to subscribe to " << topics.size() << " topics: " << RdKafka::err2str(err) << std::endl; - return; - } - //Initialize Json to J2735 Spat convertor + _spat_kafka_consumer_ptr->subscribe(); + //Initialize Json to J2735 Spat convertor JsonToJ2735SpatConverter spat_convertor; - while (true) + while (_spat_kafka_consumer_ptr->is_running()) { - auto msg = _spat_kafka_consumer->consume( 500 ); - if( msg->err() == RdKafka::ERR_NO_ERROR ) + std::string payload_str = _spat_kafka_consumer_ptr->consume(500); + if(payload_str.length() > 0) { - auto payload_str = static_cast( msg->payload() ); - if(msg->len() > 0) + PLOG(logDEBUG) << "consumed message payload: " << payload_str <(Key_SPATMessageSkipped, ++_spatMessageSkipped); + continue; + } + //Convert the SPAT JSON string into J2735 SPAT message and encode it. + auto spat_ptr = std::make_shared(); + spat_convertor.convertJson2Spat(payload_root, spat_ptr.get()); + tmx::messages::SpatEncodedMessage spatEncodedMsg; + try { - PLOG(logDEBUG) << "consumed message payload: " << payload_str <(Key_SPATMessageSkipped, ++_spatMessageSkipped); - continue; - } - //Convert the SPAT JSON string into J2735 SPAT message and encode it. - auto spat_ptr = std::make_shared(); - spat_convertor.convertJson2Spat(payload_root, spat_ptr.get()); - tmx::messages::SpatEncodedMessage spatEncodedMsg; - try - { - spat_convertor.encodeSpat(spat_ptr, spatEncodedMsg); - } - catch (TmxException &ex) - { - // Skip messages that fail to encode. - PLOG(logERROR) << "Failed to encoded SPAT message : \n" << payload_str << std::endl << "Exception encountered: " - << ex.what() << std::endl; - ASN_STRUCT_FREE_CONTENTS_ONLY(asn_DEF_SPAT, spat_ptr.get()); - SetStatus(Key_SPATMessageSkipped, ++_spatMessageSkipped); - - continue; - } - + spat_convertor.encodeSpat(spat_ptr, spatEncodedMsg); + } + catch (TmxException &ex) + { + // Skip messages that fail to encode. + PLOG(logERROR) << "Failed to encoded SPAT message : \n" << payload_str << std::endl << "Exception encountered: " + << ex.what() << std::endl; ASN_STRUCT_FREE_CONTENTS_ONLY(asn_DEF_SPAT, spat_ptr.get()); - PLOG(logDEBUG) << "SpatEncodedMessage: " << spatEncodedMsg; + SetStatus(Key_SPATMessageSkipped, ++_spatMessageSkipped); - //Broadcast the encoded SPAT message - spatEncodedMsg.set_flags(IvpMsgFlags_RouteDSRC); - spatEncodedMsg.addDsrcMetadata(0x8002); - BroadcastMessage(static_cast(spatEncodedMsg)); + continue; } + + ASN_STRUCT_FREE_CONTENTS_ONLY(asn_DEF_SPAT, spat_ptr.get()); + PLOG(logDEBUG) << "SpatEncodedMessage: " << spatEncodedMsg; + + //Broadcast the encoded SPAT message + spatEncodedMsg.set_flags(IvpMsgFlags_RouteDSRC); + spatEncodedMsg.addDsrcMetadata(0x8002); + BroadcastMessage(static_cast(spatEncodedMsg)); } - delete msg; } } } @@ -684,62 +579,49 @@ void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){ if(_subscribeToSsmTopic.length() > 0) { PLOG(logDEBUG) << "SubscribeSSMKafkaTopics:" <<_subscribeToSsmTopic << std::endl; - std::vector topics; - topics.emplace_back(_subscribeToSsmTopic); - - RdKafka::ErrorCode err = _ssm_kafka_consumer->subscribe(topics); - if (err) - { - PLOG(logERROR) << "Failed to subscribe to " << topics.size() << " topics: " << RdKafka::err2str(err) << std::endl; - return; - } + _ssm_kafka_consumer_ptr->subscribe(); //Initialize Json to J2735 SSM convertor JsonToJ2735SSMConverter ssm_convertor; - while (true) + while (_ssm_kafka_consumer_ptr->is_running()) { - auto msg = _ssm_kafka_consumer->consume( 500 ); - if( msg->err() == RdKafka::ERR_NO_ERROR ) + std::string payload_str = _ssm_kafka_consumer_ptr->consume(500); + if(payload_str.length() > 0) { - auto payload_str = static_cast( msg->payload() ); - if(msg->len() > 0) + PLOG(logDEBUG) << "consumed message payload: " << payload_str <(Key_SSMMessageSkipped, ++_ssmMessageSkipped); + continue; + } + //Convert the SSM JSON string into J2735 SSM message and encode it. + auto ssm_ptr = std::make_shared(); + ssm_convertor.toJ2735SSM(ssmDoc, ssm_ptr); + tmx::messages::SsmEncodedMessage ssmEncodedMsg; + try { - PLOG(logDEBUG) << "consumed message payload: " << payload_str <(Key_SSMMessageSkipped, ++_ssmMessageSkipped); - continue; - } - //Convert the SSM JSON string into J2735 SSM message and encode it. - auto ssm_ptr = std::make_shared(); - ssm_convertor.toJ2735SSM(ssmDoc, ssm_ptr); - tmx::messages::SsmEncodedMessage ssmEncodedMsg; - try - { - ssm_convertor.encodeSSM(ssm_ptr, ssmEncodedMsg); - } - catch (TmxException &ex) - { - // Skip messages that fail to encode. - PLOG(logERROR) << "Failed to encoded SSM message : \n" << payload_str << std::endl << "Exception encountered: " - << ex.what() << std::endl; - ASN_STRUCT_FREE_CONTENTS_ONLY(asn_DEF_SignalStatusMessage, ssm_ptr.get()); - SetStatus(Key_SSMMessageSkipped, ++_ssmMessageSkipped); - continue; - } - + ssm_convertor.encodeSSM(ssm_ptr, ssmEncodedMsg); + } + catch (TmxException &ex) + { + // Skip messages that fail to encode. + PLOG(logERROR) << "Failed to encoded SSM message : \n" << payload_str << std::endl << "Exception encountered: " + << ex.what() << std::endl; ASN_STRUCT_FREE_CONTENTS_ONLY(asn_DEF_SignalStatusMessage, ssm_ptr.get()); - PLOG(logDEBUG) << "ssmEncodedMsg: " << ssmEncodedMsg; - - //Broadcast the encoded SSM message - ssmEncodedMsg.set_flags(IvpMsgFlags_RouteDSRC); - ssmEncodedMsg.addDsrcMetadata(0x8002); - BroadcastMessage(static_cast(ssmEncodedMsg)); + SetStatus(Key_SSMMessageSkipped, ++_ssmMessageSkipped); + continue; } + + ASN_STRUCT_FREE_CONTENTS_ONLY(asn_DEF_SignalStatusMessage, ssm_ptr.get()); + PLOG(logDEBUG) << "ssmEncodedMsg: " << ssmEncodedMsg; + + //Broadcast the encoded SSM message + ssmEncodedMsg.set_flags(IvpMsgFlags_RouteDSRC); + ssmEncodedMsg.addDsrcMetadata(0x8002); + BroadcastMessage(static_cast(ssmEncodedMsg)); } - delete msg; } } @@ -870,7 +752,6 @@ int CARMAStreetsPlugin::Main() { uint64_t lastSendTime = 0; while (_plugin->state != IvpPluginState_error) { - usleep(100000); //sleep for microseconds set from config. diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h index 566aaf35c..35df38132 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h @@ -16,11 +16,14 @@ #include #include "J2735MapToJsonConverter.h" #include "JsonToJ2735SpatConverter.h" -#include "J2735ToSRMJsonConverter.h" +#include "J2735ToSRMJsonConverter.h" +#include +#include #include "JsonToJ2735SSMConverter.h" + using namespace std; using namespace tmx; using namespace tmx::utils; @@ -75,6 +78,10 @@ class CARMAStreetsPlugin: public PluginClient { * @param topic_name The name of the topic */ void produce_kafka_msg(const string &msg, const string &topic_name) const; + /** + * @brief Initialize Kafka Producers and Consumers + */ + void InitKafkaConsumerProducers(); private: @@ -93,17 +100,10 @@ class CARMAStreetsPlugin: public PluginClient { std::string _transmitSRMTopic; std::string _kafkaBrokerIp; std::string _kafkaBrokerPort; - RdKafka::Conf *kafka_conf; - RdKafka::Conf *kafka_conf_spat_consumer; - RdKafka::Conf *kafka_conf_sp_consumer; - RdKafka::Conf *kafka_conf_ssm_consumer; - RdKafka::Producer *kafka_producer; - RdKafka::KafkaConsumer *_scheduing_plan_kafka_consumer; - RdKafka::KafkaConsumer *_spat_kafka_consumer; - RdKafka::KafkaConsumer *_ssm_kafka_consumer; - RdKafka::Topic *_scheduing_plan_topic; - RdKafka::Topic *_spat_topic; - RdKafka::Topic *_ssm_topic; + std::shared_ptr _kafka_producer_ptr; + std::shared_ptr _spat_kafka_consumer_ptr; + std::shared_ptr _scheduing_plan_kafka_consumer_ptr; + std::shared_ptr _ssm_kafka_consumer_ptr; std::vector _strategies; tmx::messages::tsm3Message *_tsm3Message{NULL}; std::mutex data_lock; @@ -194,4 +194,4 @@ class CARMAStreetsPlugin: public PluginClient { std::mutex _cfgLock; } -#endif \ No newline at end of file +#endif From 639ad080a736aaa0151593ff1a0298264b806e9f Mon Sep 17 00:00:00 2001 From: Will Martin Date: Mon, 10 Jul 2023 14:21:08 -0400 Subject: [PATCH 06/28] modified: initialization.sh (#548) # PR Details ## Description Changes made to fix localhost address to reach V2X Hub Web UI and allow for script to be run multiple times without changing mysql password files. ## Related Issue ## Motivation and Context ## How Has This Been Tested? ## Types of changes - [ ] 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. - [ ] 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. --- configuration/amd64/initialization.sh | 38 +++++++++++++++++--------- configuration/arm64/initialization.sh | 39 ++++++++++++++++++--------- 2 files changed, 51 insertions(+), 26 deletions(-) diff --git a/configuration/amd64/initialization.sh b/configuration/amd64/initialization.sh index 03660e9d5..eea35fc48 100755 --- a/configuration/amd64/initialization.sh +++ b/configuration/amd64/initialization.sh @@ -1,4 +1,6 @@ #!/bin/bash +directory=$(pwd) +mysqlDir="$directory/mysql" # update and upgrade commands to update linux OS sudo apt update -y && sudo apt upgrade -y @@ -11,21 +13,31 @@ sudo apt install curl -y #Curl for downloading files over i curl -L https://raw.githubusercontent.com/usdot-fhwa-stol/carma-platform/develop/engineering_tools/install-docker.sh | bash #make passwords for mysql -mkdir secrets && cd secrets +mkdir -p secrets && cd secrets #creates password files where user inputs password -read -p "enter password for the mysql_root_password: " sql_root_pass -echo "$sql_root_pass" > sql_root_pass.txt - -read -p "enter password for mysql_password: " sql_pass -echo "$sql_pass" > sql_pass.txt - -#remove endline characters from password files -tr -d '\n' mysql_root_password.txt && tr -d '\n' mysql_password.txt -rm sql_root_pass.txt && rm sql_pass.txt +FILE1=mysql_root_password.txt +FILE2=mysql_password.txt +if test -f "$FILE1"; then + echo "$FILE1 exists." +else + read -p "enter password for the mysql_root_password: " sql_root_pass + echo "$sql_root_pass" > sql_root_pass.txt + #remove endline characters from password files + tr -d '\n' mysql_root_password.txt && rm sql_root_pass.txt +fi + +if test -f "$FILE2"; then + echo "$FILE2 exists." +else + read -p "enter password for mysql_password: " sql_pass + echo "$sql_pass" > sql_pass.txt + #remove endline characters from password files + tr -d '\n' mysql_password.txt && rm sql_pass.txt +fi #AMD64 initialzation -cd .. +cd $directory sudo apt-get -y remove docker docker-engine docker.io containerd runc sudo apt-get -y install apt-transport-https ca-certificates curl gnupg-agent software-properties-common curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo apt-key add - @@ -38,8 +50,8 @@ sudo apt update -y && sudo apt upgrade -y sudo docker-compose up -d #create v2xhub user -cd mysql +cd $mysqlDir ./add_v2xhub_user.bash -chromium-browser "https://127.0.0.1" > /dev/null 2>&1 & +chromium-browser "http://127.0.0.1" > /dev/null 2>&1 & chromium-browser "https://127.0.0.1:19760" > /dev/null 2>&1 & diff --git a/configuration/arm64/initialization.sh b/configuration/arm64/initialization.sh index 2430a1641..7769004df 100755 --- a/configuration/arm64/initialization.sh +++ b/configuration/arm64/initialization.sh @@ -1,4 +1,7 @@ #!/bin/bash +directory=$(pwd) +mysqlDir="$directory/mysql" + # update and upgrade commands to update linux OS sudo apt update -y && sudo apt upgrade -y @@ -10,21 +13,31 @@ sudo apt install curl -y #Curl for downloading files over i curl -L https://raw.githubusercontent.com/usdot-fhwa-stol/carma-platform/develop/engineering_tools/install-docker.sh | bash #make passwords for mysql -mkdir secrets && cd secrets +mkdir -p secrets && cd secrets #creates password files where user inputs password -read -p "enter password for the mysql_root_password: " sql_root_pass -echo "$sql_root_pass" > sql_root_pass.txt - -read -p "enter password for mysql_password: " sql_pass -echo "$sql_pass" > sql_pass.txt - -#remove endline characters from password files -tr -d '\n' mysql_root_password.txt && tr -d '\n' mysql_password.txt -rm sql_root_pass.txt && rm sql_pass.txt +FILE1=mysql_root_password.txt +FILE2=mysql_password.txt +if test -f "$FILE1"; then + echo "$FILE1 exists." +else + read -p "enter password for the mysql_root_password: " sql_root_pass + echo "$sql_root_pass" > sql_root_pass.txt + #remove endline characters from password files + tr -d '\n' mysql_root_password.txt && rm sql_root_pass.txt +fi + +if test -f "$FILE2"; then + echo "$FILE2 exists." +else + read -p "enter password for mysql_password: " sql_pass + echo "$sql_pass" > sql_pass.txt + #remove endline characters from password files + tr -d '\n' mysql_password.txt && rm sql_pass.txt +fi #ARM initialization -cd .. +cd $directory sudo apt-get -y remove docker docker-engine docker.io containerd runc sudo apt-get -y install apt-transport-https ca-certificates curl gnupg-agent software-properties-common OS=$(lsb_release -i | awk 'FS=":" {print $3;}' | awk '{print tolower($0)}') @@ -38,8 +51,8 @@ sudo apt update -y && sudo apt upgrade -y sudo docker-compose up -d #create v2xhub user -cd mysql +cd $mysqlDir ./add_v2xhub_user.bash -chromium-browser "https://127.0.0.1" > /dev/null 2>&1 & +chromium-browser "http://127.0.0.1" > /dev/null 2>&1 & chromium-browser "https://127.0.0.1:19760" > /dev/null 2>&1 & From e81bdc02dcd7ff7770a8bcc57ade8c6b21806bf6 Mon Sep 17 00:00:00 2001 From: Saikrishna Bairamoni <84093461+SaikrishnaBairamoni@users.noreply.github.com> Date: Wed, 19 Jul 2023 00:32:43 -0400 Subject: [PATCH 07/28] update dockercompose files to point latest images and syc with 7.5.1 release changes --- configuration/amd64/docker-compose.yml | 6 +++--- configuration/arm64/docker-compose.yml | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/configuration/amd64/docker-compose.yml b/configuration/amd64/docker-compose.yml index 201463578..6b5188297 100755 --- a/configuration/amd64/docker-compose.yml +++ b/configuration/amd64/docker-compose.yml @@ -20,7 +20,7 @@ services: - mysql-datavolume:/var/lib/mysql php: - image: usdotfhwaops/php:7.5.1 + image: usdotfhwaops/php:latest container_name: php network_mode: host depends_on: @@ -30,7 +30,7 @@ services: tty: true v2xhub: - image: usdotfhwaops/v2xhubamd:7.5.1 + image: usdotfhwaops/v2xhubamd:latest container_name: v2xhub network_mode: host restart: always @@ -44,7 +44,7 @@ services: - ./logs:/var/log/tmx - ./MAP:/var/www/plugins/MAP port_drayage_webservice: - image: usdotfhwaops/port-drayage-webservice:7.5.1 + image: usdotfhwaops/port-drayage-webservice:latest container_name: port_drayage_webservice network_mode: host secrets: diff --git a/configuration/arm64/docker-compose.yml b/configuration/arm64/docker-compose.yml index 4974d26ee..a32c31fab 100644 --- a/configuration/arm64/docker-compose.yml +++ b/configuration/arm64/docker-compose.yml @@ -20,7 +20,7 @@ services: - mysql-datavolume:/var/lib/mysql php: - image: usdotfhwaops/php_arm:7.5.1 + image: usdotfhwaops/php_arm:latest container_name: php network_mode: host depends_on: @@ -30,7 +30,7 @@ services: tty: true v2xhub: - image: usdotfhwaops/v2xhubarm:7.5.1 + image: usdotfhwaops/v2xhubarm:latest container_name: v2xhub network_mode: host restart: always @@ -44,7 +44,7 @@ services: - ./logs:/var/log/tmx - ./MAP:/var/www/plugins/MAP port_drayage_webservice: - image: usdotfhwaops/port-drayage-webservice_arm:7.5.1 + image: usdotfhwaops/port-drayage-webservice_arm:latest container_name: port_drayage_webservice network_mode: host secrets: From 77127e22f5a87322a64103fb8a82192ccd94f0e4 Mon Sep 17 00:00:00 2001 From: paulbourelly999 <77466294+paulbourelly999@users.noreply.github.com> Date: Thu, 20 Jul 2023 16:07:13 -0400 Subject: [PATCH 08/28] =?UTF-8?q?Issue-549:=20Moved=20Kafka=20time=20produ?= =?UTF-8?q?cer=20from=20CDASimAdapter=20to=20CARMA-Stre=E2=80=A6=20(#550)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit …ets plugin + Update CARMA-Streets Plugin to be simulation time aware # PR Details ## Description Move Kafka time producer for CARMA-Streets time synchronization to CARMA-Streets plugin. This removes unnecessary dependency on kafka for CDASimAdapter and also makes CARMA-Streets Plugin contain entire interface to CARMA-Streets including simulation. CARMA-Streets Plugin now extends our simulation time aware PluginClient which can be further extended to listen for simulated messages. ## Related Issue #549 ## Motivation and Context ## How Has This Been Tested? Locally integration testing with python time step script to send udp time step messages. ## Types of changes - [ ] Defect fix (non-breaking change that fixes an issue) - [x] 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 - [x] My change requires a change to the documentation. - [x] 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. --- .devcontainer/docker-compose-vscode.yml | 1 - .../TmxUtils/src/PluginClientClockAware.cpp | 11 +++++--- src/tmx/TmxUtils/src/PluginClientClockAware.h | 7 ++++- .../src/kafka/kafka_consumer_worker.cpp | 9 +++++-- .../src/kafka/kafka_consumer_worker.h | 20 +++++++++----- .../src/kafka/kafka_producer_worker.cpp | 13 ++++++--- .../src/kafka/kafka_producer_worker.h | 21 ++++++++++----- .../src/kafka/mock_kafka_consumer_worker.h | 1 - .../src/kafka/mock_kafka_producer_worker.h | 1 - .../TmxUtils/test/KafkaTestEnvironment.cpp | 27 +++++++++++++++++++ src/tmx/TmxUtils/test/Main.cpp | 2 ++ .../test/test_kafka_consumer_worker.cpp | 1 + .../test/test_kafka_producer_worker.cpp | 2 -- .../src/CARMAStreetsPlugin.cpp | 18 ++++++------- .../src/CARMAStreetsPlugin.h | 10 +++++-- src/v2i-hub/CDASimAdapter/CMakeLists.txt | 6 ++--- .../CDASimAdapter/src/CDASimAdapter.cpp | 26 ------------------ .../src/include/CDASimAdapter.hpp | 14 +--------- .../src/include/CDASimConnection.hpp | 1 - .../test/TestCARMASimulationConnection.cpp | 3 +-- src/v2i-hub/MapPlugin/src/MapPlugin.cpp | 1 + src/v2i-hub/SpatPlugin/src/SpatPlugin.h | 1 + 22 files changed, 113 insertions(+), 83 deletions(-) create mode 100644 src/tmx/TmxUtils/test/KafkaTestEnvironment.cpp diff --git a/.devcontainer/docker-compose-vscode.yml b/.devcontainer/docker-compose-vscode.yml index a34dd25c9..89b1efd18 100755 --- a/.devcontainer/docker-compose-vscode.yml +++ b/.devcontainer/docker-compose-vscode.yml @@ -24,7 +24,6 @@ services: - SIM_V2X_PORT=5757 - V2X_PORT=8686 - INFRASTRUCTURE_ID=1 - - KAFKA_BROKER_ADDRESS=127.0.0.1:9092 secrets: - mysql_password diff --git a/src/tmx/TmxUtils/src/PluginClientClockAware.cpp b/src/tmx/TmxUtils/src/PluginClientClockAware.cpp index c8317c678..4cf87fe6b 100644 --- a/src/tmx/TmxUtils/src/PluginClientClockAware.cpp +++ b/src/tmx/TmxUtils/src/PluginClientClockAware.cpp @@ -8,11 +8,11 @@ namespace tmx::utils { : PluginClient(name) { // check for simulation mode enabled by environment variable - bool simulationMode = sim::is_simulation_mode(); + _simulation_mode = sim::is_simulation_mode(); using namespace fwha_stol::lib::time; - clock = std::make_shared(simulationMode); - if (simulationMode) { + clock = std::make_shared(_simulation_mode); + if (_simulation_mode) { AddMessageFilter(this, &PluginClientClockAware::HandleTimeSyncMessage); } @@ -25,7 +25,6 @@ namespace tmx::utils { this->getClock()->update( msg.get_timestep() ); if (sim::is_simulation_mode() ) { SetStatus(Key_Simulation_Time_Step, Clock::ToUtcPreciseTimeString(msg.get_timestep())); - } } @@ -36,4 +35,8 @@ namespace tmx::utils { } } + bool PluginClientClockAware::isSimulationMode() const { + return _simulation_mode; + } + } \ No newline at end of file diff --git a/src/tmx/TmxUtils/src/PluginClientClockAware.h b/src/tmx/TmxUtils/src/PluginClientClockAware.h index e0981b8d1..974f55060 100644 --- a/src/tmx/TmxUtils/src/PluginClientClockAware.h +++ b/src/tmx/TmxUtils/src/PluginClientClockAware.h @@ -28,7 +28,7 @@ class PluginClientClockAware : public PluginClient { * @param msg TimeSyncMessage broadcast on TMX core * @param routeableMsg */ - void HandleTimeSyncMessage(tmx::messages::TimeSyncMessage &msg, routeable_message &routeableMsg ); + virtual void HandleTimeSyncMessage(tmx::messages::TimeSyncMessage &msg, routeable_message &routeableMsg ); protected: @@ -41,6 +41,9 @@ class PluginClientClockAware : public PluginClient { } void OnStateChange(IvpPluginState state) override; + + bool isSimulationMode() const; + private: /** @@ -55,6 +58,8 @@ class PluginClientClockAware : public PluginClient { * @brief Status label to indicate whether plugin is in Simulation Mode. */ const char* Key_Simulation_Mode = "Simulation Mode "; + + bool _simulation_mode; }; diff --git a/src/tmx/TmxUtils/src/kafka/kafka_consumer_worker.cpp b/src/tmx/TmxUtils/src/kafka/kafka_consumer_worker.cpp index a0a3d2c8f..97f9d4373 100644 --- a/src/tmx/TmxUtils/src/kafka/kafka_consumer_worker.cpp +++ b/src/tmx/TmxUtils/src/kafka/kafka_consumer_worker.cpp @@ -8,6 +8,10 @@ namespace tmx::utils _partition(partition) { } + kafka_consumer_worker::~kafka_consumer_worker() { + stop(); + FILE_LOG(logWARNING) << "Kafka consumer destroyed!" << std::endl; + } bool kafka_consumer_worker::init() { @@ -93,11 +97,12 @@ namespace tmx::utils void kafka_consumer_worker::stop() { + FILE_LOG(logWARNING) << "Stopping Kafka Consumer!" << std::endl; _run = false; //Close and shutdown the consumer. _consumer->close(); - /*Destroy kafka instance*/ // Wait for RdKafka to decommission. - RdKafka::wait_destroyed(5000); + FILE_LOG(logWARNING) << "Kafka Consumer Stopped!" << std::endl; + } void kafka_consumer_worker::subscribe() diff --git a/src/tmx/TmxUtils/src/kafka/kafka_consumer_worker.h b/src/tmx/TmxUtils/src/kafka/kafka_consumer_worker.h index 95a9c96a8..341492b17 100644 --- a/src/tmx/TmxUtils/src/kafka/kafka_consumer_worker.h +++ b/src/tmx/TmxUtils/src/kafka/kafka_consumer_worker.h @@ -68,6 +68,19 @@ namespace tmx::utils { * @param partition partition consumer should be assigned to. */ kafka_consumer_worker(const std::string &broker_str, const std::string &topic_str, const std::string & group_id, int64_t cur_offset = 0, int32_t partition = 0); + /** + * @brief Destroy the kafka consumer worker object. Calls stop on consumer to clean up resources. + */ + ~kafka_consumer_worker(); + // Rule of 5 because destructor is define (https://www.codementor.io/@sandesh87/the-rule-of-five-in-c-1pdgpzb04f) + // Delete copy constructor + kafka_consumer_worker(kafka_consumer_worker& other) = delete; + // Delete copy assigment + kafka_consumer_worker& operator=(const kafka_consumer_worker& other) = delete; + // delete move constructor + kafka_consumer_worker(kafka_consumer_worker &&consumer) = delete; + // delete move assignment + kafka_consumer_worker const & operator=(kafka_consumer_worker &&consumer) = delete; /** * @brief Initialize kafka_consumer_worker * @@ -89,7 +102,7 @@ namespace tmx::utils { /** * @brief Stop running kafka consumer. */ - virtual void stop(); + void stop(); /** * @brief Print current configurations. */ @@ -101,11 +114,6 @@ namespace tmx::utils { * @return false if kafka consumer is stopped. */ virtual bool is_running() const; - /** - * @brief Destroy the kafka consumer worker object - * - */ - virtual ~kafka_consumer_worker() = default; }; } diff --git a/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.cpp b/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.cpp index e81364c19..02d0a0cbb 100644 --- a/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.cpp +++ b/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.cpp @@ -38,6 +38,11 @@ namespace tmx::utils { } + kafka_producer_worker::~kafka_producer_worker() { + stop(); + FILE_LOG(logWARNING) << "Kafka Producer Worker Destroyed!" << std::endl; + } + bool kafka_producer_worker::init() { if(init_producer()) @@ -217,16 +222,18 @@ namespace tmx::utils { if (_producer) { - _producer->flush(10 * 1000 /* wait for max 10 seconds */); - + auto error =_producer->flush(10 * 1000 /* wait for max 10 seconds */); + if (error == RdKafka::ERR__TIMED_OUT) + FILE_LOG(logERROR) << "Flush attempt timed out!" << std::endl; if (_producer->outq_len() > 0) - FILE_LOG(logWARNING) << _producer->name() << _producer->outq_len() << " message(s) were not delivered." << std::endl; + FILE_LOG(logERROR) << _producer->name() << _producer->outq_len() << " message(s) were not delivered." << std::endl; } } catch (const std::runtime_error &e) { FILE_LOG(logERROR) << "Error encountered flushing producer : " << e.what() << std::endl; } + FILE_LOG(logWARNING) << "Kafka producer stopped!" << std::endl; } void kafka_producer_worker::printCurrConf() diff --git a/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.h b/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.h index 7dde427d1..ae8d0b56a 100644 --- a/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.h +++ b/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.h @@ -60,6 +60,19 @@ namespace tmx::utils * @param broker_str network address of kafka broker. */ explicit kafka_producer_worker(const std::string &brokers); + /** + * @brief Destroy the kafka producer worker object. Calls stop on producer to clean up resources. + */ + virtual ~kafka_producer_worker(); + // Rule of 5 because destructor is define (https://www.codementor.io/@sandesh87/the-rule-of-five-in-c-1pdgpzb04f) + // delete copy constructor + kafka_producer_worker(kafka_producer_worker& other) = delete; + // delete copy assignment + kafka_producer_worker& operator=(const kafka_producer_worker& other) = delete; + // delete move constructor + kafka_producer_worker(kafka_producer_worker &&producer) = delete; + // delete move assignment + kafka_producer_worker const & operator=(kafka_producer_worker &&producer) = delete; /** * @brief Initialize kafka_producer_worker. This method must be called before send! * @@ -100,16 +113,12 @@ namespace tmx::utils /** * @brief Stop running kafka producer. */ - virtual void stop(); + void stop(); /** * @brief Print current configurations. */ virtual void printCurrConf(); - /** - * @brief Destroy the kafka producer worker object - * - */ - virtual ~kafka_producer_worker() = default; + }; } diff --git a/src/tmx/TmxUtils/src/kafka/mock_kafka_consumer_worker.h b/src/tmx/TmxUtils/src/kafka/mock_kafka_consumer_worker.h index a2bbb888b..fc927d84c 100644 --- a/src/tmx/TmxUtils/src/kafka/mock_kafka_consumer_worker.h +++ b/src/tmx/TmxUtils/src/kafka/mock_kafka_consumer_worker.h @@ -31,7 +31,6 @@ namespace tmx::utils { MOCK_METHOD(bool, init,(),(override)); MOCK_METHOD(const char*, consume, (int timeout_ms), (override)); MOCK_METHOD(void, subscribe, (), (override)); - MOCK_METHOD(void, stop, (), (override)); MOCK_METHOD(void, printCurrConf, (), (override)); MOCK_METHOD(bool, is_running, (), (const override)); }; diff --git a/src/tmx/TmxUtils/src/kafka/mock_kafka_producer_worker.h b/src/tmx/TmxUtils/src/kafka/mock_kafka_producer_worker.h index c587169b4..cb41d8de5 100644 --- a/src/tmx/TmxUtils/src/kafka/mock_kafka_producer_worker.h +++ b/src/tmx/TmxUtils/src/kafka/mock_kafka_producer_worker.h @@ -26,7 +26,6 @@ namespace tmx::utils { MOCK_METHOD(bool, init,(),(override)); MOCK_METHOD(void, send, (const std::string &msg), (override)); MOCK_METHOD(bool, is_running, (), (const, override)); - MOCK_METHOD(void, stop, (), (override)); MOCK_METHOD(void, printCurrConf, (), (override)); }; } \ No newline at end of file diff --git a/src/tmx/TmxUtils/test/KafkaTestEnvironment.cpp b/src/tmx/TmxUtils/test/KafkaTestEnvironment.cpp new file mode 100644 index 000000000..85fd6cdfa --- /dev/null +++ b/src/tmx/TmxUtils/test/KafkaTestEnvironment.cpp @@ -0,0 +1,27 @@ +#include "gtest/gtest.h" +#include + +/** + * @brief Kafka Test Environment which allows for Setup/Teardown configuration at the + * test program level. Teardown waits on all rd_kafka_t objects to be destroyed. + */ +class KafkaTestEnvironment : public ::testing::Environment { + public: + ~KafkaTestEnvironment() override {} + + // Override this to define how to set up the environment. + void SetUp() override {} + + // Override this to define how to tear down the environment. + void TearDown() override { + std::cout << "Waiting for all RDKafka objects to be destroyed!" << std::endl; + // Wait for all rd_kafka_t objects to be destroyed + auto error = RdKafka::wait_destroyed(5000); + if (error == RdKafka::ERR__TIMED_OUT) { + std::cout << "Wait destroy attempted timed out!" << std::endl; + } + else { + std::cout << "All Objects are destroyed!" << std::endl; + } + } +}; \ No newline at end of file diff --git a/src/tmx/TmxUtils/test/Main.cpp b/src/tmx/TmxUtils/test/Main.cpp index 75163d417..7bffd67ea 100644 --- a/src/tmx/TmxUtils/test/Main.cpp +++ b/src/tmx/TmxUtils/test/Main.cpp @@ -6,9 +6,11 @@ */ #include +#include "KafkaTestEnvironment.cpp" int main(int argc, char **argv) { + ::testing::AddGlobalTestEnvironment(new KafkaTestEnvironment()); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/src/tmx/TmxUtils/test/test_kafka_consumer_worker.cpp b/src/tmx/TmxUtils/test/test_kafka_consumer_worker.cpp index 2d0d680a7..ba7a33c99 100644 --- a/src/tmx/TmxUtils/test/test_kafka_consumer_worker.cpp +++ b/src/tmx/TmxUtils/test/test_kafka_consumer_worker.cpp @@ -1,5 +1,6 @@ #include "gtest/gtest.h" #include "kafka/kafka_client.h" +#include "PluginLog.h" TEST(test_kafka_consumer_worker, create_consumer) { diff --git a/src/tmx/TmxUtils/test/test_kafka_producer_worker.cpp b/src/tmx/TmxUtils/test/test_kafka_producer_worker.cpp index ca04a1e19..e6a101d3e 100644 --- a/src/tmx/TmxUtils/test/test_kafka_producer_worker.cpp +++ b/src/tmx/TmxUtils/test/test_kafka_producer_worker.cpp @@ -13,7 +13,6 @@ TEST(test_kafka_producer_worker, create_producer) std::string msg = "test message"; // // Run this unit test without launching kafka broker will throw connection refused error worker->send(msg); - worker->stop(); } TEST(test_kafka_producer_worker, create_producer_no_topic) @@ -28,5 +27,4 @@ TEST(test_kafka_producer_worker, create_producer_no_topic) std::string msg = "test message"; // // Run this unit test without launching kafka broker will throw connection refused error worker->send(msg, topic); - worker->stop(); } \ No newline at end of file diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index 71703bd59..d76260ad6 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -19,23 +19,15 @@ namespace CARMAStreetsPlugin { * @param name The name to give the plugin for identification purposes */ CARMAStreetsPlugin::CARMAStreetsPlugin(string name) : - PluginClient(name) { + PluginClientClockAware(name) { AddMessageFilter < BsmMessage > (this, &CARMAStreetsPlugin::HandleBasicSafetyMessage); AddMessageFilter < tsm3Message > (this, &CARMAStreetsPlugin::HandleMobilityOperationMessage); AddMessageFilter < tsm2Message > (this, &CARMAStreetsPlugin::HandleMobilityPathMessage); AddMessageFilter < MapDataMessage > (this, &CARMAStreetsPlugin::HandleMapMessage); AddMessageFilter < SrmMessage > (this, &CARMAStreetsPlugin::HandleSRMMessage); - SubscribeToMessages(); - } -CARMAStreetsPlugin::~CARMAStreetsPlugin() { - //Todo: It does not seem the desctructor is called. - _spat_kafka_consumer_ptr->stop(); - _scheduing_plan_kafka_consumer_ptr->stop(); - _ssm_kafka_consumer_ptr->stop(); -} void CARMAStreetsPlugin::UpdateConfigSettings() { @@ -64,6 +56,7 @@ void CARMAStreetsPlugin::UpdateConfigSettings() { _strategies.clear(); while( ss.good() ) { std::string substring; + getline( ss, substring, ','); _strategies.push_back( substring); } @@ -109,6 +102,13 @@ void CARMAStreetsPlugin::OnConfigChanged(const char *key, const char *value) { UpdateConfigSettings(); } +void CARMAStreetsPlugin::HandleTimeSyncMessage(tmx::messages::TimeSyncMessage &msg, routeable_message &routeableMsg ) { + PluginClientClockAware::HandleTimeSyncMessage(msg, routeableMsg); + if ( isSimulationMode()) { + PLOG(logINFO) << "Handling TimeSync messages!" << std::endl; + produce_kafka_msg(msg.to_string(), "time_sync"); + } +} void CARMAStreetsPlugin::HandleMobilityOperationMessage(tsm3Message &msg, routeable_message &routeableMsg ) { try { diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h index 35df38132..f7167f269 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h @@ -20,6 +20,7 @@ #include #include #include "JsonToJ2735SSMConverter.h" +#include "PluginClientClockAware.h" @@ -32,10 +33,9 @@ using namespace boost::property_tree; namespace CARMAStreetsPlugin { -class CARMAStreetsPlugin: public PluginClient { +class CARMAStreetsPlugin: public PluginClientClockAware { public: CARMAStreetsPlugin(std::string); - virtual ~CARMAStreetsPlugin(); int Main(); protected: @@ -48,6 +48,12 @@ class CARMAStreetsPlugin: public PluginClient { void HandleMobilityOperationMessage(tsm3Message &msg, routeable_message &routeableMsg); void HandleMobilityPathMessage(tsm2Message &msg, routeable_message &routeableMsg); void HandleBasicSafetyMessage(BsmMessage &msg, routeable_message &routeableMsg); + /** + * @brief Overide PluginClientClockAware HandleTimeSyncMessage to producer TimeSyncMessage to kafka for CARMA Streets Time Synchronization. + * @param msg TimeSyncMessage received by plugin when in simulation mode. Message provides current simulation time to all processes. + * @param routeableMsg routeable_message for time sync message. + */ + void HandleTimeSyncMessage(TimeSyncMessage &msg, routeable_message &routeableMsg) override; /** * @brief Subscribe to MAP message broadcast by the MAPPlugin. This handler will be called automatically whenever the MAPPlugin is broadcasting a J2735 MAP message. * @param msg The J2735 MAP message received from the internal diff --git a/src/v2i-hub/CDASimAdapter/CMakeLists.txt b/src/v2i-hub/CDASimAdapter/CMakeLists.txt index 97468a2cf..8fbc4f376 100755 --- a/src/v2i-hub/CDASimAdapter/CMakeLists.txt +++ b/src/v2i-hub/CDASimAdapter/CMakeLists.txt @@ -4,17 +4,17 @@ set(CMAKE_CXX_STANDARD 17) FIND_PACKAGE( carma-clock ) BuildTmxPlugin ( ) -TARGET_LINK_LIBRARIES (${PROJECT_NAME} tmxutils ::carma-clock rdkafka++ jsoncpp) +TARGET_LINK_LIBRARIES (${PROJECT_NAME} tmxutils ::carma-clock jsoncpp) # ############ # # Testing ## # ############ ADD_LIBRARY(${PROJECT_NAME}_lib src/CDASimConnection.cpp) -TARGET_LINK_LIBRARIES(${PROJECT_NAME}_lib PUBLIC tmxutils ::carma-clock rdkafka++ jsoncpp ) +TARGET_LINK_LIBRARIES(${PROJECT_NAME}_lib PUBLIC tmxutils ::carma-clock jsoncpp ) SET(BINARY ${PROJECT_NAME}_test) FILE(GLOB_RECURSE TEST_SOURCES LIST_DIRECTORIES false test/*.h test/*.cpp) SET(SOURCES ${TEST_SOURCES} WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) ADD_EXECUTABLE(${BINARY} ${TEST_SOURCES}) ADD_TEST(NAME ${BINARY} COMMAND ${BINARY}) TARGET_INCLUDE_DIRECTORIES(${BINARY} PUBLIC /usr/local/lib src/) -TARGET_LINK_LIBRARIES(${BINARY} PUBLIC ${PROJECT_NAME}_lib gtest gmock tmxutils ::carma-clock rdkafka++ jsoncpp) \ No newline at end of file +TARGET_LINK_LIBRARIES(${BINARY} PUBLIC ${PROJECT_NAME}_lib gtest gmock tmxutils ::carma-clock jsoncpp) \ No newline at end of file diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp index 01cec61ef..44fd35e2f 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp @@ -69,35 +69,12 @@ namespace CDASimAdapter{ } } - bool CDASimAdapter::initialize_time_producer() { - try { - std::string _broker_str = sim::get_sim_config(sim::KAFKA_BROKER_ADDRESS); - std::string _topic = sim::get_sim_config(sim::TIME_SYNC_TOPIC); - - kafka_client client; - time_producer = client.create_producer(_broker_str,_topic); - return time_producer->init(); - - } - catch( const runtime_error &e ) { - PLOG(logWARNING) << "Initialization of time producer failed: " << e.what() << std::endl; - } - return false; - } void CDASimAdapter::forward_time_sync_message(tmx::messages::TimeSyncMessage &msg) { std::string payload =msg.to_string(); PLOG(logDEBUG1) << "Sending Time Sync Message " << msg << std::endl; this->BroadcastMessage(msg, _name, 0 , IvpMsgFlags_None); - if (time_producer && time_producer->is_running()) { - try { - time_producer->send(payload); - } - catch( const runtime_error &e ) { - PLOG(logERROR) << "Exception encountered during kafka time sync forward : " << e.what() << std::endl; - } - } } @@ -115,9 +92,6 @@ namespace CDASimAdapter{ PLOG(logINFO) << "CDASim connecting " << simulation_ip << "\nUsing Registration Port : " << std::to_string( simulation_registration_port) << " Time Sync Port: " << std::to_string( time_sync_port) << " and V2X Port: " << std::to_string(v2x_port) << std::endl; - if (!initialize_time_producer()) { - return false; - } if ( connection ) { connection.reset(new CDASimConnection( simulation_ip, infrastructure_id, simulation_registration_port, sim_v2x_port, local_ip, time_sync_port, v2x_port, location )); diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp index fd586fafc..b3a979373 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp @@ -18,8 +18,6 @@ #include #include #include "CDASimConnection.hpp" -#include -#include #include #include "ThreadWorker.h" @@ -61,13 +59,6 @@ namespace CDASimAdapter void OnStateChange(IvpPluginState state) override; // Virtual method overrides END. - /** - * @brief Get Kafka Connection string from environment variable KAFKA_BROKER_ADDRESS and time sync topic name from - * CARMA_INFRASTRUCTURE_TIME_SYNC_TOPIC and initialize a Kafka producer to forward time synchronization messages to - * all infrastructure services. - * @return true if initialization is successful and false if initialization fails. - */ - bool initialize_time_producer(); /** * @brief Method to attempt to establish connection between CARMA Simulation and Infrastructure Software (V2X-Hub). * @return true if successful and false if unsuccessful. @@ -94,8 +85,7 @@ namespace CDASimAdapter */ void attempt_message_from_simulation() const; /** - * @brief Forward time sychronization message to TMX message bus for other V2X-Hub Plugin and to infrastructure Kafka Broker for - * CARMA Streets services + * @brief Forward time sychronization message to TMX message bus for other V2X-Hub Plugin * @param msg TimeSyncMessage. */ void forward_time_sync_message(tmx::messages::TimeSyncMessage &msg); @@ -115,8 +105,6 @@ namespace CDASimAdapter int max_connection_attempts; // Time in seconds between connection attempts. Most be greater than zero! uint connection_sleep_time; - // Kafka producer for sending time_sync messages to carma-streets - std::shared_ptr time_producer; // CDASim connection std::unique_ptr connection; // Mutex for configuration parameter thread safety diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp index 86bd0ff95..9420cc3f3 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp @@ -30,7 +30,6 @@ namespace CDASimAdapter { * @param time_sync_port Port on which connection listens for time synchronization messages. * @param v2x_port Port on which connecction listens for incoming v2x messages. * @param location Simulationed location of infrastructure. - * @param producer Kafka Producer for forwarding time synchronization messages. */ explicit CDASimConnection( const std::string &simulation_ip, const std::string &infrastructure_id, const uint simulation_registration_port, const uint sim_v2x_port, const std::string &local_ip, const uint time_sync_port, const uint v2x_port, diff --git a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp index 28b7376ab..b239c96e0 100644 --- a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp @@ -3,7 +3,6 @@ #include "include/CDASimConnection.hpp" #include "include/CDASimAdapter.hpp" #include -#include #include #include @@ -22,7 +21,7 @@ namespace CDASimAdapter { class TestCARMASimulationConnection : public ::testing::Test { protected: void SetUp() override { - // Initialize CARMA Simulation connection with (0,0,0) location and mock kafka producer. + // Initialize CARMA Simulation connection with (0,0,0) location. Point location; connection = std::make_shared("127.0.0.1", "1212", 4567, 4678, "127.0.0.1", 1213, 1214, location); } diff --git a/src/v2i-hub/MapPlugin/src/MapPlugin.cpp b/src/v2i-hub/MapPlugin/src/MapPlugin.cpp index ba4a54b05..2317e7800 100644 --- a/src/v2i-hub/MapPlugin/src/MapPlugin.cpp +++ b/src/v2i-hub/MapPlugin/src/MapPlugin.cpp @@ -81,6 +81,7 @@ class MapPlugin: public PluginClientClockAware { void OnConfigChanged(const char *key, const char *value); void OnMessageReceived(IvpMessage *msg); void OnStateChange(IvpPluginState state); + private: std::atomic _mapAction {-1}; std::atomic _isMapFileNew {false}; diff --git a/src/v2i-hub/SpatPlugin/src/SpatPlugin.h b/src/v2i-hub/SpatPlugin/src/SpatPlugin.h index a42552618..0d0b9987f 100644 --- a/src/v2i-hub/SpatPlugin/src/SpatPlugin.h +++ b/src/v2i-hub/SpatPlugin/src/SpatPlugin.h @@ -44,6 +44,7 @@ class SpatPlugin: public tmx::utils::PluginClientClockAware { void OnConfigChanged(const char *key, const char *value); void OnStateChange(IvpPluginState state); + private: From 678784cfb2416ae4918753ecb378ba4bcd16c92a Mon Sep 17 00:00:00 2001 From: dan-du-car <62157949+dan-du-car@users.noreply.github.com> Date: Thu, 27 Jul 2023 14:41:54 -0400 Subject: [PATCH 09/28] Streets MOSAIC integration: CDASimAdapter register with Simulation ambassador and send sensors information (#554) # PR Details ## Description Update the CDASimAdapter registration message to include the sensors information. Read the sensors information from local file and store the sensors information in the CDASimAdapter plugin. ## Related Issue NA ## Motivation and Context CDA research ## How Has This Been Tested? unit test local integration test ## Types of changes - [ ] Defect fix (non-breaking change that fixes an issue) - [x] 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. --- .devcontainer/docker-compose-vscode.yml | 1 + .../src/simulation/SimulationEnvUtils.h | 5 ++ .../scripts/udp_socket_server.py | 19 ++++++++ .../CDASimAdapter/src/CDASimAdapter.cpp | 5 +- .../CDASimAdapter/src/CDASimConnection.cpp | 47 +++++++++++++++++-- .../src/include/CDASimConnection.hpp | 19 +++++++- .../test/TestCARMASimulationConnection.cpp | 34 ++++++++++++-- src/v2i-hub/CDASimAdapter/test/sensors.json | 30 ++++++++++++ 8 files changed, 149 insertions(+), 11 deletions(-) create mode 100644 src/v2i-hub/CDASimAdapter/scripts/udp_socket_server.py create mode 100755 src/v2i-hub/CDASimAdapter/test/sensors.json diff --git a/.devcontainer/docker-compose-vscode.yml b/.devcontainer/docker-compose-vscode.yml index 89b1efd18..649f451e2 100755 --- a/.devcontainer/docker-compose-vscode.yml +++ b/.devcontainer/docker-compose-vscode.yml @@ -24,6 +24,7 @@ services: - SIM_V2X_PORT=5757 - V2X_PORT=8686 - INFRASTRUCTURE_ID=1 + - SENSOR_JSON_FILE_PATH=/var/www/plugins/MAP/sensors.json secrets: - mysql_password diff --git a/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h b/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h index 1b04ffe11..7b0c4ec71 100644 --- a/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h +++ b/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h @@ -31,6 +31,11 @@ namespace tmx::utils::sim{ * in SIMULATION MODE for connecting to CDASim. */ constexpr inline static const char *SIMULATION_REGISTRATION_PORT = "SIMULATION_REGISTRATION_PORT"; + /** + * @brief sensors file location + */ + constexpr inline static const char *SENSOR_JSON_FILE_PATH = "SENSOR_JSON_FILE_PATH"; + /** * @brief Name of environment varaible for storing port for receiving time sync messages from CDASim. Only * necessary in SIMULATION MODE for CDASim time sync. diff --git a/src/v2i-hub/CDASimAdapter/scripts/udp_socket_server.py b/src/v2i-hub/CDASimAdapter/scripts/udp_socket_server.py new file mode 100644 index 000000000..b91e882c3 --- /dev/null +++ b/src/v2i-hub/CDASimAdapter/scripts/udp_socket_server.py @@ -0,0 +1,19 @@ +import socket +''' +Purpose: This script is to launch a UDP server and used to test the CDASimAdapter simulation registration. +Usage: Run this script first, it shall open a socket listenning to port 6767. + Launch the v2xhub and enable the CDASimAdapter plugin. Upon the plugin startup, + it sends registration message to port 6767. This UDP server shall receive the registration message, + and print this message on the terminal. +Run command: python3 udp_socket_server.py +''' +UDP_IP = "127.0.0.1" +UDP_SOCKET_PORT_SIM_REGISTRATION = 6767 + +sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) +sock.bind((UDP_IP, UDP_SOCKET_PORT_SIM_REGISTRATION)) +print("Server Listenning on port: %s" % UDP_SOCKET_PORT_SIM_REGISTRATION) + +while True: + data, addr = sock.recvfrom(1024) + print("recevied message: %s" % data) \ No newline at end of file diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp index 44fd35e2f..297e44bdd 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp @@ -88,17 +88,18 @@ namespace CDASimAdapter{ uint v2x_port = std::stoul(sim::get_sim_config(sim::V2X_PORT)); uint sim_v2x_port = std::stoul(sim::get_sim_config(sim::SIM_V2X_PORT)); std::string infrastructure_id = sim::get_sim_config(sim::INFRASTRUCTURE_ID); + std::string sensor_json_file_path = sim::get_sim_config(sim::SENSOR_JSON_FILE_PATH); PLOG(logINFO) << "CDASim connecting " << simulation_ip << "\nUsing Registration Port : " << std::to_string( simulation_registration_port) << " Time Sync Port: " << std::to_string( time_sync_port) << " and V2X Port: " << std::to_string(v2x_port) << std::endl; if ( connection ) { connection.reset(new CDASimConnection( simulation_ip, infrastructure_id, simulation_registration_port, sim_v2x_port, local_ip, - time_sync_port, v2x_port, location )); + time_sync_port, v2x_port, location, sensor_json_file_path)); } else { connection = std::make_unique(simulation_ip, infrastructure_id, simulation_registration_port, sim_v2x_port, local_ip, - time_sync_port, v2x_port, location); + time_sync_port, v2x_port, location, sensor_json_file_path); } } catch (const TmxException &e) { diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp index 51f55eaf2..15621be80 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp @@ -6,10 +6,10 @@ using namespace tmx::utils; namespace CDASimAdapter{ CDASimConnection::CDASimConnection(const std::string &simulation_ip, const std::string &infrastructure_id, const uint simulation_registration_port, const uint sim_v2x_port, const std::string &local_ip, const uint time_sync_port, const uint v2x_port, - const Point &location) : + const Point &location, const std::string &sensor_json_file_path) : _simulation_ip(simulation_ip), _infrastructure_id(infrastructure_id), _simulation_registration_port(simulation_registration_port), _simulation_v2x_port(sim_v2x_port), _local_ip(local_ip), _time_sync_port(time_sync_port), _v2x_port(v2x_port), - _location(location) { + _location(location) ,_sensor_json_file_path(sensor_json_file_path) { PLOG(logDEBUG) << "CARMA-Simulation connection initialized." << std::endl; } @@ -47,13 +47,22 @@ namespace CDASimAdapter{ message["location"]["x"] = location.X; message["location"]["y"] = location.Y; message["location"]["z"] = location.Z; + + //Read local sensor file and populate the sensors JSON + //Sample sensors.json: https://raw.githubusercontent.com/usdot-fhwa-OPS/V2X-Hub/develop/src/v2i-hub/CDASimAdapter/test/sensors.json + auto sensors_json_v = read_json_file(_sensor_json_file_path); + if(sensors_json_v.empty()) + { + PLOG(logWARNING) << "Sensors JSON is empty!" << std::endl; + } + message["sensors"] = sensors_json_v; Json::StyledWriter writer; message_str = writer.write(message); return message_str; } bool CDASimConnection::carma_simulation_handshake(const std::string &simulation_ip, const std::string &infrastructure_id, const uint simulation_registration_port, - const std::string &local_ip, const uint time_sync_port, const uint v2x_port, + const std::string &local_ip, const uint time_sync_port, const uint v2x_port, const Point &location) { // Create JSON message with the content @@ -217,4 +226,36 @@ namespace CDASimAdapter{ forward_message( msg , message_receiver_publisher ); } + Json::Value CDASimConnection::read_json_file(const std::string& file_path) const{ + Json::Value sensors_json_v; + //Read file from disk + std::ifstream in_strm; + in_strm.open(file_path, std::ifstream::binary); + if(!in_strm.is_open()) + { + PLOG(logERROR) << "File cannot be opened. File path: " << file_path < reader(builder.newCharReader()); + JSONCPP_STRING err; + if(!reader->parse(json_str.c_str(), json_str.c_str() + json_str.length(), &json_v, &err)) + { + PLOG(logERROR) << "Error parsing sensors from string: " << json_str << std::endl; + } + return json_v; + } } diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp index 9420cc3f3..bdd9d05ed 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp @@ -7,6 +7,7 @@ #include #include #include +#include namespace CDASimAdapter { @@ -33,7 +34,7 @@ namespace CDASimAdapter { */ explicit CDASimConnection( const std::string &simulation_ip, const std::string &infrastructure_id, const uint simulation_registration_port, const uint sim_v2x_port, const std::string &local_ip, const uint time_sync_port, const uint v2x_port, - const tmx::utils::Point &location); + const tmx::utils::Point &location, const std::string &sensor_json_file_path); /** * @brief Method to forward v2x message to CARMA Simulation @@ -133,6 +134,19 @@ namespace CDASimAdapter { std::string get_handshake_json(const std::string &infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint v2x_port, const tmx::utils::Point &location) const; + /** + * @brief Read local file that has the sensor information in JSON format from disk. Populate global sensor json variable with the information. + * @param file_path A string of file location in the host machine. + * @return A reference to the location where the sensors inforation is updated and stored. + */ + Json::Value read_json_file(const std::string& file_path) const; + /** + * @brief Read local file that has the sensor information in JSON format from disk. Populate global sensor json variable with the information. + * @param json_str A JSON string. + * @return A reference to JSON value. + */ + Json::Value string_to_json(const std::string &json_str) const; + std::string _simulation_ip; uint _simulation_registration_port; std::string _infrastructure_id; @@ -142,6 +156,7 @@ namespace CDASimAdapter { uint _v2x_port; tmx::utils::Point _location; bool _connected = false; + std::string _sensor_json_file_path; std::shared_ptr carma_simulation_listener; std::shared_ptr carma_simulation_publisher; @@ -151,6 +166,8 @@ namespace CDASimAdapter { std::shared_ptr time_sync_listener; FRIEND_TEST(TestCARMASimulationConnection, get_handshake_json); + FRIEND_TEST(TestCARMASimulationConnection, read_json_file); + FRIEND_TEST(TestCARMASimulationConnection, string_to_json); }; } diff --git a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp index b239c96e0..2c686d170 100644 --- a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp @@ -5,6 +5,7 @@ #include #include #include +#include using testing::_; @@ -23,13 +24,14 @@ namespace CDASimAdapter { void SetUp() override { // Initialize CARMA Simulation connection with (0,0,0) location. Point location; - connection = std::make_shared("127.0.0.1", "1212", 4567, 4678, "127.0.0.1", 1213, 1214, location); + connection = std::make_shared("127.0.0.1", "1212", 4567, 4678, "127.0.0.1", 1213, 1214, location, sensors_file_path); } void TearDown() override { } public: std::shared_ptr connection; + std::string sensors_file_path = "../../CDASimAdapter/test/sensors.json"; }; @@ -85,10 +87,13 @@ namespace CDASimAdapter { location.X = 1000; location.Y = 38.955; location.Z = -77.149; - - ASSERT_EQ(connection->get_handshake_json("4566", "127.0.0.1", 4567, 4568, location), - "{\n \"infrastructureId\" : \"4566\",\n \"location\" : {\n \"x\" : 1000.0,\n \"y\" : 38.954999999999998,\n \"z\" : -77.149000000000001\n },\n \"rxMessageIpAddress\" : \"127.0.0.1\",\n \"rxMessagePort\" : 4568,\n \"timeSyncPort\" : 4567\n}\n"); - + std::ifstream in_strm; + in_strm.open(sensors_file_path, std::ifstream::binary); + if(in_strm.is_open()) + { + ASSERT_EQ(connection->get_handshake_json("4566", "127.0.0.1", 4567, 4568, location), + "{\n \"infrastructureId\" : \"4566\",\n \"location\" : {\n \"x\" : 1000.0,\n \"y\" : 38.954999999999998,\n \"z\" : -77.149000000000001\n },\n \"rxMessageIpAddress\" : \"127.0.0.1\",\n \"rxMessagePort\" : 4568,\n \"sensors\" : [\n {\n \"location\" : {\n \"x\" : 0.0,\n \"y\" : 0.0,\n \"z\" : 0.0\n },\n \"orientation\" : {\n \"pitch\" : 0.0,\n \"roll\" : 0.0,\n \"yaw\" : 0.0\n },\n \"sensorId\" : \"SomeID\",\n \"type\" : \"SematicLidar\"\n },\n {\n \"location\" : {\n \"x\" : 1.0,\n \"y\" : 2.0,\n \"z\" : 0.0\n },\n \"orientation\" : {\n \"pitch\" : 0.0,\n \"roll\" : 0.0,\n \"yaw\" : 23.0\n },\n \"sensorId\" : \"SomeID2\",\n \"type\" : \"SematicLidar\"\n }\n ],\n \"timeSyncPort\" : 4567\n}\n"); + } } TEST_F( TestCARMASimulationConnection, carma_simulation_handshake) { @@ -101,4 +106,23 @@ namespace CDASimAdapter { TEST_F(TestCARMASimulationConnection, connect) { ASSERT_TRUE(connection->connect()); } + + TEST_F(TestCARMASimulationConnection, read_json_file) + { + auto sensorJsonV = connection->read_json_file("Invalid_file_path" ); + ASSERT_TRUE(sensorJsonV.empty()); + std::ifstream in_strm; + in_strm.open(sensors_file_path, std::ifstream::binary); + if(in_strm.is_open()) + { + sensorJsonV = connection->read_json_file(sensors_file_path ); + ASSERT_FALSE(sensorJsonV.empty()); + } + } + + TEST_F(TestCARMASimulationConnection, string_to_json) + { + auto sensorJsonV = connection->string_to_json("Invalid Json"); + ASSERT_TRUE(sensorJsonV.empty()); + } } \ No newline at end of file diff --git a/src/v2i-hub/CDASimAdapter/test/sensors.json b/src/v2i-hub/CDASimAdapter/test/sensors.json new file mode 100755 index 000000000..2979e5a21 --- /dev/null +++ b/src/v2i-hub/CDASimAdapter/test/sensors.json @@ -0,0 +1,30 @@ +[ + { + "sensorId": "SomeID", + "type": "SematicLidar", + "location": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "orientation": { + "yaw": 0.0, + "pitch": 0.0, + "roll": 0.0 + } + }, + { + "sensorId": "SomeID2", + "type": "SematicLidar", + "location": { + "x": 1.0, + "y": 2.0, + "z": 0.0 + }, + "orientation": { + "yaw": 23.0, + "pitch": 0.0, + "roll": 0.0 + } + } +] \ No newline at end of file From d4a370f96b7a90b1fb8679c0ab60fedb075044b0 Mon Sep 17 00:00:00 2001 From: dan-du-car <62157949+dan-du-car@users.noreply.github.com> Date: Fri, 28 Jul 2023 14:06:53 -0400 Subject: [PATCH 10/28] MOSAIC-CARMA-Streets: CDASimAdapter listens on port for incoming Simulated Object Dectections and publish the serialized object to Kafka (#552) # PR Details ## Description Implement the design from https://usdot-carma.atlassian.net/browse/CDAR-148 Part 2.B: Implement the logic in CDASimAdapter to listen to UDP socket and receive DetectedObject. Then serialize this C++ object to TMX bus so that CARMAStreetPlugin can receive it form the TMX bus. As a V2X-Hub user, I need the CDASimAdapter to listen for Simulated Object Detection Messages on an open port and broadcast received messages on the TMX message bus. ## Related Issue https://usdot-carma.atlassian.net/browse/CDAR-191 https://usdot-carma.atlassian.net/browse/CDAR-184 ## Related Jira Key https://usdot-carma.atlassian.net/browse/CDAR-191 https://usdot-carma.atlassian.net/browse/CDAR-184 ## Motivation and Context CDA research ## How Has This Been Tested? Unit test Local integration test ## Types of changes - [ ] Defect fix (non-breaking change that fixes an issue) - [x] 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. --- .devcontainer/docker-compose-vscode.yml | 1 + src/tmx/Messages/include/MessageTypes.h | 1 + .../include/simulation/SensorDetectedObject.h | 35 ++++++++ .../src/simulation/SimulationEnvUtils.h | 5 ++ src/v2i-hub/CARMAStreetsPlugin/manifest.json | 10 +++ .../src/CARMAStreetsPlugin.cpp | 12 ++- .../src/CARMAStreetsPlugin.h | 8 ++ .../scripts/send_sim_detected_object_udp.py | 81 +++++++++++++++++++ .../CDASimAdapter/src/CDASimAdapter.cpp | 39 ++++++++- .../CDASimAdapter/src/CDASimConnection.cpp | 54 +++++++++---- .../src/include/CDASimAdapter.hpp | 11 +++ .../src/include/CDASimConnection.hpp | 30 ++++--- .../test/TestCARMASimulationConnection.cpp | 10 +-- 13 files changed, 261 insertions(+), 36 deletions(-) create mode 100644 src/tmx/Messages/include/simulation/SensorDetectedObject.h create mode 100755 src/v2i-hub/CDASimAdapter/scripts/send_sim_detected_object_udp.py diff --git a/.devcontainer/docker-compose-vscode.yml b/.devcontainer/docker-compose-vscode.yml index 649f451e2..cc113ce62 100755 --- a/.devcontainer/docker-compose-vscode.yml +++ b/.devcontainer/docker-compose-vscode.yml @@ -22,6 +22,7 @@ services: - TIME_SYNC_TOPIC=time_sync - TIME_SYNC_PORT=7575 - SIM_V2X_PORT=5757 + - SIM_INTERACTION_PORT=7576 - V2X_PORT=8686 - INFRASTRUCTURE_ID=1 - SENSOR_JSON_FILE_PATH=/var/www/plugins/MAP/sensors.json diff --git a/src/tmx/Messages/include/MessageTypes.h b/src/tmx/Messages/include/MessageTypes.h index a4a84f6f4..e0fd09c70 100644 --- a/src/tmx/Messages/include/MessageTypes.h +++ b/src/tmx/Messages/include/MessageTypes.h @@ -91,6 +91,7 @@ static CONSTEXPR const char *MSGSUBTYPE_INCOMING_STRING = "Incoming"; static CONSTEXPR const char *MSGSUBTYPE_OUTGOING_STRING = "Outgoing"; static CONSTEXPR const char *MSGSUBTYPE_SHUTDOWN_STRING = "Shutdown"; static CONSTEXPR const char *MSGSUBTYPE_TIMESYNC_STRING = "TimeSync"; +static CONSTEXPR const char *MSGSUBTYPE_SENSOR_DETECTED_OBJECT_STRING = "SensorDetectedObject"; } /* End namespace messages */ diff --git a/src/tmx/Messages/include/simulation/SensorDetectedObject.h b/src/tmx/Messages/include/simulation/SensorDetectedObject.h new file mode 100644 index 000000000..179f48a13 --- /dev/null +++ b/src/tmx/Messages/include/simulation/SensorDetectedObject.h @@ -0,0 +1,35 @@ +#ifndef INCLUDE_SIMULATED_SensorDetectedObject_H_ +#define INCLUDE_SIMULATED_SensorDetectedObject_H_ + +#include +#include + +namespace tmx +{ + namespace messages + { + namespace simulation + { + /** + * This SensorDetectedObject is used to communicate the sensor detected object information with various applications + * including internal infrastructure applications and external road user applications through simulated environment. + * It defines the message type and sub type and all data members. + */ + class SensorDetectedObject : public tmx::message + { + public: + SensorDetectedObject(){}; + SensorDetectedObject(const tmx::message_container_type &contents) : tmx::message(contents) {}; + ~SensorDetectedObject(){}; + // Message type for routing this message through TMX core + static constexpr const char *MessageType = MSGTYPE_APPLICATION_STRING; + + // // Message sub type for routing this message through TMX core + static constexpr const char *MessageSubType = MSGSUBTYPE_SENSOR_DETECTED_OBJECT_STRING; + }; + + } + } + +}; // namespace tmx +#endif diff --git a/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h b/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h index 7b0c4ec71..7498a7922 100644 --- a/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h +++ b/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h @@ -46,6 +46,11 @@ namespace tmx::utils::sim{ * necessary in SIMULATION MODE for CDASim message forwarding. */ constexpr inline static const char *SIM_V2X_PORT = "SIM_V2X_PORT"; + /** + * @brief Name of environment variable for storing port for forwarding v2x messages to CDASim. Only + * necessary in SIMULATION MODE for CDASim message forwarding. + */ + constexpr inline static const char *SIM_INTERACTION_PORT= "SIM_INTERACTION_PORT"; /** * @brief Name of environment variable for storing port for receiving v2x messages from CDASim. Only * necessary in SIMULATION MODE for CDASim message forwarding. diff --git a/src/v2i-hub/CARMAStreetsPlugin/manifest.json b/src/v2i-hub/CARMAStreetsPlugin/manifest.json index 790ef1af9..f0daaf9a9 100644 --- a/src/v2i-hub/CARMAStreetsPlugin/manifest.json +++ b/src/v2i-hub/CARMAStreetsPlugin/manifest.json @@ -34,6 +34,11 @@ "type":"J2735", "subtype":"SPAT-P", "description":"Signal Phase and Timing (SPAT) status for the signalized intersection." + }, + { + "type":"Application", + "subtype":"SensorDetectedObject", + "description": "Sensor detected object for cooperative perception." } ], "configuration": [ @@ -116,6 +121,11 @@ "key": "SpatConsumerGroupId", "default": "v2xhub_spat", "description": "Apache Kafka consumer group ID for spat consumer." + }, + { + "key": "SimSensorDetectedObjTopic", + "default": "v2xhub_sim_sensor_detected_object", + "description": "Apache Kafka topic plugin will transmit simulated sensor detected object to." } ] diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index d76260ad6..c5bb9a7e6 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -25,6 +25,8 @@ CARMAStreetsPlugin::CARMAStreetsPlugin(string name) : AddMessageFilter < tsm2Message > (this, &CARMAStreetsPlugin::HandleMobilityPathMessage); AddMessageFilter < MapDataMessage > (this, &CARMAStreetsPlugin::HandleMapMessage); AddMessageFilter < SrmMessage > (this, &CARMAStreetsPlugin::HandleSRMMessage); + AddMessageFilter < simulation::SensorDetectedObject > (this, &CARMAStreetsPlugin::HandleSimulatedSensorDetectedMessage ); + SubscribeToMessages(); } @@ -48,7 +50,8 @@ void CARMAStreetsPlugin::UpdateConfigSettings() { GetConfigValue("MobilityOperationTopic", _transmitMobilityOperationTopic); GetConfigValue("MobilityPathTopic", _transmitMobilityPathTopic); GetConfigValue("MapTopic", _transmitMAPTopic); - GetConfigValue("SRMTopic", _transmitSRMTopic); + GetConfigValue("SRMTopic", _transmitSRMTopic); + GetConfigValue("SimSensorDetectedObjTopic", _transmitSimSensorDetectedObjTopic); // Populate strategies config string config; GetConfigValue("MobilityOperationStrategies", config); @@ -626,6 +629,13 @@ void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){ } } + +void CARMAStreetsPlugin::HandleSimulatedSensorDetectedMessage(simulation::SensorDetectedObject &msg, routeable_message &routeableMsg) +{ + PLOG(logDEBUG) << "Produce sensor detected message in JSON format: " << msg.to_string() < #include #include "JsonToJ2735SSMConverter.h" +#include #include "PluginClientClockAware.h" @@ -48,6 +49,12 @@ class CARMAStreetsPlugin: public PluginClientClockAware { void HandleMobilityOperationMessage(tsm3Message &msg, routeable_message &routeableMsg); void HandleMobilityPathMessage(tsm2Message &msg, routeable_message &routeableMsg); void HandleBasicSafetyMessage(BsmMessage &msg, routeable_message &routeableMsg); + /** + * @brief Callback function when the plugin received detected object, and forward the detected object to Kafka topic. + * @param msg Detected object received from TMX bus. + * @param routeableMsg routeable_message for detected object. + */ + void HandleSimulatedSensorDetectedMessage(simulation::SensorDetectedObject &msg, routeable_message &routeableMsg); /** * @brief Overide PluginClientClockAware HandleTimeSyncMessage to producer TimeSyncMessage to kafka for CARMA Streets Time Synchronization. * @param msg TimeSyncMessage received by plugin when in simulation mode. Message provides current simulation time to all processes. @@ -104,6 +111,7 @@ class CARMAStreetsPlugin: public PluginClientClockAware { std::string _transmitBSMTopic; std::string _transmitMAPTopic; std::string _transmitSRMTopic; + std::string _transmitSimSensorDetectedObjTopic; std::string _kafkaBrokerIp; std::string _kafkaBrokerPort; std::shared_ptr _kafka_producer_ptr; diff --git a/src/v2i-hub/CDASimAdapter/scripts/send_sim_detected_object_udp.py b/src/v2i-hub/CDASimAdapter/scripts/send_sim_detected_object_udp.py new file mode 100755 index 000000000..e90eaf9c3 --- /dev/null +++ b/src/v2i-hub/CDASimAdapter/scripts/send_sim_detected_object_udp.py @@ -0,0 +1,81 @@ + +import socket +import sys +import json +import time + +# Script for integration testing CDASimAdapter Time Sync functionality. +# This python script sends periodic time sync messages to a configurable +# host and port. To Test the Time Sync functionality of the CDASimAdapter +# set port to the value of the TIME_SYNC_PORT environment variable. +# +# TODO Move this script into a more permanent location +count_num = 0 + +def generate_sim_external_object(): + jsonResult = { + "metadata": { + "type": "Application", + "subtype": "SensorDetectedObject", + "timestamp": 123, + "isSimulated": True + }, + "payload": { + "sensor_id": "sensor1", + "proj_string": "asdlasdkasd", + "type": "Car", + "confidence": "0.7", + "objectId": "Object1", + "position": { + "x": 1.0, + "y": 2.5, + "z": 1.1 + }, + "positionCovariance" : [12,12,2, 34, 34, 55], + "velocity": { + "x": 1.0, + "y": 2.5, + "z": 1.1 + }, + "velocityCovariance" : ["a11", "a12", "a13", "a21", "a22", "a23", "a31", "a32", "a33"], + "angularVelocity":{ + "x": 1.0, + "y": 2.5, + "z": 1.1 + }, + "angularVelocityCovariance" : ["a11", "a12", "a13", "a21", "a22", "a23", "a31", "a32", "a33"], + "size": { + "length": 0.1, + "width": 0.4, + "height": 1.5 + } + } + } + jsonResult = json.dumps(jsonResult) + return jsonResult +port = 7576 +address = "127.0.0.1" +host = (address, port) +try: + sock = socket.socket(family=socket.AF_INET, type=socket.SOCK_DGRAM) +except socket.error as err: + print('Socket error because of %s' %(err)) + + +while True : + try: + msg = generate_sim_external_object() + encoded_msg = str.encode(msg) + count_num += 1 + sock.sendto(encoded_msg,host) + print( encoded_msg.decode(encoding= 'UTF-8'), 'was sent to ', host) + print(f'Message sent at ${time.time()}') + time.sleep(5) + except socket.gaierror: + + print ('There an error resolving the host') + break + +sock.close() + + diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp index 297e44bdd..c662e6e9c 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp @@ -60,6 +60,7 @@ namespace CDASimAdapter{ if ( connection->is_connected() ) { start_time_sync_thread_timer(); + start_sensor_detected_object_detection_thread(); start_immediate_forward_thread(); start_message_receiver_thread(); }else { @@ -78,6 +79,11 @@ namespace CDASimAdapter{ } + void CDASimAdapter::forward_simulated_detected_message(tmx::messages::simulation::SensorDetectedObject &msg) { + PLOG(logDEBUG1) << "Sending Simulated SensorDetectedObject Message " << msg << std::endl; + this->BroadcastMessage(msg, _name, 0 , IvpMsgFlags_None); + } + bool CDASimAdapter::connect() { try { std::string simulation_ip = sim::get_sim_config(sim::SIMULATION_IP); @@ -85,6 +91,7 @@ namespace CDASimAdapter{ PLOG(logINFO) << "Simulation and local IP successfully initialized!"<< std::endl; uint simulation_registration_port = std::stoul(sim::get_sim_config(sim::SIMULATION_REGISTRATION_PORT)); uint time_sync_port = std::stoul(sim::get_sim_config(sim::TIME_SYNC_PORT)); + auto simulated_interaction_port =static_cast(std::stoi(sim::get_sim_config(sim::SIM_INTERACTION_PORT))); uint v2x_port = std::stoul(sim::get_sim_config(sim::V2X_PORT)); uint sim_v2x_port = std::stoul(sim::get_sim_config(sim::SIM_V2X_PORT)); std::string infrastructure_id = sim::get_sim_config(sim::INFRASTRUCTURE_ID); @@ -95,11 +102,11 @@ namespace CDASimAdapter{ " Time Sync Port: " << std::to_string( time_sync_port) << " and V2X Port: " << std::to_string(v2x_port) << std::endl; if ( connection ) { connection.reset(new CDASimConnection( simulation_ip, infrastructure_id, simulation_registration_port, sim_v2x_port, local_ip, - time_sync_port, v2x_port, location, sensor_json_file_path)); + time_sync_port, simulated_interaction_port, v2x_port, location, sensor_json_file_path )); } else { connection = std::make_unique(simulation_ip, infrastructure_id, simulation_registration_port, sim_v2x_port, local_ip, - time_sync_port, v2x_port, location, sensor_json_file_path); + time_sync_port, simulated_interaction_port, v2x_port, location, sensor_json_file_path); } } catch (const TmxException &e) { @@ -161,6 +168,34 @@ namespace CDASimAdapter{ } } + void CDASimAdapter::start_sensor_detected_object_detection_thread() { + PLOG(logDEBUG) << "Creating Thread Timer for simulated external object" << std::endl; + try + { + if(!external_object_detection_thread_timer) + { + external_object_detection_thread_timer = std::make_unique(); + } + external_object_detection_thread_timer->AddPeriodicTick([this](){ + PLOG(logDEBUG1) << "Listening for Sensor Detected Message from CDASim." << std::endl; + auto msg = connection->consume_sensor_detected_object_message(); + if ( !msg.is_empty()) { + this->forward_simulated_detected_message(msg); + } + else + { + PLOG(logDEBUG1) << "CDASim connection has not yet received an simulated sensor detected message!" << std::endl; + } + }//End lambda + , std::chrono::milliseconds(100)); + external_object_detection_thread_timer->Start(); + } + catch ( const UdpServerRuntimeError &e ) + { + PLOG(logERROR) << "Error occured :" << e.what() << std::endl; + } + } + void CDASimAdapter::attempt_message_from_v2xhub() const { try { std::string msg = connection->consume_v2x_message_from_v2xhub(); diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp index 15621be80..982ac5f64 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp @@ -5,11 +5,11 @@ using namespace tmx::utils; namespace CDASimAdapter{ CDASimConnection::CDASimConnection(const std::string &simulation_ip, const std::string &infrastructure_id, const uint simulation_registration_port, const uint sim_v2x_port, - const std::string &local_ip, const uint time_sync_port, const uint v2x_port, + const std::string &local_ip, const uint time_sync_port,const uint simulated_interaction_port, const uint v2x_port, const Point &location, const std::string &sensor_json_file_path) : _simulation_ip(simulation_ip), _infrastructure_id(infrastructure_id), _simulation_registration_port(simulation_registration_port), - _simulation_v2x_port(sim_v2x_port), _local_ip(local_ip), _time_sync_port(time_sync_port), _v2x_port(v2x_port), - _location(location) ,_sensor_json_file_path(sensor_json_file_path) { + _simulation_v2x_port(sim_v2x_port), _local_ip(local_ip), _time_sync_port(time_sync_port), _simulated_interaction_port(simulated_interaction_port),_v2x_port(v2x_port), + _location(location) ,_sensor_json_file_path(sensor_json_file_path) { PLOG(logDEBUG) << "CARMA-Simulation connection initialized." << std::endl; } @@ -20,11 +20,11 @@ namespace CDASimAdapter{ } bool CDASimConnection::connect() { - if (!carma_simulation_handshake(_simulation_ip, _infrastructure_id, _simulation_registration_port, _local_ip, _time_sync_port, _v2x_port, _location)) { + if (!carma_simulation_handshake(_simulation_ip, _infrastructure_id, _simulation_registration_port, _local_ip, _time_sync_port, _simulated_interaction_port, _v2x_port, _location)) { _connected = false; return _connected; } - if (!setup_udp_connection(_simulation_ip, _local_ip, _time_sync_port, _v2x_port, _simulation_v2x_port )) { + if (!setup_udp_connection(_simulation_ip, _local_ip, _time_sync_port,_simulated_interaction_port, _v2x_port, _simulation_v2x_port )) { _connected = false; return _connected; } @@ -33,8 +33,9 @@ namespace CDASimAdapter{ return _connected; } - std::string CDASimConnection::get_handshake_json(const std::string &infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint v2x_port, - const Point &location) const + + std::string CDASimConnection::get_handshake_json(const std::string &infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint simulated_interaction_port, const uint v2x_port, + const tmx::utils::Point &location) const { Json::Value message; @@ -44,6 +45,7 @@ namespace CDASimAdapter{ message["infrastructureId"] = infrastructure_id; message["rxMessagePort"] = v2x_port; message["timeSyncPort"] = time_sync_port; + message["simulatedInteractionPort"] = simulated_interaction_port; message["location"]["x"] = location.X; message["location"]["y"] = location.Y; message["location"]["z"] = location.Z; @@ -62,13 +64,13 @@ namespace CDASimAdapter{ } bool CDASimConnection::carma_simulation_handshake(const std::string &simulation_ip, const std::string &infrastructure_id, const uint simulation_registration_port, - const std::string &local_ip, const uint time_sync_port, const uint v2x_port, - const Point &location) + const std::string &local_ip, const uint time_sync_port, const uint simulated_interaction_port, const uint v2x_port, + const tmx::utils::Point &location) { // Create JSON message with the content std::string payload = ""; - payload = get_handshake_json(infrastructure_id, local_ip, time_sync_port, v2x_port, location); + payload = get_handshake_json(infrastructure_id, local_ip, time_sync_port, simulated_interaction_port, v2x_port, location); try { @@ -84,7 +86,7 @@ namespace CDASimAdapter{ return true; } - bool CDASimConnection::setup_udp_connection(const std::string &simulation_ip, const std::string &local_ip, const uint time_sync_port, + bool CDASimConnection::setup_udp_connection(const std::string &simulation_ip, const std::string &local_ip, const uint time_sync_port, const uint simulated_interaction_port, const uint v2x_port, const uint simulation_v2x_port) { try { // Iniitialize CARMA Simulation UDP Server and Client to foward V2X messages between CARMA simulation @@ -101,12 +103,14 @@ namespace CDASimAdapter{ // TODO: Replace 0 with message receiver port message_receiver_publisher = std::make_shared( local_ip, 8765); // Initialize UDP Server for listening for incoming CARMA-Simulation time synchronization. - PLOG(logDEBUG) << "Creating UDPServer Time Sync Messages" << local_ip << ":" << std::to_string(time_sync_port) << "\n" - << "Creating UDPServer for CDA V2X message forwarding" << local_ip << ":" << std::to_string(v2x_port) << "\n" - << "Creating UDPClient for CDA V2X message forwarding " << simulation_ip << ":" << std::to_string(simulation_v2x_port) << "\n" + PLOG(logDEBUG) << "Creating UDPServer for Time Sync Messages: " << local_ip << ":" << std::to_string(time_sync_port) << "\n" + << "Creating UDPServer for Simulated External Object detection: " << local_ip << ":" << std::to_string(simulated_interaction_port) << "\n" + << "Creating UDPServer for CDA V2X message forwarding: " << local_ip << ":" << std::to_string(v2x_port) << "\n" + << "Creating UDPClient for CDA V2X message forwarding: " << simulation_ip << ":" << std::to_string(simulation_v2x_port) << "\n" << "Creating UDPServer for Immediate Forward " << local_ip << ":" << std::to_string(5678) << "\n" << "Creating UDPClient for Message Receiver " << local_ip << ":" << std::to_string(8765) << std::endl; time_sync_listener = std::make_shared(local_ip,time_sync_port); + sensor_detected_object_listener = std::make_shared (local_ip, simulated_interaction_port); } catch (const UdpClientRuntimeError &e) { PLOG(logERROR) << "Encountered UDPClient Runtime error during UDP connection initialization : " << e.what() << std::endl; @@ -128,12 +132,28 @@ namespace CDASimAdapter{ msg.set_contents( str_msg ); } else { - throw std::runtime_error("Time Sync UDP Server is not initialized"); + throw UdpServerRuntimeError("Time Sync UDP Server is not initialized"); } return msg; } + tmx::messages::simulation::SensorDetectedObject CDASimConnection::consume_sensor_detected_object_message() const + { + tmx::messages::simulation::SensorDetectedObject externalObj; + externalObj.clear(); + if(sensor_detected_object_listener) + { + std::string str_msg = consume_server_message(sensor_detected_object_listener); + externalObj.set_contents(str_msg); + } + else + { + throw UdpServerRuntimeError("Simulated External Object UDP Server is not initialized."); + } + return externalObj; + } + std::string CDASimConnection::consume_hex_server_message( const std::shared_ptr _server) const { std::vector msg(4000); int num_of_bytes = _server->TimedReceive(msg.data(),4000, 5); @@ -180,7 +200,7 @@ namespace CDASimAdapter{ return msg; } else { - throw std::runtime_error("CARMA Simulation UDP Server is not initialized!"); + throw UdpServerRuntimeError("CARMA Simulation UDP Server is not initialized!"); } return ""; @@ -192,7 +212,7 @@ namespace CDASimAdapter{ return msg; } else { - throw std::runtime_error("Immediate Forward UDP Server is not initialized!"); + throw UdpServerRuntimeError("Immediate Forward UDP Server is not initialized!"); } return ""; diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp index b3a979373..529cd9dc9 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp @@ -89,6 +89,11 @@ namespace CDASimAdapter * @param msg TimeSyncMessage. */ void forward_time_sync_message(tmx::messages::TimeSyncMessage &msg); + /** + * @brief Forward simulated sensor detected object message to TMX message bus for other V2X-Hub Plugin + * @param msg simulation::SensorDetectedObject. + */ + void forward_simulated_detected_message(tmx::messages::simulation::SensorDetectedObject &msg); /** * @brief Method to start thread timer for regular interval actions lauched on seperate thread. */ @@ -98,6 +103,11 @@ namespace CDASimAdapter */ void attempt_time_sync(); + /** + * @brief Method to start thread timer for regular interval actions lauched on seperate thread. + */ + void start_sensor_detected_object_detection_thread(); + private: // Simulated location of RSU tmx::utils::Point location; @@ -109,6 +119,7 @@ namespace CDASimAdapter std::unique_ptr connection; // Mutex for configuration parameter thread safety std::mutex _lock; + std::unique_ptr external_object_detection_thread_timer; // Time sync thread to forward time sync messages to PluginClientClockAware V2X-Hub plugins. std::unique_ptr time_sync_timer; // Time sync thread id diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp index bdd9d05ed..b6bb55424 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -33,9 +34,8 @@ namespace CDASimAdapter { * @param location Simulationed location of infrastructure. */ explicit CDASimConnection( const std::string &simulation_ip, const std::string &infrastructure_id, const uint simulation_registration_port, - const uint sim_v2x_port, const std::string &local_ip, const uint time_sync_port, const uint v2x_port, + const uint sim_v2x_port, const std::string &local_ip, const uint time_sync_port, const uint simulated_interaction_port, const uint v2x_port, const tmx::utils::Point &location, const std::string &sensor_json_file_path); - /** * @brief Method to forward v2x message to CARMA Simulation * @param v2x_message string @@ -83,6 +83,12 @@ namespace CDASimAdapter { * @return TimeSyncMessage. */ tmx::messages::TimeSyncMessage consume_time_sync_message() const; + /** + * @brief Method to consume incoming external object message. + * To populate the simulation external object, this JSON string has to follow this specification: https://usdot-carma.atlassian.net/wiki/spaces/CRMSIM/pages/2563899417/Detected+Objects+Specification#CARMA-Street-and-V2xHub + * @return simulation::SensorDetectedObject. + */ + tmx::messages::simulation::SensorDetectedObject consume_sensor_detected_object_message() const; /** * @brief Perform handshake with CARMA-Simulation. Will return true on successful handshakes and false if * unsuccessful. As part of the handshake should set simulation_v2x_port for forwarding v2x messages to simulation, @@ -96,7 +102,7 @@ namespace CDASimAdapter { * @return true if handshake successful and false if handshake unsuccessful. */ bool carma_simulation_handshake(const std::string &simulation_ip, const std::string &infrastructure_id, const uint simulation_registration_port, - const std::string &local_ip, const uint time_sync_port, const uint v2x_port, + const std::string &local_ip, const uint time_sync_port, const uint simulated_interaction_port, const uint v2x_port, const tmx::utils::Point &location); /** @@ -108,7 +114,7 @@ namespace CDASimAdapter { * @param simulation_v2x_port port on which CARMA-Simulation is listening for incoming v2x messages. * @return true if setup is successful and false otherwise. */ - bool setup_udp_connection(const std::string &simulation_ip, const std::string &local_ip, const uint time_sync_port, + bool setup_udp_connection(const std::string &simulation_ip, const std::string &local_ip, const uint time_sync_port, const uint simulated_interaction_port, const uint v2x_port, const uint simulation_v2x_port); /** * @brief Method to attempt to establish connection between CARMA-Simulation and infrastucture. Returns true if succesful @@ -131,19 +137,19 @@ namespace CDASimAdapter { * @param location simulated location of infrastructure hardware. * @return true if handshake successful and false if handshake unsuccessful. */ - std::string get_handshake_json(const std::string &infrastructure_id, const std::string &local_ip, const uint time_sync_port, - const uint v2x_port, const tmx::utils::Point &location) const; + std::string get_handshake_json(const std::string &infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint simulated_interaction_port, const uint v2x_port, + const tmx::utils::Point &location) const; /** - * @brief Read local file that has the sensor information in JSON format from disk. Populate global sensor json variable with the information. - * @param file_path A string of file location in the host machine. - * @return A reference to the location where the sensors inforation is updated and stored. + * @brief Read Json file specified by the file path from disk, and convert the json into Json::Value object. + * @param file_path A string of file path in the host machine. + * @return A Json::Value object. */ Json::Value read_json_file(const std::string& file_path) const; /** - * @brief Read local file that has the sensor information in JSON format from disk. Populate global sensor json variable with the information. + * @brief Convert the Json string into Json::Value object. * @param json_str A JSON string. - * @return A reference to JSON value. + * @return A Json::Value object. */ Json::Value string_to_json(const std::string &json_str) const; @@ -151,6 +157,7 @@ namespace CDASimAdapter { uint _simulation_registration_port; std::string _infrastructure_id; uint _simulation_v2x_port; + uint _simulated_interaction_port; std::string _local_ip; uint _time_sync_port; uint _v2x_port; @@ -164,6 +171,7 @@ namespace CDASimAdapter { std::shared_ptr immediate_forward_listener; std::shared_ptr message_receiver_publisher; std::shared_ptr time_sync_listener; + std::shared_ptr sensor_detected_object_listener; FRIEND_TEST(TestCARMASimulationConnection, get_handshake_json); FRIEND_TEST(TestCARMASimulationConnection, read_json_file); diff --git a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp index 2c686d170..2149ce8b4 100644 --- a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp @@ -24,7 +24,7 @@ namespace CDASimAdapter { void SetUp() override { // Initialize CARMA Simulation connection with (0,0,0) location. Point location; - connection = std::make_shared("127.0.0.1", "1212", 4567, 4678, "127.0.0.1", 1213, 1214, location, sensors_file_path); + connection = std::make_shared("127.0.0.1", "1212", 4567, 4678, "127.0.0.1", 1213, 1214, 1215, location, sensors_file_path); } void TearDown() override { @@ -79,7 +79,7 @@ namespace CDASimAdapter { } TEST_F( TestCARMASimulationConnection, setup_upd_connection) { - ASSERT_TRUE(connection->setup_udp_connection("127.0.0.1", "127.0.0.1", 4567, 4568, 4569)); + ASSERT_TRUE(connection->setup_udp_connection("127.0.0.1", "127.0.0.1", 4567, 4568, 4569, 4570)); } TEST_F( TestCARMASimulationConnection, get_handshake_json) { @@ -91,8 +91,8 @@ namespace CDASimAdapter { in_strm.open(sensors_file_path, std::ifstream::binary); if(in_strm.is_open()) { - ASSERT_EQ(connection->get_handshake_json("4566", "127.0.0.1", 4567, 4568, location), - "{\n \"infrastructureId\" : \"4566\",\n \"location\" : {\n \"x\" : 1000.0,\n \"y\" : 38.954999999999998,\n \"z\" : -77.149000000000001\n },\n \"rxMessageIpAddress\" : \"127.0.0.1\",\n \"rxMessagePort\" : 4568,\n \"sensors\" : [\n {\n \"location\" : {\n \"x\" : 0.0,\n \"y\" : 0.0,\n \"z\" : 0.0\n },\n \"orientation\" : {\n \"pitch\" : 0.0,\n \"roll\" : 0.0,\n \"yaw\" : 0.0\n },\n \"sensorId\" : \"SomeID\",\n \"type\" : \"SematicLidar\"\n },\n {\n \"location\" : {\n \"x\" : 1.0,\n \"y\" : 2.0,\n \"z\" : 0.0\n },\n \"orientation\" : {\n \"pitch\" : 0.0,\n \"roll\" : 0.0,\n \"yaw\" : 23.0\n },\n \"sensorId\" : \"SomeID2\",\n \"type\" : \"SematicLidar\"\n }\n ],\n \"timeSyncPort\" : 4567\n}\n"); + ASSERT_EQ(connection->get_handshake_json("4566", "127.0.0.1", 4567, 4568, 4569, location), + "{\n \"infrastructureId\" : \"4566\",\n \"location\" : {\n \"x\" : 1000.0,\n \"y\" : 38.954999999999998,\n \"z\" : -77.149000000000001\n },\n \"rxMessageIpAddress\" : \"127.0.0.1\",\n \"rxMessagePort\" : 4569,\n \"sensors\" : [\n {\n \"location\" : {\n \"x\" : 0.0,\n \"y\" : 0.0,\n \"z\" : 0.0\n },\n \"orientation\" : {\n \"pitch\" : 0.0,\n \"roll\" : 0.0,\n \"yaw\" : 0.0\n },\n \"sensorId\" : \"SomeID\",\n \"type\" : \"SematicLidar\"\n },\n {\n \"location\" : {\n \"x\" : 1.0,\n \"y\" : 2.0,\n \"z\" : 0.0\n },\n \"orientation\" : {\n \"pitch\" : 0.0,\n \"roll\" : 0.0,\n \"yaw\" : 23.0\n },\n \"sensorId\" : \"SomeID2\",\n \"type\" : \"SematicLidar\"\n }\n ],\n \"simulatedInteractionPort\" : 4568,\n \"timeSyncPort\" : 4567\n}\n"); } } @@ -100,7 +100,7 @@ namespace CDASimAdapter { Point location; // UDP creation error ASSERT_FALSE(connection->carma_simulation_handshake("", "45", NULL, - "", 45, 45, location)); + "", 45, 45, 45, location)); } TEST_F(TestCARMASimulationConnection, connect) { From f4a2e61db9dbcb9ab963eb028de1043812222c6e Mon Sep 17 00:00:00 2001 From: dan-du-car <62157949+dan-du-car@users.noreply.github.com> Date: Thu, 10 Aug 2023 09:03:31 -0400 Subject: [PATCH 11/28] V2xHub: Update the code for ASN1.c parser to recognize SDSM message type + unit test (#555) # PR Details ## Description As an V2XHub developer, I want to update the code for ASN.1 parser to recognize these SDSM message type. https://github.com/usdot-fhwa-stol/CARMASensitive/commit/a11650b48e350f5c54a2dfc0eb0afe14125c4080 ## Related Issue NA ## Motivation and Context CDA research ## How Has This Been Tested? Unit test ## Types of changes - [ ] Defect fix (non-breaking change that fixes an issue) - [x] 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. --- .../include/asn_j2735_r63/AngularVelocity.h | 42 ++++ .../asn_j2735_r63/AngularVelocityConfidence.h | 42 ++++ .../include/asn_j2735_r63/Attitude.h | 44 ++++ .../asn_j2735_r63/AttitudeConfidence.h | 42 ++++ .../asn_j2735_r63/ClassificationConfidence.h | 44 ++++ .../asn_j2735_r63/DetectedObjectCommonData.h | 79 +++++++ .../asn_j2735_r63/DetectedObjectData.h | 47 ++++ .../asn_j2735_r63/DetectedObjectList.h | 47 ++++ .../DetectedObjectOptionalData.h | 56 +++++ .../asn_j2735_r63/DetectedObstacleData.h | 42 ++++ .../include/asn_j2735_r63/DetectedVRUData.h | 51 ++++ .../asn_j2735_r63/DetectedVehicleData.h | 72 ++++++ .../include/asn_j2735_r63/EquipmentType.h | 56 +++++ .../asn_j2735_r63/MeasurementTimeOffset.h | 44 ++++ .../include/asn_j2735_r63/MessageFrame.h | 9 +- .../include/asn_j2735_r63/ObjectDistance.h | 44 ++++ .../include/asn_j2735_r63/ObjectID.h | 44 ++++ .../include/asn_j2735_r63/ObjectType.h | 56 +++++ .../include/asn_j2735_r63/ObstacleSize.h | 42 ++++ .../asn_j2735_r63/ObstacleSizeConfidence.h | 42 ++++ .../include/asn_j2735_r63/PitchDetected.h | 44 ++++ .../include/asn_j2735_r63/PitchRate.h | 44 ++++ .../asn_j2735_r63/PitchRateConfidence.h | 57 +++++ .../include/asn_j2735_r63/PositionOffsetXYZ.h | 42 ++++ .../include/asn_j2735_r63/RollDetected.h | 44 ++++ .../include/asn_j2735_r63/RollRate.h | 44 ++++ .../asn_j2735_r63/RollRateConfidence.h | 57 +++++ .../asn_j2735_r63/SensorDataSharingMessage.h | 54 +++++ .../include/asn_j2735_r63/SizeValue.h | 44 ++++ .../asn_j2735_r63/SizeValueConfidence.h | 63 +++++ .../asn_j2735_r63/VehicleSizeConfidence.h | 42 ++++ .../include/asn_j2735_r63/YawDetected.h | 44 ++++ src/tmx/Asn_J2735/src/r63/AngularVelocity.c | 60 +++++ .../src/r63/AngularVelocityConfidence.c | 62 +++++ src/tmx/Asn_J2735/src/r63/Attitude.c | 70 ++++++ .../Asn_J2735/src/r63/AttitudeConfidence.c | 70 ++++++ .../src/r63/ClassificationConfidence.c | 64 +++++ .../src/r63/DetectedObjectCommonData.c | 222 ++++++++++++++++++ .../Asn_J2735/src/r63/DetectedObjectData.c | 62 +++++ .../Asn_J2735/src/r63/DetectedObjectList.c | 52 ++++ .../src/r63/DetectedObjectOptionalData.c | 75 ++++++ .../Asn_J2735/src/r63/DetectedObstacleData.c | 60 +++++ src/tmx/Asn_J2735/src/r63/DetectedVRUData.c | 82 +++++++ .../Asn_J2735/src/r63/DetectedVehicleData.c | 142 +++++++++++ src/tmx/Asn_J2735/src/r63/EquipmentType.c | 62 +++++ .../Asn_J2735/src/r63/MeasurementTimeOffset.c | 64 +++++ src/tmx/Asn_J2735/src/r63/MessageFrame.c | 87 ++++--- src/tmx/Asn_J2735/src/r63/ObjectDistance.c | 64 +++++ src/tmx/Asn_J2735/src/r63/ObjectID.c | 64 +++++ src/tmx/Asn_J2735/src/r63/ObjectType.c | 62 +++++ src/tmx/Asn_J2735/src/r63/ObstacleSize.c | 72 ++++++ .../src/r63/ObstacleSizeConfidence.c | 72 ++++++ src/tmx/Asn_J2735/src/r63/PitchDetected.c | 64 +++++ src/tmx/Asn_J2735/src/r63/PitchRate.c | 64 +++++ .../Asn_J2735/src/r63/PitchRateConfidence.c | 68 ++++++ src/tmx/Asn_J2735/src/r63/PositionOffsetXYZ.c | 72 ++++++ src/tmx/Asn_J2735/src/r63/RollDetected.c | 64 +++++ src/tmx/Asn_J2735/src/r63/RollRate.c | 64 +++++ .../Asn_J2735/src/r63/RollRateConfidence.c | 68 ++++++ .../src/r63/SensorDataSharingMessage.c | 122 ++++++++++ src/tmx/Asn_J2735/src/r63/SizeValue.c | 64 +++++ .../Asn_J2735/src/r63/SizeValueConfidence.c | 80 +++++++ .../Asn_J2735/src/r63/VehicleSizeConfidence.c | 72 ++++++ src/tmx/Asn_J2735/src/r63/YawDetected.c | 64 +++++ src/tmx/TmxApi/tmx/TmxApiMessages.h | 4 + .../j2735_messages/J2735MessageFactory.hpp | 5 +- .../SensorDataSharingMessage.hpp | 15 ++ src/tmx/TmxUtils/test/J2735MessageTest.cpp | 57 +++++ 68 files changed, 3998 insertions(+), 41 deletions(-) create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/AngularVelocity.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/AngularVelocityConfidence.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/Attitude.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/AttitudeConfidence.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/ClassificationConfidence.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObjectCommonData.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObjectData.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObjectList.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObjectOptionalData.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObstacleData.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedVRUData.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedVehicleData.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/EquipmentType.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/MeasurementTimeOffset.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/ObjectDistance.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/ObjectID.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/ObjectType.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/ObstacleSize.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/ObstacleSizeConfidence.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/PitchDetected.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/PitchRate.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/PitchRateConfidence.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/PositionOffsetXYZ.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/RollDetected.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/RollRate.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/RollRateConfidence.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/SensorDataSharingMessage.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/SizeValue.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/SizeValueConfidence.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/VehicleSizeConfidence.h create mode 100644 src/tmx/Asn_J2735/include/asn_j2735_r63/YawDetected.h create mode 100644 src/tmx/Asn_J2735/src/r63/AngularVelocity.c create mode 100644 src/tmx/Asn_J2735/src/r63/AngularVelocityConfidence.c create mode 100644 src/tmx/Asn_J2735/src/r63/Attitude.c create mode 100644 src/tmx/Asn_J2735/src/r63/AttitudeConfidence.c create mode 100644 src/tmx/Asn_J2735/src/r63/ClassificationConfidence.c create mode 100644 src/tmx/Asn_J2735/src/r63/DetectedObjectCommonData.c create mode 100644 src/tmx/Asn_J2735/src/r63/DetectedObjectData.c create mode 100644 src/tmx/Asn_J2735/src/r63/DetectedObjectList.c create mode 100644 src/tmx/Asn_J2735/src/r63/DetectedObjectOptionalData.c create mode 100644 src/tmx/Asn_J2735/src/r63/DetectedObstacleData.c create mode 100644 src/tmx/Asn_J2735/src/r63/DetectedVRUData.c create mode 100644 src/tmx/Asn_J2735/src/r63/DetectedVehicleData.c create mode 100644 src/tmx/Asn_J2735/src/r63/EquipmentType.c create mode 100644 src/tmx/Asn_J2735/src/r63/MeasurementTimeOffset.c create mode 100644 src/tmx/Asn_J2735/src/r63/ObjectDistance.c create mode 100644 src/tmx/Asn_J2735/src/r63/ObjectID.c create mode 100644 src/tmx/Asn_J2735/src/r63/ObjectType.c create mode 100644 src/tmx/Asn_J2735/src/r63/ObstacleSize.c create mode 100644 src/tmx/Asn_J2735/src/r63/ObstacleSizeConfidence.c create mode 100644 src/tmx/Asn_J2735/src/r63/PitchDetected.c create mode 100644 src/tmx/Asn_J2735/src/r63/PitchRate.c create mode 100644 src/tmx/Asn_J2735/src/r63/PitchRateConfidence.c create mode 100644 src/tmx/Asn_J2735/src/r63/PositionOffsetXYZ.c create mode 100644 src/tmx/Asn_J2735/src/r63/RollDetected.c create mode 100644 src/tmx/Asn_J2735/src/r63/RollRate.c create mode 100644 src/tmx/Asn_J2735/src/r63/RollRateConfidence.c create mode 100644 src/tmx/Asn_J2735/src/r63/SensorDataSharingMessage.c create mode 100644 src/tmx/Asn_J2735/src/r63/SizeValue.c create mode 100644 src/tmx/Asn_J2735/src/r63/SizeValueConfidence.c create mode 100644 src/tmx/Asn_J2735/src/r63/VehicleSizeConfidence.c create mode 100644 src/tmx/Asn_J2735/src/r63/YawDetected.c create mode 100644 src/tmx/TmxApi/tmx/j2735_messages/SensorDataSharingMessage.hpp diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/AngularVelocity.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/AngularVelocity.h new file mode 100644 index 000000000..2cb35471e --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/AngularVelocity.h @@ -0,0 +1,42 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _AngularVelocity_H_ +#define _AngularVelocity_H_ + + +#include + +/* Including external dependencies */ +#include "PitchRate.h" +#include "RollRate.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* AngularVelocity */ +typedef struct AngularVelocity { + PitchRate_t pitchRate; + RollRate_t rollRate; + + /* Context for parsing across buffer boundaries */ + asn_struct_ctx_t _asn_ctx; +} AngularVelocity_t; + +/* Implementation */ +extern asn_TYPE_descriptor_t asn_DEF_AngularVelocity; +extern asn_SEQUENCE_specifics_t asn_SPC_AngularVelocity_specs_1; +extern asn_TYPE_member_t asn_MBR_AngularVelocity_1[2]; + +#ifdef __cplusplus +} +#endif + +#endif /* _AngularVelocity_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/AngularVelocityConfidence.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/AngularVelocityConfidence.h new file mode 100644 index 000000000..e1d1e73f1 --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/AngularVelocityConfidence.h @@ -0,0 +1,42 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _AngularVelocityConfidence_H_ +#define _AngularVelocityConfidence_H_ + + +#include + +/* Including external dependencies */ +#include "PitchRateConfidence.h" +#include "RollRateConfidence.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* AngularVelocityConfidence */ +typedef struct AngularVelocityConfidence { + PitchRateConfidence_t *pitchRateConfidence /* OPTIONAL */; + RollRateConfidence_t *rollRateConfidence /* OPTIONAL */; + + /* Context for parsing across buffer boundaries */ + asn_struct_ctx_t _asn_ctx; +} AngularVelocityConfidence_t; + +/* Implementation */ +extern asn_TYPE_descriptor_t asn_DEF_AngularVelocityConfidence; +extern asn_SEQUENCE_specifics_t asn_SPC_AngularVelocityConfidence_specs_1; +extern asn_TYPE_member_t asn_MBR_AngularVelocityConfidence_1[2]; + +#ifdef __cplusplus +} +#endif + +#endif /* _AngularVelocityConfidence_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/Attitude.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/Attitude.h new file mode 100644 index 000000000..b2316ff61 --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/Attitude.h @@ -0,0 +1,44 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _Attitude_H_ +#define _Attitude_H_ + + +#include + +/* Including external dependencies */ +#include "PitchDetected.h" +#include "RollDetected.h" +#include "YawDetected.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Attitude */ +typedef struct Attitude { + PitchDetected_t pitch; + RollDetected_t roll; + YawDetected_t yaw; + + /* Context for parsing across buffer boundaries */ + asn_struct_ctx_t _asn_ctx; +} Attitude_t; + +/* Implementation */ +extern asn_TYPE_descriptor_t asn_DEF_Attitude; +extern asn_SEQUENCE_specifics_t asn_SPC_Attitude_specs_1; +extern asn_TYPE_member_t asn_MBR_Attitude_1[3]; + +#ifdef __cplusplus +} +#endif + +#endif /* _Attitude_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/AttitudeConfidence.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/AttitudeConfidence.h new file mode 100644 index 000000000..b6377270b --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/AttitudeConfidence.h @@ -0,0 +1,42 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _AttitudeConfidence_H_ +#define _AttitudeConfidence_H_ + + +#include + +/* Including external dependencies */ +#include "HeadingConfidence.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* AttitudeConfidence */ +typedef struct AttitudeConfidence { + HeadingConfidence_t pitchConfidence; + HeadingConfidence_t rollConfidence; + HeadingConfidence_t yawConfidence; + + /* Context for parsing across buffer boundaries */ + asn_struct_ctx_t _asn_ctx; +} AttitudeConfidence_t; + +/* Implementation */ +extern asn_TYPE_descriptor_t asn_DEF_AttitudeConfidence; +extern asn_SEQUENCE_specifics_t asn_SPC_AttitudeConfidence_specs_1; +extern asn_TYPE_member_t asn_MBR_AttitudeConfidence_1[3]; + +#ifdef __cplusplus +} +#endif + +#endif /* _AttitudeConfidence_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/ClassificationConfidence.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/ClassificationConfidence.h new file mode 100644 index 000000000..155280700 --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/ClassificationConfidence.h @@ -0,0 +1,44 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _ClassificationConfidence_H_ +#define _ClassificationConfidence_H_ + + +#include + +/* Including external dependencies */ +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* ClassificationConfidence */ +typedef long ClassificationConfidence_t; + +/* Implementation */ +extern asn_per_constraints_t asn_PER_type_ClassificationConfidence_constr_1; +extern asn_TYPE_descriptor_t asn_DEF_ClassificationConfidence; +asn_struct_free_f ClassificationConfidence_free; +asn_struct_print_f ClassificationConfidence_print; +asn_constr_check_f ClassificationConfidence_constraint; +ber_type_decoder_f ClassificationConfidence_decode_ber; +der_type_encoder_f ClassificationConfidence_encode_der; +xer_type_decoder_f ClassificationConfidence_decode_xer; +xer_type_encoder_f ClassificationConfidence_encode_xer; +oer_type_decoder_f ClassificationConfidence_decode_oer; +oer_type_encoder_f ClassificationConfidence_encode_oer; +per_type_decoder_f ClassificationConfidence_decode_uper; +per_type_encoder_f ClassificationConfidence_encode_uper; + +#ifdef __cplusplus +} +#endif + +#endif /* _ClassificationConfidence_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObjectCommonData.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObjectCommonData.h new file mode 100644 index 000000000..47a21dd85 --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObjectCommonData.h @@ -0,0 +1,79 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _DetectedObjectCommonData_H_ +#define _DetectedObjectCommonData_H_ + + +#include + +/* Including external dependencies */ +#include "ObjectType.h" +#include "ClassificationConfidence.h" +#include "ObjectID.h" +#include "MeasurementTimeOffset.h" +#include "TimeConfidence.h" +#include "PositionOffsetXYZ.h" +#include "PositionConfidenceSet.h" +#include "Speed.h" +#include "SpeedConfidence.h" +#include "Heading.h" +#include "HeadingConfidence.h" +#include "AccelerationConfidence.h" +#include "YawRateConfidence.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Forward declarations */ +struct AccelerationSet4Way; + +/* DetectedObjectCommonData */ +typedef struct DetectedObjectCommonData { + ObjectType_t objType; + ClassificationConfidence_t objTypeCfd; + ObjectID_t objectID; + MeasurementTimeOffset_t measurementTime; + TimeConfidence_t timeConfidence; + PositionOffsetXYZ_t pos; + PositionConfidenceSet_t posConfidence; + Speed_t speed; + SpeedConfidence_t speedConfidence; + Speed_t *speedZ /* OPTIONAL */; + SpeedConfidence_t *speedConfidenceZ /* OPTIONAL */; + Heading_t heading; + HeadingConfidence_t headingConf; + struct AccelerationSet4Way *accel4way /* OPTIONAL */; + AccelerationConfidence_t *accCfdX /* OPTIONAL */; + AccelerationConfidence_t *accCfdY /* OPTIONAL */; + AccelerationConfidence_t *accCfdZ /* OPTIONAL */; + YawRateConfidence_t *accCfdYaw /* OPTIONAL */; + /* + * This type is extensible, + * possible extensions are below. + */ + + /* Context for parsing across buffer boundaries */ + asn_struct_ctx_t _asn_ctx; +} DetectedObjectCommonData_t; + +/* Implementation */ +extern asn_TYPE_descriptor_t asn_DEF_DetectedObjectCommonData; +extern asn_SEQUENCE_specifics_t asn_SPC_DetectedObjectCommonData_specs_1; +extern asn_TYPE_member_t asn_MBR_DetectedObjectCommonData_1[18]; + +#ifdef __cplusplus +} +#endif + +/* Referred external types */ +#include "AccelerationSet4Way.h" + +#endif /* _DetectedObjectCommonData_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObjectData.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObjectData.h new file mode 100644 index 000000000..e856ddf5f --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObjectData.h @@ -0,0 +1,47 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _DetectedObjectData_H_ +#define _DetectedObjectData_H_ + + +#include + +/* Including external dependencies */ +#include "DetectedObjectCommonData.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Forward declarations */ +struct DetectedObjectOptionalData; + +/* DetectedObjectData */ +typedef struct DetectedObjectData { + DetectedObjectCommonData_t detObjCommon; + struct DetectedObjectOptionalData *detObjOptData /* OPTIONAL */; + + /* Context for parsing across buffer boundaries */ + asn_struct_ctx_t _asn_ctx; +} DetectedObjectData_t; + +/* Implementation */ +extern asn_TYPE_descriptor_t asn_DEF_DetectedObjectData; +extern asn_SEQUENCE_specifics_t asn_SPC_DetectedObjectData_specs_1; +extern asn_TYPE_member_t asn_MBR_DetectedObjectData_1[2]; + +#ifdef __cplusplus +} +#endif + +/* Referred external types */ +#include "DetectedObjectOptionalData.h" + +#endif /* _DetectedObjectData_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObjectList.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObjectList.h new file mode 100644 index 000000000..c114380dd --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObjectList.h @@ -0,0 +1,47 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _DetectedObjectList_H_ +#define _DetectedObjectList_H_ + + +#include + +/* Including external dependencies */ +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Forward declarations */ +struct DetectedObjectData; + +/* DetectedObjectList */ +typedef struct DetectedObjectList { + A_SEQUENCE_OF(struct DetectedObjectData) list; + + /* Context for parsing across buffer boundaries */ + asn_struct_ctx_t _asn_ctx; +} DetectedObjectList_t; + +/* Implementation */ +extern asn_TYPE_descriptor_t asn_DEF_DetectedObjectList; +extern asn_SET_OF_specifics_t asn_SPC_DetectedObjectList_specs_1; +extern asn_TYPE_member_t asn_MBR_DetectedObjectList_1[1]; +extern asn_per_constraints_t asn_PER_type_DetectedObjectList_constr_1; + +#ifdef __cplusplus +} +#endif + +/* Referred external types */ +#include "DetectedObjectData.h" + +#endif /* _DetectedObjectList_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObjectOptionalData.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObjectOptionalData.h new file mode 100644 index 000000000..97b9624cf --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObjectOptionalData.h @@ -0,0 +1,56 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _DetectedObjectOptionalData_H_ +#define _DetectedObjectOptionalData_H_ + + +#include + +/* Including external dependencies */ +#include "DetectedVehicleData.h" +#include "DetectedVRUData.h" +#include "DetectedObstacleData.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Dependencies */ +typedef enum DetectedObjectOptionalData_PR { + DetectedObjectOptionalData_PR_NOTHING, /* No components present */ + DetectedObjectOptionalData_PR_detVeh, + DetectedObjectOptionalData_PR_detVRU, + DetectedObjectOptionalData_PR_detObst +} DetectedObjectOptionalData_PR; + +/* DetectedObjectOptionalData */ +typedef struct DetectedObjectOptionalData { + DetectedObjectOptionalData_PR present; + union DetectedObjectOptionalData_u { + DetectedVehicleData_t detVeh; + DetectedVRUData_t detVRU; + DetectedObstacleData_t detObst; + } choice; + + /* Context for parsing across buffer boundaries */ + asn_struct_ctx_t _asn_ctx; +} DetectedObjectOptionalData_t; + +/* Implementation */ +extern asn_TYPE_descriptor_t asn_DEF_DetectedObjectOptionalData; +extern asn_CHOICE_specifics_t asn_SPC_DetectedObjectOptionalData_specs_1; +extern asn_TYPE_member_t asn_MBR_DetectedObjectOptionalData_1[3]; +extern asn_per_constraints_t asn_PER_type_DetectedObjectOptionalData_constr_1; + +#ifdef __cplusplus +} +#endif + +#endif /* _DetectedObjectOptionalData_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObstacleData.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObstacleData.h new file mode 100644 index 000000000..b6f5ad78e --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedObstacleData.h @@ -0,0 +1,42 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _DetectedObstacleData_H_ +#define _DetectedObstacleData_H_ + + +#include + +/* Including external dependencies */ +#include "ObstacleSize.h" +#include "ObstacleSizeConfidence.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* DetectedObstacleData */ +typedef struct DetectedObstacleData { + ObstacleSize_t obstSize; + ObstacleSizeConfidence_t obstSizeConfidence; + + /* Context for parsing across buffer boundaries */ + asn_struct_ctx_t _asn_ctx; +} DetectedObstacleData_t; + +/* Implementation */ +extern asn_TYPE_descriptor_t asn_DEF_DetectedObstacleData; +extern asn_SEQUENCE_specifics_t asn_SPC_DetectedObstacleData_specs_1; +extern asn_TYPE_member_t asn_MBR_DetectedObstacleData_1[2]; + +#ifdef __cplusplus +} +#endif + +#endif /* _DetectedObstacleData_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedVRUData.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedVRUData.h new file mode 100644 index 000000000..5bfaffda3 --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedVRUData.h @@ -0,0 +1,51 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _DetectedVRUData_H_ +#define _DetectedVRUData_H_ + + +#include + +/* Including external dependencies */ +#include "PersonalDeviceUserType.h" +#include "Attachment.h" +#include "AttachmentRadius.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Forward declarations */ +struct PropelledInformation; + +/* DetectedVRUData */ +typedef struct DetectedVRUData { + PersonalDeviceUserType_t *basicType /* OPTIONAL */; + struct PropelledInformation *propulsion /* OPTIONAL */; + Attachment_t *attachment /* OPTIONAL */; + AttachmentRadius_t *radius /* OPTIONAL */; + + /* Context for parsing across buffer boundaries */ + asn_struct_ctx_t _asn_ctx; +} DetectedVRUData_t; + +/* Implementation */ +extern asn_TYPE_descriptor_t asn_DEF_DetectedVRUData; +extern asn_SEQUENCE_specifics_t asn_SPC_DetectedVRUData_specs_1; +extern asn_TYPE_member_t asn_MBR_DetectedVRUData_1[4]; + +#ifdef __cplusplus +} +#endif + +/* Referred external types */ +#include "PropelledInformation.h" + +#endif /* _DetectedVRUData_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedVehicleData.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedVehicleData.h new file mode 100644 index 000000000..49a988e5d --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/DetectedVehicleData.h @@ -0,0 +1,72 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _DetectedVehicleData_H_ +#define _DetectedVehicleData_H_ + + +#include + +/* Including external dependencies */ +#include "ExteriorLights.h" +#include "VehicleHeight.h" +#include "BasicVehicleClass.h" +#include "ClassificationConfidence.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Forward declarations */ +struct Attitude; +struct AttitudeConfidence; +struct AngularVelocity; +struct AngularVelocityConfidence; +struct VehicleSize; +struct VehicleSizeConfidence; + +/* DetectedVehicleData */ +typedef struct DetectedVehicleData { + ExteriorLights_t *lights /* OPTIONAL */; + struct Attitude *vehAttitude /* OPTIONAL */; + struct AttitudeConfidence *vehAttitudeConfidence /* OPTIONAL */; + struct AngularVelocity *vehAngVel /* OPTIONAL */; + struct AngularVelocityConfidence *vehAngVelConfidence /* OPTIONAL */; + struct VehicleSize *size /* OPTIONAL */; + VehicleHeight_t *height /* OPTIONAL */; + struct VehicleSizeConfidence *vehicleSizeConfidence /* OPTIONAL */; + BasicVehicleClass_t *vehicleClass /* OPTIONAL */; + ClassificationConfidence_t *classConf /* OPTIONAL */; + /* + * This type is extensible, + * possible extensions are below. + */ + + /* Context for parsing across buffer boundaries */ + asn_struct_ctx_t _asn_ctx; +} DetectedVehicleData_t; + +/* Implementation */ +extern asn_TYPE_descriptor_t asn_DEF_DetectedVehicleData; +extern asn_SEQUENCE_specifics_t asn_SPC_DetectedVehicleData_specs_1; +extern asn_TYPE_member_t asn_MBR_DetectedVehicleData_1[10]; + +#ifdef __cplusplus +} +#endif + +/* Referred external types */ +#include "Attitude.h" +#include "AttitudeConfidence.h" +#include "AngularVelocity.h" +#include "AngularVelocityConfidence.h" +#include "VehicleSize.h" +#include "VehicleSizeConfidence.h" + +#endif /* _DetectedVehicleData_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/EquipmentType.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/EquipmentType.h new file mode 100644 index 000000000..2d1799bb4 --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/EquipmentType.h @@ -0,0 +1,56 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _EquipmentType_H_ +#define _EquipmentType_H_ + + +#include + +/* Including external dependencies */ +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Dependencies */ +typedef enum EquipmentType { + EquipmentType_unknown = 0, + EquipmentType_rsu = 1, + EquipmentType_obu = 2, + EquipmentType_vru = 3 + /* + * Enumeration is extensible + */ +} e_EquipmentType; + +/* EquipmentType */ +typedef long EquipmentType_t; + +/* Implementation */ +extern asn_per_constraints_t asn_PER_type_EquipmentType_constr_1; +extern asn_TYPE_descriptor_t asn_DEF_EquipmentType; +extern const asn_INTEGER_specifics_t asn_SPC_EquipmentType_specs_1; +asn_struct_free_f EquipmentType_free; +asn_struct_print_f EquipmentType_print; +asn_constr_check_f EquipmentType_constraint; +ber_type_decoder_f EquipmentType_decode_ber; +der_type_encoder_f EquipmentType_encode_der; +xer_type_decoder_f EquipmentType_decode_xer; +xer_type_encoder_f EquipmentType_encode_xer; +oer_type_decoder_f EquipmentType_decode_oer; +oer_type_encoder_f EquipmentType_encode_oer; +per_type_decoder_f EquipmentType_decode_uper; +per_type_encoder_f EquipmentType_encode_uper; + +#ifdef __cplusplus +} +#endif + +#endif /* _EquipmentType_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/MeasurementTimeOffset.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/MeasurementTimeOffset.h new file mode 100644 index 000000000..59c1c0f1f --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/MeasurementTimeOffset.h @@ -0,0 +1,44 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names` + */ + +#ifndef _MeasurementTimeOffset_H_ +#define _MeasurementTimeOffset_H_ + + +#include + +/* Including external dependencies */ +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* MeasurementTimeOffset */ +typedef long MeasurementTimeOffset_t; + +/* Implementation */ +extern asn_per_constraints_t asn_PER_type_MeasurementTimeOffset_constr_1; +extern asn_TYPE_descriptor_t asn_DEF_MeasurementTimeOffset; +asn_struct_free_f MeasurementTimeOffset_free; +asn_struct_print_f MeasurementTimeOffset_print; +asn_constr_check_f MeasurementTimeOffset_constraint; +ber_type_decoder_f MeasurementTimeOffset_decode_ber; +der_type_encoder_f MeasurementTimeOffset_encode_der; +xer_type_decoder_f MeasurementTimeOffset_decode_xer; +xer_type_encoder_f MeasurementTimeOffset_encode_xer; +oer_type_decoder_f MeasurementTimeOffset_decode_oer; +oer_type_encoder_f MeasurementTimeOffset_encode_oer; +per_type_decoder_f MeasurementTimeOffset_decode_uper; +per_type_encoder_f MeasurementTimeOffset_encode_uper; + +#ifdef __cplusplus +} +#endif + +#endif /* _MeasurementTimeOffset_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/MessageFrame.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/MessageFrame.h index ff77ab6f5..422fc814a 100644 --- a/src/tmx/Asn_J2735/include/asn_j2735_r63/MessageFrame.h +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/MessageFrame.h @@ -1,8 +1,8 @@ /* * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) * From ASN.1 module "DSRC" - * found in "J2735_201603_ASN_CC.asn" - * `asn1c -gen-PER -fcompound-names -fincludes-quoted` + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names` */ #ifndef _MessageFrame_H_ @@ -46,6 +46,7 @@ #include "TestMessage13.h" #include "TestMessage14.h" #include "TestMessage15.h" +#include "SensorDataSharingMessage.h" #include "OPEN_TYPE.h" #include "constr_CHOICE.h" #include "constr_SEQUENCE.h" @@ -87,7 +88,8 @@ typedef enum value_PR { value_PR_TestMessage12, value_PR_TestMessage13, value_PR_TestMessage14, - value_PR_TestMessage15 + value_PR_TestMessage15, + value_PR_SensorDataSharingMessage } value_PR; /* MessageFrame */ @@ -127,6 +129,7 @@ typedef struct MessageFrame { TestMessage13_t TestMessage13; TestMessage14_t TestMessage14; TestMessage15_t TestMessage15; + SensorDataSharingMessage_t SensorDataSharingMessage; } choice; /* Context for parsing across buffer boundaries */ diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/ObjectDistance.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/ObjectDistance.h new file mode 100644 index 000000000..db97608e2 --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/ObjectDistance.h @@ -0,0 +1,44 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _ObjectDistance_H_ +#define _ObjectDistance_H_ + + +#include + +/* Including external dependencies */ +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* ObjectDistance */ +typedef long ObjectDistance_t; + +/* Implementation */ +extern asn_per_constraints_t asn_PER_type_ObjectDistance_constr_1; +extern asn_TYPE_descriptor_t asn_DEF_ObjectDistance; +asn_struct_free_f ObjectDistance_free; +asn_struct_print_f ObjectDistance_print; +asn_constr_check_f ObjectDistance_constraint; +ber_type_decoder_f ObjectDistance_decode_ber; +der_type_encoder_f ObjectDistance_encode_der; +xer_type_decoder_f ObjectDistance_decode_xer; +xer_type_encoder_f ObjectDistance_encode_xer; +oer_type_decoder_f ObjectDistance_decode_oer; +oer_type_encoder_f ObjectDistance_encode_oer; +per_type_decoder_f ObjectDistance_decode_uper; +per_type_encoder_f ObjectDistance_encode_uper; + +#ifdef __cplusplus +} +#endif + +#endif /* _ObjectDistance_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/ObjectID.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/ObjectID.h new file mode 100644 index 000000000..3b0a53b78 --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/ObjectID.h @@ -0,0 +1,44 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _ObjectID_H_ +#define _ObjectID_H_ + + +#include + +/* Including external dependencies */ +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* ObjectID */ +typedef long ObjectID_t; + +/* Implementation */ +extern asn_per_constraints_t asn_PER_type_ObjectID_constr_1; +extern asn_TYPE_descriptor_t asn_DEF_ObjectID; +asn_struct_free_f ObjectID_free; +asn_struct_print_f ObjectID_print; +asn_constr_check_f ObjectID_constraint; +ber_type_decoder_f ObjectID_decode_ber; +der_type_encoder_f ObjectID_encode_der; +xer_type_decoder_f ObjectID_decode_xer; +xer_type_encoder_f ObjectID_encode_xer; +oer_type_decoder_f ObjectID_decode_oer; +oer_type_encoder_f ObjectID_encode_oer; +per_type_decoder_f ObjectID_decode_uper; +per_type_encoder_f ObjectID_encode_uper; + +#ifdef __cplusplus +} +#endif + +#endif /* _ObjectID_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/ObjectType.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/ObjectType.h new file mode 100644 index 000000000..52837430b --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/ObjectType.h @@ -0,0 +1,56 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _ObjectType_H_ +#define _ObjectType_H_ + + +#include + +/* Including external dependencies */ +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Dependencies */ +typedef enum ObjectType { + ObjectType_unknown = 0, + ObjectType_vehicle = 1, + ObjectType_vru = 2, + ObjectType_animal = 3 + /* + * Enumeration is extensible + */ +} e_ObjectType; + +/* ObjectType */ +typedef long ObjectType_t; + +/* Implementation */ +extern asn_per_constraints_t asn_PER_type_ObjectType_constr_1; +extern asn_TYPE_descriptor_t asn_DEF_ObjectType; +extern const asn_INTEGER_specifics_t asn_SPC_ObjectType_specs_1; +asn_struct_free_f ObjectType_free; +asn_struct_print_f ObjectType_print; +asn_constr_check_f ObjectType_constraint; +ber_type_decoder_f ObjectType_decode_ber; +der_type_encoder_f ObjectType_encode_der; +xer_type_decoder_f ObjectType_decode_xer; +xer_type_encoder_f ObjectType_encode_xer; +oer_type_decoder_f ObjectType_decode_oer; +oer_type_encoder_f ObjectType_encode_oer; +per_type_decoder_f ObjectType_decode_uper; +per_type_encoder_f ObjectType_encode_uper; + +#ifdef __cplusplus +} +#endif + +#endif /* _ObjectType_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/ObstacleSize.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/ObstacleSize.h new file mode 100644 index 000000000..12206d92c --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/ObstacleSize.h @@ -0,0 +1,42 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _ObstacleSize_H_ +#define _ObstacleSize_H_ + + +#include + +/* Including external dependencies */ +#include "SizeValue.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* ObstacleSize */ +typedef struct ObstacleSize { + SizeValue_t width; + SizeValue_t length; + SizeValue_t *height /* OPTIONAL */; + + /* Context for parsing across buffer boundaries */ + asn_struct_ctx_t _asn_ctx; +} ObstacleSize_t; + +/* Implementation */ +extern asn_TYPE_descriptor_t asn_DEF_ObstacleSize; +extern asn_SEQUENCE_specifics_t asn_SPC_ObstacleSize_specs_1; +extern asn_TYPE_member_t asn_MBR_ObstacleSize_1[3]; + +#ifdef __cplusplus +} +#endif + +#endif /* _ObstacleSize_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/ObstacleSizeConfidence.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/ObstacleSizeConfidence.h new file mode 100644 index 000000000..1ef36fbc5 --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/ObstacleSizeConfidence.h @@ -0,0 +1,42 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _ObstacleSizeConfidence_H_ +#define _ObstacleSizeConfidence_H_ + + +#include + +/* Including external dependencies */ +#include "SizeValueConfidence.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* ObstacleSizeConfidence */ +typedef struct ObstacleSizeConfidence { + SizeValueConfidence_t widthConfidence; + SizeValueConfidence_t lengthConfidence; + SizeValueConfidence_t *heightConfidence /* OPTIONAL */; + + /* Context for parsing across buffer boundaries */ + asn_struct_ctx_t _asn_ctx; +} ObstacleSizeConfidence_t; + +/* Implementation */ +extern asn_TYPE_descriptor_t asn_DEF_ObstacleSizeConfidence; +extern asn_SEQUENCE_specifics_t asn_SPC_ObstacleSizeConfidence_specs_1; +extern asn_TYPE_member_t asn_MBR_ObstacleSizeConfidence_1[3]; + +#ifdef __cplusplus +} +#endif + +#endif /* _ObstacleSizeConfidence_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/PitchDetected.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/PitchDetected.h new file mode 100644 index 000000000..a028235b2 --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/PitchDetected.h @@ -0,0 +1,44 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _PitchDetected_H_ +#define _PitchDetected_H_ + + +#include + +/* Including external dependencies */ +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* PitchDetected */ +typedef long PitchDetected_t; + +/* Implementation */ +extern asn_per_constraints_t asn_PER_type_PitchDetected_constr_1; +extern asn_TYPE_descriptor_t asn_DEF_PitchDetected; +asn_struct_free_f PitchDetected_free; +asn_struct_print_f PitchDetected_print; +asn_constr_check_f PitchDetected_constraint; +ber_type_decoder_f PitchDetected_decode_ber; +der_type_encoder_f PitchDetected_encode_der; +xer_type_decoder_f PitchDetected_decode_xer; +xer_type_encoder_f PitchDetected_encode_xer; +oer_type_decoder_f PitchDetected_decode_oer; +oer_type_encoder_f PitchDetected_encode_oer; +per_type_decoder_f PitchDetected_decode_uper; +per_type_encoder_f PitchDetected_encode_uper; + +#ifdef __cplusplus +} +#endif + +#endif /* _PitchDetected_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/PitchRate.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/PitchRate.h new file mode 100644 index 000000000..145045cd2 --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/PitchRate.h @@ -0,0 +1,44 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _PitchRate_H_ +#define _PitchRate_H_ + + +#include + +/* Including external dependencies */ +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* PitchRate */ +typedef long PitchRate_t; + +/* Implementation */ +extern asn_per_constraints_t asn_PER_type_PitchRate_constr_1; +extern asn_TYPE_descriptor_t asn_DEF_PitchRate; +asn_struct_free_f PitchRate_free; +asn_struct_print_f PitchRate_print; +asn_constr_check_f PitchRate_constraint; +ber_type_decoder_f PitchRate_decode_ber; +der_type_encoder_f PitchRate_encode_der; +xer_type_decoder_f PitchRate_decode_xer; +xer_type_encoder_f PitchRate_encode_xer; +oer_type_decoder_f PitchRate_decode_oer; +oer_type_encoder_f PitchRate_encode_oer; +per_type_decoder_f PitchRate_decode_uper; +per_type_encoder_f PitchRate_encode_uper; + +#ifdef __cplusplus +} +#endif + +#endif /* _PitchRate_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/PitchRateConfidence.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/PitchRateConfidence.h new file mode 100644 index 000000000..df6f4e7a0 --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/PitchRateConfidence.h @@ -0,0 +1,57 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _PitchRateConfidence_H_ +#define _PitchRateConfidence_H_ + + +#include + +/* Including external dependencies */ +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Dependencies */ +typedef enum PitchRateConfidence { + PitchRateConfidence_unavailable = 0, + PitchRateConfidence_degSec_100_00 = 1, + PitchRateConfidence_degSec_010_00 = 2, + PitchRateConfidence_degSec_005_00 = 3, + PitchRateConfidence_degSec_001_00 = 4, + PitchRateConfidence_degSec_000_10 = 5, + PitchRateConfidence_degSec_000_05 = 6, + PitchRateConfidence_degSec_000_01 = 7 +} e_PitchRateConfidence; + +/* PitchRateConfidence */ +typedef long PitchRateConfidence_t; + +/* Implementation */ +extern asn_per_constraints_t asn_PER_type_PitchRateConfidence_constr_1; +extern asn_TYPE_descriptor_t asn_DEF_PitchRateConfidence; +extern const asn_INTEGER_specifics_t asn_SPC_PitchRateConfidence_specs_1; +asn_struct_free_f PitchRateConfidence_free; +asn_struct_print_f PitchRateConfidence_print; +asn_constr_check_f PitchRateConfidence_constraint; +ber_type_decoder_f PitchRateConfidence_decode_ber; +der_type_encoder_f PitchRateConfidence_encode_der; +xer_type_decoder_f PitchRateConfidence_decode_xer; +xer_type_encoder_f PitchRateConfidence_encode_xer; +oer_type_decoder_f PitchRateConfidence_decode_oer; +oer_type_encoder_f PitchRateConfidence_encode_oer; +per_type_decoder_f PitchRateConfidence_decode_uper; +per_type_encoder_f PitchRateConfidence_encode_uper; + +#ifdef __cplusplus +} +#endif + +#endif /* _PitchRateConfidence_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/PositionOffsetXYZ.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/PositionOffsetXYZ.h new file mode 100644 index 000000000..9a9890818 --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/PositionOffsetXYZ.h @@ -0,0 +1,42 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _PositionOffsetXYZ_H_ +#define _PositionOffsetXYZ_H_ + + +#include + +/* Including external dependencies */ +#include "ObjectDistance.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* PositionOffsetXYZ */ +typedef struct PositionOffsetXYZ { + ObjectDistance_t offsetX; + ObjectDistance_t offsetY; + ObjectDistance_t *offsetZ /* OPTIONAL */; + + /* Context for parsing across buffer boundaries */ + asn_struct_ctx_t _asn_ctx; +} PositionOffsetXYZ_t; + +/* Implementation */ +extern asn_TYPE_descriptor_t asn_DEF_PositionOffsetXYZ; +extern asn_SEQUENCE_specifics_t asn_SPC_PositionOffsetXYZ_specs_1; +extern asn_TYPE_member_t asn_MBR_PositionOffsetXYZ_1[3]; + +#ifdef __cplusplus +} +#endif + +#endif /* _PositionOffsetXYZ_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/RollDetected.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/RollDetected.h new file mode 100644 index 000000000..5326257bd --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/RollDetected.h @@ -0,0 +1,44 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _RollDetected_H_ +#define _RollDetected_H_ + + +#include + +/* Including external dependencies */ +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* RollDetected */ +typedef long RollDetected_t; + +/* Implementation */ +extern asn_per_constraints_t asn_PER_type_RollDetected_constr_1; +extern asn_TYPE_descriptor_t asn_DEF_RollDetected; +asn_struct_free_f RollDetected_free; +asn_struct_print_f RollDetected_print; +asn_constr_check_f RollDetected_constraint; +ber_type_decoder_f RollDetected_decode_ber; +der_type_encoder_f RollDetected_encode_der; +xer_type_decoder_f RollDetected_decode_xer; +xer_type_encoder_f RollDetected_encode_xer; +oer_type_decoder_f RollDetected_decode_oer; +oer_type_encoder_f RollDetected_encode_oer; +per_type_decoder_f RollDetected_decode_uper; +per_type_encoder_f RollDetected_encode_uper; + +#ifdef __cplusplus +} +#endif + +#endif /* _RollDetected_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/RollRate.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/RollRate.h new file mode 100644 index 000000000..86d4d3b25 --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/RollRate.h @@ -0,0 +1,44 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _RollRate_H_ +#define _RollRate_H_ + + +#include + +/* Including external dependencies */ +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* RollRate */ +typedef long RollRate_t; + +/* Implementation */ +extern asn_per_constraints_t asn_PER_type_RollRate_constr_1; +extern asn_TYPE_descriptor_t asn_DEF_RollRate; +asn_struct_free_f RollRate_free; +asn_struct_print_f RollRate_print; +asn_constr_check_f RollRate_constraint; +ber_type_decoder_f RollRate_decode_ber; +der_type_encoder_f RollRate_encode_der; +xer_type_decoder_f RollRate_decode_xer; +xer_type_encoder_f RollRate_encode_xer; +oer_type_decoder_f RollRate_decode_oer; +oer_type_encoder_f RollRate_encode_oer; +per_type_decoder_f RollRate_decode_uper; +per_type_encoder_f RollRate_encode_uper; + +#ifdef __cplusplus +} +#endif + +#endif /* _RollRate_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/RollRateConfidence.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/RollRateConfidence.h new file mode 100644 index 000000000..9fbfec7fd --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/RollRateConfidence.h @@ -0,0 +1,57 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _RollRateConfidence_H_ +#define _RollRateConfidence_H_ + + +#include + +/* Including external dependencies */ +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Dependencies */ +typedef enum RollRateConfidence { + RollRateConfidence_unavailable = 0, + RollRateConfidence_degSec_100_00 = 1, + RollRateConfidence_degSec_010_00 = 2, + RollRateConfidence_degSec_005_00 = 3, + RollRateConfidence_degSec_001_00 = 4, + RollRateConfidence_degSec_000_10 = 5, + RollRateConfidence_degSec_000_05 = 6, + RollRateConfidence_degSec_000_01 = 7 +} e_RollRateConfidence; + +/* RollRateConfidence */ +typedef long RollRateConfidence_t; + +/* Implementation */ +extern asn_per_constraints_t asn_PER_type_RollRateConfidence_constr_1; +extern asn_TYPE_descriptor_t asn_DEF_RollRateConfidence; +extern const asn_INTEGER_specifics_t asn_SPC_RollRateConfidence_specs_1; +asn_struct_free_f RollRateConfidence_free; +asn_struct_print_f RollRateConfidence_print; +asn_constr_check_f RollRateConfidence_constraint; +ber_type_decoder_f RollRateConfidence_decode_ber; +der_type_encoder_f RollRateConfidence_encode_der; +xer_type_decoder_f RollRateConfidence_decode_xer; +xer_type_encoder_f RollRateConfidence_encode_xer; +oer_type_decoder_f RollRateConfidence_decode_oer; +oer_type_encoder_f RollRateConfidence_encode_oer; +per_type_decoder_f RollRateConfidence_decode_uper; +per_type_encoder_f RollRateConfidence_encode_uper; + +#ifdef __cplusplus +} +#endif + +#endif /* _RollRateConfidence_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/SensorDataSharingMessage.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/SensorDataSharingMessage.h new file mode 100644 index 000000000..5a4d08db2 --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/SensorDataSharingMessage.h @@ -0,0 +1,54 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _SensorDataSharingMessage_H_ +#define _SensorDataSharingMessage_H_ + + +#include + +/* Including external dependencies */ +#include "DSRC_MsgCount.h" +#include "TemporaryID.h" +#include "EquipmentType.h" +#include "DDateTime.h" +#include "Position3D.h" +#include "PositionalAccuracy.h" +#include "ElevationConfidence.h" +#include "DetectedObjectList.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* SensorDataSharingMessage */ +typedef struct SensorDataSharingMessage { + DSRC_MsgCount_t msgCnt; + TemporaryID_t sourceID; + EquipmentType_t equipmentType; + DDateTime_t sDSMTimeStamp; + Position3D_t refPos; + PositionalAccuracy_t refPosXYConf; + ElevationConfidence_t *refPosElConf /* OPTIONAL */; + DetectedObjectList_t objects; + + /* Context for parsing across buffer boundaries */ + asn_struct_ctx_t _asn_ctx; +} SensorDataSharingMessage_t; + +/* Implementation */ +extern asn_TYPE_descriptor_t asn_DEF_SensorDataSharingMessage; +extern asn_SEQUENCE_specifics_t asn_SPC_SensorDataSharingMessage_specs_1; +extern asn_TYPE_member_t asn_MBR_SensorDataSharingMessage_1[8]; + +#ifdef __cplusplus +} +#endif + +#endif /* _SensorDataSharingMessage_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/SizeValue.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/SizeValue.h new file mode 100644 index 000000000..7b35e76ef --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/SizeValue.h @@ -0,0 +1,44 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _SizeValue_H_ +#define _SizeValue_H_ + + +#include + +/* Including external dependencies */ +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* SizeValue */ +typedef long SizeValue_t; + +/* Implementation */ +extern asn_per_constraints_t asn_PER_type_SizeValue_constr_1; +extern asn_TYPE_descriptor_t asn_DEF_SizeValue; +asn_struct_free_f SizeValue_free; +asn_struct_print_f SizeValue_print; +asn_constr_check_f SizeValue_constraint; +ber_type_decoder_f SizeValue_decode_ber; +der_type_encoder_f SizeValue_encode_der; +xer_type_decoder_f SizeValue_decode_xer; +xer_type_encoder_f SizeValue_encode_xer; +oer_type_decoder_f SizeValue_decode_oer; +oer_type_encoder_f SizeValue_encode_oer; +per_type_decoder_f SizeValue_decode_uper; +per_type_encoder_f SizeValue_encode_uper; + +#ifdef __cplusplus +} +#endif + +#endif /* _SizeValue_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/SizeValueConfidence.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/SizeValueConfidence.h new file mode 100644 index 000000000..b719b5570 --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/SizeValueConfidence.h @@ -0,0 +1,63 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _SizeValueConfidence_H_ +#define _SizeValueConfidence_H_ + + +#include + +/* Including external dependencies */ +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Dependencies */ +typedef enum SizeValueConfidence { + SizeValueConfidence_unavailable = 0, + SizeValueConfidence_size_100_00 = 1, + SizeValueConfidence_size_050_00 = 2, + SizeValueConfidence_size_020_00 = 3, + SizeValueConfidence_size_010_00 = 4, + SizeValueConfidence_size_005_00 = 5, + SizeValueConfidence_size_002_00 = 6, + SizeValueConfidence_size_001_00 = 7, + SizeValueConfidence_size_000_50 = 8, + SizeValueConfidence_size_000_20 = 9, + SizeValueConfidence_size_000_10 = 10, + SizeValueConfidence_size_000_05 = 11, + SizeValueConfidence_size_000_02 = 12, + SizeValueConfidence_size_000_01 = 13 +} e_SizeValueConfidence; + +/* SizeValueConfidence */ +typedef long SizeValueConfidence_t; + +/* Implementation */ +extern asn_per_constraints_t asn_PER_type_SizeValueConfidence_constr_1; +extern asn_TYPE_descriptor_t asn_DEF_SizeValueConfidence; +extern const asn_INTEGER_specifics_t asn_SPC_SizeValueConfidence_specs_1; +asn_struct_free_f SizeValueConfidence_free; +asn_struct_print_f SizeValueConfidence_print; +asn_constr_check_f SizeValueConfidence_constraint; +ber_type_decoder_f SizeValueConfidence_decode_ber; +der_type_encoder_f SizeValueConfidence_encode_der; +xer_type_decoder_f SizeValueConfidence_decode_xer; +xer_type_encoder_f SizeValueConfidence_encode_xer; +oer_type_decoder_f SizeValueConfidence_decode_oer; +oer_type_encoder_f SizeValueConfidence_encode_oer; +per_type_decoder_f SizeValueConfidence_decode_uper; +per_type_encoder_f SizeValueConfidence_encode_uper; + +#ifdef __cplusplus +} +#endif + +#endif /* _SizeValueConfidence_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/VehicleSizeConfidence.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/VehicleSizeConfidence.h new file mode 100644 index 000000000..7ab00134d --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/VehicleSizeConfidence.h @@ -0,0 +1,42 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _VehicleSizeConfidence_H_ +#define _VehicleSizeConfidence_H_ + + +#include + +/* Including external dependencies */ +#include "SizeValueConfidence.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* VehicleSizeConfidence */ +typedef struct VehicleSizeConfidence { + SizeValueConfidence_t vehicleWidthConfidence; + SizeValueConfidence_t vehicleLengthConfidence; + SizeValueConfidence_t *vehicleHeightConfidence /* OPTIONAL */; + + /* Context for parsing across buffer boundaries */ + asn_struct_ctx_t _asn_ctx; +} VehicleSizeConfidence_t; + +/* Implementation */ +extern asn_TYPE_descriptor_t asn_DEF_VehicleSizeConfidence; +extern asn_SEQUENCE_specifics_t asn_SPC_VehicleSizeConfidence_specs_1; +extern asn_TYPE_member_t asn_MBR_VehicleSizeConfidence_1[3]; + +#ifdef __cplusplus +} +#endif + +#endif /* _VehicleSizeConfidence_H_ */ +#include diff --git a/src/tmx/Asn_J2735/include/asn_j2735_r63/YawDetected.h b/src/tmx/Asn_J2735/include/asn_j2735_r63/YawDetected.h new file mode 100644 index 000000000..394867a92 --- /dev/null +++ b/src/tmx/Asn_J2735/include/asn_j2735_r63/YawDetected.h @@ -0,0 +1,44 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#ifndef _YawDetected_H_ +#define _YawDetected_H_ + + +#include + +/* Including external dependencies */ +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* YawDetected */ +typedef long YawDetected_t; + +/* Implementation */ +extern asn_per_constraints_t asn_PER_type_YawDetected_constr_1; +extern asn_TYPE_descriptor_t asn_DEF_YawDetected; +asn_struct_free_f YawDetected_free; +asn_struct_print_f YawDetected_print; +asn_constr_check_f YawDetected_constraint; +ber_type_decoder_f YawDetected_decode_ber; +der_type_encoder_f YawDetected_encode_der; +xer_type_decoder_f YawDetected_decode_xer; +xer_type_encoder_f YawDetected_encode_xer; +oer_type_decoder_f YawDetected_decode_oer; +oer_type_encoder_f YawDetected_encode_oer; +per_type_decoder_f YawDetected_decode_uper; +per_type_encoder_f YawDetected_encode_uper; + +#ifdef __cplusplus +} +#endif + +#endif /* _YawDetected_H_ */ +#include diff --git a/src/tmx/Asn_J2735/src/r63/AngularVelocity.c b/src/tmx/Asn_J2735/src/r63/AngularVelocity.c new file mode 100644 index 000000000..eedd75054 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/AngularVelocity.c @@ -0,0 +1,60 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "AngularVelocity.h" + +asn_TYPE_member_t asn_MBR_AngularVelocity_1[] = { + { ATF_NOFLAGS, 0, offsetof(struct AngularVelocity, pitchRate), + (ASN_TAG_CLASS_CONTEXT | (0 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_PitchRate, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "pitchRate" + }, + { ATF_NOFLAGS, 0, offsetof(struct AngularVelocity, rollRate), + (ASN_TAG_CLASS_CONTEXT | (1 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_RollRate, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "rollRate" + }, +}; +static const ber_tlv_tag_t asn_DEF_AngularVelocity_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)) +}; +static const asn_TYPE_tag2member_t asn_MAP_AngularVelocity_tag2el_1[] = { + { (ASN_TAG_CLASS_CONTEXT | (0 << 2)), 0, 0, 0 }, /* pitchRate */ + { (ASN_TAG_CLASS_CONTEXT | (1 << 2)), 1, 0, 0 } /* rollRate */ +}; +asn_SEQUENCE_specifics_t asn_SPC_AngularVelocity_specs_1 = { + sizeof(struct AngularVelocity), + offsetof(struct AngularVelocity, _asn_ctx), + asn_MAP_AngularVelocity_tag2el_1, + 2, /* Count of tags in the map */ + 0, 0, 0, /* Optional elements (not needed) */ + -1, /* First extension addition */ +}; +asn_TYPE_descriptor_t asn_DEF_AngularVelocity = { + "AngularVelocity", + "AngularVelocity", + &asn_OP_SEQUENCE, + asn_DEF_AngularVelocity_tags_1, + sizeof(asn_DEF_AngularVelocity_tags_1) + /sizeof(asn_DEF_AngularVelocity_tags_1[0]), /* 1 */ + asn_DEF_AngularVelocity_tags_1, /* Same as above */ + sizeof(asn_DEF_AngularVelocity_tags_1) + /sizeof(asn_DEF_AngularVelocity_tags_1[0]), /* 1 */ + { 0, 0, SEQUENCE_constraint }, + asn_MBR_AngularVelocity_1, + 2, /* Elements count */ + &asn_SPC_AngularVelocity_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/AngularVelocityConfidence.c b/src/tmx/Asn_J2735/src/r63/AngularVelocityConfidence.c new file mode 100644 index 000000000..8ff44510f --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/AngularVelocityConfidence.c @@ -0,0 +1,62 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "AngularVelocityConfidence.h" + +asn_TYPE_member_t asn_MBR_AngularVelocityConfidence_1[] = { + { ATF_POINTER, 2, offsetof(struct AngularVelocityConfidence, pitchRateConfidence), + (ASN_TAG_CLASS_CONTEXT | (0 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_PitchRateConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "pitchRateConfidence" + }, + { ATF_POINTER, 1, offsetof(struct AngularVelocityConfidence, rollRateConfidence), + (ASN_TAG_CLASS_CONTEXT | (1 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_RollRateConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "rollRateConfidence" + }, +}; +static const int asn_MAP_AngularVelocityConfidence_oms_1[] = { 0, 1 }; +static const ber_tlv_tag_t asn_DEF_AngularVelocityConfidence_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)) +}; +static const asn_TYPE_tag2member_t asn_MAP_AngularVelocityConfidence_tag2el_1[] = { + { (ASN_TAG_CLASS_CONTEXT | (0 << 2)), 0, 0, 0 }, /* pitchRateConfidence */ + { (ASN_TAG_CLASS_CONTEXT | (1 << 2)), 1, 0, 0 } /* rollRateConfidence */ +}; +asn_SEQUENCE_specifics_t asn_SPC_AngularVelocityConfidence_specs_1 = { + sizeof(struct AngularVelocityConfidence), + offsetof(struct AngularVelocityConfidence, _asn_ctx), + asn_MAP_AngularVelocityConfidence_tag2el_1, + 2, /* Count of tags in the map */ + asn_MAP_AngularVelocityConfidence_oms_1, /* Optional members */ + 2, 0, /* Root/Additions */ + -1, /* First extension addition */ +}; +asn_TYPE_descriptor_t asn_DEF_AngularVelocityConfidence = { + "AngularVelocityConfidence", + "AngularVelocityConfidence", + &asn_OP_SEQUENCE, + asn_DEF_AngularVelocityConfidence_tags_1, + sizeof(asn_DEF_AngularVelocityConfidence_tags_1) + /sizeof(asn_DEF_AngularVelocityConfidence_tags_1[0]), /* 1 */ + asn_DEF_AngularVelocityConfidence_tags_1, /* Same as above */ + sizeof(asn_DEF_AngularVelocityConfidence_tags_1) + /sizeof(asn_DEF_AngularVelocityConfidence_tags_1[0]), /* 1 */ + { 0, 0, SEQUENCE_constraint }, + asn_MBR_AngularVelocityConfidence_1, + 2, /* Elements count */ + &asn_SPC_AngularVelocityConfidence_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/Attitude.c b/src/tmx/Asn_J2735/src/r63/Attitude.c new file mode 100644 index 000000000..4af80a825 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/Attitude.c @@ -0,0 +1,70 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "Attitude.h" + +asn_TYPE_member_t asn_MBR_Attitude_1[] = { + { ATF_NOFLAGS, 0, offsetof(struct Attitude, pitch), + (ASN_TAG_CLASS_CONTEXT | (0 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_PitchDetected, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "pitch" + }, + { ATF_NOFLAGS, 0, offsetof(struct Attitude, roll), + (ASN_TAG_CLASS_CONTEXT | (1 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_RollDetected, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "roll" + }, + { ATF_NOFLAGS, 0, offsetof(struct Attitude, yaw), + (ASN_TAG_CLASS_CONTEXT | (2 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_YawDetected, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "yaw" + }, +}; +static const ber_tlv_tag_t asn_DEF_Attitude_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)) +}; +static const asn_TYPE_tag2member_t asn_MAP_Attitude_tag2el_1[] = { + { (ASN_TAG_CLASS_CONTEXT | (0 << 2)), 0, 0, 0 }, /* pitch */ + { (ASN_TAG_CLASS_CONTEXT | (1 << 2)), 1, 0, 0 }, /* roll */ + { (ASN_TAG_CLASS_CONTEXT | (2 << 2)), 2, 0, 0 } /* yaw */ +}; +asn_SEQUENCE_specifics_t asn_SPC_Attitude_specs_1 = { + sizeof(struct Attitude), + offsetof(struct Attitude, _asn_ctx), + asn_MAP_Attitude_tag2el_1, + 3, /* Count of tags in the map */ + 0, 0, 0, /* Optional elements (not needed) */ + -1, /* First extension addition */ +}; +asn_TYPE_descriptor_t asn_DEF_Attitude = { + "Attitude", + "Attitude", + &asn_OP_SEQUENCE, + asn_DEF_Attitude_tags_1, + sizeof(asn_DEF_Attitude_tags_1) + /sizeof(asn_DEF_Attitude_tags_1[0]), /* 1 */ + asn_DEF_Attitude_tags_1, /* Same as above */ + sizeof(asn_DEF_Attitude_tags_1) + /sizeof(asn_DEF_Attitude_tags_1[0]), /* 1 */ + { 0, 0, SEQUENCE_constraint }, + asn_MBR_Attitude_1, + 3, /* Elements count */ + &asn_SPC_Attitude_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/AttitudeConfidence.c b/src/tmx/Asn_J2735/src/r63/AttitudeConfidence.c new file mode 100644 index 000000000..f521222c5 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/AttitudeConfidence.c @@ -0,0 +1,70 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "AttitudeConfidence.h" + +asn_TYPE_member_t asn_MBR_AttitudeConfidence_1[] = { + { ATF_NOFLAGS, 0, offsetof(struct AttitudeConfidence, pitchConfidence), + (ASN_TAG_CLASS_CONTEXT | (0 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_HeadingConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "pitchConfidence" + }, + { ATF_NOFLAGS, 0, offsetof(struct AttitudeConfidence, rollConfidence), + (ASN_TAG_CLASS_CONTEXT | (1 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_HeadingConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "rollConfidence" + }, + { ATF_NOFLAGS, 0, offsetof(struct AttitudeConfidence, yawConfidence), + (ASN_TAG_CLASS_CONTEXT | (2 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_HeadingConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "yawConfidence" + }, +}; +static const ber_tlv_tag_t asn_DEF_AttitudeConfidence_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)) +}; +static const asn_TYPE_tag2member_t asn_MAP_AttitudeConfidence_tag2el_1[] = { + { (ASN_TAG_CLASS_CONTEXT | (0 << 2)), 0, 0, 0 }, /* pitchConfidence */ + { (ASN_TAG_CLASS_CONTEXT | (1 << 2)), 1, 0, 0 }, /* rollConfidence */ + { (ASN_TAG_CLASS_CONTEXT | (2 << 2)), 2, 0, 0 } /* yawConfidence */ +}; +asn_SEQUENCE_specifics_t asn_SPC_AttitudeConfidence_specs_1 = { + sizeof(struct AttitudeConfidence), + offsetof(struct AttitudeConfidence, _asn_ctx), + asn_MAP_AttitudeConfidence_tag2el_1, + 3, /* Count of tags in the map */ + 0, 0, 0, /* Optional elements (not needed) */ + -1, /* First extension addition */ +}; +asn_TYPE_descriptor_t asn_DEF_AttitudeConfidence = { + "AttitudeConfidence", + "AttitudeConfidence", + &asn_OP_SEQUENCE, + asn_DEF_AttitudeConfidence_tags_1, + sizeof(asn_DEF_AttitudeConfidence_tags_1) + /sizeof(asn_DEF_AttitudeConfidence_tags_1[0]), /* 1 */ + asn_DEF_AttitudeConfidence_tags_1, /* Same as above */ + sizeof(asn_DEF_AttitudeConfidence_tags_1) + /sizeof(asn_DEF_AttitudeConfidence_tags_1[0]), /* 1 */ + { 0, 0, SEQUENCE_constraint }, + asn_MBR_AttitudeConfidence_1, + 3, /* Elements count */ + &asn_SPC_AttitudeConfidence_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/ClassificationConfidence.c b/src/tmx/Asn_J2735/src/r63/ClassificationConfidence.c new file mode 100644 index 000000000..423fdca04 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/ClassificationConfidence.c @@ -0,0 +1,64 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "ClassificationConfidence.h" + +int +ClassificationConfidence_constraint(const asn_TYPE_descriptor_t *td, const void *sptr, + asn_app_constraint_failed_f *ctfailcb, void *app_key) { + long value; + + if(!sptr) { + ASN__CTFAIL(app_key, td, sptr, + "%s: value not given (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } + + value = *(const long *)sptr; + + if((value >= 0 && value <= 101)) { + /* Constraint check succeeded */ + return 0; + } else { + ASN__CTFAIL(app_key, td, sptr, + "%s: constraint failed (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } +} + +/* + * This type is implemented using NativeInteger, + * so here we adjust the DEF accordingly. + */ +static asn_oer_constraints_t asn_OER_type_ClassificationConfidence_constr_1 CC_NOTUSED = { + { 1, 1 } /* (0..101) */, + -1}; +asn_per_constraints_t asn_PER_type_ClassificationConfidence_constr_1 CC_NOTUSED = { + { APC_CONSTRAINED, 7, 7, 0, 101 } /* (0..101) */, + { APC_UNCONSTRAINED, -1, -1, 0, 0 }, + 0, 0 /* No PER value map */ +}; +static const ber_tlv_tag_t asn_DEF_ClassificationConfidence_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (2 << 2)) +}; +asn_TYPE_descriptor_t asn_DEF_ClassificationConfidence = { + "ClassificationConfidence", + "ClassificationConfidence", + &asn_OP_NativeInteger, + asn_DEF_ClassificationConfidence_tags_1, + sizeof(asn_DEF_ClassificationConfidence_tags_1) + /sizeof(asn_DEF_ClassificationConfidence_tags_1[0]), /* 1 */ + asn_DEF_ClassificationConfidence_tags_1, /* Same as above */ + sizeof(asn_DEF_ClassificationConfidence_tags_1) + /sizeof(asn_DEF_ClassificationConfidence_tags_1[0]), /* 1 */ + { &asn_OER_type_ClassificationConfidence_constr_1, &asn_PER_type_ClassificationConfidence_constr_1, ClassificationConfidence_constraint }, + 0, 0, /* No members */ + 0 /* No specifics */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/DetectedObjectCommonData.c b/src/tmx/Asn_J2735/src/r63/DetectedObjectCommonData.c new file mode 100644 index 000000000..d22ee61a2 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/DetectedObjectCommonData.c @@ -0,0 +1,222 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "DetectedObjectCommonData.h" + +asn_TYPE_member_t asn_MBR_DetectedObjectCommonData_1[] = { + { ATF_NOFLAGS, 0, offsetof(struct DetectedObjectCommonData, objType), + (ASN_TAG_CLASS_CONTEXT | (0 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_ObjectType, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "objType" + }, + { ATF_NOFLAGS, 0, offsetof(struct DetectedObjectCommonData, objTypeCfd), + (ASN_TAG_CLASS_CONTEXT | (1 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_ClassificationConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "objTypeCfd" + }, + { ATF_NOFLAGS, 0, offsetof(struct DetectedObjectCommonData, objectID), + (ASN_TAG_CLASS_CONTEXT | (2 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_ObjectID, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "objectID" + }, + { ATF_NOFLAGS, 0, offsetof(struct DetectedObjectCommonData, measurementTime), + (ASN_TAG_CLASS_CONTEXT | (3 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_MeasurementTimeOffset, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "measurementTime" + }, + { ATF_NOFLAGS, 0, offsetof(struct DetectedObjectCommonData, timeConfidence), + (ASN_TAG_CLASS_CONTEXT | (4 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_TimeConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "timeConfidence" + }, + { ATF_NOFLAGS, 0, offsetof(struct DetectedObjectCommonData, pos), + (ASN_TAG_CLASS_CONTEXT | (5 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_PositionOffsetXYZ, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "pos" + }, + { ATF_NOFLAGS, 0, offsetof(struct DetectedObjectCommonData, posConfidence), + (ASN_TAG_CLASS_CONTEXT | (6 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_PositionConfidenceSet, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "posConfidence" + }, + { ATF_NOFLAGS, 0, offsetof(struct DetectedObjectCommonData, speed), + (ASN_TAG_CLASS_CONTEXT | (7 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_Speed, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "speed" + }, + { ATF_NOFLAGS, 0, offsetof(struct DetectedObjectCommonData, speedConfidence), + (ASN_TAG_CLASS_CONTEXT | (8 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_SpeedConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "speedConfidence" + }, + { ATF_POINTER, 2, offsetof(struct DetectedObjectCommonData, speedZ), + (ASN_TAG_CLASS_CONTEXT | (9 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_Speed, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "speedZ" + }, + { ATF_POINTER, 1, offsetof(struct DetectedObjectCommonData, speedConfidenceZ), + (ASN_TAG_CLASS_CONTEXT | (10 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_SpeedConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "speedConfidenceZ" + }, + { ATF_NOFLAGS, 0, offsetof(struct DetectedObjectCommonData, heading), + (ASN_TAG_CLASS_CONTEXT | (11 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_Heading, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "heading" + }, + { ATF_NOFLAGS, 0, offsetof(struct DetectedObjectCommonData, headingConf), + (ASN_TAG_CLASS_CONTEXT | (12 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_HeadingConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "headingConf" + }, + { ATF_POINTER, 5, offsetof(struct DetectedObjectCommonData, accel4way), + (ASN_TAG_CLASS_CONTEXT | (13 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_AccelerationSet4Way, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "accel4way" + }, + { ATF_POINTER, 4, offsetof(struct DetectedObjectCommonData, accCfdX), + (ASN_TAG_CLASS_CONTEXT | (14 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_AccelerationConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "accCfdX" + }, + { ATF_POINTER, 3, offsetof(struct DetectedObjectCommonData, accCfdY), + (ASN_TAG_CLASS_CONTEXT | (15 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_AccelerationConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "accCfdY" + }, + { ATF_POINTER, 2, offsetof(struct DetectedObjectCommonData, accCfdZ), + (ASN_TAG_CLASS_CONTEXT | (16 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_AccelerationConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "accCfdZ" + }, + { ATF_POINTER, 1, offsetof(struct DetectedObjectCommonData, accCfdYaw), + (ASN_TAG_CLASS_CONTEXT | (17 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_YawRateConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "accCfdYaw" + }, +}; +static const int asn_MAP_DetectedObjectCommonData_oms_1[] = { 9, 10, 13, 14, 15, 16, 17 }; +static const ber_tlv_tag_t asn_DEF_DetectedObjectCommonData_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)) +}; +static const asn_TYPE_tag2member_t asn_MAP_DetectedObjectCommonData_tag2el_1[] = { + { (ASN_TAG_CLASS_CONTEXT | (0 << 2)), 0, 0, 0 }, /* objType */ + { (ASN_TAG_CLASS_CONTEXT | (1 << 2)), 1, 0, 0 }, /* objTypeCfd */ + { (ASN_TAG_CLASS_CONTEXT | (2 << 2)), 2, 0, 0 }, /* objectID */ + { (ASN_TAG_CLASS_CONTEXT | (3 << 2)), 3, 0, 0 }, /* measurementTime */ + { (ASN_TAG_CLASS_CONTEXT | (4 << 2)), 4, 0, 0 }, /* timeConfidence */ + { (ASN_TAG_CLASS_CONTEXT | (5 << 2)), 5, 0, 0 }, /* pos */ + { (ASN_TAG_CLASS_CONTEXT | (6 << 2)), 6, 0, 0 }, /* posConfidence */ + { (ASN_TAG_CLASS_CONTEXT | (7 << 2)), 7, 0, 0 }, /* speed */ + { (ASN_TAG_CLASS_CONTEXT | (8 << 2)), 8, 0, 0 }, /* speedConfidence */ + { (ASN_TAG_CLASS_CONTEXT | (9 << 2)), 9, 0, 0 }, /* speedZ */ + { (ASN_TAG_CLASS_CONTEXT | (10 << 2)), 10, 0, 0 }, /* speedConfidenceZ */ + { (ASN_TAG_CLASS_CONTEXT | (11 << 2)), 11, 0, 0 }, /* heading */ + { (ASN_TAG_CLASS_CONTEXT | (12 << 2)), 12, 0, 0 }, /* headingConf */ + { (ASN_TAG_CLASS_CONTEXT | (13 << 2)), 13, 0, 0 }, /* accel4way */ + { (ASN_TAG_CLASS_CONTEXT | (14 << 2)), 14, 0, 0 }, /* accCfdX */ + { (ASN_TAG_CLASS_CONTEXT | (15 << 2)), 15, 0, 0 }, /* accCfdY */ + { (ASN_TAG_CLASS_CONTEXT | (16 << 2)), 16, 0, 0 }, /* accCfdZ */ + { (ASN_TAG_CLASS_CONTEXT | (17 << 2)), 17, 0, 0 } /* accCfdYaw */ +}; +asn_SEQUENCE_specifics_t asn_SPC_DetectedObjectCommonData_specs_1 = { + sizeof(struct DetectedObjectCommonData), + offsetof(struct DetectedObjectCommonData, _asn_ctx), + asn_MAP_DetectedObjectCommonData_tag2el_1, + 18, /* Count of tags in the map */ + asn_MAP_DetectedObjectCommonData_oms_1, /* Optional members */ + 7, 0, /* Root/Additions */ + 18, /* First extension addition */ +}; +asn_TYPE_descriptor_t asn_DEF_DetectedObjectCommonData = { + "DetectedObjectCommonData", + "DetectedObjectCommonData", + &asn_OP_SEQUENCE, + asn_DEF_DetectedObjectCommonData_tags_1, + sizeof(asn_DEF_DetectedObjectCommonData_tags_1) + /sizeof(asn_DEF_DetectedObjectCommonData_tags_1[0]), /* 1 */ + asn_DEF_DetectedObjectCommonData_tags_1, /* Same as above */ + sizeof(asn_DEF_DetectedObjectCommonData_tags_1) + /sizeof(asn_DEF_DetectedObjectCommonData_tags_1[0]), /* 1 */ + { 0, 0, SEQUENCE_constraint }, + asn_MBR_DetectedObjectCommonData_1, + 18, /* Elements count */ + &asn_SPC_DetectedObjectCommonData_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/DetectedObjectData.c b/src/tmx/Asn_J2735/src/r63/DetectedObjectData.c new file mode 100644 index 000000000..f15b14725 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/DetectedObjectData.c @@ -0,0 +1,62 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "DetectedObjectData.h" + +asn_TYPE_member_t asn_MBR_DetectedObjectData_1[] = { + { ATF_NOFLAGS, 0, offsetof(struct DetectedObjectData, detObjCommon), + (ASN_TAG_CLASS_CONTEXT | (0 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_DetectedObjectCommonData, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "detObjCommon" + }, + { ATF_POINTER, 1, offsetof(struct DetectedObjectData, detObjOptData), + (ASN_TAG_CLASS_CONTEXT | (1 << 2)), + +1, /* EXPLICIT tag at current level */ + &asn_DEF_DetectedObjectOptionalData, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "detObjOptData" + }, +}; +static const int asn_MAP_DetectedObjectData_oms_1[] = { 1 }; +static const ber_tlv_tag_t asn_DEF_DetectedObjectData_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)) +}; +static const asn_TYPE_tag2member_t asn_MAP_DetectedObjectData_tag2el_1[] = { + { (ASN_TAG_CLASS_CONTEXT | (0 << 2)), 0, 0, 0 }, /* detObjCommon */ + { (ASN_TAG_CLASS_CONTEXT | (1 << 2)), 1, 0, 0 } /* detObjOptData */ +}; +asn_SEQUENCE_specifics_t asn_SPC_DetectedObjectData_specs_1 = { + sizeof(struct DetectedObjectData), + offsetof(struct DetectedObjectData, _asn_ctx), + asn_MAP_DetectedObjectData_tag2el_1, + 2, /* Count of tags in the map */ + asn_MAP_DetectedObjectData_oms_1, /* Optional members */ + 1, 0, /* Root/Additions */ + -1, /* First extension addition */ +}; +asn_TYPE_descriptor_t asn_DEF_DetectedObjectData = { + "DetectedObjectData", + "DetectedObjectData", + &asn_OP_SEQUENCE, + asn_DEF_DetectedObjectData_tags_1, + sizeof(asn_DEF_DetectedObjectData_tags_1) + /sizeof(asn_DEF_DetectedObjectData_tags_1[0]), /* 1 */ + asn_DEF_DetectedObjectData_tags_1, /* Same as above */ + sizeof(asn_DEF_DetectedObjectData_tags_1) + /sizeof(asn_DEF_DetectedObjectData_tags_1[0]), /* 1 */ + { 0, 0, SEQUENCE_constraint }, + asn_MBR_DetectedObjectData_1, + 2, /* Elements count */ + &asn_SPC_DetectedObjectData_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/DetectedObjectList.c b/src/tmx/Asn_J2735/src/r63/DetectedObjectList.c new file mode 100644 index 000000000..9cf444254 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/DetectedObjectList.c @@ -0,0 +1,52 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "DetectedObjectList.h" + +static asn_oer_constraints_t asn_OER_type_DetectedObjectList_constr_1 CC_NOTUSED = { + { 0, 0 }, + -1 /* (SIZE(1..256)) */}; +asn_per_constraints_t asn_PER_type_DetectedObjectList_constr_1 CC_NOTUSED = { + { APC_UNCONSTRAINED, -1, -1, 0, 0 }, + { APC_CONSTRAINED, 8, 8, 1, 256 } /* (SIZE(1..256)) */, + 0, 0 /* No PER value map */ +}; +asn_TYPE_member_t asn_MBR_DetectedObjectList_1[] = { + { ATF_POINTER, 0, 0, + (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), + 0, + &asn_DEF_DetectedObjectData, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "" + }, +}; +static const ber_tlv_tag_t asn_DEF_DetectedObjectList_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)) +}; +asn_SET_OF_specifics_t asn_SPC_DetectedObjectList_specs_1 = { + sizeof(struct DetectedObjectList), + offsetof(struct DetectedObjectList, _asn_ctx), + 0, /* XER encoding is XMLDelimitedItemList */ +}; +asn_TYPE_descriptor_t asn_DEF_DetectedObjectList = { + "DetectedObjectList", + "DetectedObjectList", + &asn_OP_SEQUENCE_OF, + asn_DEF_DetectedObjectList_tags_1, + sizeof(asn_DEF_DetectedObjectList_tags_1) + /sizeof(asn_DEF_DetectedObjectList_tags_1[0]), /* 1 */ + asn_DEF_DetectedObjectList_tags_1, /* Same as above */ + sizeof(asn_DEF_DetectedObjectList_tags_1) + /sizeof(asn_DEF_DetectedObjectList_tags_1[0]), /* 1 */ + { &asn_OER_type_DetectedObjectList_constr_1, &asn_PER_type_DetectedObjectList_constr_1, SEQUENCE_OF_constraint }, + asn_MBR_DetectedObjectList_1, + 1, /* Single element */ + &asn_SPC_DetectedObjectList_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/DetectedObjectOptionalData.c b/src/tmx/Asn_J2735/src/r63/DetectedObjectOptionalData.c new file mode 100644 index 000000000..c00b24eeb --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/DetectedObjectOptionalData.c @@ -0,0 +1,75 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "DetectedObjectOptionalData.h" + +static asn_oer_constraints_t asn_OER_type_DetectedObjectOptionalData_constr_1 CC_NOTUSED = { + { 0, 0 }, + -1}; +asn_per_constraints_t asn_PER_type_DetectedObjectOptionalData_constr_1 CC_NOTUSED = { + { APC_CONSTRAINED, 2, 2, 0, 2 } /* (0..2) */, + { APC_UNCONSTRAINED, -1, -1, 0, 0 }, + 0, 0 /* No PER value map */ +}; +asn_TYPE_member_t asn_MBR_DetectedObjectOptionalData_1[] = { + { ATF_NOFLAGS, 0, offsetof(struct DetectedObjectOptionalData, choice.detVeh), + (ASN_TAG_CLASS_CONTEXT | (0 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_DetectedVehicleData, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "detVeh" + }, + { ATF_NOFLAGS, 0, offsetof(struct DetectedObjectOptionalData, choice.detVRU), + (ASN_TAG_CLASS_CONTEXT | (1 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_DetectedVRUData, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "detVRU" + }, + { ATF_NOFLAGS, 0, offsetof(struct DetectedObjectOptionalData, choice.detObst), + (ASN_TAG_CLASS_CONTEXT | (2 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_DetectedObstacleData, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "detObst" + }, +}; +static const asn_TYPE_tag2member_t asn_MAP_DetectedObjectOptionalData_tag2el_1[] = { + { (ASN_TAG_CLASS_CONTEXT | (0 << 2)), 0, 0, 0 }, /* detVeh */ + { (ASN_TAG_CLASS_CONTEXT | (1 << 2)), 1, 0, 0 }, /* detVRU */ + { (ASN_TAG_CLASS_CONTEXT | (2 << 2)), 2, 0, 0 } /* detObst */ +}; +asn_CHOICE_specifics_t asn_SPC_DetectedObjectOptionalData_specs_1 = { + sizeof(struct DetectedObjectOptionalData), + offsetof(struct DetectedObjectOptionalData, _asn_ctx), + offsetof(struct DetectedObjectOptionalData, present), + sizeof(((struct DetectedObjectOptionalData *)0)->present), + asn_MAP_DetectedObjectOptionalData_tag2el_1, + 3, /* Count of tags in the map */ + 0, 0, + -1 /* Extensions start */ +}; +asn_TYPE_descriptor_t asn_DEF_DetectedObjectOptionalData = { + "DetectedObjectOptionalData", + "DetectedObjectOptionalData", + &asn_OP_CHOICE, + 0, /* No effective tags (pointer) */ + 0, /* No effective tags (count) */ + 0, /* No tags (pointer) */ + 0, /* No tags (count) */ + { &asn_OER_type_DetectedObjectOptionalData_constr_1, &asn_PER_type_DetectedObjectOptionalData_constr_1, CHOICE_constraint }, + asn_MBR_DetectedObjectOptionalData_1, + 3, /* Elements count */ + &asn_SPC_DetectedObjectOptionalData_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/DetectedObstacleData.c b/src/tmx/Asn_J2735/src/r63/DetectedObstacleData.c new file mode 100644 index 000000000..fafab50a5 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/DetectedObstacleData.c @@ -0,0 +1,60 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "DetectedObstacleData.h" + +asn_TYPE_member_t asn_MBR_DetectedObstacleData_1[] = { + { ATF_NOFLAGS, 0, offsetof(struct DetectedObstacleData, obstSize), + (ASN_TAG_CLASS_CONTEXT | (0 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_ObstacleSize, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "obstSize" + }, + { ATF_NOFLAGS, 0, offsetof(struct DetectedObstacleData, obstSizeConfidence), + (ASN_TAG_CLASS_CONTEXT | (1 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_ObstacleSizeConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "obstSizeConfidence" + }, +}; +static const ber_tlv_tag_t asn_DEF_DetectedObstacleData_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)) +}; +static const asn_TYPE_tag2member_t asn_MAP_DetectedObstacleData_tag2el_1[] = { + { (ASN_TAG_CLASS_CONTEXT | (0 << 2)), 0, 0, 0 }, /* obstSize */ + { (ASN_TAG_CLASS_CONTEXT | (1 << 2)), 1, 0, 0 } /* obstSizeConfidence */ +}; +asn_SEQUENCE_specifics_t asn_SPC_DetectedObstacleData_specs_1 = { + sizeof(struct DetectedObstacleData), + offsetof(struct DetectedObstacleData, _asn_ctx), + asn_MAP_DetectedObstacleData_tag2el_1, + 2, /* Count of tags in the map */ + 0, 0, 0, /* Optional elements (not needed) */ + -1, /* First extension addition */ +}; +asn_TYPE_descriptor_t asn_DEF_DetectedObstacleData = { + "DetectedObstacleData", + "DetectedObstacleData", + &asn_OP_SEQUENCE, + asn_DEF_DetectedObstacleData_tags_1, + sizeof(asn_DEF_DetectedObstacleData_tags_1) + /sizeof(asn_DEF_DetectedObstacleData_tags_1[0]), /* 1 */ + asn_DEF_DetectedObstacleData_tags_1, /* Same as above */ + sizeof(asn_DEF_DetectedObstacleData_tags_1) + /sizeof(asn_DEF_DetectedObstacleData_tags_1[0]), /* 1 */ + { 0, 0, SEQUENCE_constraint }, + asn_MBR_DetectedObstacleData_1, + 2, /* Elements count */ + &asn_SPC_DetectedObstacleData_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/DetectedVRUData.c b/src/tmx/Asn_J2735/src/r63/DetectedVRUData.c new file mode 100644 index 000000000..29d981cbe --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/DetectedVRUData.c @@ -0,0 +1,82 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "DetectedVRUData.h" + +asn_TYPE_member_t asn_MBR_DetectedVRUData_1[] = { + { ATF_POINTER, 4, offsetof(struct DetectedVRUData, basicType), + (ASN_TAG_CLASS_CONTEXT | (0 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_PersonalDeviceUserType, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "basicType" + }, + { ATF_POINTER, 3, offsetof(struct DetectedVRUData, propulsion), + (ASN_TAG_CLASS_CONTEXT | (1 << 2)), + +1, /* EXPLICIT tag at current level */ + &asn_DEF_PropelledInformation, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "propulsion" + }, + { ATF_POINTER, 2, offsetof(struct DetectedVRUData, attachment), + (ASN_TAG_CLASS_CONTEXT | (2 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_Attachment, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "attachment" + }, + { ATF_POINTER, 1, offsetof(struct DetectedVRUData, radius), + (ASN_TAG_CLASS_CONTEXT | (3 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_AttachmentRadius, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "radius" + }, +}; +static const int asn_MAP_DetectedVRUData_oms_1[] = { 0, 1, 2, 3 }; +static const ber_tlv_tag_t asn_DEF_DetectedVRUData_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)) +}; +static const asn_TYPE_tag2member_t asn_MAP_DetectedVRUData_tag2el_1[] = { + { (ASN_TAG_CLASS_CONTEXT | (0 << 2)), 0, 0, 0 }, /* basicType */ + { (ASN_TAG_CLASS_CONTEXT | (1 << 2)), 1, 0, 0 }, /* propulsion */ + { (ASN_TAG_CLASS_CONTEXT | (2 << 2)), 2, 0, 0 }, /* attachment */ + { (ASN_TAG_CLASS_CONTEXT | (3 << 2)), 3, 0, 0 } /* radius */ +}; +asn_SEQUENCE_specifics_t asn_SPC_DetectedVRUData_specs_1 = { + sizeof(struct DetectedVRUData), + offsetof(struct DetectedVRUData, _asn_ctx), + asn_MAP_DetectedVRUData_tag2el_1, + 4, /* Count of tags in the map */ + asn_MAP_DetectedVRUData_oms_1, /* Optional members */ + 4, 0, /* Root/Additions */ + -1, /* First extension addition */ +}; +asn_TYPE_descriptor_t asn_DEF_DetectedVRUData = { + "DetectedVRUData", + "DetectedVRUData", + &asn_OP_SEQUENCE, + asn_DEF_DetectedVRUData_tags_1, + sizeof(asn_DEF_DetectedVRUData_tags_1) + /sizeof(asn_DEF_DetectedVRUData_tags_1[0]), /* 1 */ + asn_DEF_DetectedVRUData_tags_1, /* Same as above */ + sizeof(asn_DEF_DetectedVRUData_tags_1) + /sizeof(asn_DEF_DetectedVRUData_tags_1[0]), /* 1 */ + { 0, 0, SEQUENCE_constraint }, + asn_MBR_DetectedVRUData_1, + 4, /* Elements count */ + &asn_SPC_DetectedVRUData_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/DetectedVehicleData.c b/src/tmx/Asn_J2735/src/r63/DetectedVehicleData.c new file mode 100644 index 000000000..6fdf3e2e1 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/DetectedVehicleData.c @@ -0,0 +1,142 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "DetectedVehicleData.h" + +asn_TYPE_member_t asn_MBR_DetectedVehicleData_1[] = { + { ATF_POINTER, 10, offsetof(struct DetectedVehicleData, lights), + (ASN_TAG_CLASS_CONTEXT | (0 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_ExteriorLights, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "lights" + }, + { ATF_POINTER, 9, offsetof(struct DetectedVehicleData, vehAttitude), + (ASN_TAG_CLASS_CONTEXT | (1 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_Attitude, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "vehAttitude" + }, + { ATF_POINTER, 8, offsetof(struct DetectedVehicleData, vehAttitudeConfidence), + (ASN_TAG_CLASS_CONTEXT | (2 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_AttitudeConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "vehAttitudeConfidence" + }, + { ATF_POINTER, 7, offsetof(struct DetectedVehicleData, vehAngVel), + (ASN_TAG_CLASS_CONTEXT | (3 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_AngularVelocity, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "vehAngVel" + }, + { ATF_POINTER, 6, offsetof(struct DetectedVehicleData, vehAngVelConfidence), + (ASN_TAG_CLASS_CONTEXT | (4 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_AngularVelocityConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "vehAngVelConfidence" + }, + { ATF_POINTER, 5, offsetof(struct DetectedVehicleData, size), + (ASN_TAG_CLASS_CONTEXT | (5 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_VehicleSize, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "size" + }, + { ATF_POINTER, 4, offsetof(struct DetectedVehicleData, height), + (ASN_TAG_CLASS_CONTEXT | (6 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_VehicleHeight, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "height" + }, + { ATF_POINTER, 3, offsetof(struct DetectedVehicleData, vehicleSizeConfidence), + (ASN_TAG_CLASS_CONTEXT | (7 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_VehicleSizeConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "vehicleSizeConfidence" + }, + { ATF_POINTER, 2, offsetof(struct DetectedVehicleData, vehicleClass), + (ASN_TAG_CLASS_CONTEXT | (8 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_BasicVehicleClass, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "vehicleClass" + }, + { ATF_POINTER, 1, offsetof(struct DetectedVehicleData, classConf), + (ASN_TAG_CLASS_CONTEXT | (9 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_ClassificationConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "classConf" + }, +}; +static const int asn_MAP_DetectedVehicleData_oms_1[] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9 }; +static const ber_tlv_tag_t asn_DEF_DetectedVehicleData_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)) +}; +static const asn_TYPE_tag2member_t asn_MAP_DetectedVehicleData_tag2el_1[] = { + { (ASN_TAG_CLASS_CONTEXT | (0 << 2)), 0, 0, 0 }, /* lights */ + { (ASN_TAG_CLASS_CONTEXT | (1 << 2)), 1, 0, 0 }, /* vehAttitude */ + { (ASN_TAG_CLASS_CONTEXT | (2 << 2)), 2, 0, 0 }, /* vehAttitudeConfidence */ + { (ASN_TAG_CLASS_CONTEXT | (3 << 2)), 3, 0, 0 }, /* vehAngVel */ + { (ASN_TAG_CLASS_CONTEXT | (4 << 2)), 4, 0, 0 }, /* vehAngVelConfidence */ + { (ASN_TAG_CLASS_CONTEXT | (5 << 2)), 5, 0, 0 }, /* size */ + { (ASN_TAG_CLASS_CONTEXT | (6 << 2)), 6, 0, 0 }, /* height */ + { (ASN_TAG_CLASS_CONTEXT | (7 << 2)), 7, 0, 0 }, /* vehicleSizeConfidence */ + { (ASN_TAG_CLASS_CONTEXT | (8 << 2)), 8, 0, 0 }, /* vehicleClass */ + { (ASN_TAG_CLASS_CONTEXT | (9 << 2)), 9, 0, 0 } /* classConf */ +}; +asn_SEQUENCE_specifics_t asn_SPC_DetectedVehicleData_specs_1 = { + sizeof(struct DetectedVehicleData), + offsetof(struct DetectedVehicleData, _asn_ctx), + asn_MAP_DetectedVehicleData_tag2el_1, + 10, /* Count of tags in the map */ + asn_MAP_DetectedVehicleData_oms_1, /* Optional members */ + 10, 0, /* Root/Additions */ + 10, /* First extension addition */ +}; +asn_TYPE_descriptor_t asn_DEF_DetectedVehicleData = { + "DetectedVehicleData", + "DetectedVehicleData", + &asn_OP_SEQUENCE, + asn_DEF_DetectedVehicleData_tags_1, + sizeof(asn_DEF_DetectedVehicleData_tags_1) + /sizeof(asn_DEF_DetectedVehicleData_tags_1[0]), /* 1 */ + asn_DEF_DetectedVehicleData_tags_1, /* Same as above */ + sizeof(asn_DEF_DetectedVehicleData_tags_1) + /sizeof(asn_DEF_DetectedVehicleData_tags_1[0]), /* 1 */ + { 0, 0, SEQUENCE_constraint }, + asn_MBR_DetectedVehicleData_1, + 10, /* Elements count */ + &asn_SPC_DetectedVehicleData_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/EquipmentType.c b/src/tmx/Asn_J2735/src/r63/EquipmentType.c new file mode 100644 index 000000000..7707fbfe9 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/EquipmentType.c @@ -0,0 +1,62 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "EquipmentType.h" + +/* + * This type is implemented using NativeEnumerated, + * so here we adjust the DEF accordingly. + */ +static asn_oer_constraints_t asn_OER_type_EquipmentType_constr_1 CC_NOTUSED = { + { 0, 0 }, + -1}; +asn_per_constraints_t asn_PER_type_EquipmentType_constr_1 CC_NOTUSED = { + { APC_CONSTRAINED | APC_EXTENSIBLE, 2, 2, 0, 3 } /* (0..3,...) */, + { APC_UNCONSTRAINED, -1, -1, 0, 0 }, + 0, 0 /* No PER value map */ +}; +static const asn_INTEGER_enum_map_t asn_MAP_EquipmentType_value2enum_1[] = { + { 0, 7, "unknown" }, + { 1, 3, "rsu" }, + { 2, 3, "obu" }, + { 3, 3, "vru" } + /* This list is extensible */ +}; +static const unsigned int asn_MAP_EquipmentType_enum2value_1[] = { + 2, /* obu(2) */ + 1, /* rsu(1) */ + 0, /* unknown(0) */ + 3 /* vru(3) */ + /* This list is extensible */ +}; +const asn_INTEGER_specifics_t asn_SPC_EquipmentType_specs_1 = { + asn_MAP_EquipmentType_value2enum_1, /* "tag" => N; sorted by tag */ + asn_MAP_EquipmentType_enum2value_1, /* N => "tag"; sorted by N */ + 4, /* Number of elements in the maps */ + 5, /* Extensions before this member */ + 1, /* Strict enumeration */ + 0, /* Native long size */ + 0 +}; +static const ber_tlv_tag_t asn_DEF_EquipmentType_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (10 << 2)) +}; +asn_TYPE_descriptor_t asn_DEF_EquipmentType = { + "EquipmentType", + "EquipmentType", + &asn_OP_NativeEnumerated, + asn_DEF_EquipmentType_tags_1, + sizeof(asn_DEF_EquipmentType_tags_1) + /sizeof(asn_DEF_EquipmentType_tags_1[0]), /* 1 */ + asn_DEF_EquipmentType_tags_1, /* Same as above */ + sizeof(asn_DEF_EquipmentType_tags_1) + /sizeof(asn_DEF_EquipmentType_tags_1[0]), /* 1 */ + { &asn_OER_type_EquipmentType_constr_1, &asn_PER_type_EquipmentType_constr_1, NativeEnumerated_constraint }, + 0, 0, /* Defined elsewhere */ + &asn_SPC_EquipmentType_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/MeasurementTimeOffset.c b/src/tmx/Asn_J2735/src/r63/MeasurementTimeOffset.c new file mode 100644 index 000000000..604caf91c --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/MeasurementTimeOffset.c @@ -0,0 +1,64 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "MeasurementTimeOffset.h" + +int +MeasurementTimeOffset_constraint(const asn_TYPE_descriptor_t *td, const void *sptr, + asn_app_constraint_failed_f *ctfailcb, void *app_key) { + long value; + + if(!sptr) { + ASN__CTFAIL(app_key, td, sptr, + "%s: value not given (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } + + value = *(const long *)sptr; + + if((value >= -1500 && value <= 1500)) { + /* Constraint check succeeded */ + return 0; + } else { + ASN__CTFAIL(app_key, td, sptr, + "%s: constraint failed (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } +} + +/* + * This type is implemented using NativeInteger, + * so here we adjust the DEF accordingly. + */ +static asn_oer_constraints_t asn_OER_type_MeasurementTimeOffset_constr_1 CC_NOTUSED = { + { 2, 0 } /* (-1500..1500) */, + -1}; +asn_per_constraints_t asn_PER_type_MeasurementTimeOffset_constr_1 CC_NOTUSED = { + { APC_CONSTRAINED, 12, 12, -1500, 1500 } /* (-1500..1500) */, + { APC_UNCONSTRAINED, -1, -1, 0, 0 }, + 0, 0 /* No PER value map */ +}; +static const ber_tlv_tag_t asn_DEF_MeasurementTimeOffset_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (2 << 2)) +}; +asn_TYPE_descriptor_t asn_DEF_MeasurementTimeOffset = { + "MeasurementTimeOffset", + "MeasurementTimeOffset", + &asn_OP_NativeInteger, + asn_DEF_MeasurementTimeOffset_tags_1, + sizeof(asn_DEF_MeasurementTimeOffset_tags_1) + /sizeof(asn_DEF_MeasurementTimeOffset_tags_1[0]), /* 1 */ + asn_DEF_MeasurementTimeOffset_tags_1, /* Same as above */ + sizeof(asn_DEF_MeasurementTimeOffset_tags_1) + /sizeof(asn_DEF_MeasurementTimeOffset_tags_1[0]), /* 1 */ + { &asn_OER_type_MeasurementTimeOffset_constr_1, &asn_PER_type_MeasurementTimeOffset_constr_1, MeasurementTimeOffset_constraint }, + 0, 0, /* No members */ + 0 /* No specifics */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/MessageFrame.c b/src/tmx/Asn_J2735/src/r63/MessageFrame.c index edd668078..b3925827e 100644 --- a/src/tmx/Asn_J2735/src/r63/MessageFrame.c +++ b/src/tmx/Asn_J2735/src/r63/MessageFrame.c @@ -1,8 +1,8 @@ /* * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) * From ASN.1 module "DSRC" - * found in "J2735_201603_ASN_CC.asn" - * `asn1c -gen-PER -fcompound-names -fincludes-quoted` + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names` */ #include "MessageFrame.h" @@ -38,6 +38,7 @@ static const long asn_VAL_28_testMessage12 = 252; static const long asn_VAL_29_testMessage13 = 253; static const long asn_VAL_30_testMessage14 = 254; static const long asn_VAL_31_testMessage15 = 255; +static const long asn_VAL_32_sensorDataSharingMessage = 41; static const asn_ioc_cell_t asn_IOS_MessageTypes_1_rows[] = { { "&id", aioc__value, &asn_DEF_DSRCmsgID, &asn_VAL_1_basicSafetyMessage }, { "&Type", aioc__type, &asn_DEF_BasicSafetyMessage }, @@ -100,10 +101,12 @@ static const asn_ioc_cell_t asn_IOS_MessageTypes_1_rows[] = { { "&id", aioc__value, &asn_DEF_DSRCmsgID, &asn_VAL_30_testMessage14 }, { "&Type", aioc__type, &asn_DEF_TestMessage14 }, { "&id", aioc__value, &asn_DEF_DSRCmsgID, &asn_VAL_31_testMessage15 }, - { "&Type", aioc__type, &asn_DEF_TestMessage15 } + { "&Type", aioc__type, &asn_DEF_TestMessage15 }, + { "&id", aioc__value, &asn_DEF_DSRCmsgID, &asn_VAL_32_sensorDataSharingMessage }, + { "&Type", aioc__type, &asn_DEF_SensorDataSharingMessage } }; static const asn_ioc_set_t asn_IOS_MessageTypes_1[] = { - { 31, 2, asn_IOS_MessageTypes_1_rows } + 32, 2, asn_IOS_MessageTypes_1_rows }; static int memb_messageId_constraint_1(const asn_TYPE_descriptor_t *td, const void *sptr, @@ -472,39 +475,49 @@ static asn_TYPE_member_t asn_MBR_value_3[] = { 0, 0, /* No default value */ "TestMessage15" }, + { ATF_NOFLAGS, 0, offsetof(struct value, choice.SensorDataSharingMessage), + (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), + 0, + &asn_DEF_SensorDataSharingMessage, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "SensorDataSharingMessage" + }, }; static const asn_TYPE_tag2member_t asn_MAP_value_tag2el_3[] = { - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 0, 0, 30 }, /* BasicSafetyMessage */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 1, -1, 29 }, /* MapData */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 2, -2, 28 }, /* SPAT */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 3, -3, 27 }, /* CommonSafetyRequest */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 4, -4, 26 }, /* EmergencyVehicleAlert */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 5, -5, 25 }, /* IntersectionCollision */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 6, -6, 24 }, /* NMEAcorrections */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 7, -7, 23 }, /* ProbeDataManagement */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 8, -8, 22 }, /* ProbeVehicleData */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 9, -9, 21 }, /* RoadSideAlert */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 10, -10, 20 }, /* RTCMcorrections */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 11, -11, 19 }, /* SignalRequestMessage */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 12, -12, 18 }, /* SignalStatusMessage */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 13, -13, 17 }, /* TravelerInformation */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 14, -14, 16 }, /* PersonalSafetyMessage */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 15, -15, 15 }, /* TestMessage00 */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 16, -16, 14 }, /* TestMessage01 */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 17, -17, 13 }, /* TestMessage02 */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 18, -18, 12 }, /* TestMessage03 */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 19, -19, 11 }, /* TestMessage04 */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 20, -20, 10 }, /* TestMessage05 */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 21, -21, 9 }, /* TestMessage06 */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 22, -22, 8 }, /* TestMessage07 */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 23, -23, 7 }, /* TestMessage08 */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 24, -24, 6 }, /* TestMessage09 */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 25, -25, 5 }, /* TestMessage10 */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 26, -26, 4 }, /* TestMessage11 */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 27, -27, 3 }, /* TestMessage12 */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 28, -28, 2 }, /* TestMessage13 */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 29, -29, 1 }, /* TestMessage14 */ - { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 30, -30, 0 } /* TestMessage15 */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 0, 0, 31 }, /* BasicSafetyMessage */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 1, -1, 30 }, /* MapData */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 2, -2, 29 }, /* SPAT */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 3, -3, 28 }, /* CommonSafetyRequest */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 4, -4, 27 }, /* EmergencyVehicleAlert */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 5, -5, 26 }, /* IntersectionCollision */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 6, -6, 25 }, /* NMEAcorrections */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 7, -7, 24 }, /* ProbeDataManagement */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 8, -8, 23 }, /* ProbeVehicleData */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 9, -9, 22 }, /* RoadSideAlert */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 10, -10, 21 }, /* RTCMcorrections */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 11, -11, 20 }, /* SignalRequestMessage */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 12, -12, 19 }, /* SignalStatusMessage */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 13, -13, 18 }, /* TravelerInformation */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 14, -14, 17 }, /* PersonalSafetyMessage */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 15, -15, 16 }, /* TestMessage00 */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 16, -16, 15 }, /* TestMessage01 */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 17, -17, 14 }, /* TestMessage02 */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 18, -18, 13 }, /* TestMessage03 */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 19, -19, 12 }, /* TestMessage04 */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 20, -20, 11 }, /* TestMessage05 */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 21, -21, 10 }, /* TestMessage06 */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 22, -22, 9 }, /* TestMessage07 */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 23, -23, 8 }, /* TestMessage08 */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 24, -24, 7 }, /* TestMessage09 */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 25, -25, 6 }, /* TestMessage10 */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 26, -26, 5 }, /* TestMessage11 */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 27, -27, 4 }, /* TestMessage12 */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 28, -28, 3 }, /* TestMessage13 */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 29, -29, 2 }, /* TestMessage14 */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 30, -30, 1 }, /* TestMessage15 */ + { (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)), 31, -31, 0 } /* SensorDataSharingMessage */ }; static asn_CHOICE_specifics_t asn_SPC_value_specs_3 = { sizeof(struct value), @@ -512,7 +525,7 @@ static asn_CHOICE_specifics_t asn_SPC_value_specs_3 = { offsetof(struct value, present), sizeof(((struct value *)0)->present), asn_MAP_value_tag2el_3, - 31, /* Count of tags in the map */ + 32, /* Count of tags in the map */ 0, 0, -1 /* Extensions start */ }; @@ -527,7 +540,7 @@ asn_TYPE_descriptor_t asn_DEF_value_3 = { 0, /* No tags (count) */ { 0, 0, OPEN_TYPE_constraint }, asn_MBR_value_3, - 31, /* Elements count */ + 32, /* Elements count */ &asn_SPC_value_specs_3 /* Additional specs */ }; diff --git a/src/tmx/Asn_J2735/src/r63/ObjectDistance.c b/src/tmx/Asn_J2735/src/r63/ObjectDistance.c new file mode 100644 index 000000000..79ec8c88b --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/ObjectDistance.c @@ -0,0 +1,64 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "ObjectDistance.h" + +int +ObjectDistance_constraint(const asn_TYPE_descriptor_t *td, const void *sptr, + asn_app_constraint_failed_f *ctfailcb, void *app_key) { + long value; + + if(!sptr) { + ASN__CTFAIL(app_key, td, sptr, + "%s: value not given (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } + + value = *(const long *)sptr; + + if((value >= -32767 && value <= 32767)) { + /* Constraint check succeeded */ + return 0; + } else { + ASN__CTFAIL(app_key, td, sptr, + "%s: constraint failed (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } +} + +/* + * This type is implemented using NativeInteger, + * so here we adjust the DEF accordingly. + */ +static asn_oer_constraints_t asn_OER_type_ObjectDistance_constr_1 CC_NOTUSED = { + { 2, 0 } /* (-32767..32767) */, + -1}; +asn_per_constraints_t asn_PER_type_ObjectDistance_constr_1 CC_NOTUSED = { + { APC_CONSTRAINED, 16, 16, -32767, 32767 } /* (-32767..32767) */, + { APC_UNCONSTRAINED, -1, -1, 0, 0 }, + 0, 0 /* No PER value map */ +}; +static const ber_tlv_tag_t asn_DEF_ObjectDistance_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (2 << 2)) +}; +asn_TYPE_descriptor_t asn_DEF_ObjectDistance = { + "ObjectDistance", + "ObjectDistance", + &asn_OP_NativeInteger, + asn_DEF_ObjectDistance_tags_1, + sizeof(asn_DEF_ObjectDistance_tags_1) + /sizeof(asn_DEF_ObjectDistance_tags_1[0]), /* 1 */ + asn_DEF_ObjectDistance_tags_1, /* Same as above */ + sizeof(asn_DEF_ObjectDistance_tags_1) + /sizeof(asn_DEF_ObjectDistance_tags_1[0]), /* 1 */ + { &asn_OER_type_ObjectDistance_constr_1, &asn_PER_type_ObjectDistance_constr_1, ObjectDistance_constraint }, + 0, 0, /* No members */ + 0 /* No specifics */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/ObjectID.c b/src/tmx/Asn_J2735/src/r63/ObjectID.c new file mode 100644 index 000000000..5b46a3671 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/ObjectID.c @@ -0,0 +1,64 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "ObjectID.h" + +int +ObjectID_constraint(const asn_TYPE_descriptor_t *td, const void *sptr, + asn_app_constraint_failed_f *ctfailcb, void *app_key) { + long value; + + if(!sptr) { + ASN__CTFAIL(app_key, td, sptr, + "%s: value not given (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } + + value = *(const long *)sptr; + + if((value >= 0 && value <= 65535)) { + /* Constraint check succeeded */ + return 0; + } else { + ASN__CTFAIL(app_key, td, sptr, + "%s: constraint failed (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } +} + +/* + * This type is implemented using NativeInteger, + * so here we adjust the DEF accordingly. + */ +static asn_oer_constraints_t asn_OER_type_ObjectID_constr_1 CC_NOTUSED = { + { 2, 1 } /* (0..65535) */, + -1}; +asn_per_constraints_t asn_PER_type_ObjectID_constr_1 CC_NOTUSED = { + { APC_CONSTRAINED, 16, 16, 0, 65535 } /* (0..65535) */, + { APC_UNCONSTRAINED, -1, -1, 0, 0 }, + 0, 0 /* No PER value map */ +}; +static const ber_tlv_tag_t asn_DEF_ObjectID_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (2 << 2)) +}; +asn_TYPE_descriptor_t asn_DEF_ObjectID = { + "ObjectID", + "ObjectID", + &asn_OP_NativeInteger, + asn_DEF_ObjectID_tags_1, + sizeof(asn_DEF_ObjectID_tags_1) + /sizeof(asn_DEF_ObjectID_tags_1[0]), /* 1 */ + asn_DEF_ObjectID_tags_1, /* Same as above */ + sizeof(asn_DEF_ObjectID_tags_1) + /sizeof(asn_DEF_ObjectID_tags_1[0]), /* 1 */ + { &asn_OER_type_ObjectID_constr_1, &asn_PER_type_ObjectID_constr_1, ObjectID_constraint }, + 0, 0, /* No members */ + 0 /* No specifics */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/ObjectType.c b/src/tmx/Asn_J2735/src/r63/ObjectType.c new file mode 100644 index 000000000..43ce3726e --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/ObjectType.c @@ -0,0 +1,62 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "ObjectType.h" + +/* + * This type is implemented using NativeEnumerated, + * so here we adjust the DEF accordingly. + */ +static asn_oer_constraints_t asn_OER_type_ObjectType_constr_1 CC_NOTUSED = { + { 0, 0 }, + -1}; +asn_per_constraints_t asn_PER_type_ObjectType_constr_1 CC_NOTUSED = { + { APC_CONSTRAINED | APC_EXTENSIBLE, 2, 2, 0, 3 } /* (0..3,...) */, + { APC_UNCONSTRAINED, -1, -1, 0, 0 }, + 0, 0 /* No PER value map */ +}; +static const asn_INTEGER_enum_map_t asn_MAP_ObjectType_value2enum_1[] = { + { 0, 7, "unknown" }, + { 1, 7, "vehicle" }, + { 2, 3, "vru" }, + { 3, 6, "animal" } + /* This list is extensible */ +}; +static const unsigned int asn_MAP_ObjectType_enum2value_1[] = { + 3, /* animal(3) */ + 0, /* unknown(0) */ + 1, /* vehicle(1) */ + 2 /* vru(2) */ + /* This list is extensible */ +}; +const asn_INTEGER_specifics_t asn_SPC_ObjectType_specs_1 = { + asn_MAP_ObjectType_value2enum_1, /* "tag" => N; sorted by tag */ + asn_MAP_ObjectType_enum2value_1, /* N => "tag"; sorted by N */ + 4, /* Number of elements in the maps */ + 5, /* Extensions before this member */ + 1, /* Strict enumeration */ + 0, /* Native long size */ + 0 +}; +static const ber_tlv_tag_t asn_DEF_ObjectType_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (10 << 2)) +}; +asn_TYPE_descriptor_t asn_DEF_ObjectType = { + "ObjectType", + "ObjectType", + &asn_OP_NativeEnumerated, + asn_DEF_ObjectType_tags_1, + sizeof(asn_DEF_ObjectType_tags_1) + /sizeof(asn_DEF_ObjectType_tags_1[0]), /* 1 */ + asn_DEF_ObjectType_tags_1, /* Same as above */ + sizeof(asn_DEF_ObjectType_tags_1) + /sizeof(asn_DEF_ObjectType_tags_1[0]), /* 1 */ + { &asn_OER_type_ObjectType_constr_1, &asn_PER_type_ObjectType_constr_1, NativeEnumerated_constraint }, + 0, 0, /* Defined elsewhere */ + &asn_SPC_ObjectType_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/ObstacleSize.c b/src/tmx/Asn_J2735/src/r63/ObstacleSize.c new file mode 100644 index 000000000..8edc1baf8 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/ObstacleSize.c @@ -0,0 +1,72 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "ObstacleSize.h" + +asn_TYPE_member_t asn_MBR_ObstacleSize_1[] = { + { ATF_NOFLAGS, 0, offsetof(struct ObstacleSize, width), + (ASN_TAG_CLASS_CONTEXT | (0 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_SizeValue, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "width" + }, + { ATF_NOFLAGS, 0, offsetof(struct ObstacleSize, length), + (ASN_TAG_CLASS_CONTEXT | (1 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_SizeValue, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "length" + }, + { ATF_POINTER, 1, offsetof(struct ObstacleSize, height), + (ASN_TAG_CLASS_CONTEXT | (2 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_SizeValue, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "height" + }, +}; +static const int asn_MAP_ObstacleSize_oms_1[] = { 2 }; +static const ber_tlv_tag_t asn_DEF_ObstacleSize_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)) +}; +static const asn_TYPE_tag2member_t asn_MAP_ObstacleSize_tag2el_1[] = { + { (ASN_TAG_CLASS_CONTEXT | (0 << 2)), 0, 0, 0 }, /* width */ + { (ASN_TAG_CLASS_CONTEXT | (1 << 2)), 1, 0, 0 }, /* length */ + { (ASN_TAG_CLASS_CONTEXT | (2 << 2)), 2, 0, 0 } /* height */ +}; +asn_SEQUENCE_specifics_t asn_SPC_ObstacleSize_specs_1 = { + sizeof(struct ObstacleSize), + offsetof(struct ObstacleSize, _asn_ctx), + asn_MAP_ObstacleSize_tag2el_1, + 3, /* Count of tags in the map */ + asn_MAP_ObstacleSize_oms_1, /* Optional members */ + 1, 0, /* Root/Additions */ + -1, /* First extension addition */ +}; +asn_TYPE_descriptor_t asn_DEF_ObstacleSize = { + "ObstacleSize", + "ObstacleSize", + &asn_OP_SEQUENCE, + asn_DEF_ObstacleSize_tags_1, + sizeof(asn_DEF_ObstacleSize_tags_1) + /sizeof(asn_DEF_ObstacleSize_tags_1[0]), /* 1 */ + asn_DEF_ObstacleSize_tags_1, /* Same as above */ + sizeof(asn_DEF_ObstacleSize_tags_1) + /sizeof(asn_DEF_ObstacleSize_tags_1[0]), /* 1 */ + { 0, 0, SEQUENCE_constraint }, + asn_MBR_ObstacleSize_1, + 3, /* Elements count */ + &asn_SPC_ObstacleSize_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/ObstacleSizeConfidence.c b/src/tmx/Asn_J2735/src/r63/ObstacleSizeConfidence.c new file mode 100644 index 000000000..d5ddfbc43 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/ObstacleSizeConfidence.c @@ -0,0 +1,72 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "ObstacleSizeConfidence.h" + +asn_TYPE_member_t asn_MBR_ObstacleSizeConfidence_1[] = { + { ATF_NOFLAGS, 0, offsetof(struct ObstacleSizeConfidence, widthConfidence), + (ASN_TAG_CLASS_CONTEXT | (0 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_SizeValueConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "widthConfidence" + }, + { ATF_NOFLAGS, 0, offsetof(struct ObstacleSizeConfidence, lengthConfidence), + (ASN_TAG_CLASS_CONTEXT | (1 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_SizeValueConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "lengthConfidence" + }, + { ATF_POINTER, 1, offsetof(struct ObstacleSizeConfidence, heightConfidence), + (ASN_TAG_CLASS_CONTEXT | (2 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_SizeValueConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "heightConfidence" + }, +}; +static const int asn_MAP_ObstacleSizeConfidence_oms_1[] = { 2 }; +static const ber_tlv_tag_t asn_DEF_ObstacleSizeConfidence_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)) +}; +static const asn_TYPE_tag2member_t asn_MAP_ObstacleSizeConfidence_tag2el_1[] = { + { (ASN_TAG_CLASS_CONTEXT | (0 << 2)), 0, 0, 0 }, /* widthConfidence */ + { (ASN_TAG_CLASS_CONTEXT | (1 << 2)), 1, 0, 0 }, /* lengthConfidence */ + { (ASN_TAG_CLASS_CONTEXT | (2 << 2)), 2, 0, 0 } /* heightConfidence */ +}; +asn_SEQUENCE_specifics_t asn_SPC_ObstacleSizeConfidence_specs_1 = { + sizeof(struct ObstacleSizeConfidence), + offsetof(struct ObstacleSizeConfidence, _asn_ctx), + asn_MAP_ObstacleSizeConfidence_tag2el_1, + 3, /* Count of tags in the map */ + asn_MAP_ObstacleSizeConfidence_oms_1, /* Optional members */ + 1, 0, /* Root/Additions */ + -1, /* First extension addition */ +}; +asn_TYPE_descriptor_t asn_DEF_ObstacleSizeConfidence = { + "ObstacleSizeConfidence", + "ObstacleSizeConfidence", + &asn_OP_SEQUENCE, + asn_DEF_ObstacleSizeConfidence_tags_1, + sizeof(asn_DEF_ObstacleSizeConfidence_tags_1) + /sizeof(asn_DEF_ObstacleSizeConfidence_tags_1[0]), /* 1 */ + asn_DEF_ObstacleSizeConfidence_tags_1, /* Same as above */ + sizeof(asn_DEF_ObstacleSizeConfidence_tags_1) + /sizeof(asn_DEF_ObstacleSizeConfidence_tags_1[0]), /* 1 */ + { 0, 0, SEQUENCE_constraint }, + asn_MBR_ObstacleSizeConfidence_1, + 3, /* Elements count */ + &asn_SPC_ObstacleSizeConfidence_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/PitchDetected.c b/src/tmx/Asn_J2735/src/r63/PitchDetected.c new file mode 100644 index 000000000..531398fb6 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/PitchDetected.c @@ -0,0 +1,64 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "PitchDetected.h" + +int +PitchDetected_constraint(const asn_TYPE_descriptor_t *td, const void *sptr, + asn_app_constraint_failed_f *ctfailcb, void *app_key) { + long value; + + if(!sptr) { + ASN__CTFAIL(app_key, td, sptr, + "%s: value not given (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } + + value = *(const long *)sptr; + + if((value >= -7200 && value <= 7200)) { + /* Constraint check succeeded */ + return 0; + } else { + ASN__CTFAIL(app_key, td, sptr, + "%s: constraint failed (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } +} + +/* + * This type is implemented using NativeInteger, + * so here we adjust the DEF accordingly. + */ +static asn_oer_constraints_t asn_OER_type_PitchDetected_constr_1 CC_NOTUSED = { + { 2, 0 } /* (-7200..7200) */, + -1}; +asn_per_constraints_t asn_PER_type_PitchDetected_constr_1 CC_NOTUSED = { + { APC_CONSTRAINED, 14, 14, -7200, 7200 } /* (-7200..7200) */, + { APC_UNCONSTRAINED, -1, -1, 0, 0 }, + 0, 0 /* No PER value map */ +}; +static const ber_tlv_tag_t asn_DEF_PitchDetected_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (2 << 2)) +}; +asn_TYPE_descriptor_t asn_DEF_PitchDetected = { + "PitchDetected", + "PitchDetected", + &asn_OP_NativeInteger, + asn_DEF_PitchDetected_tags_1, + sizeof(asn_DEF_PitchDetected_tags_1) + /sizeof(asn_DEF_PitchDetected_tags_1[0]), /* 1 */ + asn_DEF_PitchDetected_tags_1, /* Same as above */ + sizeof(asn_DEF_PitchDetected_tags_1) + /sizeof(asn_DEF_PitchDetected_tags_1[0]), /* 1 */ + { &asn_OER_type_PitchDetected_constr_1, &asn_PER_type_PitchDetected_constr_1, PitchDetected_constraint }, + 0, 0, /* No members */ + 0 /* No specifics */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/PitchRate.c b/src/tmx/Asn_J2735/src/r63/PitchRate.c new file mode 100644 index 000000000..368ba352d --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/PitchRate.c @@ -0,0 +1,64 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "PitchRate.h" + +int +PitchRate_constraint(const asn_TYPE_descriptor_t *td, const void *sptr, + asn_app_constraint_failed_f *ctfailcb, void *app_key) { + long value; + + if(!sptr) { + ASN__CTFAIL(app_key, td, sptr, + "%s: value not given (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } + + value = *(const long *)sptr; + + if((value >= -32767 && value <= 32767)) { + /* Constraint check succeeded */ + return 0; + } else { + ASN__CTFAIL(app_key, td, sptr, + "%s: constraint failed (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } +} + +/* + * This type is implemented using NativeInteger, + * so here we adjust the DEF accordingly. + */ +static asn_oer_constraints_t asn_OER_type_PitchRate_constr_1 CC_NOTUSED = { + { 2, 0 } /* (-32767..32767) */, + -1}; +asn_per_constraints_t asn_PER_type_PitchRate_constr_1 CC_NOTUSED = { + { APC_CONSTRAINED, 16, 16, -32767, 32767 } /* (-32767..32767) */, + { APC_UNCONSTRAINED, -1, -1, 0, 0 }, + 0, 0 /* No PER value map */ +}; +static const ber_tlv_tag_t asn_DEF_PitchRate_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (2 << 2)) +}; +asn_TYPE_descriptor_t asn_DEF_PitchRate = { + "PitchRate", + "PitchRate", + &asn_OP_NativeInteger, + asn_DEF_PitchRate_tags_1, + sizeof(asn_DEF_PitchRate_tags_1) + /sizeof(asn_DEF_PitchRate_tags_1[0]), /* 1 */ + asn_DEF_PitchRate_tags_1, /* Same as above */ + sizeof(asn_DEF_PitchRate_tags_1) + /sizeof(asn_DEF_PitchRate_tags_1[0]), /* 1 */ + { &asn_OER_type_PitchRate_constr_1, &asn_PER_type_PitchRate_constr_1, PitchRate_constraint }, + 0, 0, /* No members */ + 0 /* No specifics */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/PitchRateConfidence.c b/src/tmx/Asn_J2735/src/r63/PitchRateConfidence.c new file mode 100644 index 000000000..d510e4c1f --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/PitchRateConfidence.c @@ -0,0 +1,68 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "PitchRateConfidence.h" + +/* + * This type is implemented using NativeEnumerated, + * so here we adjust the DEF accordingly. + */ +static asn_oer_constraints_t asn_OER_type_PitchRateConfidence_constr_1 CC_NOTUSED = { + { 0, 0 }, + -1}; +asn_per_constraints_t asn_PER_type_PitchRateConfidence_constr_1 CC_NOTUSED = { + { APC_CONSTRAINED, 3, 3, 0, 7 } /* (0..7) */, + { APC_UNCONSTRAINED, -1, -1, 0, 0 }, + 0, 0 /* No PER value map */ +}; +static const asn_INTEGER_enum_map_t asn_MAP_PitchRateConfidence_value2enum_1[] = { + { 0, 11, "unavailable" }, + { 1, 13, "degSec-100-00" }, + { 2, 13, "degSec-010-00" }, + { 3, 13, "degSec-005-00" }, + { 4, 13, "degSec-001-00" }, + { 5, 13, "degSec-000-10" }, + { 6, 13, "degSec-000-05" }, + { 7, 13, "degSec-000-01" } +}; +static const unsigned int asn_MAP_PitchRateConfidence_enum2value_1[] = { + 7, /* degSec-000-01(7) */ + 6, /* degSec-000-05(6) */ + 5, /* degSec-000-10(5) */ + 4, /* degSec-001-00(4) */ + 3, /* degSec-005-00(3) */ + 2, /* degSec-010-00(2) */ + 1, /* degSec-100-00(1) */ + 0 /* unavailable(0) */ +}; +const asn_INTEGER_specifics_t asn_SPC_PitchRateConfidence_specs_1 = { + asn_MAP_PitchRateConfidence_value2enum_1, /* "tag" => N; sorted by tag */ + asn_MAP_PitchRateConfidence_enum2value_1, /* N => "tag"; sorted by N */ + 8, /* Number of elements in the maps */ + 0, /* Enumeration is not extensible */ + 1, /* Strict enumeration */ + 0, /* Native long size */ + 0 +}; +static const ber_tlv_tag_t asn_DEF_PitchRateConfidence_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (10 << 2)) +}; +asn_TYPE_descriptor_t asn_DEF_PitchRateConfidence = { + "PitchRateConfidence", + "PitchRateConfidence", + &asn_OP_NativeEnumerated, + asn_DEF_PitchRateConfidence_tags_1, + sizeof(asn_DEF_PitchRateConfidence_tags_1) + /sizeof(asn_DEF_PitchRateConfidence_tags_1[0]), /* 1 */ + asn_DEF_PitchRateConfidence_tags_1, /* Same as above */ + sizeof(asn_DEF_PitchRateConfidence_tags_1) + /sizeof(asn_DEF_PitchRateConfidence_tags_1[0]), /* 1 */ + { &asn_OER_type_PitchRateConfidence_constr_1, &asn_PER_type_PitchRateConfidence_constr_1, NativeEnumerated_constraint }, + 0, 0, /* Defined elsewhere */ + &asn_SPC_PitchRateConfidence_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/PositionOffsetXYZ.c b/src/tmx/Asn_J2735/src/r63/PositionOffsetXYZ.c new file mode 100644 index 000000000..fd2d344d1 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/PositionOffsetXYZ.c @@ -0,0 +1,72 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "PositionOffsetXYZ.h" + +asn_TYPE_member_t asn_MBR_PositionOffsetXYZ_1[] = { + { ATF_NOFLAGS, 0, offsetof(struct PositionOffsetXYZ, offsetX), + (ASN_TAG_CLASS_CONTEXT | (0 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_ObjectDistance, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "offsetX" + }, + { ATF_NOFLAGS, 0, offsetof(struct PositionOffsetXYZ, offsetY), + (ASN_TAG_CLASS_CONTEXT | (1 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_ObjectDistance, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "offsetY" + }, + { ATF_POINTER, 1, offsetof(struct PositionOffsetXYZ, offsetZ), + (ASN_TAG_CLASS_CONTEXT | (2 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_ObjectDistance, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "offsetZ" + }, +}; +static const int asn_MAP_PositionOffsetXYZ_oms_1[] = { 2 }; +static const ber_tlv_tag_t asn_DEF_PositionOffsetXYZ_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)) +}; +static const asn_TYPE_tag2member_t asn_MAP_PositionOffsetXYZ_tag2el_1[] = { + { (ASN_TAG_CLASS_CONTEXT | (0 << 2)), 0, 0, 0 }, /* offsetX */ + { (ASN_TAG_CLASS_CONTEXT | (1 << 2)), 1, 0, 0 }, /* offsetY */ + { (ASN_TAG_CLASS_CONTEXT | (2 << 2)), 2, 0, 0 } /* offsetZ */ +}; +asn_SEQUENCE_specifics_t asn_SPC_PositionOffsetXYZ_specs_1 = { + sizeof(struct PositionOffsetXYZ), + offsetof(struct PositionOffsetXYZ, _asn_ctx), + asn_MAP_PositionOffsetXYZ_tag2el_1, + 3, /* Count of tags in the map */ + asn_MAP_PositionOffsetXYZ_oms_1, /* Optional members */ + 1, 0, /* Root/Additions */ + -1, /* First extension addition */ +}; +asn_TYPE_descriptor_t asn_DEF_PositionOffsetXYZ = { + "PositionOffsetXYZ", + "PositionOffsetXYZ", + &asn_OP_SEQUENCE, + asn_DEF_PositionOffsetXYZ_tags_1, + sizeof(asn_DEF_PositionOffsetXYZ_tags_1) + /sizeof(asn_DEF_PositionOffsetXYZ_tags_1[0]), /* 1 */ + asn_DEF_PositionOffsetXYZ_tags_1, /* Same as above */ + sizeof(asn_DEF_PositionOffsetXYZ_tags_1) + /sizeof(asn_DEF_PositionOffsetXYZ_tags_1[0]), /* 1 */ + { 0, 0, SEQUENCE_constraint }, + asn_MBR_PositionOffsetXYZ_1, + 3, /* Elements count */ + &asn_SPC_PositionOffsetXYZ_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/RollDetected.c b/src/tmx/Asn_J2735/src/r63/RollDetected.c new file mode 100644 index 000000000..017e10e77 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/RollDetected.c @@ -0,0 +1,64 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "RollDetected.h" + +int +RollDetected_constraint(const asn_TYPE_descriptor_t *td, const void *sptr, + asn_app_constraint_failed_f *ctfailcb, void *app_key) { + long value; + + if(!sptr) { + ASN__CTFAIL(app_key, td, sptr, + "%s: value not given (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } + + value = *(const long *)sptr; + + if((value >= -14400 && value <= 14400)) { + /* Constraint check succeeded */ + return 0; + } else { + ASN__CTFAIL(app_key, td, sptr, + "%s: constraint failed (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } +} + +/* + * This type is implemented using NativeInteger, + * so here we adjust the DEF accordingly. + */ +static asn_oer_constraints_t asn_OER_type_RollDetected_constr_1 CC_NOTUSED = { + { 2, 0 } /* (-14400..14400) */, + -1}; +asn_per_constraints_t asn_PER_type_RollDetected_constr_1 CC_NOTUSED = { + { APC_CONSTRAINED, 15, 15, -14400, 14400 } /* (-14400..14400) */, + { APC_UNCONSTRAINED, -1, -1, 0, 0 }, + 0, 0 /* No PER value map */ +}; +static const ber_tlv_tag_t asn_DEF_RollDetected_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (2 << 2)) +}; +asn_TYPE_descriptor_t asn_DEF_RollDetected = { + "RollDetected", + "RollDetected", + &asn_OP_NativeInteger, + asn_DEF_RollDetected_tags_1, + sizeof(asn_DEF_RollDetected_tags_1) + /sizeof(asn_DEF_RollDetected_tags_1[0]), /* 1 */ + asn_DEF_RollDetected_tags_1, /* Same as above */ + sizeof(asn_DEF_RollDetected_tags_1) + /sizeof(asn_DEF_RollDetected_tags_1[0]), /* 1 */ + { &asn_OER_type_RollDetected_constr_1, &asn_PER_type_RollDetected_constr_1, RollDetected_constraint }, + 0, 0, /* No members */ + 0 /* No specifics */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/RollRate.c b/src/tmx/Asn_J2735/src/r63/RollRate.c new file mode 100644 index 000000000..d5c525e3a --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/RollRate.c @@ -0,0 +1,64 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "RollRate.h" + +int +RollRate_constraint(const asn_TYPE_descriptor_t *td, const void *sptr, + asn_app_constraint_failed_f *ctfailcb, void *app_key) { + long value; + + if(!sptr) { + ASN__CTFAIL(app_key, td, sptr, + "%s: value not given (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } + + value = *(const long *)sptr; + + if((value >= -32767 && value <= 32767)) { + /* Constraint check succeeded */ + return 0; + } else { + ASN__CTFAIL(app_key, td, sptr, + "%s: constraint failed (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } +} + +/* + * This type is implemented using NativeInteger, + * so here we adjust the DEF accordingly. + */ +static asn_oer_constraints_t asn_OER_type_RollRate_constr_1 CC_NOTUSED = { + { 2, 0 } /* (-32767..32767) */, + -1}; +asn_per_constraints_t asn_PER_type_RollRate_constr_1 CC_NOTUSED = { + { APC_CONSTRAINED, 16, 16, -32767, 32767 } /* (-32767..32767) */, + { APC_UNCONSTRAINED, -1, -1, 0, 0 }, + 0, 0 /* No PER value map */ +}; +static const ber_tlv_tag_t asn_DEF_RollRate_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (2 << 2)) +}; +asn_TYPE_descriptor_t asn_DEF_RollRate = { + "RollRate", + "RollRate", + &asn_OP_NativeInteger, + asn_DEF_RollRate_tags_1, + sizeof(asn_DEF_RollRate_tags_1) + /sizeof(asn_DEF_RollRate_tags_1[0]), /* 1 */ + asn_DEF_RollRate_tags_1, /* Same as above */ + sizeof(asn_DEF_RollRate_tags_1) + /sizeof(asn_DEF_RollRate_tags_1[0]), /* 1 */ + { &asn_OER_type_RollRate_constr_1, &asn_PER_type_RollRate_constr_1, RollRate_constraint }, + 0, 0, /* No members */ + 0 /* No specifics */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/RollRateConfidence.c b/src/tmx/Asn_J2735/src/r63/RollRateConfidence.c new file mode 100644 index 000000000..3b578a211 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/RollRateConfidence.c @@ -0,0 +1,68 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "RollRateConfidence.h" + +/* + * This type is implemented using NativeEnumerated, + * so here we adjust the DEF accordingly. + */ +static asn_oer_constraints_t asn_OER_type_RollRateConfidence_constr_1 CC_NOTUSED = { + { 0, 0 }, + -1}; +asn_per_constraints_t asn_PER_type_RollRateConfidence_constr_1 CC_NOTUSED = { + { APC_CONSTRAINED, 3, 3, 0, 7 } /* (0..7) */, + { APC_UNCONSTRAINED, -1, -1, 0, 0 }, + 0, 0 /* No PER value map */ +}; +static const asn_INTEGER_enum_map_t asn_MAP_RollRateConfidence_value2enum_1[] = { + { 0, 11, "unavailable" }, + { 1, 13, "degSec-100-00" }, + { 2, 13, "degSec-010-00" }, + { 3, 13, "degSec-005-00" }, + { 4, 13, "degSec-001-00" }, + { 5, 13, "degSec-000-10" }, + { 6, 13, "degSec-000-05" }, + { 7, 13, "degSec-000-01" } +}; +static const unsigned int asn_MAP_RollRateConfidence_enum2value_1[] = { + 7, /* degSec-000-01(7) */ + 6, /* degSec-000-05(6) */ + 5, /* degSec-000-10(5) */ + 4, /* degSec-001-00(4) */ + 3, /* degSec-005-00(3) */ + 2, /* degSec-010-00(2) */ + 1, /* degSec-100-00(1) */ + 0 /* unavailable(0) */ +}; +const asn_INTEGER_specifics_t asn_SPC_RollRateConfidence_specs_1 = { + asn_MAP_RollRateConfidence_value2enum_1, /* "tag" => N; sorted by tag */ + asn_MAP_RollRateConfidence_enum2value_1, /* N => "tag"; sorted by N */ + 8, /* Number of elements in the maps */ + 0, /* Enumeration is not extensible */ + 1, /* Strict enumeration */ + 0, /* Native long size */ + 0 +}; +static const ber_tlv_tag_t asn_DEF_RollRateConfidence_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (10 << 2)) +}; +asn_TYPE_descriptor_t asn_DEF_RollRateConfidence = { + "RollRateConfidence", + "RollRateConfidence", + &asn_OP_NativeEnumerated, + asn_DEF_RollRateConfidence_tags_1, + sizeof(asn_DEF_RollRateConfidence_tags_1) + /sizeof(asn_DEF_RollRateConfidence_tags_1[0]), /* 1 */ + asn_DEF_RollRateConfidence_tags_1, /* Same as above */ + sizeof(asn_DEF_RollRateConfidence_tags_1) + /sizeof(asn_DEF_RollRateConfidence_tags_1[0]), /* 1 */ + { &asn_OER_type_RollRateConfidence_constr_1, &asn_PER_type_RollRateConfidence_constr_1, NativeEnumerated_constraint }, + 0, 0, /* Defined elsewhere */ + &asn_SPC_RollRateConfidence_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/SensorDataSharingMessage.c b/src/tmx/Asn_J2735/src/r63/SensorDataSharingMessage.c new file mode 100644 index 000000000..d45d92e5a --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/SensorDataSharingMessage.c @@ -0,0 +1,122 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "SensorDataSharingMessage.h" + +asn_TYPE_member_t asn_MBR_SensorDataSharingMessage_1[] = { + { ATF_NOFLAGS, 0, offsetof(struct SensorDataSharingMessage, msgCnt), + (ASN_TAG_CLASS_CONTEXT | (0 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_DSRC_MsgCount, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "msgCnt" + }, + { ATF_NOFLAGS, 0, offsetof(struct SensorDataSharingMessage, sourceID), + (ASN_TAG_CLASS_CONTEXT | (1 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_TemporaryID, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "sourceID" + }, + { ATF_NOFLAGS, 0, offsetof(struct SensorDataSharingMessage, equipmentType), + (ASN_TAG_CLASS_CONTEXT | (2 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_EquipmentType, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "equipmentType" + }, + { ATF_NOFLAGS, 0, offsetof(struct SensorDataSharingMessage, sDSMTimeStamp), + (ASN_TAG_CLASS_CONTEXT | (3 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_DDateTime, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "sDSMTimeStamp" + }, + { ATF_NOFLAGS, 0, offsetof(struct SensorDataSharingMessage, refPos), + (ASN_TAG_CLASS_CONTEXT | (4 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_Position3D, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "refPos" + }, + { ATF_NOFLAGS, 0, offsetof(struct SensorDataSharingMessage, refPosXYConf), + (ASN_TAG_CLASS_CONTEXT | (5 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_PositionalAccuracy, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "refPosXYConf" + }, + { ATF_POINTER, 1, offsetof(struct SensorDataSharingMessage, refPosElConf), + (ASN_TAG_CLASS_CONTEXT | (6 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_ElevationConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "refPosElConf" + }, + { ATF_NOFLAGS, 0, offsetof(struct SensorDataSharingMessage, objects), + (ASN_TAG_CLASS_CONTEXT | (7 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_DetectedObjectList, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "objects" + }, +}; +static const int asn_MAP_SensorDataSharingMessage_oms_1[] = { 6 }; +static const ber_tlv_tag_t asn_DEF_SensorDataSharingMessage_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)) +}; +static const asn_TYPE_tag2member_t asn_MAP_SensorDataSharingMessage_tag2el_1[] = { + { (ASN_TAG_CLASS_CONTEXT | (0 << 2)), 0, 0, 0 }, /* msgCnt */ + { (ASN_TAG_CLASS_CONTEXT | (1 << 2)), 1, 0, 0 }, /* sourceID */ + { (ASN_TAG_CLASS_CONTEXT | (2 << 2)), 2, 0, 0 }, /* equipmentType */ + { (ASN_TAG_CLASS_CONTEXT | (3 << 2)), 3, 0, 0 }, /* sDSMTimeStamp */ + { (ASN_TAG_CLASS_CONTEXT | (4 << 2)), 4, 0, 0 }, /* refPos */ + { (ASN_TAG_CLASS_CONTEXT | (5 << 2)), 5, 0, 0 }, /* refPosXYConf */ + { (ASN_TAG_CLASS_CONTEXT | (6 << 2)), 6, 0, 0 }, /* refPosElConf */ + { (ASN_TAG_CLASS_CONTEXT | (7 << 2)), 7, 0, 0 } /* objects */ +}; +asn_SEQUENCE_specifics_t asn_SPC_SensorDataSharingMessage_specs_1 = { + sizeof(struct SensorDataSharingMessage), + offsetof(struct SensorDataSharingMessage, _asn_ctx), + asn_MAP_SensorDataSharingMessage_tag2el_1, + 8, /* Count of tags in the map */ + asn_MAP_SensorDataSharingMessage_oms_1, /* Optional members */ + 1, 0, /* Root/Additions */ + -1, /* First extension addition */ +}; +asn_TYPE_descriptor_t asn_DEF_SensorDataSharingMessage = { + "SensorDataSharingMessage", + "SensorDataSharingMessage", + &asn_OP_SEQUENCE, + asn_DEF_SensorDataSharingMessage_tags_1, + sizeof(asn_DEF_SensorDataSharingMessage_tags_1) + /sizeof(asn_DEF_SensorDataSharingMessage_tags_1[0]), /* 1 */ + asn_DEF_SensorDataSharingMessage_tags_1, /* Same as above */ + sizeof(asn_DEF_SensorDataSharingMessage_tags_1) + /sizeof(asn_DEF_SensorDataSharingMessage_tags_1[0]), /* 1 */ + { 0, 0, SEQUENCE_constraint }, + asn_MBR_SensorDataSharingMessage_1, + 8, /* Elements count */ + &asn_SPC_SensorDataSharingMessage_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/SizeValue.c b/src/tmx/Asn_J2735/src/r63/SizeValue.c new file mode 100644 index 000000000..19ec9d837 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/SizeValue.c @@ -0,0 +1,64 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "SizeValue.h" + +int +SizeValue_constraint(const asn_TYPE_descriptor_t *td, const void *sptr, + asn_app_constraint_failed_f *ctfailcb, void *app_key) { + long value; + + if(!sptr) { + ASN__CTFAIL(app_key, td, sptr, + "%s: value not given (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } + + value = *(const long *)sptr; + + if((value >= 0 && value <= 1023)) { + /* Constraint check succeeded */ + return 0; + } else { + ASN__CTFAIL(app_key, td, sptr, + "%s: constraint failed (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } +} + +/* + * This type is implemented using NativeInteger, + * so here we adjust the DEF accordingly. + */ +static asn_oer_constraints_t asn_OER_type_SizeValue_constr_1 CC_NOTUSED = { + { 2, 1 } /* (0..1023) */, + -1}; +asn_per_constraints_t asn_PER_type_SizeValue_constr_1 CC_NOTUSED = { + { APC_CONSTRAINED, 10, 10, 0, 1023 } /* (0..1023) */, + { APC_UNCONSTRAINED, -1, -1, 0, 0 }, + 0, 0 /* No PER value map */ +}; +static const ber_tlv_tag_t asn_DEF_SizeValue_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (2 << 2)) +}; +asn_TYPE_descriptor_t asn_DEF_SizeValue = { + "SizeValue", + "SizeValue", + &asn_OP_NativeInteger, + asn_DEF_SizeValue_tags_1, + sizeof(asn_DEF_SizeValue_tags_1) + /sizeof(asn_DEF_SizeValue_tags_1[0]), /* 1 */ + asn_DEF_SizeValue_tags_1, /* Same as above */ + sizeof(asn_DEF_SizeValue_tags_1) + /sizeof(asn_DEF_SizeValue_tags_1[0]), /* 1 */ + { &asn_OER_type_SizeValue_constr_1, &asn_PER_type_SizeValue_constr_1, SizeValue_constraint }, + 0, 0, /* No members */ + 0 /* No specifics */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/SizeValueConfidence.c b/src/tmx/Asn_J2735/src/r63/SizeValueConfidence.c new file mode 100644 index 000000000..ac70f8af5 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/SizeValueConfidence.c @@ -0,0 +1,80 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "SizeValueConfidence.h" + +/* + * This type is implemented using NativeEnumerated, + * so here we adjust the DEF accordingly. + */ +static asn_oer_constraints_t asn_OER_type_SizeValueConfidence_constr_1 CC_NOTUSED = { + { 0, 0 }, + -1}; +asn_per_constraints_t asn_PER_type_SizeValueConfidence_constr_1 CC_NOTUSED = { + { APC_CONSTRAINED, 4, 4, 0, 13 } /* (0..13) */, + { APC_UNCONSTRAINED, -1, -1, 0, 0 }, + 0, 0 /* No PER value map */ +}; +static const asn_INTEGER_enum_map_t asn_MAP_SizeValueConfidence_value2enum_1[] = { + { 0, 11, "unavailable" }, + { 1, 11, "size-100-00" }, + { 2, 11, "size-050-00" }, + { 3, 11, "size-020-00" }, + { 4, 11, "size-010-00" }, + { 5, 11, "size-005-00" }, + { 6, 11, "size-002-00" }, + { 7, 11, "size-001-00" }, + { 8, 11, "size-000-50" }, + { 9, 11, "size-000-20" }, + { 10, 11, "size-000-10" }, + { 11, 11, "size-000-05" }, + { 12, 11, "size-000-02" }, + { 13, 11, "size-000-01" } +}; +static const unsigned int asn_MAP_SizeValueConfidence_enum2value_1[] = { + 13, /* size-000-01(13) */ + 12, /* size-000-02(12) */ + 11, /* size-000-05(11) */ + 10, /* size-000-10(10) */ + 9, /* size-000-20(9) */ + 8, /* size-000-50(8) */ + 7, /* size-001-00(7) */ + 6, /* size-002-00(6) */ + 5, /* size-005-00(5) */ + 4, /* size-010-00(4) */ + 3, /* size-020-00(3) */ + 2, /* size-050-00(2) */ + 1, /* size-100-00(1) */ + 0 /* unavailable(0) */ +}; +const asn_INTEGER_specifics_t asn_SPC_SizeValueConfidence_specs_1 = { + asn_MAP_SizeValueConfidence_value2enum_1, /* "tag" => N; sorted by tag */ + asn_MAP_SizeValueConfidence_enum2value_1, /* N => "tag"; sorted by N */ + 14, /* Number of elements in the maps */ + 0, /* Enumeration is not extensible */ + 1, /* Strict enumeration */ + 0, /* Native long size */ + 0 +}; +static const ber_tlv_tag_t asn_DEF_SizeValueConfidence_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (10 << 2)) +}; +asn_TYPE_descriptor_t asn_DEF_SizeValueConfidence = { + "SizeValueConfidence", + "SizeValueConfidence", + &asn_OP_NativeEnumerated, + asn_DEF_SizeValueConfidence_tags_1, + sizeof(asn_DEF_SizeValueConfidence_tags_1) + /sizeof(asn_DEF_SizeValueConfidence_tags_1[0]), /* 1 */ + asn_DEF_SizeValueConfidence_tags_1, /* Same as above */ + sizeof(asn_DEF_SizeValueConfidence_tags_1) + /sizeof(asn_DEF_SizeValueConfidence_tags_1[0]), /* 1 */ + { &asn_OER_type_SizeValueConfidence_constr_1, &asn_PER_type_SizeValueConfidence_constr_1, NativeEnumerated_constraint }, + 0, 0, /* Defined elsewhere */ + &asn_SPC_SizeValueConfidence_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/VehicleSizeConfidence.c b/src/tmx/Asn_J2735/src/r63/VehicleSizeConfidence.c new file mode 100644 index 000000000..1e1b5b372 --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/VehicleSizeConfidence.c @@ -0,0 +1,72 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "VehicleSizeConfidence.h" + +asn_TYPE_member_t asn_MBR_VehicleSizeConfidence_1[] = { + { ATF_NOFLAGS, 0, offsetof(struct VehicleSizeConfidence, vehicleWidthConfidence), + (ASN_TAG_CLASS_CONTEXT | (0 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_SizeValueConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "vehicleWidthConfidence" + }, + { ATF_NOFLAGS, 0, offsetof(struct VehicleSizeConfidence, vehicleLengthConfidence), + (ASN_TAG_CLASS_CONTEXT | (1 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_SizeValueConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "vehicleLengthConfidence" + }, + { ATF_POINTER, 1, offsetof(struct VehicleSizeConfidence, vehicleHeightConfidence), + (ASN_TAG_CLASS_CONTEXT | (2 << 2)), + -1, /* IMPLICIT tag at current level */ + &asn_DEF_SizeValueConfidence, + 0, + { 0, 0, 0 }, + 0, 0, /* No default value */ + "vehicleHeightConfidence" + }, +}; +static const int asn_MAP_VehicleSizeConfidence_oms_1[] = { 2 }; +static const ber_tlv_tag_t asn_DEF_VehicleSizeConfidence_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (16 << 2)) +}; +static const asn_TYPE_tag2member_t asn_MAP_VehicleSizeConfidence_tag2el_1[] = { + { (ASN_TAG_CLASS_CONTEXT | (0 << 2)), 0, 0, 0 }, /* vehicleWidthConfidence */ + { (ASN_TAG_CLASS_CONTEXT | (1 << 2)), 1, 0, 0 }, /* vehicleLengthConfidence */ + { (ASN_TAG_CLASS_CONTEXT | (2 << 2)), 2, 0, 0 } /* vehicleHeightConfidence */ +}; +asn_SEQUENCE_specifics_t asn_SPC_VehicleSizeConfidence_specs_1 = { + sizeof(struct VehicleSizeConfidence), + offsetof(struct VehicleSizeConfidence, _asn_ctx), + asn_MAP_VehicleSizeConfidence_tag2el_1, + 3, /* Count of tags in the map */ + asn_MAP_VehicleSizeConfidence_oms_1, /* Optional members */ + 1, 0, /* Root/Additions */ + -1, /* First extension addition */ +}; +asn_TYPE_descriptor_t asn_DEF_VehicleSizeConfidence = { + "VehicleSizeConfidence", + "VehicleSizeConfidence", + &asn_OP_SEQUENCE, + asn_DEF_VehicleSizeConfidence_tags_1, + sizeof(asn_DEF_VehicleSizeConfidence_tags_1) + /sizeof(asn_DEF_VehicleSizeConfidence_tags_1[0]), /* 1 */ + asn_DEF_VehicleSizeConfidence_tags_1, /* Same as above */ + sizeof(asn_DEF_VehicleSizeConfidence_tags_1) + /sizeof(asn_DEF_VehicleSizeConfidence_tags_1[0]), /* 1 */ + { 0, 0, SEQUENCE_constraint }, + asn_MBR_VehicleSizeConfidence_1, + 3, /* Elements count */ + &asn_SPC_VehicleSizeConfidence_specs_1 /* Additional specs */ +}; + diff --git a/src/tmx/Asn_J2735/src/r63/YawDetected.c b/src/tmx/Asn_J2735/src/r63/YawDetected.c new file mode 100644 index 000000000..bc7c0018b --- /dev/null +++ b/src/tmx/Asn_J2735/src/r63/YawDetected.c @@ -0,0 +1,64 @@ +/* + * Generated by asn1c-0.9.29 (http://lionet.info/asn1c) + * From ASN.1 module "SDSM" + * found in "J2735_201603_2023-06-22.asn" + * `asn1c -fcompound-names ` + */ + +#include "YawDetected.h" + +int +YawDetected_constraint(const asn_TYPE_descriptor_t *td, const void *sptr, + asn_app_constraint_failed_f *ctfailcb, void *app_key) { + long value; + + if(!sptr) { + ASN__CTFAIL(app_key, td, sptr, + "%s: value not given (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } + + value = *(const long *)sptr; + + if((value >= -14400 && value <= 14400)) { + /* Constraint check succeeded */ + return 0; + } else { + ASN__CTFAIL(app_key, td, sptr, + "%s: constraint failed (%s:%d)", + td->name, __FILE__, __LINE__); + return -1; + } +} + +/* + * This type is implemented using NativeInteger, + * so here we adjust the DEF accordingly. + */ +static asn_oer_constraints_t asn_OER_type_YawDetected_constr_1 CC_NOTUSED = { + { 2, 0 } /* (-14400..14400) */, + -1}; +asn_per_constraints_t asn_PER_type_YawDetected_constr_1 CC_NOTUSED = { + { APC_CONSTRAINED, 15, 15, -14400, 14400 } /* (-14400..14400) */, + { APC_UNCONSTRAINED, -1, -1, 0, 0 }, + 0, 0 /* No PER value map */ +}; +static const ber_tlv_tag_t asn_DEF_YawDetected_tags_1[] = { + (ASN_TAG_CLASS_UNIVERSAL | (2 << 2)) +}; +asn_TYPE_descriptor_t asn_DEF_YawDetected = { + "YawDetected", + "YawDetected", + &asn_OP_NativeInteger, + asn_DEF_YawDetected_tags_1, + sizeof(asn_DEF_YawDetected_tags_1) + /sizeof(asn_DEF_YawDetected_tags_1[0]), /* 1 */ + asn_DEF_YawDetected_tags_1, /* Same as above */ + sizeof(asn_DEF_YawDetected_tags_1) + /sizeof(asn_DEF_YawDetected_tags_1[0]), /* 1 */ + { &asn_OER_type_YawDetected_constr_1, &asn_PER_type_YawDetected_constr_1, YawDetected_constraint }, + 0, 0, /* No members */ + 0 /* No specifics */ +}; + diff --git a/src/tmx/TmxApi/tmx/TmxApiMessages.h b/src/tmx/TmxApi/tmx/TmxApiMessages.h index b680329ff..f44406061 100644 --- a/src/tmx/TmxApi/tmx/TmxApiMessages.h +++ b/src/tmx/TmxApi/tmx/TmxApiMessages.h @@ -88,6 +88,7 @@ enum MsgSubType signalStatusMessage = 30, travelerInformation = 31, personalSafetyMessage = 32, + sensorDataSharingMessage = 41, personalMobilityMessage = 245, testMessage00 = 240, testMessage01 = 241, @@ -143,6 +144,7 @@ static CONSTEXPR const char *MSGSUBTYPE_TESTMESSAGE02_STRING = "TMSG02-P"; static CONSTEXPR const char *MSGSUBTYPE_TESTMESSAGE03_STRING = "TMSG03-P"; static CONSTEXPR const char *MSGSUBTYPE_J2735_END_STRING = "J2735_end"; static CONSTEXPR const char *MSGSUBTYPE_GID_STRING = "GID"; +static CONSTEXPR const char *MSGSUBTYPE_SENSORDATASHARINGMESSAGE_STRING = "SDSM"; enum Encoding { @@ -194,6 +196,7 @@ enum msgPSID rtcmCorrections_PSID = 0x8000, signalRequestMessage_PSID = 0xE0000016, signalStatusMessage_PSID = 0x8002, + sensorDataSharingMessage_PSID = 0x8002, travelerInformation_PSID = 0x8003, personalSafetyMessage_PSID = 0x27, testMessage00_PSID = 0xBFEE, @@ -218,6 +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_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/tmx/TmxApi/tmx/j2735_messages/J2735MessageFactory.hpp b/src/tmx/TmxApi/tmx/j2735_messages/J2735MessageFactory.hpp index 88609ec81..1fa6eadf8 100644 --- a/src/tmx/TmxApi/tmx/j2735_messages/J2735MessageFactory.hpp +++ b/src/tmx/TmxApi/tmx/j2735_messages/J2735MessageFactory.hpp @@ -43,6 +43,7 @@ #include #include #include +#include namespace tmx { namespace messages { @@ -78,7 +79,8 @@ using message_types = message_type_list< tsm0Message, tsm1Message, tsm2Message, - tsm3Message + tsm3Message, + SdsmMessage >; /// Base allocator type @@ -203,6 +205,7 @@ class J2735MessageFactory add_allocator_to_maps(); add_allocator_to_maps(); add_allocator_to_maps(); + add_allocator_to_maps(); #if SAEJ2735_SPEC < 63 add_allocator_to_maps(); #endif diff --git a/src/tmx/TmxApi/tmx/j2735_messages/SensorDataSharingMessage.hpp b/src/tmx/TmxApi/tmx/j2735_messages/SensorDataSharingMessage.hpp new file mode 100644 index 000000000..66b910e51 --- /dev/null +++ b/src/tmx/TmxApi/tmx/j2735_messages/SensorDataSharingMessage.hpp @@ -0,0 +1,15 @@ +/* + * @file SensorDataSharingMessage.hpp + * + * Created on: August 4, 2023 + */ +#ifndef TMX_J2735_MESSAGES_SENSORDATASHARINGMESSAGE_HPP_ +#define TMX_J2735_MESSAGES_SENSORDATASHARINGMESSAGE_HPP_ + +#include +#include +#include + +TMX_J2735_DECLARE(Sdsm, SensorDataSharingMessage, api::sensorDataSharingMessage, api::MSGSUBTYPE_SENSORDATASHARINGMESSAGE_STRING) + +#endif /* TMX_J2735_MESSAGES_SENSORDATASHARINGMESSAGE_HPP_ */ diff --git a/src/tmx/TmxUtils/test/J2735MessageTest.cpp b/src/tmx/TmxUtils/test/J2735MessageTest.cpp index 5a72f9458..c885ecdd9 100644 --- a/src/tmx/TmxUtils/test/J2735MessageTest.cpp +++ b/src/tmx/TmxUtils/test/J2735MessageTest.cpp @@ -748,4 +748,61 @@ TEST_F(J2735MessageTest, EncodeTravelerInformation){ string expectedHex = "001f526011c35d000000000023667bac0407299b9ef9e7a9b9408230dfffe4386ba00078005a53373df3cf5372810461b90ffff53373df3cf53728104618129800010704a04c7d7976ca3501872e1bb66ad19b2620"; ASSERT_EQ(expectedHex, timEnc.get_payload_str()); } + +TEST_F(J2735MessageTest, EncodeSDSM) +{ + auto message = (SensorDataSharingMessage_t*)calloc(1, sizeof(SensorDataSharingMessage_t)); + message->msgCnt = 10; + uint8_t my_bytes_id[4] = {(uint8_t)1, (uint8_t)12, (uint8_t)12, (uint8_t)10}; + message->sourceID.buf = my_bytes_id; + message->sourceID.size = sizeof(my_bytes_id); + message->equipmentType = EquipmentType_unknown; + + + auto sDSMTimeStamp = (DDateTime_t*) calloc(1, sizeof(DDateTime_t)); + auto year = (DYear_t*) calloc(1, sizeof(DYear_t)); + *year= 2023; + sDSMTimeStamp->year = year; + message->sDSMTimeStamp = *sDSMTimeStamp; + + message->refPos.lat = 38.121212; + message->refPos.Long = -77.121212; + + message->refPosXYConf.orientation = 10; + message->refPosXYConf.semiMajor = 12; + message->refPosXYConf.semiMinor = 52; + + auto objects = (DetectedObjectList_t*) calloc(1, sizeof(DetectedObjectList_t)); + auto objectData = (DetectedObjectData_t*) calloc(1, sizeof(DetectedObjectData_t)); + objectData->detObjCommon.objType = ObjectType_unknown; + objectData->detObjCommon.objTypeCfd = 1; + objectData->detObjCommon.measurementTime = 1; + objectData->detObjCommon.timeConfidence = 1; + objectData->detObjCommon.pos.offsetX = 1; + objectData->detObjCommon.pos.offsetY = 1; + objectData->detObjCommon.posConfidence.elevation = 1; + objectData->detObjCommon.posConfidence.pos = 1; + objectData->detObjCommon.speed = 1; + objectData->detObjCommon.speedConfidence = 1; + objectData->detObjCommon.heading = 1; + objectData->detObjCommon.headingConf = 1; + ASN_SEQUENCE_ADD(&objects->list.array, objectData); + message->objects = *objects; + xer_fprint(stdout, &asn_DEF_SensorDataSharingMessage, message); + + //Encode SDSM + tmx::messages::SdsmEncodedMessage SdsmEncodeMessage; + auto _sdsmMessage = new tmx::messages::SdsmMessage(message); + tmx::messages::MessageFrameMessage frame_msg(_sdsmMessage->get_j2735_data()); + SdsmEncodeMessage.set_data(TmxJ2735EncodedMessage::encode_j2735_message>(frame_msg)); + free(message); + free(frame_msg.get_j2735_data().get()); + ASSERT_EQ(41, SdsmEncodeMessage.get_msgId()); + std::string expectedSDSMEncHex = "0029250a010c0c0a101f9c35a4e9266b49d1b20c34000a00000020000bba0a000200004400240009"; + ASSERT_EQ(expectedSDSMEncHex, SdsmEncodeMessage.get_payload_str()); + + //Decode SDSM + auto sdsm_ptr = SdsmEncodeMessage.decode_j2735_message().get_j2735_data(); + ASSERT_EQ(10, sdsm_ptr->msgCnt); +} } From e1b7cf8b0a01f200094932a978ac9ceb5fffa81b Mon Sep 17 00:00:00 2001 From: paulbourelly999 <77466294+paulbourelly999@users.noreply.github.com> Date: Wed, 16 Aug 2023 13:29:21 -0400 Subject: [PATCH 12/28] Fixed spelling of SemanticLidar sensors in test config file (#556) # PR Details ## Description Corrected sensor type spelling in sensor config file ## Related Issue ## Motivation and Context Fix sensor config file ## How Has This Been Tested? CDASim Infrastructure Registration Integration testing in XIL-Cloud environment. ## 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. - [ ] 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. - [x] All new and existing tests passed. --- .../CDASimAdapter/test/TestCARMASimulationConnection.cpp | 2 +- src/v2i-hub/CDASimAdapter/test/sensors.json | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp index 2149ce8b4..2e992933e 100644 --- a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp @@ -92,7 +92,7 @@ namespace CDASimAdapter { if(in_strm.is_open()) { ASSERT_EQ(connection->get_handshake_json("4566", "127.0.0.1", 4567, 4568, 4569, location), - "{\n \"infrastructureId\" : \"4566\",\n \"location\" : {\n \"x\" : 1000.0,\n \"y\" : 38.954999999999998,\n \"z\" : -77.149000000000001\n },\n \"rxMessageIpAddress\" : \"127.0.0.1\",\n \"rxMessagePort\" : 4569,\n \"sensors\" : [\n {\n \"location\" : {\n \"x\" : 0.0,\n \"y\" : 0.0,\n \"z\" : 0.0\n },\n \"orientation\" : {\n \"pitch\" : 0.0,\n \"roll\" : 0.0,\n \"yaw\" : 0.0\n },\n \"sensorId\" : \"SomeID\",\n \"type\" : \"SematicLidar\"\n },\n {\n \"location\" : {\n \"x\" : 1.0,\n \"y\" : 2.0,\n \"z\" : 0.0\n },\n \"orientation\" : {\n \"pitch\" : 0.0,\n \"roll\" : 0.0,\n \"yaw\" : 23.0\n },\n \"sensorId\" : \"SomeID2\",\n \"type\" : \"SematicLidar\"\n }\n ],\n \"simulatedInteractionPort\" : 4568,\n \"timeSyncPort\" : 4567\n}\n"); + "{\n \"infrastructureId\" : \"4566\",\n \"location\" : {\n \"x\" : 1000.0,\n \"y\" : 38.954999999999998,\n \"z\" : -77.149000000000001\n },\n \"rxMessageIpAddress\" : \"127.0.0.1\",\n \"rxMessagePort\" : 4569,\n \"sensors\" : [\n {\n \"location\" : {\n \"x\" : 0.0,\n \"y\" : 0.0,\n \"z\" : 0.0\n },\n \"orientation\" : {\n \"pitch\" : 0.0,\n \"roll\" : 0.0,\n \"yaw\" : 0.0\n },\n \"sensorId\" : \"SomeID\",\n \"type\" : \"SemanticLidar\"\n },\n {\n \"location\" : {\n \"x\" : 1.0,\n \"y\" : 2.0,\n \"z\" : 0.0\n },\n \"orientation\" : {\n \"pitch\" : 0.0,\n \"roll\" : 0.0,\n \"yaw\" : 23.0\n },\n \"sensorId\" : \"SomeID2\",\n \"type\" : \"SemanticLidar\"\n }\n ],\n \"simulatedInteractionPort\" : 4568,\n \"timeSyncPort\" : 4567\n}\n"); } } diff --git a/src/v2i-hub/CDASimAdapter/test/sensors.json b/src/v2i-hub/CDASimAdapter/test/sensors.json index 2979e5a21..985496f8d 100755 --- a/src/v2i-hub/CDASimAdapter/test/sensors.json +++ b/src/v2i-hub/CDASimAdapter/test/sensors.json @@ -1,7 +1,7 @@ [ { "sensorId": "SomeID", - "type": "SematicLidar", + "type": "SemanticLidar", "location": { "x": 0.0, "y": 0.0, @@ -15,7 +15,7 @@ }, { "sensorId": "SomeID2", - "type": "SematicLidar", + "type": "SemanticLidar", "location": { "x": 1.0, "y": 2.0, From aae33e6a16b8a30e1faee31a7ee863d191be06b8 Mon Sep 17 00:00:00 2001 From: paulbourelly999 <77466294+paulbourelly999@users.noreply.github.com> Date: Thu, 24 Aug 2023 10:58:21 -0400 Subject: [PATCH 13/28] Fix CARMAStreets Plugin undefined behavior for Kafka Producer (#559) # PR Details ## Description Changes : - Added return statement for init method of kafka producer to avoid undefine behavior - Replaced error logs and null returns with throwing exceptions in Kafka consumer producer initialization in carma-streets plugin - Added stop command for devcontainer setup - Removed consumer group ID configuration, using plugin name instead ## Related Issue #558 ## Motivation and Context ## How Has This Been Tested? ## 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. - [x] All new and existing tests passed. --- .devcontainer/devcontainer.json | 4 ++- .../src/kafka/kafka_producer_worker.cpp | 1 + src/v2i-hub/CARMAStreetsPlugin/manifest.json | 15 ----------- .../src/CARMAStreetsPlugin.cpp | 27 +++++++++---------- 4 files changed, 17 insertions(+), 30 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 09591f645..b306616f5 100755 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -35,7 +35,9 @@ // "forwardPorts": [], // Uncomment the next line to run commands after the container is created - for example installing curl. - "postCreateCommand": "container/database.sh && container/library.sh && container/setup.sh && ldconfig" + "postCreateCommand": "container/database.sh && container/library.sh && container/setup.sh && ldconfig", + + "shutdownAction": "stopCompose" // Uncomment to use the Docker CLI from inside the container. See https://aka.ms/vscode-remote/samples/docker-from-docker. // "mounts": [ diff --git a/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.cpp b/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.cpp index 02d0a0cbb..ae074e4aa 100644 --- a/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.cpp +++ b/src/tmx/TmxUtils/src/kafka/kafka_producer_worker.cpp @@ -93,6 +93,7 @@ namespace tmx::utils delete conf; FILE_LOG(logINFO) << "Created producer: " << _producer->name() << std::endl; + return true; } bool kafka_producer_worker::init_topic() diff --git a/src/v2i-hub/CARMAStreetsPlugin/manifest.json b/src/v2i-hub/CARMAStreetsPlugin/manifest.json index f0daaf9a9..5faa1f8e2 100644 --- a/src/v2i-hub/CARMAStreetsPlugin/manifest.json +++ b/src/v2i-hub/CARMAStreetsPlugin/manifest.json @@ -97,11 +97,6 @@ "default": "v2xhub_scheduling_plan_sub", "description": "Apache Kafka topic plugin will transmit message to." }, - { - "key": "SchedulingPlanConsumerGroupId", - "default": "v2xhub_scheduling_plan", - "description": "Apache Kafka consumer group ID for scheduling plan consumer." - }, { "key": "SpatTopic", "default": "modified_spat", @@ -112,16 +107,6 @@ "default": "v2xhub_ssm_sub", "description": "Apache Kafka topic plugin will transmit message to." }, - { - "key": "SsmConsumerGroupId", - "default": "v2xhub_ssm", - "description": "Apache Kafka consumer group ID for spat consumer." - }, - { - "key": "SpatConsumerGroupId", - "default": "v2xhub_spat", - "description": "Apache Kafka consumer group ID for spat consumer." - }, { "key": "SimSensorDetectedObjTopic", "default": "v2xhub_sim_sensor_detected_object", diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index c5bb9a7e6..ed04c2112 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -41,11 +41,8 @@ void CARMAStreetsPlugin::UpdateConfigSettings() { GetConfigValue("KafkaBrokerPort", _kafkaBrokerPort); // GetConfigValue("SchedulingPlanTopic", _subscribeToSchedulingPlanTopic); - GetConfigValue("SchedulingPlanConsumerGroupId", _subscribeToSchedulingPlanConsumerGroupId); GetConfigValue("SpatTopic", _subscribeToSpatTopic); GetConfigValue("SsmTopic", _subscribeToSsmTopic); - GetConfigValue("SpatConsumerGroupId", _subscribeToSpatConsumerGroupId); - GetConfigValue("SsmConsumerGroupId", _subscribeToSSMConsumerGroupId); GetConfigValue("BsmTopic", _transmitBSMTopic); GetConfigValue("MobilityOperationTopic", _transmitMobilityOperationTopic); GetConfigValue("MobilityPathTopic", _transmitMobilityPathTopic); @@ -76,28 +73,28 @@ void CARMAStreetsPlugin::InitKafkaConsumerProducers() _kafka_producer_ptr = client.create_producer(kafkaConnectString); if(!_kafka_producer_ptr->init_producer()) { - return; + throw TmxException("Failed to create Kafka producer."); + } //Consumers - _spat_kafka_consumer_ptr = client.create_consumer(kafkaConnectString, _subscribeToSpatTopic,_subscribeToSpatConsumerGroupId); - _scheduing_plan_kafka_consumer_ptr = client.create_consumer(kafkaConnectString, _subscribeToSchedulingPlanTopic,_subscribeToSchedulingPlanConsumerGroupId); - _ssm_kafka_consumer_ptr = client.create_consumer(kafkaConnectString, _subscribeToSsmTopic,_subscribeToSSMConsumerGroupId); + _spat_kafka_consumer_ptr = client.create_consumer(kafkaConnectString, _subscribeToSpatTopic,this->_name); + _scheduing_plan_kafka_consumer_ptr = client.create_consumer(kafkaConnectString, _subscribeToSchedulingPlanTopic, this->_name); + _ssm_kafka_consumer_ptr = client.create_consumer(kafkaConnectString, _subscribeToSsmTopic,this->_name); if(!_scheduing_plan_kafka_consumer_ptr || !_spat_kafka_consumer_ptr || !_ssm_kafka_consumer_ptr) { - PLOG(logERROR) <<"Failed to create Kafka consumers."; - return; + throw TmxException("Failed to create Kafka consumers."); } PLOG(logDEBUG) <<"Kafka consumers created"; if(!_spat_kafka_consumer_ptr->init() || !_scheduing_plan_kafka_consumer_ptr->init() || !_ssm_kafka_consumer_ptr->init()) { - PLOG(logERROR) <<"Kafka consumers init() failed!"; - return; + throw TmxException("Kafka consumers init() failed!"); } - + // TODO: Replace with tmxutil ThreadTimer or some other more appropriate Thread wrapper. boost::thread thread_schpl(&CARMAStreetsPlugin::SubscribeSchedulingPlanKafkaTopic, this); boost::thread thread_spat(&CARMAStreetsPlugin::SubscribeSpatKafkaTopic, this); - boost::thread thread_ssm(&CARMAStreetsPlugin::SubscribeSSMKafkaTopic, this); + boost::thread thread_ssm(&CARMAStreetsPlugin::SubscribeSSMKafkaTopic, this); + } void CARMAStreetsPlugin::OnConfigChanged(const char *key, const char *value) { @@ -468,6 +465,7 @@ void CARMAStreetsPlugin::OnStateChange(IvpPluginState state) { void CARMAStreetsPlugin::SubscribeSchedulingPlanKafkaTopic() { + // TODO: Update methods to represent consuming a single message from Kafka topic if(_subscribeToSchedulingPlanTopic.length() > 0) { PLOG(logDEBUG) << "SubscribeSchedulingPlanKafkaTopics:" <<_subscribeToSchedulingPlanTopic << std::endl; @@ -525,6 +523,7 @@ void CARMAStreetsPlugin::SubscribeSchedulingPlanKafkaTopic() } void CARMAStreetsPlugin::SubscribeSpatKafkaTopic(){ + // TODO: Update methods to represent consuming a single message from Kafka topic if(_subscribeToSpatTopic.length() > 0) { PLOG(logDEBUG) << "SubscribeSpatKafkaTopics:" <<_subscribeToSpatTopic << std::endl; @@ -578,7 +577,7 @@ void CARMAStreetsPlugin::SubscribeSpatKafkaTopic(){ } void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){ - + // TODO: Update methods to represent consuming a single message from Kafka topic if(_subscribeToSsmTopic.length() > 0) { PLOG(logDEBUG) << "SubscribeSSMKafkaTopics:" <<_subscribeToSsmTopic << std::endl; From 2079d6416bfc8f9b1d844921721237b0f08e1800 Mon Sep 17 00:00:00 2001 From: Peyton Johnson <35665039+willjohnsonk@users.noreply.github.com> Date: Mon, 28 Aug 2023 09:33:22 -0400 Subject: [PATCH 14/28] Added conversion between ASN.1 and JSON for SDSM (#560) # PR Details ## Description Support added for converting inbound ASN.1 SDSMs to a more usable json format for data manipulation and passing. May be deployed for data fusion on infrastructure in the future. ## Related Issue NA ## Motivation and Context CDA Research ## How Has This Been Tested? V2X-hub builds, added unit tests pass for test objects ## Types of changes - [ ] Defect fix (non-breaking change that fixes an issue) - [x] 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/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt | 2 +- src/v2i-hub/CARMAStreetsPlugin/manifest.json | 5 + .../src/J3224ToSDSMJsonConverter.cpp | 177 +++++++ .../src/J3224ToSDSMJsonConverter.h | 43 ++ .../test/test_J3224ToSDSMJsonConverter.cpp | 435 ++++++++++++++++++ 5 files changed, 661 insertions(+), 1 deletion(-) create mode 100644 src/v2i-hub/CARMAStreetsPlugin/src/J3224ToSDSMJsonConverter.cpp create mode 100644 src/v2i-hub/CARMAStreetsPlugin/src/J3224ToSDSMJsonConverter.h create mode 100644 src/v2i-hub/CARMAStreetsPlugin/test/test_J3224ToSDSMJsonConverter.cpp diff --git a/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt b/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt index 53c8ca0ef..f8c11c896 100644 --- a/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt +++ b/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt @@ -9,7 +9,7 @@ TARGET_LINK_LIBRARIES (${PROJECT_NAME} tmxutils rdkafka++ jsoncpp) ############# enable_testing() include_directories(${PROJECT_SOURCE_DIR}/src) -add_library(${PROJECT_NAME}_lib src/J2735MapToJsonConverter.cpp src/JsonToJ2735SSMConverter.cpp src/JsonToJ2735SpatConverter.cpp src/J2735ToSRMJsonConverter.cpp) +add_library(${PROJECT_NAME}_lib src/J2735MapToJsonConverter.cpp src/JsonToJ2735SSMConverter.cpp src/JsonToJ2735SpatConverter.cpp src/J2735ToSRMJsonConverter.cpp src/J3224ToSDSMJsonConverter.cpp) target_link_libraries(${PROJECT_NAME}_lib PUBLIC ${TMXAPI_LIBRARIES} ${ASN_J2735_LIBRARIES} ${MYSQL_LIBRARIES} diff --git a/src/v2i-hub/CARMAStreetsPlugin/manifest.json b/src/v2i-hub/CARMAStreetsPlugin/manifest.json index 5faa1f8e2..22fb4a912 100644 --- a/src/v2i-hub/CARMAStreetsPlugin/manifest.json +++ b/src/v2i-hub/CARMAStreetsPlugin/manifest.json @@ -39,6 +39,11 @@ "type":"Application", "subtype":"SensorDetectedObject", "description": "Sensor detected object for cooperative perception." + }, + { + "type":"J2735", + "subtype": "SensorDataSharingMessage", + "description": "Message to share detected object data." } ], "configuration": [ diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/J3224ToSDSMJsonConverter.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/J3224ToSDSMJsonConverter.cpp new file mode 100644 index 000000000..33fd68e2c --- /dev/null +++ b/src/v2i-hub/CARMAStreetsPlugin/src/J3224ToSDSMJsonConverter.cpp @@ -0,0 +1,177 @@ +#include "J3224ToSDSMJsonConverter.h" + +namespace CARMAStreetsPlugin +{ + + void J3224ToSDSMJsonConverter::convertJ3224ToSDSMJSON(const std::shared_ptr sdsmMsgPtr, Json::Value &SDSMDataJson) const + { + // Define shared pointers for optional data + std::vector> shared_ptrs; + + // SDSM header data + SDSMDataJson["msg_cnt"] = sdsmMsgPtr->msgCnt; + + auto id_len = sdsmMsgPtr->sourceID.size; + unsigned long id_num = 0; + for(auto i = 0; i < id_len; i++) + { + id_num = (id_num << 8) | sdsmMsgPtr->sourceID.buf[i]; + } + std::stringstream id_fill_ss; + id_fill_ss << std::setfill('0') << std::setw(8) <equipmentType; + + Json::Value sDSMTimeStamp; + sDSMTimeStamp["year"] = *sdsmMsgPtr->sDSMTimeStamp.year; + sDSMTimeStamp["month"] = *sdsmMsgPtr->sDSMTimeStamp.month; + sDSMTimeStamp["day"] = *sdsmMsgPtr->sDSMTimeStamp.day; + sDSMTimeStamp["hour"] = *sdsmMsgPtr->sDSMTimeStamp.hour; + sDSMTimeStamp["minute"] = *sdsmMsgPtr->sDSMTimeStamp.minute; + sDSMTimeStamp["second"] = *sdsmMsgPtr->sDSMTimeStamp.second; + sDSMTimeStamp["offset"] = *sdsmMsgPtr->sDSMTimeStamp.offset; + SDSMDataJson["sdsm_time_stamp"] = sDSMTimeStamp; + + Json::Value refPos; + refPos["lat"] = sdsmMsgPtr->refPos.lat; + refPos["Long"] = sdsmMsgPtr->refPos.Long; + refPos["elevation"] = *sdsmMsgPtr->refPos.elevation; + SDSMDataJson["ref_pos"] = refPos; + + Json::Value refPosXYConf; + refPosXYConf["semi_major"] = sdsmMsgPtr->refPosXYConf.semiMajor; + refPosXYConf["semi_minor"] = sdsmMsgPtr->refPosXYConf.semiMinor; + refPosXYConf["orientation"] = sdsmMsgPtr->refPosXYConf.orientation; + SDSMDataJson["ref_pos_xy_conf"] = refPosXYConf; + + SDSMDataJson["ref_pos_el_conf"] = *sdsmMsgPtr->refPosElConf; + + + // Create list to append detected objects to + const DetectedObjectList det_obj_list = sdsmMsgPtr->objects; + + if(det_obj_list.list.array != nullptr){ + + Json::Value objectsJson; + + for(size_t i = 0; i < det_obj_list.list.count; i++){ + + Json::Value detectedObjectJson; + auto det_object = det_obj_list.list.array[i]; + + // Detected object common data + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["object_type"] = det_object->detObjCommon.objType; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["object_type_conf"] = det_object->detObjCommon.objTypeCfd; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["object_id"] = det_object->detObjCommon.objectID; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["measurement_time"] = det_object->detObjCommon.measurementTime; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["time_confidence"] = det_object->detObjCommon.timeConfidence; + + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["pos"]["offset_x"] = det_object->detObjCommon.pos.offsetX; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["pos"]["offset_y"] = det_object->detObjCommon.pos.offsetY; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["pos"]["offset_z"] = *det_object->detObjCommon.pos.offsetZ; + + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["pos_confidence"]["pos"] = det_object->detObjCommon.posConfidence.pos; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["pos_confidence"]["elevation"] = det_object->detObjCommon.posConfidence.elevation; + + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["speed"] = det_object->detObjCommon.speed; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["speed_confidence"] = det_object->detObjCommon.speedConfidence; + + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["heading"] = det_object->detObjCommon.heading; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["heading_conf"] = det_object->detObjCommon.headingConf; + + // Optional common data + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["speed_z"] = *det_object->detObjCommon.speedZ; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["speed_confidence_z"] = *det_object->detObjCommon.speedConfidenceZ; + + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["long"] = det_object->detObjCommon.accel4way->Long; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["lat"] = det_object->detObjCommon.accel4way->lat; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["vert"] = det_object->detObjCommon.accel4way->vert; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["yaw"] = det_object->detObjCommon.accel4way->yaw; + + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["acc_cfd_x"] = *det_object->detObjCommon.accCfdX; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["acc_cfd_y"] = *det_object->detObjCommon.accCfdY; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["acc_cfd_z"] = *det_object->detObjCommon.accCfdZ; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["acc_cfd_yaw"] = *det_object->detObjCommon.accCfdYaw; + + // Detected object optional data for vehicles, VRUs, obstacles + if(det_object->detObjOptData != nullptr){ + Json::Value optionalDataJson; + + switch(det_object->detObjOptData->present) + { + case DetectedObjectOptionalData_PR_NOTHING: + break; + + case DetectedObjectOptionalData_PR_detVeh: + + // TODO: find a better way to convert bit string data (JsonTo2735SpatConverter) + optionalDataJson["detected_vehicle_data"]["lights"] = std::to_string(*det_object->detObjOptData->choice.detVeh.lights->buf); + optionalDataJson["detected_vehicle_data"]["veh_attitude"]["pitch"] = det_object->detObjOptData->choice.detVeh.vehAttitude->pitch; + optionalDataJson["detected_vehicle_data"]["veh_attitude"]["roll"] = det_object->detObjOptData->choice.detVeh.vehAttitude->roll; + optionalDataJson["detected_vehicle_data"]["veh_attitude"]["yaw"] = det_object->detObjOptData->choice.detVeh.vehAttitude->yaw; + + optionalDataJson["detected_vehicle_data"]["veh_attitude_confidence"]["pitch_confidence"] = det_object->detObjOptData->choice.detVeh.vehAttitudeConfidence->pitchConfidence; + optionalDataJson["detected_vehicle_data"]["veh_attitude_confidence"]["roll_confidence"] = det_object->detObjOptData->choice.detVeh.vehAttitudeConfidence->rollConfidence; + optionalDataJson["detected_vehicle_data"]["veh_attitude_confidence"]["yaw_confidence"] = det_object->detObjOptData->choice.detVeh.vehAttitudeConfidence->yawConfidence; + + optionalDataJson["detected_vehicle_data"]["veh_ang_vel"]["pitch_rate"] = det_object->detObjOptData->choice.detVeh.vehAngVel->pitchRate; + optionalDataJson["detected_vehicle_data"]["veh_ang_vel"]["roll_rate"] = det_object->detObjOptData->choice.detVeh.vehAngVel->rollRate; + + optionalDataJson["detected_vehicle_data"]["veh_ang_vel_confidence"]["pitch_rate_confidence"] = *det_object->detObjOptData->choice.detVeh.vehAngVelConfidence->pitchRateConfidence; + optionalDataJson["detected_vehicle_data"]["veh_ang_vel_confidence"]["roll_rate_confidence"] = *det_object->detObjOptData->choice.detVeh.vehAngVelConfidence->rollRateConfidence; + + optionalDataJson["detected_vehicle_data"]["size"]["width"] = det_object->detObjOptData->choice.detVeh.size->width; + optionalDataJson["detected_vehicle_data"]["size"]["length"] = det_object->detObjOptData->choice.detVeh.size->length; + optionalDataJson["detected_vehicle_data"]["height"] = *det_object->detObjOptData->choice.detVeh.height; + + optionalDataJson["detected_vehicle_data"]["vehicle_size_confidence"]["vehicle_width_confidence"] = det_object->detObjOptData->choice.detVeh.vehicleSizeConfidence->vehicleWidthConfidence; + optionalDataJson["detected_vehicle_data"]["vehicle_size_confidence"]["vehicle_length_confidence"] = det_object->detObjOptData->choice.detVeh.vehicleSizeConfidence->vehicleLengthConfidence; + optionalDataJson["detected_vehicle_data"]["vehicle_size_confidence"]["vehicle_height_confidence"] = *det_object->detObjOptData->choice.detVeh.vehicleSizeConfidence->vehicleHeightConfidence; + + optionalDataJson["detected_vehicle_data"]["vehicle_class"] = *det_object->detObjOptData->choice.detVeh.vehicleClass; + optionalDataJson["detected_vehicle_data"]["vehicle_class_conf"] = *det_object->detObjOptData->choice.detVeh.classConf; + break; + + case DetectedObjectOptionalData_PR_detVRU: + optionalDataJson["detected_vru_data"]["basic_type"] = *det_object->detObjOptData->choice.detVRU.basicType; + + switch(det_object->detObjOptData->choice.detVRU.propulsion->present) + { + case PropelledInformation_PR_NOTHING: + break; + case PropelledInformation_PR_human: + optionalDataJson["detected_vru_data"]["propulsion"]["human"] = det_object->detObjOptData->choice.detVRU.propulsion->choice.human; + break; + case PropelledInformation_PR_animal: + optionalDataJson["detected_vru_data"]["propulsion"]["human"] = det_object->detObjOptData->choice.detVRU.propulsion->choice.animal; + break; + case PropelledInformation_PR_motor: + optionalDataJson["detected_vru_data"]["propulsion"]["human"] = det_object->detObjOptData->choice.detVRU.propulsion->choice.motor; + break; + } + + optionalDataJson["detected_vru_data"]["attachment"] = *det_object->detObjOptData->choice.detVRU.attachment; + optionalDataJson["detected_vru_data"]["radius"] = *det_object->detObjOptData->choice.detVRU.radius; + break; + + case DetectedObjectOptionalData_PR_detObst: + optionalDataJson["detected_obst_data"]["obst_size"]["width"] = det_object->detObjOptData->choice.detObst.obstSize.width; + optionalDataJson["detected_obst_data"]["obst_size"]["length"] = det_object->detObjOptData->choice.detObst.obstSize.length; + optionalDataJson["detected_obst_data"]["obst_size"]["height"] = *det_object->detObjOptData->choice.detObst.obstSize.height; // optional + + optionalDataJson["detected_obst_data"]["obst_size"]["width_confidence"] = det_object->detObjOptData->choice.detObst.obstSizeConfidence.widthConfidence; + optionalDataJson["detected_obst_data"]["obst_size"]["length_confidence"] = det_object->detObjOptData->choice.detObst.obstSizeConfidence.lengthConfidence; + optionalDataJson["detected_obst_data"]["obst_size"]["height_confidence"] = *det_object->detObjOptData->choice.detObst.obstSizeConfidence.heightConfidence; + break; + } + detectedObjectJson["detected_object_data"]["detected_object_optional_data"] = optionalDataJson; + } + + // Add the generated object jsons to the detected object list + SDSMDataJson["objects"].append(detectedObjectJson); + + } + } + } +} \ No newline at end of file diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/J3224ToSDSMJsonConverter.h b/src/v2i-hub/CARMAStreetsPlugin/src/J3224ToSDSMJsonConverter.h new file mode 100644 index 000000000..273d7efdd --- /dev/null +++ b/src/v2i-hub/CARMAStreetsPlugin/src/J3224ToSDSMJsonConverter.h @@ -0,0 +1,43 @@ +#include "jsoncpp/json/json.h" +#include + +namespace CARMAStreetsPlugin +{ + + // Template to use when created shared pointer objects for optional data + template + T *create_store_shared(std::vector> &shared_pointers) + { + auto obj_shared = std::make_shared(); + shared_pointers.push_back(obj_shared); + return obj_shared.get(); + } + + // Template for shared pointers with array elements + template + T *create_store_shared_array(std::vector> &shared_pointers, int size) + { + std::shared_ptr array_shared(new T[size]{0}); + shared_pointers.push_back(array_shared); + return array_shared.get(); + } + + + class J3224ToSDSMJsonConverter + { + public: + + J3224ToSDSMJsonConverter() = default; + ~J3224ToSDSMJsonConverter() = default; + + /** + * @brief Convert the J3224 SensorDataSharingMessage into JSON format. + * @param sdsmMsgPtr The input is a constant SensorDataSharingMessage pointer. This prevent any modification to the original SDSM + * @param sdsmJson Pass by reference to allow the method to populate this object with the SensorDataSharingMessage data. + */ + void convertJ3224ToSDSMJSON(const std::shared_ptr sdsmMsgPtr, Json::Value &sdsmJson) const; + + + }; + +} \ No newline at end of file diff --git a/src/v2i-hub/CARMAStreetsPlugin/test/test_J3224ToSDSMJsonConverter.cpp b/src/v2i-hub/CARMAStreetsPlugin/test/test_J3224ToSDSMJsonConverter.cpp new file mode 100644 index 000000000..085e3ba24 --- /dev/null +++ b/src/v2i-hub/CARMAStreetsPlugin/test/test_J3224ToSDSMJsonConverter.cpp @@ -0,0 +1,435 @@ +#include +#include "jsoncpp/json/json.h" +#include +#include + +class test_J3224ToSDSMJsonConverter : public ::testing::Test +{ + public: + test_J3224ToSDSMJsonConverter() = default; + ~test_J3224ToSDSMJsonConverter() = default; +}; + +namespace unit_test +{ + + TEST_F(test_J3224ToSDSMJsonConverter, convertJ3224ToSDSMJSON) + { + std::vector> shared_ptrs; + CARMAStreetsPlugin::J3224ToSDSMJsonConverter sdsmConverter; + + SensorDataSharingMessage *message = (SensorDataSharingMessage_t *)calloc(1, sizeof(SensorDataSharingMessage_t)); + + message->msgCnt = 15; + + uint8_t source_id_bytes[4] = {(uint8_t)1, (uint8_t)2, (uint8_t)3, (uint8_t)4}; + TemporaryID_t source_id; + source_id.buf = source_id_bytes; + source_id.size = sizeof(source_id_bytes); + message->sourceID = source_id; + + message->equipmentType = EquipmentType_rsu; + + auto year_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *year_ptr = 2000; + message->sDSMTimeStamp.year = year_ptr; + auto month_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *month_ptr = 7; + message->sDSMTimeStamp.month = month_ptr; + auto day_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *day_ptr = 4; + message->sDSMTimeStamp.day = day_ptr; + auto hour_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *hour_ptr = 21; + message->sDSMTimeStamp.hour = hour_ptr; + auto minute_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *minute_ptr = 15; + message->sDSMTimeStamp.minute = minute_ptr; + auto second_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *second_ptr = 10000; + message->sDSMTimeStamp.second = second_ptr; + auto offset_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *offset_ptr = 2000; + message->sDSMTimeStamp.offset = offset_ptr; + + message->refPos.lat = 400000000; + message->refPos.Long = 800000000; + auto pos_elevation_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *pos_elevation_ptr = 30; + message->refPos.elevation = pos_elevation_ptr; + + message->refPosXYConf.semiMajor = 250; + message->refPosXYConf.semiMinor = 200; + message->refPosXYConf.orientation = 20000; + + auto elevation_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *elevation_conf_ptr = ElevationConfidence_elev_000_05; + message->refPosElConf = elevation_conf_ptr; + + DetectedObjectList_t detected_obj_list; + + + // Generate an object to test common data and detected vehicle data + DetectedObjectData_t test_obj1; + + test_obj1.detObjCommon.objType = ObjectType_vehicle; + test_obj1.detObjCommon.objTypeCfd = 65; + test_obj1.detObjCommon.objectID = 1200; + test_obj1.detObjCommon.measurementTime = 500; + test_obj1.detObjCommon.timeConfidence = TimeConfidence_time_000_200; + + test_obj1.detObjCommon.pos.offsetX = 1000; + test_obj1.detObjCommon.pos.offsetY = 750; + auto offsetZ_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *offsetZ_ptr = 50; + test_obj1.detObjCommon.pos.offsetZ = offsetZ_ptr; + + test_obj1.detObjCommon.posConfidence.pos = PositionConfidence_a200m; + test_obj1.detObjCommon.posConfidence.elevation = ElevationConfidence_elev_100_00; + + test_obj1.detObjCommon.speed = 2100; + test_obj1.detObjCommon.speedConfidence = SpeedConfidence_prec5ms; + + auto speedZ_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *speedZ_ptr = 1200; + test_obj1.detObjCommon.speedZ = speedZ_ptr; + auto speedZ_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *speedZ_conf_ptr = SpeedConfidence_prec0_1ms; + test_obj1.detObjCommon.speedConfidenceZ = speedZ_conf_ptr; + + test_obj1.detObjCommon.heading = 15000; + test_obj1.detObjCommon.headingConf = HeadingConfidence_prec0_05deg; + + auto accel_4_way_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + accel_4_way_ptr->Long = 200; + accel_4_way_ptr->lat = -500; + accel_4_way_ptr->vert = 1; + accel_4_way_ptr->yaw = 400; + test_obj1.detObjCommon.accel4way = accel_4_way_ptr; + + auto acc_cfd_x_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_x_ptr = AccelerationConfidence_accl_100_00; + test_obj1.detObjCommon.accCfdX = acc_cfd_x_ptr; + auto acc_cfd_y_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_y_ptr = AccelerationConfidence_accl_010_00; + test_obj1.detObjCommon.accCfdY = acc_cfd_y_ptr; + auto acc_cfd_z_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_z_ptr = AccelerationConfidence_accl_005_00; + test_obj1.detObjCommon.accCfdZ = acc_cfd_z_ptr; + + auto acc_cfd_yaw_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_yaw_ptr = YawRateConfidence_degSec_001_00; + test_obj1.detObjCommon.accCfdYaw = acc_cfd_yaw_ptr; + + + auto det_obj_opt_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + + det_obj_opt_ptr->present = DetectedObjectOptionalData_PR_detVeh; + + auto lights_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + // TODO: Make sure lights are being set and converted properly + uint8_t lights_bytes[9] = {(uint8_t)8}; + lights_ptr->buf = lights_bytes; + lights_ptr->size = sizeof(lights_bytes); + det_obj_opt_ptr->choice.detVeh.lights = lights_ptr; + + auto veh_attitude_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + veh_attitude_ptr->pitch = 2400; + veh_attitude_ptr->roll = -12000; + veh_attitude_ptr->yaw = 600; + det_obj_opt_ptr->choice.detVeh.vehAttitude = veh_attitude_ptr; + + auto veh_attitude_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + veh_attitude_conf_ptr->pitchConfidence = HeadingConfidence_prec0_05deg; + veh_attitude_conf_ptr->rollConfidence = HeadingConfidence_prec0_01deg; + veh_attitude_conf_ptr->yawConfidence = HeadingConfidence_prec0_0125deg; + det_obj_opt_ptr->choice.detVeh.vehAttitudeConfidence = veh_attitude_conf_ptr; + + auto veh_ang_vel_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + veh_ang_vel_ptr->pitchRate = 800; + veh_ang_vel_ptr->rollRate = -600; + det_obj_opt_ptr->choice.detVeh.vehAngVel = veh_ang_vel_ptr; + + auto veh_ang_vel_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + auto pitch_rate_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *pitch_rate_conf_ptr = PitchRateConfidence_degSec_001_00; + veh_ang_vel_conf_ptr->pitchRateConfidence = pitch_rate_conf_ptr; + auto roll_rate_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *roll_rate_conf_ptr = RollRateConfidence_degSec_000_10; + veh_ang_vel_conf_ptr->rollRateConfidence = roll_rate_conf_ptr; + det_obj_opt_ptr->choice.detVeh.vehAngVelConfidence = veh_ang_vel_conf_ptr; + + auto veh_size_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + veh_size_ptr->width = 300; + veh_size_ptr->length = 700; + det_obj_opt_ptr->choice.detVeh.size = veh_size_ptr; + + auto veh_height_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *veh_height_ptr = 70; + det_obj_opt_ptr->choice.detVeh.height= veh_height_ptr; + + auto veh_size_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + veh_size_conf_ptr->vehicleWidthConfidence = SizeValueConfidence_size_000_10; + veh_size_conf_ptr->vehicleLengthConfidence = SizeValueConfidence_size_000_05; + auto veh_height_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *veh_height_conf_ptr = SizeValueConfidence_size_000_02; + veh_size_conf_ptr->vehicleHeightConfidence = veh_height_conf_ptr; + det_obj_opt_ptr->choice.detVeh.vehicleSizeConfidence = veh_size_conf_ptr; + + auto veh_class_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *veh_class_ptr = 45; + det_obj_opt_ptr->choice.detVeh.vehicleClass = veh_class_ptr; + + auto veh_class_conf_ptr = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *veh_class_conf_ptr = 85; + det_obj_opt_ptr->choice.detVeh.classConf = veh_class_conf_ptr; + + test_obj1.detObjOptData = det_obj_opt_ptr; + + asn_sequence_add(&message->objects.list, &test_obj1); + + + // Generate a second object to test detected VRU data + DetectedObjectData_t test_obj2; + + test_obj2.detObjCommon.objType = ObjectType_vehicle; + test_obj2.detObjCommon.objTypeCfd = 65; + test_obj2.detObjCommon.objectID = 110; + test_obj2.detObjCommon.measurementTime = 500; + test_obj2.detObjCommon.timeConfidence = TimeConfidence_time_000_200; + + test_obj2.detObjCommon.pos.offsetX = 1000; + test_obj2.detObjCommon.pos.offsetY = 750; + auto offsetZ_ptr2 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *offsetZ_ptr2 = 50; + test_obj2.detObjCommon.pos.offsetZ = offsetZ_ptr2; + + test_obj2.detObjCommon.posConfidence.pos = PositionConfidence_a200m; + test_obj2.detObjCommon.posConfidence.elevation = ElevationConfidence_elev_100_00; + + test_obj2.detObjCommon.speed = 2100; + test_obj2.detObjCommon.speedConfidence = SpeedConfidence_prec5ms; + + auto speedZ_ptr2 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *speedZ_ptr2 = 1200; + test_obj2.detObjCommon.speedZ = speedZ_ptr2; + auto speedZ_conf_ptr2 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *speedZ_conf_ptr2 = SpeedConfidence_prec0_1ms; + test_obj2.detObjCommon.speedConfidenceZ = speedZ_conf_ptr2; + + test_obj2.detObjCommon.heading = 15000; + test_obj2.detObjCommon.headingConf = HeadingConfidence_prec0_05deg; + + auto accel_4_way_ptr2 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + accel_4_way_ptr2->Long = 200; + accel_4_way_ptr2->lat = -500; + accel_4_way_ptr2->vert = 1; + accel_4_way_ptr2->yaw = 400; + test_obj2.detObjCommon.accel4way = accel_4_way_ptr2; + + auto acc_cfd_x_ptr2 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_x_ptr2 = AccelerationConfidence_accl_100_00; + test_obj2.detObjCommon.accCfdX = acc_cfd_x_ptr2; + auto acc_cfd_y_ptr2 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_y_ptr2 = AccelerationConfidence_accl_010_00; + test_obj2.detObjCommon.accCfdY = acc_cfd_y_ptr2; + auto acc_cfd_z_ptr2 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_z_ptr2 = AccelerationConfidence_accl_005_00; + test_obj2.detObjCommon.accCfdZ = acc_cfd_z_ptr2; + + auto acc_cfd_yaw_ptr2 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_yaw_ptr2 = YawRateConfidence_degSec_001_00; + test_obj2.detObjCommon.accCfdYaw = acc_cfd_yaw_ptr2; + + auto det_obj_opt_ptr2 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + det_obj_opt_ptr2->present = DetectedObjectOptionalData_PR_detVRU; + + auto basic_type = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *basic_type = PersonalDeviceUserType_aPEDESTRIAN; + det_obj_opt_ptr2->choice.detVRU.basicType = basic_type; + + auto propulsion = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + propulsion->present = PropelledInformation_PR_human; + propulsion->choice.human = HumanPropelledType_onFoot; + det_obj_opt_ptr2->choice.detVRU.propulsion = propulsion; + + auto attachment = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *attachment = Attachment_wheelchair; + det_obj_opt_ptr2->choice.detVRU.attachment = attachment; + + auto radius = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *radius = 30; + det_obj_opt_ptr2->choice.detVRU.radius = radius; + + test_obj2.detObjOptData = det_obj_opt_ptr2; + asn_sequence_add(&message->objects.list, &test_obj2); + + + + // Generate a third object to test detected obstacle data + DetectedObjectData_t test_obj3; + + test_obj3.detObjCommon.objType = ObjectType_vehicle; + test_obj3.detObjCommon.objTypeCfd = 65; + test_obj3.detObjCommon.objectID = 1000; + test_obj3.detObjCommon.measurementTime = 500; + test_obj3.detObjCommon.timeConfidence = TimeConfidence_time_000_200; + + test_obj3.detObjCommon.pos.offsetX = 1000; + test_obj3.detObjCommon.pos.offsetY = 750; + auto offsetZ_ptr3 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *offsetZ_ptr3 = 50; + test_obj3.detObjCommon.pos.offsetZ = offsetZ_ptr3; + + test_obj3.detObjCommon.posConfidence.pos = PositionConfidence_a200m; + test_obj3.detObjCommon.posConfidence.elevation = ElevationConfidence_elev_100_00; + + test_obj3.detObjCommon.speed = 2100; + test_obj3.detObjCommon.speedConfidence = SpeedConfidence_prec5ms; + + auto speedZ_ptr3 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *speedZ_ptr3 = 1200; + test_obj3.detObjCommon.speedZ = speedZ_ptr3; + auto speedZ_conf_ptr3 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *speedZ_conf_ptr3 = SpeedConfidence_prec0_1ms; + test_obj3.detObjCommon.speedConfidenceZ = speedZ_conf_ptr3; + + test_obj3.detObjCommon.heading = 15000; + test_obj3.detObjCommon.headingConf = HeadingConfidence_prec0_05deg; + + auto accel_4_way_ptr3 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + accel_4_way_ptr3->Long = 200; + accel_4_way_ptr3->lat = -500; + accel_4_way_ptr3->vert = 1; + accel_4_way_ptr3->yaw = 400; + test_obj3.detObjCommon.accel4way = accel_4_way_ptr3; + + auto acc_cfd_x_ptr3 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_x_ptr3 = AccelerationConfidence_accl_100_00; + test_obj3.detObjCommon.accCfdX = acc_cfd_x_ptr3; + auto acc_cfd_y_ptr3 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_y_ptr3 = AccelerationConfidence_accl_010_00; + test_obj3.detObjCommon.accCfdY = acc_cfd_y_ptr3; + auto acc_cfd_z_ptr3 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_z_ptr3 = AccelerationConfidence_accl_005_00; + test_obj3.detObjCommon.accCfdZ = acc_cfd_z_ptr3; + + auto acc_cfd_yaw_ptr3 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *acc_cfd_yaw_ptr3 = YawRateConfidence_degSec_001_00; + test_obj3.detObjCommon.accCfdYaw = acc_cfd_yaw_ptr3; + + + + auto det_obj_opt_ptr3 = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + det_obj_opt_ptr3->present = DetectedObjectOptionalData_PR_detObst; + + det_obj_opt_ptr3->choice.detObst.obstSize.width = 400; + det_obj_opt_ptr3->choice.detObst.obstSize.length = 300; + auto obj_height = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *obj_height = 100; + det_obj_opt_ptr3->choice.detObst.obstSize.height = obj_height; + + det_obj_opt_ptr3->choice.detObst.obstSizeConfidence.widthConfidence = SizeValueConfidence_size_010_00; + det_obj_opt_ptr3->choice.detObst.obstSizeConfidence.lengthConfidence = SizeValueConfidence_size_005_00; + auto obj_height_conf = CARMAStreetsPlugin::create_store_shared(shared_ptrs); + *obj_height_conf = SizeValueConfidence_size_002_00; + det_obj_opt_ptr3->choice.detObst.obstSizeConfidence.heightConfidence = obj_height_conf; + + test_obj3.detObjOptData = det_obj_opt_ptr3; + + asn_sequence_add(&message->objects.list, &test_obj3); + + // Create a SensorDataSharingMessage with the test data to load into a Json (sdsmJson) + std::shared_ptr sdsmMsgPtr(message); + Json::Value sdsmJson; + sdsmConverter.convertJ3224ToSDSMJSON(sdsmMsgPtr, sdsmJson); + + // Tests + // Testing SDSM header data + ASSERT_EQ(15, sdsmJson["msg_cnt"].asInt()); + ASSERT_EQ("01020304", sdsmJson["source_id"].asString()); + ASSERT_EQ(1, sdsmJson["equipment_type"].asInt()); + ASSERT_EQ(2000, sdsmJson["sdsm_time_stamp"]["year"].asInt()); + ASSERT_EQ(7, sdsmJson["sdsm_time_stamp"]["month"].asInt()); + ASSERT_EQ(4, sdsmJson["sdsm_time_stamp"]["day"].asInt()); + ASSERT_EQ(21, sdsmJson["sdsm_time_stamp"]["hour"].asInt()); + ASSERT_EQ(15, sdsmJson["sdsm_time_stamp"]["minute"].asInt()); + ASSERT_EQ(10000, sdsmJson["sdsm_time_stamp"]["second"].asInt()); + ASSERT_EQ(2000, sdsmJson["sdsm_time_stamp"]["offset"].asInt()); + ASSERT_EQ(400000000, sdsmJson["ref_pos"]["lat"].asInt()); + ASSERT_EQ(800000000, sdsmJson["ref_pos"]["Long"].asInt()); + ASSERT_EQ(30, sdsmJson["ref_pos"]["elevation"].asInt()); + ASSERT_EQ(250, sdsmJson["ref_pos_xy_conf"]["semi_major"].asInt()); + ASSERT_EQ(200, sdsmJson["ref_pos_xy_conf"]["semi_minor"].asInt()); + ASSERT_EQ(20000, sdsmJson["ref_pos_xy_conf"]["orientation"].asInt()); + ASSERT_EQ(13, sdsmJson["ref_pos_el_conf"].asInt()); + + // Object 1 detected object common data + ASSERT_EQ(1, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["object_type"].asInt()); + ASSERT_EQ(65, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["object_type_conf"].asInt()); + ASSERT_EQ(1200, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["object_id"].asInt()); + ASSERT_EQ(500, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["measurement_time"].asInt()); + ASSERT_EQ(8, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["time_confidence"].asInt()); + ASSERT_EQ(1000, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["pos"]["offset_x"].asInt()); + ASSERT_EQ(750, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["pos"]["offset_y"].asInt()); + ASSERT_EQ(50, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["pos"]["offset_z"].asInt()); + ASSERT_EQ(2, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["pos_confidence"]["pos"].asInt()); + ASSERT_EQ(3, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["pos_confidence"]["elevation"].asInt()); + ASSERT_EQ(2100, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["speed"].asInt()); + ASSERT_EQ(3, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["speed_confidence"].asInt()); + ASSERT_EQ(1200, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["speed_z"].asInt()); + ASSERT_EQ(5, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["speed_confidence_z"].asInt()); + ASSERT_EQ(15000, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["heading"].asInt()); + ASSERT_EQ(5, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["heading_conf"].asInt()); + ASSERT_EQ(200, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["long"].asInt()); + ASSERT_EQ(-500, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["lat"].asInt()); + ASSERT_EQ(1, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["vert"].asInt()); + ASSERT_EQ(400, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["accel_4_way"]["yaw"].asInt()); + ASSERT_EQ(1, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["acc_cfd_x"].asInt()); + ASSERT_EQ(2, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["acc_cfd_y"].asInt()); + ASSERT_EQ(3, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["acc_cfd_z"].asInt()); + ASSERT_EQ(4, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["acc_cfd_yaw"].asInt()); + + // Object 1 detected vehicle data (optional) + // TODO: Better construction/test of ExteriorLights message and resulting bitstring + // ASSERT_EQ(8, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["lights"].asInt()); + ASSERT_EQ(2400, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude"]["pitch"].asInt()); + ASSERT_EQ(-12000, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude"]["roll"].asInt()); + ASSERT_EQ(600, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude"]["yaw"].asInt()); + ASSERT_EQ(2400, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude"]["pitch"].asInt()); + ASSERT_EQ(5, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude_confidence"]["pitch_confidence"].asInt()); + ASSERT_EQ(6, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude_confidence"]["roll_confidence"].asInt()); + ASSERT_EQ(7, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_attitude_confidence"]["yaw_confidence"].asInt()); + ASSERT_EQ(800, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_ang_vel"]["pitch_rate"].asInt()); + ASSERT_EQ(-600, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_ang_vel"]["roll_rate"].asInt()); + ASSERT_EQ(4, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_ang_vel_confidence"]["pitch_rate_confidence"].asInt()); + ASSERT_EQ(5, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["veh_ang_vel_confidence"]["roll_rate_confidence"].asInt()); + ASSERT_EQ(300, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["size"]["width"].asInt()); + ASSERT_EQ(700, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["size"]["length"].asInt()); + ASSERT_EQ(70, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["height"].asInt()); + ASSERT_EQ(10, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["vehicle_size_confidence"]["vehicle_width_confidence"].asInt()); + ASSERT_EQ(11, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["vehicle_size_confidence"]["vehicle_length_confidence"].asInt()); + ASSERT_EQ(12, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["vehicle_size_confidence"]["vehicle_height_confidence"].asInt()); + ASSERT_EQ(45, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["vehicle_class"].asInt()); + ASSERT_EQ(85, sdsmJson["objects"][0]["detected_object_data"]["detected_object_optional_data"]["detected_vehicle_data"]["vehicle_class_conf"].asInt()); + + // Only test the ID and optional VRU data for the 2nd object + ASSERT_EQ(110, sdsmJson["objects"][1]["detected_object_data"]["detected_object_common_data"]["object_id"].asInt()); + ASSERT_EQ(1, sdsmJson["objects"][1]["detected_object_data"]["detected_object_optional_data"]["detected_vru_data"]["basic_type"].asInt()); + ASSERT_EQ(2, sdsmJson["objects"][1]["detected_object_data"]["detected_object_optional_data"]["detected_vru_data"]["propulsion"]["human"].asInt()); + ASSERT_EQ(4, sdsmJson["objects"][1]["detected_object_data"]["detected_object_optional_data"]["detected_vru_data"]["attachment"].asInt()); + ASSERT_EQ(30, sdsmJson["objects"][1]["detected_object_data"]["detected_object_optional_data"]["detected_vru_data"]["radius"].asInt()); + + // Only test the ID and optional obstacle data for the 3rd object + ASSERT_EQ(1000, sdsmJson["objects"][2]["detected_object_data"]["detected_object_common_data"]["object_id"].asInt()); + ASSERT_EQ(400, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obst_data"]["obst_size"]["width"].asInt()); + ASSERT_EQ(300, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obst_data"]["obst_size"]["length"].asInt()); + ASSERT_EQ(100, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obst_data"]["obst_size"]["height"].asInt()); + ASSERT_EQ(4, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obst_data"]["obst_size"]["width_confidence"].asInt()); + ASSERT_EQ(5, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obst_data"]["obst_size"]["length_confidence"].asInt()); + ASSERT_EQ(6, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obst_data"]["obst_size"]["height_confidence"].asInt()); + + } + +}; + From 746e8e61d5380eb611c50b5a5f5145b31ed87ee3 Mon Sep 17 00:00:00 2001 From: dev Date: Thu, 7 Sep 2023 17:59:52 -0400 Subject: [PATCH 15/28] Temporary fix for TimeSyncMessage invalid json serialization --- .../CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index ed04c2112..85dd69709 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -105,8 +105,17 @@ void CARMAStreetsPlugin::OnConfigChanged(const char *key, const char *value) { void CARMAStreetsPlugin::HandleTimeSyncMessage(tmx::messages::TimeSyncMessage &msg, routeable_message &routeableMsg ) { PluginClientClockAware::HandleTimeSyncMessage(msg, routeableMsg); if ( isSimulationMode()) { - PLOG(logINFO) << "Handling TimeSync messages!" << std::endl; - produce_kafka_msg(msg.to_string(), "time_sync"); + // 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. + boost::regex exp("\"(null|true|false|[0-9]+(\\.[0-9]+)?)\""); + std::stringstream ss; + std::string rv = boost::regex_replace(msg.to_string(), exp, "$1"); + PLOG(logINFO) << "Sending Time Sync Message " << rv << std::endl; + + produce_kafka_msg(rv, "time_sync"); } } void CARMAStreetsPlugin::HandleMobilityOperationMessage(tsm3Message &msg, routeable_message &routeableMsg ) { From 5328f89f9f7b1d52834e84c78bed1853fb244425 Mon Sep 17 00:00:00 2001 From: Peyton Johnson <35665039+willjohnsonk@users.noreply.github.com> Date: Mon, 11 Sep 2023 16:24:43 -0400 Subject: [PATCH 16/28] Added conversion between JSON and ASN.1 C-struct for SDSM with Kafka updates (#562) # PR Details ## Description Added functionality to convert SDSMs from inbound json strings to outbound ASN.1 styled C-structs to facilitate communication over the CARMA Streets plugin in both directions. ## Related Issue CDAR-308 ## Motivation and Context ## How Has This Been Tested? Tested locally in VSCode with new unit tests for the conversion between jsons and the ASN.1 C-struct messages. ## Types of changes - [X] Defect fix (non-breaking change that fixes an issue) - [X] 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/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt | 2 +- src/v2i-hub/CARMAStreetsPlugin/manifest.json | 10 + .../src/CARMAStreetsPlugin.cpp | 74 +++- .../src/CARMAStreetsPlugin.h | 25 ++ .../src/J3224ToSDSMJsonConverter.cpp | 18 +- .../src/JsonToJ3224SDSMConverter.cpp | 368 ++++++++++++++++++ .../src/JsonToJ3224SDSMConverter.h | 46 +++ .../test/test_J3224ToSDSMJsonConverter.cpp | 18 +- .../test/test_JsonToJ3224SDSMConverter.cpp | 188 +++++++++ 9 files changed, 728 insertions(+), 21 deletions(-) create mode 100644 src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.cpp create mode 100644 src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.h create mode 100644 src/v2i-hub/CARMAStreetsPlugin/test/test_JsonToJ3224SDSMConverter.cpp diff --git a/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt b/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt index f8c11c896..08981e25d 100644 --- a/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt +++ b/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt @@ -9,7 +9,7 @@ TARGET_LINK_LIBRARIES (${PROJECT_NAME} tmxutils rdkafka++ jsoncpp) ############# enable_testing() include_directories(${PROJECT_SOURCE_DIR}/src) -add_library(${PROJECT_NAME}_lib src/J2735MapToJsonConverter.cpp src/JsonToJ2735SSMConverter.cpp src/JsonToJ2735SpatConverter.cpp src/J2735ToSRMJsonConverter.cpp src/J3224ToSDSMJsonConverter.cpp) +add_library(${PROJECT_NAME}_lib src/J2735MapToJsonConverter.cpp src/JsonToJ2735SSMConverter.cpp src/JsonToJ2735SpatConverter.cpp src/J2735ToSRMJsonConverter.cpp src/J3224ToSDSMJsonConverter.cpp src/JsonToJ3224SDSMConverter.cpp) target_link_libraries(${PROJECT_NAME}_lib PUBLIC ${TMXAPI_LIBRARIES} ${ASN_J2735_LIBRARIES} ${MYSQL_LIBRARIES} diff --git a/src/v2i-hub/CARMAStreetsPlugin/manifest.json b/src/v2i-hub/CARMAStreetsPlugin/manifest.json index 22fb4a912..acd398414 100644 --- a/src/v2i-hub/CARMAStreetsPlugin/manifest.json +++ b/src/v2i-hub/CARMAStreetsPlugin/manifest.json @@ -116,6 +116,16 @@ "key": "SimSensorDetectedObjTopic", "default": "v2xhub_sim_sensor_detected_object", "description": "Apache Kafka topic plugin will transmit simulated sensor detected object to." + }, + { + "key": "SdsmSubscribeTopic", + "default": "v2xhub_sdsm_sub", + "description": "Apache Kafka topic plugin that will subscribe to SDSM streams." + }, + { + "key": "SdsmTransmitTopic", + "default": "v2xhub_sdsm_tra", + "description": "Apache Kafka topic plugin that will transmit SDSMs." } ] diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index 85dd69709..23a21d6bf 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -49,6 +49,8 @@ void CARMAStreetsPlugin::UpdateConfigSettings() { GetConfigValue("MapTopic", _transmitMAPTopic); GetConfigValue("SRMTopic", _transmitSRMTopic); GetConfigValue("SimSensorDetectedObjTopic", _transmitSimSensorDetectedObjTopic); + GetConfigValue("SdsmSubscribeTopic", _subscribeToSdsmTopic); + GetConfigValue("SdsmTransmitTopic", _transmitSDSMTopic); // Populate strategies config string config; GetConfigValue("MobilityOperationStrategies", config); @@ -81,12 +83,13 @@ void CARMAStreetsPlugin::InitKafkaConsumerProducers() _spat_kafka_consumer_ptr = client.create_consumer(kafkaConnectString, _subscribeToSpatTopic,this->_name); _scheduing_plan_kafka_consumer_ptr = client.create_consumer(kafkaConnectString, _subscribeToSchedulingPlanTopic, this->_name); _ssm_kafka_consumer_ptr = client.create_consumer(kafkaConnectString, _subscribeToSsmTopic,this->_name); - if(!_scheduing_plan_kafka_consumer_ptr || !_spat_kafka_consumer_ptr || !_ssm_kafka_consumer_ptr) + _sdsm_kafka_consumer_ptr = client.create_consumer(kafkaConnectString, _subscribeToSdsmTopic,this->_name); + if(!_scheduing_plan_kafka_consumer_ptr || !_spat_kafka_consumer_ptr || !_ssm_kafka_consumer_ptr || !_sdsm_kafka_consumer_ptr) { throw TmxException("Failed to create Kafka consumers."); } PLOG(logDEBUG) <<"Kafka consumers created"; - if(!_spat_kafka_consumer_ptr->init() || !_scheduing_plan_kafka_consumer_ptr->init() || !_ssm_kafka_consumer_ptr->init()) + if(!_spat_kafka_consumer_ptr->init() || !_scheduing_plan_kafka_consumer_ptr->init() || !_ssm_kafka_consumer_ptr->init() || !_sdsm_kafka_consumer_ptr->init()) { throw TmxException("Kafka consumers init() failed!"); } @@ -94,6 +97,7 @@ void CARMAStreetsPlugin::InitKafkaConsumerProducers() boost::thread thread_schpl(&CARMAStreetsPlugin::SubscribeSchedulingPlanKafkaTopic, this); boost::thread thread_spat(&CARMAStreetsPlugin::SubscribeSpatKafkaTopic, this); boost::thread thread_ssm(&CARMAStreetsPlugin::SubscribeSSMKafkaTopic, this); + boost::thread thread_sdsm(&CARMAStreetsPlugin::SubscribeSDSMKafkaTopic, this); } @@ -458,6 +462,19 @@ void CARMAStreetsPlugin::HandleMapMessage(MapDataMessage &msg, routeable_message produce_kafka_msg(message, _transmitMAPTopic); } +void CARMAStreetsPlugin::HandleSDSMMessage(SdsmMessage &msg, routeable_message &routeableMsg) +{ + std::shared_ptr sdsmMsgPtr = msg.get_j2735_data(); + PLOG(logDEBUG) << "Detected object count: " << sdsmMsgPtr->objects.list.count << std::endl; + Json::Value sdsmJson; + Json::StreamWriterBuilder builder; + J3224ToSDSMJsonConverter jsonConverter; + jsonConverter.convertJ3224ToSDSMJSON(sdsmMsgPtr, sdsmJson); + PLOG(logDEBUG) << "sdsmJson: " << sdsmJson << std::endl; + const std::string message = Json::writeString(builder, sdsmJson); + produce_kafka_msg(message, _transmitSDSMTopic); +} + void CARMAStreetsPlugin::produce_kafka_msg(const string& message, const string& topic_name) const { _kafka_producer_ptr->send(message, topic_name); @@ -638,6 +655,59 @@ void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){ } +void CARMAStreetsPlugin::SubscribeSDSMKafkaTopic(){ + // TODO: Update methods to represent consuming a single message from Kafka topic + if(_subscribeToSdsmTopic.length() > 0) + { + PLOG(logDEBUG) << "SubscribeSDSMKafkaTopics:" <<_subscribeToSdsmTopic << std::endl; + _sdsm_kafka_consumer_ptr->subscribe(); + //Initialize Json to J3224 SDSM convertor + JsonToJ3224SDSMConverter sdsm_convertor; + while (_sdsm_kafka_consumer_ptr->is_running()) + { + std::string payload_str = _sdsm_kafka_consumer_ptr->consume(500); + if(payload_str.length() > 0) + { + PLOG(logDEBUG) << "consumed message payload: " << payload_str <(Key_SDSMMessageSkipped, ++_sdsmMessageSkipped); + continue; + } + //Convert the SDSM JSON string into J3224 SDSM message and encode it. + auto sdsm_ptr = std::make_shared(); + sdsm_convertor.convertJsonToSDSM(sdsmDoc, sdsm_ptr); + tmx::messages::SdsmEncodedMessage sdsmEncodedMsg; + try + { + sdsm_convertor.encodeSDSM(sdsm_ptr, sdsmEncodedMsg); + } + catch (TmxException &ex) + { + // 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 + 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)); + } + } + } + +} + void CARMAStreetsPlugin::HandleSimulatedSensorDetectedMessage(simulation::SensorDetectedObject &msg, routeable_message &routeableMsg) { PLOG(logDEBUG) << "Produce sensor detected message in JSON format: " << msg.to_string() < #include "JsonToJ2735SSMConverter.h" #include +#include "JsonToJ3224SDSMConverter.h" +#include "J3224ToSDSMJsonConverter.h" #include "PluginClientClockAware.h" @@ -67,6 +69,12 @@ class CARMAStreetsPlugin: public PluginClientClockAware { * @param routeableMsg */ void HandleMapMessage(MapDataMessage &msg, routeable_message &routeableMsg); + /** + * @brief Subscribes to incoming ASN.1, C-Struct formatted SDSMs generated from broadcasting RSUs. These SDSM C-Structs are then converted to JSON to be forwarded to CARMA Streets/Kafka by the handler. + * @param msg The J3224 SDSM received from the internal + * @param routeableMsg + */ + void HandleSDSMMessage(SdsmMessage &msg, routeable_message &routeableMsg); /** * @brief Subscribe to SRM message received from RSU and publish the message to a Kafka topic */ @@ -83,6 +91,10 @@ class CARMAStreetsPlugin: public PluginClientClockAware { * @brief Subcribe to SSM Kafka topic created by carma-streets */ void SubscribeSSMKafkaTopic(); + /** + * @brief Subcribe to SDSM Kafka topic created by carma-streets + */ + void SubscribeSDSMKafkaTopic(); bool getEncodedtsm3(tsm3EncodedMessage *tsm3EncodedMsg, Json::Value metadata, Json::Value payload_json); /** @@ -105,6 +117,7 @@ class CARMAStreetsPlugin: public PluginClientClockAware { std::string _subscribeToSchedulingPlanConsumerGroupId; std::string _subscribeToSpatTopic; std::string _subscribeToSsmTopic; + std::string _subscribeToSdsmTopic; std::string _subscribeToSpatConsumerGroupId; std::string _subscribeToSSMConsumerGroupId; std::string _transmitMobilityPathTopic; @@ -112,12 +125,14 @@ class CARMAStreetsPlugin: public PluginClientClockAware { std::string _transmitMAPTopic; std::string _transmitSRMTopic; std::string _transmitSimSensorDetectedObjTopic; + std::string _transmitSDSMTopic; std::string _kafkaBrokerIp; std::string _kafkaBrokerPort; std::shared_ptr _kafka_producer_ptr; std::shared_ptr _spat_kafka_consumer_ptr; std::shared_ptr _scheduing_plan_kafka_consumer_ptr; std::shared_ptr _ssm_kafka_consumer_ptr; + std::shared_ptr _sdsm_kafka_consumer_ptr; std::vector _strategies; tmx::messages::tsm3Message *_tsm3Message{NULL}; std::mutex data_lock; @@ -192,6 +207,16 @@ class CARMAStreetsPlugin: public PluginClientClockAware { */ uint _ssmMessageSkipped = 0; + /** + * @brief Status label for SDSM messages skipped due to errors. + */ + const char* Key_SDSMMessageSkipped = "SDSM messages skipped due to errors."; + + /** + * @brief Count for SDSM messages skipped due to errors. + */ + uint _sdsmMessageSkipped = 0; + /** * @brief Intersection Id for intersection */ diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/J3224ToSDSMJsonConverter.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/J3224ToSDSMJsonConverter.cpp index 33fd68e2c..6e03bce7e 100644 --- a/src/v2i-hub/CARMAStreetsPlugin/src/J3224ToSDSMJsonConverter.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/J3224ToSDSMJsonConverter.cpp @@ -35,7 +35,7 @@ namespace CARMAStreetsPlugin Json::Value refPos; refPos["lat"] = sdsmMsgPtr->refPos.lat; - refPos["Long"] = sdsmMsgPtr->refPos.Long; + refPos["long"] = sdsmMsgPtr->refPos.Long; refPos["elevation"] = *sdsmMsgPtr->refPos.elevation; SDSMDataJson["ref_pos"] = refPos; @@ -61,8 +61,8 @@ namespace CARMAStreetsPlugin auto det_object = det_obj_list.list.array[i]; // Detected object common data - detectedObjectJson["detected_object_data"]["detected_object_common_data"]["object_type"] = det_object->detObjCommon.objType; - detectedObjectJson["detected_object_data"]["detected_object_common_data"]["object_type_conf"] = det_object->detObjCommon.objTypeCfd; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["obj_type"] = det_object->detObjCommon.objType; + detectedObjectJson["detected_object_data"]["detected_object_common_data"]["obj_type_cfd"] = det_object->detObjCommon.objTypeCfd; detectedObjectJson["detected_object_data"]["detected_object_common_data"]["object_id"] = det_object->detObjCommon.objectID; detectedObjectJson["detected_object_data"]["detected_object_common_data"]["measurement_time"] = det_object->detObjCommon.measurementTime; detectedObjectJson["detected_object_data"]["detected_object_common_data"]["time_confidence"] = det_object->detObjCommon.timeConfidence; @@ -156,13 +156,13 @@ namespace CARMAStreetsPlugin break; case DetectedObjectOptionalData_PR_detObst: - optionalDataJson["detected_obst_data"]["obst_size"]["width"] = det_object->detObjOptData->choice.detObst.obstSize.width; - optionalDataJson["detected_obst_data"]["obst_size"]["length"] = det_object->detObjOptData->choice.detObst.obstSize.length; - optionalDataJson["detected_obst_data"]["obst_size"]["height"] = *det_object->detObjOptData->choice.detObst.obstSize.height; // optional + optionalDataJson["detected_obstacle_data"]["obst_size"]["width"] = det_object->detObjOptData->choice.detObst.obstSize.width; + optionalDataJson["detected_obstacle_data"]["obst_size"]["length"] = det_object->detObjOptData->choice.detObst.obstSize.length; + optionalDataJson["detected_obstacle_data"]["obst_size"]["height"] = *det_object->detObjOptData->choice.detObst.obstSize.height; // optional - optionalDataJson["detected_obst_data"]["obst_size"]["width_confidence"] = det_object->detObjOptData->choice.detObst.obstSizeConfidence.widthConfidence; - optionalDataJson["detected_obst_data"]["obst_size"]["length_confidence"] = det_object->detObjOptData->choice.detObst.obstSizeConfidence.lengthConfidence; - optionalDataJson["detected_obst_data"]["obst_size"]["height_confidence"] = *det_object->detObjOptData->choice.detObst.obstSizeConfidence.heightConfidence; + optionalDataJson["detected_obstacle_data"]["obst_size_confidence"]["width_confidence"] = det_object->detObjOptData->choice.detObst.obstSizeConfidence.widthConfidence; + optionalDataJson["detected_obstacle_data"]["obst_size_confidence"]["length_confidence"] = det_object->detObjOptData->choice.detObst.obstSizeConfidence.lengthConfidence; + optionalDataJson["detected_obstacle_data"]["obst_size_confidence"]["height_confidence"] = *det_object->detObjOptData->choice.detObst.obstSizeConfidence.heightConfidence; break; } detectedObjectJson["detected_object_data"]["detected_object_optional_data"] = optionalDataJson; diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.cpp new file mode 100644 index 000000000..55e4cd8bf --- /dev/null +++ b/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.cpp @@ -0,0 +1,368 @@ +#include "JsonToJ3224SDSMConverter.h" + +namespace CARMAStreetsPlugin +{ + // TODO: Move these template functions to a more central location such as in a utility file + // Template to use when created shared pointer objects for optional data + template + T *create_store_shared(std::vector> &shared_pointers) + { + auto obj_shared = std::make_shared(); + shared_pointers.push_back(obj_shared); + return obj_shared.get(); + } + + // Template for shared pointers with array elements + template + T *create_store_shared_array(std::vector> &shared_pointers, int size) + { + std::shared_ptr array_shared(new T[size]{0}); + shared_pointers.push_back(array_shared); + return array_shared.get(); + } + + // TODO: Consolidate parseJsonString into a more central location since it is being used verbatim in other converters + bool JsonToJ3224SDSMConverter::parseJsonString(const std::string &consumedMsg, Json::Value &sdsmJsonOut) const + { + const auto jsonLen = static_cast(consumedMsg.length()); + Json::CharReaderBuilder builder; + JSONCPP_STRING err; + const std::unique_ptr reader(builder.newCharReader()); + bool parseResult = reader->parse(consumedMsg.c_str(), consumedMsg.c_str() + jsonLen, &sdsmJsonOut, &err); + if (!parseResult) + { + PLOG(logERROR) << "Parse error: " << err << std::endl; + } + return parseResult; + } + + + void JsonToJ3224SDSMConverter::convertJsonToSDSM(const Json::Value &sdsm_json, std::shared_ptr sdsm) const + { + std::vector> shared_ptrs; + + 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; + + 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()){ + 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; + } + + // 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; + } + + + // 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; + } + + + // Add optional data to object data + object_data->detObjOptData = optional_data_ptr; + + + } + } + + // 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; + + + } + + + void JsonToJ3224SDSMConverter::encodeSDSM(const std::shared_ptr &sdsmPtr, tmx::messages::SdsmEncodedMessage &encodedSDSM) const + { + tmx::messages::MessageFrameMessage frame(sdsmPtr); + encodedSDSM.set_data(tmx::messages::TmxJ2735EncodedMessage::encode_j2735_message>(frame)); + free(frame.get_j2735_data().get()); + } + + +} \ No newline at end of file diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.h b/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.h new file mode 100644 index 000000000..ea13123f2 --- /dev/null +++ b/src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.h @@ -0,0 +1,46 @@ +#include "jsoncpp/json/json.h" +#include +#include +#include +#include +#include +#include +#include + +using namespace tmx::utils; + +namespace CARMAStreetsPlugin +{ + class JsonToJ3224SDSMConverter + { + + public: + + JsonToJ3224SDSMConverter() = default; + + /*** + * @brief Parse Json string into Json object + * @param jsonstring that is consumed from a kafka topic + * @param JsonObject that is populated by the input json string + * @return boolean. True if parse successfully, false if parse with errors + */ + bool parseJsonString(const std::string &consumed_msg, Json::Value &sdsm_json_output) const; + + /** + * @brief Convert the Json value with sdsm information info tmx SDSM object. + * @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; + + /*** + * @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; + + ~JsonToJ3224SDSMConverter() = default; + }; + +} \ No newline at end of file diff --git a/src/v2i-hub/CARMAStreetsPlugin/test/test_J3224ToSDSMJsonConverter.cpp b/src/v2i-hub/CARMAStreetsPlugin/test/test_J3224ToSDSMJsonConverter.cpp index 085e3ba24..132cd2e20 100644 --- a/src/v2i-hub/CARMAStreetsPlugin/test/test_J3224ToSDSMJsonConverter.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/test/test_J3224ToSDSMJsonConverter.cpp @@ -357,7 +357,7 @@ namespace unit_test ASSERT_EQ(10000, sdsmJson["sdsm_time_stamp"]["second"].asInt()); ASSERT_EQ(2000, sdsmJson["sdsm_time_stamp"]["offset"].asInt()); ASSERT_EQ(400000000, sdsmJson["ref_pos"]["lat"].asInt()); - ASSERT_EQ(800000000, sdsmJson["ref_pos"]["Long"].asInt()); + ASSERT_EQ(800000000, sdsmJson["ref_pos"]["long"].asInt()); ASSERT_EQ(30, sdsmJson["ref_pos"]["elevation"].asInt()); ASSERT_EQ(250, sdsmJson["ref_pos_xy_conf"]["semi_major"].asInt()); ASSERT_EQ(200, sdsmJson["ref_pos_xy_conf"]["semi_minor"].asInt()); @@ -365,8 +365,8 @@ namespace unit_test ASSERT_EQ(13, sdsmJson["ref_pos_el_conf"].asInt()); // Object 1 detected object common data - ASSERT_EQ(1, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["object_type"].asInt()); - ASSERT_EQ(65, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["object_type_conf"].asInt()); + ASSERT_EQ(1, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["obj_type"].asInt()); + ASSERT_EQ(65, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["obj_type_cfd"].asInt()); ASSERT_EQ(1200, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["object_id"].asInt()); ASSERT_EQ(500, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["measurement_time"].asInt()); ASSERT_EQ(8, sdsmJson["objects"][0]["detected_object_data"]["detected_object_common_data"]["time_confidence"].asInt()); @@ -422,12 +422,12 @@ namespace unit_test // Only test the ID and optional obstacle data for the 3rd object ASSERT_EQ(1000, sdsmJson["objects"][2]["detected_object_data"]["detected_object_common_data"]["object_id"].asInt()); - ASSERT_EQ(400, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obst_data"]["obst_size"]["width"].asInt()); - ASSERT_EQ(300, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obst_data"]["obst_size"]["length"].asInt()); - ASSERT_EQ(100, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obst_data"]["obst_size"]["height"].asInt()); - ASSERT_EQ(4, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obst_data"]["obst_size"]["width_confidence"].asInt()); - ASSERT_EQ(5, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obst_data"]["obst_size"]["length_confidence"].asInt()); - ASSERT_EQ(6, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obst_data"]["obst_size"]["height_confidence"].asInt()); + ASSERT_EQ(400, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obstacle_data"]["obst_size"]["width"].asInt()); + ASSERT_EQ(300, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obstacle_data"]["obst_size"]["length"].asInt()); + ASSERT_EQ(100, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obstacle_data"]["obst_size"]["height"].asInt()); + ASSERT_EQ(4, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obstacle_data"]["obst_size_confidence"]["width_confidence"].asInt()); + ASSERT_EQ(5, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obstacle_data"]["obst_size_confidence"]["length_confidence"].asInt()); + ASSERT_EQ(6, sdsmJson["objects"][2]["detected_object_data"]["detected_object_optional_data"]["detected_obstacle_data"]["obst_size_confidence"]["height_confidence"].asInt()); } diff --git a/src/v2i-hub/CARMAStreetsPlugin/test/test_JsonToJ3224SDSMConverter.cpp b/src/v2i-hub/CARMAStreetsPlugin/test/test_JsonToJ3224SDSMConverter.cpp new file mode 100644 index 000000000..5d38d4716 --- /dev/null +++ b/src/v2i-hub/CARMAStreetsPlugin/test/test_JsonToJ3224SDSMConverter.cpp @@ -0,0 +1,188 @@ +#include +#include +#include +#include +#include +#include "jsoncpp/json/json.h" +#include "JsonToJ3224SDSMConverter.h" + +namespace CARMAStreetsPlugin +{ + class test_JsonToJ3224SDSMConverter : public ::testing::Test + { + }; + + // Test for SDSM string json parsing + TEST_F(test_JsonToJ3224SDSMConverter, parseJsonString) + { + // TODO: consider adding functionality for converting to input strings from existing json files + JsonToJ3224SDSMConverter converter; + 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); + 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) + { + 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}}}]}"; + Json::Value root; + bool result = converter.parseJsonString(valid_json_str, root); + ASSERT_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); + + } + + // // Test for SDSM common data + TEST_F(test_JsonToJ3224SDSMConverter, convertJsonToSDSM_common) + { + 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}}}]}"; + Json::Value root; + bool result = converter.parseJsonString(valid_json_str, root); + ASSERT_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); + + ASSERT_EQ(2, sdsmPtr->objects.list.array[0]->detObjCommon.posConfidence.pos); + ASSERT_EQ(5, sdsmPtr->objects.list.array[0]->detObjCommon.posConfidence.elevation); + + 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); + + ASSERT_EQ(16000, sdsmPtr->objects.list.array[0]->detObjCommon.heading); + ASSERT_EQ(4, sdsmPtr->objects.list.array[0]->detObjCommon.headingConf); + + 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); + + } + + // 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,\"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); + 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); + + } + + // 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_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); + 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); + + } + + // 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_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); + 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); + + 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); + + } + +} \ No newline at end of file From a0986356b972c8e4a8c466bc766d33bd4aae0671 Mon Sep 17 00:00:00 2001 From: dan-du-car <62157949+dan-du-car@users.noreply.github.com> Date: Tue, 14 Nov 2023 15:33:44 -0500 Subject: [PATCH 17/28] Rsu health monitor (#564) # PR Details ## Description RSU health monitor plugin is a V2xHub plugin that interface with its connected RSU directly via SNMP protocol. The plugin is responsible for monitoring the connected RSU status by constantly ping RSU device. For detailed design of this plugin, refer to https://usdot-carma.atlassian.net/wiki/spaces/WFD2/pages/2640740360/RSU+Health+Monitor+Plugin+Design . ## Related Issue NA ## Motivation and Context Data visualization ## How Has This Been Tested? Integration test ## Types of changes - [ ] Defect fix (non-breaking change that fixes an issue) - [x] 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. --- .sonarqube/sonar-scanner.properties | 8 +- src/tmx/Messages/include/MessageTypes.h | 1 + src/tmx/Messages/include/RSUStatusMessage.h | 25 ++ src/tmx/TmxUtils/CMakeLists.txt | 12 +- src/tmx/TmxUtils/src/RSU_MIB_4_1.h | 56 +++++ src/tmx/TmxUtils/src/SNMPClient.cpp | 227 ++++++++++++++++++ src/tmx/TmxUtils/src/SNMPClient.h | 113 +++++++++ src/tmx/TmxUtils/src/SNMPClientException.cpp | 8 + src/tmx/TmxUtils/src/SNMPClientException.h | 23 ++ src/tmx/TmxUtils/test/MockSNMPClient.h | 18 ++ src/tmx/TmxUtils/test/test_SNMPClient.cpp | 93 +++++++ .../RSUHealthMonitorPlugin/CMakeLists.txt | 26 ++ .../RSUHealthMonitorPlugin/manifest.json | 51 ++++ .../src/RSUHealthMonitorPlugin.cpp | 90 +++++++ .../src/RSUHealthMonitorPlugin.h | 46 ++++ .../src/RSUHealthMonitorWorker.cpp | 217 +++++++++++++++++ .../src/RSUHealthMonitorWorker.h | 120 +++++++++ .../RSUHealthMonitorPlugin/test/main.cpp | 8 + .../test/test_RSUHealthMonitorWorker.cpp | 108 +++++++++ 19 files changed, 1246 insertions(+), 4 deletions(-) create mode 100644 src/tmx/Messages/include/RSUStatusMessage.h create mode 100644 src/tmx/TmxUtils/src/RSU_MIB_4_1.h create mode 100644 src/tmx/TmxUtils/src/SNMPClient.cpp create mode 100644 src/tmx/TmxUtils/src/SNMPClient.h create mode 100644 src/tmx/TmxUtils/src/SNMPClientException.cpp create mode 100644 src/tmx/TmxUtils/src/SNMPClientException.h create mode 100644 src/tmx/TmxUtils/test/MockSNMPClient.h create mode 100644 src/tmx/TmxUtils/test/test_SNMPClient.cpp create mode 100755 src/v2i-hub/RSUHealthMonitorPlugin/CMakeLists.txt create mode 100755 src/v2i-hub/RSUHealthMonitorPlugin/manifest.json create mode 100755 src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorPlugin.cpp create mode 100755 src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorPlugin.h create mode 100644 src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorWorker.cpp create mode 100644 src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorWorker.h create mode 100644 src/v2i-hub/RSUHealthMonitorPlugin/test/main.cpp create mode 100644 src/v2i-hub/RSUHealthMonitorPlugin/test/test_RSUHealthMonitorWorker.cpp diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties index f6b40e299..d5f118b33 100644 --- a/.sonarqube/sonar-scanner.properties +++ b/.sonarqube/sonar-scanner.properties @@ -55,7 +55,8 @@ sonar.modules= PedestrianPlugin, \ MessageReceiverPlugin, \ CARMAStreetsPlugin, \ ERVCloudForwardingPlugin, \ - CDASimAdapter + CDASimAdapter, \ + RSUHealthMonitorPlugin @@ -82,6 +83,7 @@ TimPlugin.sonar.projectBaseDir =src/v2i-hub/TimPlugin CARMAStreetsPlugin.sonar.projectBaseDir =src/v2i-hub/CARMAStreetsPlugin ERVCloudForwardingPlugin.sonar.projectBaseDir =src/v2i-hub/ERVCloudForwardingPlugin CDASimAdapter.sonar.projectBaseDir =src/v2i-hub/CDASimAdapter +RSUHealthMonitorPlugin.sonar.projectBaseDir =src/v2i-hub/RSUHealthMonitorPlugin @@ -117,7 +119,8 @@ CARMAStreetsPlugin.sonar.exclusions =test/** ERVCloudForwardingPlugin.sonar.sources =src CDASimAdapter.sonar.sources =src CDASimAdapter.sonar.exclusions =test/** - +RSUHealthMonitorPlugin.sonar.sources =src +RSUHealthMonitorPlugin.sonar.exclusions =test/** # Tests # Note: For C++ setting this field does not cause test analysis to occur. It only allows the test source code to be evaluated. @@ -145,3 +148,4 @@ SpatPlugin.sonar.tests=test CARMAStreetsPlugin.sonar.tests=test ERVCloudForwardingPlugin.sonar.tests=test CDASimAdapter.sonar.tests=test +RSUHealthMonitorPlugin.sonar.tests=test diff --git a/src/tmx/Messages/include/MessageTypes.h b/src/tmx/Messages/include/MessageTypes.h index e0fd09c70..3360788fe 100644 --- a/src/tmx/Messages/include/MessageTypes.h +++ b/src/tmx/Messages/include/MessageTypes.h @@ -92,6 +92,7 @@ static CONSTEXPR const char *MSGSUBTYPE_OUTGOING_STRING = "Outgoing"; static CONSTEXPR const char *MSGSUBTYPE_SHUTDOWN_STRING = "Shutdown"; static CONSTEXPR const char *MSGSUBTYPE_TIMESYNC_STRING = "TimeSync"; static CONSTEXPR const char *MSGSUBTYPE_SENSOR_DETECTED_OBJECT_STRING = "SensorDetectedObject"; +static CONSTEXPR const char *MSGSUBTYPE_RSU_STATUS_STRING = "RSUStatus"; } /* End namespace messages */ diff --git a/src/tmx/Messages/include/RSUStatusMessage.h b/src/tmx/Messages/include/RSUStatusMessage.h new file mode 100644 index 000000000..a5e276268 --- /dev/null +++ b/src/tmx/Messages/include/RSUStatusMessage.h @@ -0,0 +1,25 @@ +#pragma once + + +#include +#include "MessageTypes.h" + + +namespace tmx::messages { + + +class RSUStatusMessage : public tmx::message +{ + public: + RSUStatusMessage() {} + + /// Message type for routing this message through TMX core. + static constexpr const char* MessageType = MSGTYPE_APPLICATION_STRING; + + /// Message sub type for routing this message through TMX core. + static constexpr const char* MessageSubType = MSGSUBTYPE_RSU_STATUS_STRING; + }; + +} /* namespace tmx::messages */ + + diff --git a/src/tmx/TmxUtils/CMakeLists.txt b/src/tmx/TmxUtils/CMakeLists.txt index 30c30fb41..ee3c12c5f 100644 --- a/src/tmx/TmxUtils/CMakeLists.txt +++ b/src/tmx/TmxUtils/CMakeLists.txt @@ -1,7 +1,9 @@ PROJECT ( tmxutils CXX ) FILE (GLOB_RECURSE HEADERS "src/" "*.h*") FILE (GLOB_RECURSE SOURCES "src/" "*.c*") - +find_library(NETSNMPAGENT "netsnmpagent") +find_library(NETSNMPMIBS "netsnmpmibs") +find_library(NETSNMP "netsnmp") FIND_PACKAGE (carma-clock) FIND_LIBRARY (UUID_LIBRARY uuid) @@ -16,12 +18,18 @@ TARGET_INCLUDE_DIRECTORIES (${PROJECT_NAME} SYSTEM PUBLIC $ $ ${MYSQL_INCLUDE_DIR} + ${NETSNMP_INCLUDE_DIRS} ${MYSQLCPPCONN_INCLUDE_DIR}) TARGET_LINK_LIBRARIES (${PROJECT_NAME} PUBLIC ${TMXAPI_LIBRARIES} ${MYSQL_LIBRARIES} ${MYSQLCPPCONN_LIBRARIES} ${UUID_LIBRARY} + ${UUID_LIBRARY} + ${NETSNMPAGENT} + ${NETSNMPMIBS} + ${NETSNMP} + ${NETSNMP_LIBRARIES} rdkafka++ ::carma-clock gmock @@ -54,4 +62,4 @@ add_executable(${BINARY} ${TEST_SOURCES}) add_test(NAME ${BINARY} COMMAND ${BINARY}) -target_link_libraries(${BINARY} PUBLIC ${PROJECT_NAME} rdkafka++ gmock ${TMXAPI_LIBRARIES} ${ASN_J2735_LIBRARIES} ${UUID_LIBRARY} gtest) \ No newline at end of file +target_link_libraries(${BINARY} PUBLIC ${PROJECT_NAME} rdkafka++ gmock ${TMXAPI_LIBRARIES} ${ASN_J2735_LIBRARIES} ${UUID_LIBRARY} ${NETSNMPAGENT} ${NETSNMPMIBS} ${NETSNMP} ${NETSNMP_LIBRARIES} gtest) \ No newline at end of file diff --git a/src/tmx/TmxUtils/src/RSU_MIB_4_1.h b/src/tmx/TmxUtils/src/RSU_MIB_4_1.h new file mode 100644 index 000000000..793bf4bea --- /dev/null +++ b/src/tmx/TmxUtils/src/RSU_MIB_4_1.h @@ -0,0 +1,56 @@ +#pragma once +namespace tmx::utils::rsu41::mib::oid +{ + /** + * @brief This header file contains a subset of RSU MIB definition from https://github.com/certificationoperatingcouncil/COC_TestSpecs/blob/master/AppNotes/RSU/RSU-MIB.txt + */ + // Contains the ID given to this RSU. + static constexpr const char *RSU_ID_OID = "1.0.15628.4.1.17.4.0"; + + // Contains the version of this MIB supported by this RSU, e.g. rsuMIB 4.1 rev201812060000Z + static constexpr const char *RSU_MIB_VERSION = "1.0.15628.4.1.17.1.0"; + + // Contains the version of firmware running on this RSU. + static constexpr const char *RSU_FIRMWARE_VERSION = "1.0.15628.4.1.17.2.0"; + + // Contains the name of the manufacturer of this RSU. + static constexpr const char *RSU_MANUFACTURER = "1.0.15628.4.1.17.5.0"; + + // Contains GPS NMEA GPGGA output string. + static constexpr const char *RSU_GPS_OUTPUT_STRING = "1.0.15628.4.1.8.5.0"; + + // Immediate Forward Message Index + static constexpr const char *RSU_IFM_INDEX = "1.0.15628.4.1.5.1.1.0"; + + // Immediate Forward Message PSID. + static constexpr const char *RSU_IFM_PSID = "1.0.15628.4.1.5.1.2.0"; + + // Immediate Forward Message DSRC Message ID + static constexpr const char *RSU_IFM_DSRC_MSG_ID = "1.0.15628.4.1.5.1.3.0"; + + // Immediate Forward Message Transmit Mode + static constexpr const char *RSU_IFM_TX_MODE = "1.0.15628.4.1.5.1.4.0"; + + // DSRC channel set for Immediate Forward Message transmit + static constexpr const char *RSU_IFM_TX_CHANNEL = "1.0.15628.4.1.5.1.5.0"; + + // Set this bit to enable transmission of the message 0=off, 1=on + static constexpr const char *RSU_IFM_ENABLE = "1.0.15628.4.1.5.1.6.0"; + + // Create (4) or Destroy (6) row entry + static constexpr const char *RSU_IFM_STATUS = "1.0.15628.4.1.5.1.7.0"; + + // Specifies the current mode of operation of the RSU and provides capability to transition the device into a new mode, e.g. from the current mode to off, etc + static constexpr const char *RSU_MODE = "1.0.15628.4.1.99.0"; + + /* + SYNTAX INTEGER { + bothOp (0), --both Continuous and Alternating modes are operational + altOp (1), --Alternating mode is operational, + --Continuous mode is not operational + contOp (2), --Continuous mode is operational, + --Alternating mode is not operational + noneOp (3) --neither Continuous nor Alternating mode is operational + */ + static constexpr const char *RSU_CHAN_STATUS = "1.0.15628.4.1.19.1.0"; +} \ No newline at end of file diff --git a/src/tmx/TmxUtils/src/SNMPClient.cpp b/src/tmx/TmxUtils/src/SNMPClient.cpp new file mode 100644 index 000000000..e3398ebbd --- /dev/null +++ b/src/tmx/TmxUtils/src/SNMPClient.cpp @@ -0,0 +1,227 @@ +#include "SNMPClient.h" + +namespace tmx::utils +{ + + // Client defaults to SNMPv3 + snmp_client::snmp_client(const std::string &ip, const int &port, const std::string &community, + const std::string &snmp_user, const std::string &securityLevel, const std::string &authPassPhrase, int snmp_version, int timeout) + + : ip_(ip), port_(port), community_(community), snmp_version_(snmp_version), timeout_(timeout) + { + + PLOG(logDEBUG1) << "Starting SNMP Client. Target device IP address: " << ip_ << ", Target device SNMP port: " << port_; + + // Bring the IP address and port of the target SNMP device in the required form, which is "IPADDRESS:PORT": + std::string ip_port_string = ip_ + ":" + std::to_string(port_); + char *ip_port = &ip_port_string[0]; + + // Initialize SNMP session parameters + init_snmp("carma_snmp"); + snmp_sess_init(&session); + session.peername = ip_port; + session.version = snmp_version_; // SNMP_VERSION_3 + session.securityName = (char *)snmp_user.c_str(); + session.securityNameLen = snmp_user.length(); + + // Fallback behavior to setup a community for SNMP V1/V2 + if (snmp_version_ != SNMP_VERSION_3) + { + session.community = (unsigned char *)community.c_str(); + session.community_len = community_.length(); + } + + // SNMP authorization/privach config + if (securityLevel == "authPriv") + { + session.securityLevel = SNMP_SEC_LEVEL_AUTHPRIV; + } + + else if (securityLevel == "authNoPriv") + { + session.securityLevel = SNMP_SEC_LEVEL_AUTHNOPRIV; + } + + else + session.securityLevel = SNMP_SEC_LEVEL_NOAUTH; + + // Passphrase used for both authentication and privacy + auto phrase_len = authPassPhrase.length(); + auto phrase = (u_char *)authPassPhrase.c_str(); + + // Defining and generating auth config with SHA1 + session.securityAuthProto = snmp_duplicate_objid(usmHMACSHA1AuthProtocol, USM_AUTH_PROTO_SHA_LEN); + session.securityAuthProtoLen = USM_AUTH_PROTO_SHA_LEN; + session.securityAuthKeyLen = USM_AUTH_KU_LEN; + if (session.securityLevel != SNMP_SEC_LEVEL_NOAUTH && generate_Ku(session.securityAuthProto, + session.securityAuthProtoLen, + phrase, phrase_len, + session.securityAuthKey, + &session.securityAuthKeyLen) != SNMPERR_SUCCESS) + { + std::string errMsg = "Error generating Ku from authentication pass phrase. \n"; + throw snmp_client_exception(errMsg); + } + + // Defining and generating priv config with AES (since using SHA1) + session.securityPrivKeyLen = USM_PRIV_KU_LEN; + session.securityPrivProto = + snmp_duplicate_objid(usmAESPrivProtocol, + OID_LENGTH(usmAESPrivProtocol)); + session.securityPrivProtoLen = OID_LENGTH(usmAESPrivProtocol); + + if (session.securityLevel == SNMP_SEC_LEVEL_AUTHPRIV && generate_Ku(session.securityAuthProto, + session.securityAuthProtoLen, + phrase, phrase_len, + session.securityPrivKey, + &session.securityPrivKeyLen) != SNMPERR_SUCCESS) + { + std::string errMsg = "Error generating Ku from privacy pass phrase. \n"; + throw snmp_client_exception(errMsg); + } + + session.timeout = timeout_; + + // Opens the snmp session if it exists + ss = snmp_open(&session); + + if (ss == nullptr) + { + PLOG(logERROR) << "Failed to establish session with target device"; + snmp_sess_perror("snmpget", &session); + throw snmp_client_exception("Failed to establish session with target device"); + } + else + { + PLOG(logINFO) << "Established session with device at " << ip_; + } + } + + snmp_client::~snmp_client() + { + PLOG(logINFO) << "Closing SNMP session"; + snmp_close(ss); + } + + // Original implementation used in Carma Streets https://github.com/usdot-fhwa-stol/snmp-client + bool snmp_client::process_snmp_request(const std::string &input_oid, const request_type &request_type, snmp_response_obj &val) + { + + /*Structure to hold response from the remote host*/ + snmp_pdu *response; + + // Create pdu for the data + if (request_type == request_type::GET) + { + PLOG(logDEBUG1) << "Attempting to GET value for: " << input_oid; + pdu = snmp_pdu_create(SNMP_MSG_GET); + } + else if (request_type == request_type::SET) + { + PLOG(logDEBUG1) << "Attempting to SET value for " << input_oid << " to " << val.val_int; + pdu = snmp_pdu_create(SNMP_MSG_SET); + } + else + { + PLOG(logERROR) << "Invalid request type, method accpets only GET and SET"; + return false; + } + + // Read input OID into an OID variable: + // net-snmp has several methods for creating an OID object + // their documentation suggests using get_node. read_objid seems like a simpler approach + // TO DO: investigate update to get_node + if (!read_objid(input_oid.c_str(), OID, &OID_len)) + { + // If oid cannot be created + PLOG(logERROR) << "OID could not be created from input: " << input_oid; + return false; + } + else + { + + if (request_type == request_type::GET) + { + // Add OID to pdu for get request + snmp_add_null_var(pdu, OID, OID_len); + } + else if (request_type == request_type::SET) + { + if (val.type == snmp_response_obj::response_type::INTEGER) + { + snmp_add_var(pdu, OID, OID_len, 'i', (std::to_string(val.val_int)).c_str()); + } + // Needs to be finalized to support octet string use + else if (val.type == snmp_response_obj::response_type::STRING) + { + PLOG(logERROR) << "Setting string value is currently not supported"; + } + } + + PLOG(logINFO) << "Created OID for input: " << input_oid; + } + // Send the request + int status = snmp_synch_response(ss, pdu, &response); + PLOG(logINFO) << "Response request status: " << status << " (=" << (status == STAT_SUCCESS ? "SUCCESS" : "FAILED") << ")"; + + // Check GET response + if (status == STAT_SUCCESS && response && response->errstat == SNMP_ERR_NOERROR && request_type == request_type::GET) + { + for (auto vars = response->variables; vars; vars = vars->next_variable) + { + // Get value of variable depending on ASN.1 type + // Variable could be a integer, string, bitstring, ojbid, counter : defined here https://github.com/net-snmp/net-snmp/blob/master/include/net-snmp/types.h + // get Integer value + if (vars->type == ASN_INTEGER && vars->val.integer) + { + val.type = snmp_response_obj::response_type::INTEGER; + val.val_int = *vars->val.integer; + } + else if (vars->type == ASN_OCTET_STR && vars->val.string) + { + size_t str_len = vars->val_len; + for (size_t i = 0; i < str_len; ++i) + { + val.val_string.push_back(vars->val.string[i]); + } + val.type = snmp_response_obj::response_type::STRING; + } + } + } + else + { + log_error(status, request_type, response); + return false; + } + + if (response) + { + snmp_free_pdu(response); + OID_len = MAX_OID_LEN; + } + + return true; + } + + int snmp_client::get_port() const + { + return port_; + } + + void snmp_client::log_error(const int &status, const request_type &request_type, const snmp_pdu *response) const + { + + if (status == STAT_SUCCESS) + { + PLOG(logERROR) << "Variable type: " << response->variables->type << ". Error in packet " << static_cast(snmp_errstring(static_cast(response->errstat))); + } + else if (status == STAT_TIMEOUT) + { + PLOG(logERROR) << "Timeout, no response from server"; + } + else + { + PLOG(logERROR) << "Unknown SNMP Error for " << (request_type == request_type::GET ? "GET" : "SET"); + } + } +} // namespace \ No newline at end of file diff --git a/src/tmx/TmxUtils/src/SNMPClient.h b/src/tmx/TmxUtils/src/SNMPClient.h new file mode 100644 index 000000000..75eaa9360 --- /dev/null +++ b/src/tmx/TmxUtils/src/SNMPClient.h @@ -0,0 +1,113 @@ +#pragma once + +#include +#include +#include +#include "PluginLog.h" + +#include "SNMPClientException.h" + +namespace tmx::utils +{ + + enum class request_type + { + GET, + SET, + OTHER // Processing this request type is not a defined behavior, included for testing only + }; + + /** @brief A struct to hold the value being sent to the TSC, can be integer or string. Type needs to be defined*/ + struct snmp_response_obj + { + /** @brief The type of value being requested or set, on the TSC */ + enum class response_type + { + INTEGER, + STRING + }; + + // snmp response values can be any asn.1 supported types. + // Integer and string values can be processed here + int64_t val_int = 0; + std::vector val_string; + response_type type; + + inline bool operator==(const snmp_response_obj &obj2) const + { + return val_int == obj2.val_int && val_string == obj2.val_string && type == obj2.type; + } + }; + + class snmp_client + { + private: + /*variables to store an snmp session*/ + // struct that holds information about who we're going to be talking to + // We need to declare 2 of these, one to fill info with and second which is + // a pointer returned by the library + snmp_session session; + snmp_session *ss; + + /*Structure to hold all of the information that we're going to send to the remote host*/ + snmp_pdu *pdu; + + /*OID is going to hold the location of the information which we want to receive. It will need a size as well*/ + oid OID[MAX_OID_LEN]; + size_t OID_len = MAX_OID_LEN; + + // Values from config + /*Target device IP address*/ + std::string ip_; + /*Target device NTCIP port*/ + int port_ = 0; + /*Target community for establishing snmp communication*/ + std::string community_ = "public"; + /* net-snmp version definition: SNMP_VERSION_1:0 SNMP_VERSION_2c:1 SNMP_VERSION_2u:2 SNMP_VERSION_3:3 + https://github.com/net-snmp/net-snmp/blob/master/include/net-snmp/library/snmp.h */ + int snmp_version_ = 3; // default to 3 since previous versions not compatable currently + /*Time after which the the snmp request times out*/ + int timeout_ = 10000; + + public: + /** @brief Constructor for Traffic Signal Controller Service client. + * Uses the arguments provided to establish an snmp connection + * @param ip The ip ,as a string, for the tsc_client_service to establish an snmp communication with. + * @param port Target port as integer on the host for snmp communication. + * @param community The community id as a string. Defaults to "public" if unassigned. + * @param snmp_version The snmp_version as defined in net-snmp.Default to 0 if unassigned. + * net-snmp version definition: SNMP_VERSION_1:0 SNMP_VERSION_2c:1 SNMP_VERSION_2u:2 SNMP_VERSION_3:3" + * @param timeout The time in microseconds after which an snmp session request expires. Defaults to 100 if unassigned + * **/ + snmp_client(const std::string &ip, const int &port, const std::string &community, const std::string &snmp_user, const std::string &securityLevel, const std::string &authPassPhrase, int snmp_version = 0, int timeout = 100); + + /* Disable default copy constructor*/ + snmp_client(snmp_client &sc) = delete; + + /* Disable default move constructor*/ + snmp_client(snmp_client &&sc) = delete; + + /** @brief Returns true or false depending on whether the request could be processed for given input OID at the Traffic Signal Controller. + * @param input_oid The OID to request information for. + * @param request_type The request type for which the error is being logged. Accepted values are "GET" and "SET" only. + * @param value_int The integer value for the object returned by reference. For "SET" it is the value to be set. + * For "GET", it is the value returned for the returned object by reference. + * This is an optional argument, if not provided, defaults to 0. + * @param value_str String value for the object, returned by reference. Optional argument, if not provided the value is set as an empty string + * @return Integer value at the oid, returns false if value cannot be set/requested or oid doesn't have an integer value to return.*/ + + virtual bool process_snmp_request(const std::string &input_oid, const request_type &request_type, snmp_response_obj &val); + /** @brief Finds error type from status and logs an error. + * @param status The integer value corresponding to net-snmp defined errors. macros considered are STAT_SUCCESS(0) and STAT_TIMEOUT(2) + * @param request_type The request type for which the error is being logged (GET/SET). + * @param response The snmp_pdu struct */ + + virtual int get_port() const; // Returns the current port (should always be 161 or 162) + + void log_error(const int &status, const request_type &request_type, const snmp_pdu *response) const; + + /** @brief Destructor for client. Closes the snmp session**/ + virtual ~snmp_client(); + }; + +} // namespace \ No newline at end of file diff --git a/src/tmx/TmxUtils/src/SNMPClientException.cpp b/src/tmx/TmxUtils/src/SNMPClientException.cpp new file mode 100644 index 000000000..67f82fabe --- /dev/null +++ b/src/tmx/TmxUtils/src/SNMPClientException.cpp @@ -0,0 +1,8 @@ +#include "SNMPClientException.h" + +namespace tmx::utils { + + snmp_client_exception::snmp_client_exception(const std::string &msg): std::runtime_error(msg){}; + + snmp_client_exception::~snmp_client_exception() = default; +} \ No newline at end of file diff --git a/src/tmx/TmxUtils/src/SNMPClientException.h b/src/tmx/TmxUtils/src/SNMPClientException.h new file mode 100644 index 000000000..76020a5b1 --- /dev/null +++ b/src/tmx/TmxUtils/src/SNMPClientException.h @@ -0,0 +1,23 @@ +#pragma once + +#include + +namespace tmx::utils { + /** + * @brief Runtime error related to SNMP client used to communicate with Traffic Signal Controller (NTCIP). + * + * @author Paul Bourelly + */ + class snmp_client_exception : public std::runtime_error{ + public: + /** + * @brief Destructor. + */ + ~snmp_client_exception() override; + /** + * @brief Constructor. + * @param msg String exception message. + */ + explicit snmp_client_exception(const std::string &msg ); + }; +} \ No newline at end of file diff --git a/src/tmx/TmxUtils/test/MockSNMPClient.h b/src/tmx/TmxUtils/test/MockSNMPClient.h new file mode 100644 index 000000000..7e07fe47f --- /dev/null +++ b/src/tmx/TmxUtils/test/MockSNMPClient.h @@ -0,0 +1,18 @@ +#pragma once +#include "SNMPClient.h" +#include +#include + +using namespace tmx::utils; +using namespace std; + +namespace unit_test +{ + class mock_snmp_client : public snmp_client + { + public: + mock_snmp_client(const std::string &ip, const int &port, const std::string &community, const std::string &snmp_user, const std::string &securityLevel, const std::string &authPassPhrase, int snmp_version = 0, int timeout = 100) : snmp_client(ip, port, community, snmp_user, securityLevel, authPassPhrase, snmp_version, timeout){}; + ~mock_snmp_client() = default; + MOCK_METHOD(bool, process_snmp_request, (const std::string &input_oid, const request_type &request_type, snmp_response_obj &val), (override)); + }; +} \ No newline at end of file diff --git a/src/tmx/TmxUtils/test/test_SNMPClient.cpp b/src/tmx/TmxUtils/test/test_SNMPClient.cpp new file mode 100644 index 000000000..3e775945c --- /dev/null +++ b/src/tmx/TmxUtils/test/test_SNMPClient.cpp @@ -0,0 +1,93 @@ + +#include "MockSNMPClient.h" +#include "gtest/gtest.h" +#include "RSU_MIB_4_1.h" + +using namespace tmx::utils; +using namespace std; +using namespace tmx::utils::rsu41::mib::oid; +using testing::_; +using testing::Action; +using testing::DoDefault; +using testing::Return; +using testing::SetArgReferee; +using testing::Throw; + +namespace unit_test +{ + class test_SNMPClient : public ::testing::Test + { + public: + shared_ptr scPtr; + uint16_t port = 161; + test_SNMPClient() + { + scPtr = make_shared("127.0.0.1", port, "public", "test", "authPriv", "testtesttest", SNMP_VERSION_3, 1000); + } + }; + + TEST_F(test_SNMPClient, constructor_error) + { + ASSERT_THROW(snmp_client("127.0.0.1", port, "public", "test", "authPriv", "test", SNMP_VERSION_3, 1000), snmp_client_exception); + ASSERT_NO_THROW(snmp_client("127.0.0.1", port, "public", "test", "authPriv", "testtesttest", SNMP_VERSION_3, 1000)); + ASSERT_NO_THROW(snmp_client("127.0.0.1", port, "public", "test", "authNoPriv", "testtesttest", SNMP_VERSION_3, 1000)); + ASSERT_NO_THROW(snmp_client("127.0.0.1", port, "public", "test", "authNoPriv", "testtesttest", SNMP_VERSION_1, 1000)); + ASSERT_NO_THROW(snmp_client("127.0.0.1", port, "public", "test", "", "testtesttest", SNMP_VERSION_3, 1000)); + ASSERT_THROW(snmp_client("127.0.XX.XX", port, "public", "test", "", "testtesttest", SNMP_VERSION_3, 1000), snmp_client_exception); + } + + TEST_F(test_SNMPClient, get_port) + { + ASSERT_EQ(161, scPtr->get_port()); + } + + TEST_F(test_SNMPClient, log_error) + { + snmp_pdu response; + ASSERT_NO_THROW(scPtr->log_error(STAT_ERROR, request_type::GET, &response)); + ASSERT_NO_THROW(scPtr->log_error(STAT_ERROR, request_type::SET, &response)); + ASSERT_NO_THROW(scPtr->log_error(STAT_SUCCESS, request_type::OTHER, &response)); + ASSERT_NO_THROW(scPtr->log_error(STAT_TIMEOUT, request_type::OTHER, &response)); + } + + TEST_F(test_SNMPClient, process_snmp_request) + { + snmp_response_obj reqponseRSUID; + string rsuId = "RSU4.1"; + vector rsuId_c; + copy(rsuId.begin(), rsuId.end(), back_inserter(rsuId_c)); + reqponseRSUID.val_string = rsuId_c; + reqponseRSUID.type = snmp_response_obj::response_type::STRING; + EXPECT_CALL(*scPtr, process_snmp_request(RSU_ID_OID, request_type::GET, _)).WillRepeatedly(testing::DoAll(SetArgReferee<2>(reqponseRSUID), Return(true))); + EXPECT_CALL(*scPtr, process_snmp_request(RSU_ID_OID, request_type::SET, _)).WillRepeatedly(testing::DoAll(SetArgReferee<2>(reqponseRSUID), Return(true))); + + snmp_response_obj reqponseMode; + reqponseMode.val_int = 2; + reqponseMode.type = snmp_response_obj::response_type::INTEGER; + EXPECT_CALL(*scPtr, process_snmp_request(RSU_MODE, request_type::GET, _)).WillRepeatedly(testing::DoAll(SetArgReferee<2>(reqponseMode), Return(true))); + EXPECT_CALL(*scPtr, process_snmp_request(RSU_MODE, request_type::SET, _)).WillRepeatedly(testing::DoAll(SetArgReferee<2>(reqponseRSUID), Return(true))); + + snmp_response_obj reqponseInvalidOID; + EXPECT_CALL(*scPtr, process_snmp_request("Invalid OID", request_type::GET, _)).WillRepeatedly(testing::DoAll(SetArgReferee<2>(reqponseInvalidOID), Return(false))); + EXPECT_CALL(*scPtr, process_snmp_request("Invalid OID", request_type::SET, _)).WillRepeatedly(testing::DoAll(SetArgReferee<2>(reqponseInvalidOID), Return(false))); + + snmp_response_obj response; + scPtr->process_snmp_request(RSU_ID_OID, request_type::GET, response); + scPtr->process_snmp_request(RSU_ID_OID, request_type::SET, response); + scPtr->process_snmp_request(RSU_MODE, request_type::GET, response); + scPtr->process_snmp_request(RSU_MODE, request_type::SET, response); + scPtr->process_snmp_request("Invalid OID", request_type::GET, response); + scPtr->process_snmp_request("Invalid OID", request_type::SET, response); + + snmp_client scClient("127.0.0.1", port, "public", "test", "authPriv", "testtesttest", SNMP_VERSION_3, 1000); + scClient.process_snmp_request(RSU_ID_OID, request_type::GET, reqponseRSUID); + scClient.process_snmp_request(RSU_ID_OID, request_type::SET, reqponseRSUID); + scClient.process_snmp_request(RSU_ID_OID, request_type::OTHER, reqponseRSUID); + scClient.process_snmp_request("INVALID OID", request_type::GET, reqponseRSUID); + + scClient.process_snmp_request(RSU_MODE, request_type::GET, reqponseMode); + scClient.process_snmp_request(RSU_MODE, request_type::SET, reqponseMode); + scClient.process_snmp_request(RSU_MODE, request_type::OTHER, reqponseMode); + } + +} \ No newline at end of file diff --git a/src/v2i-hub/RSUHealthMonitorPlugin/CMakeLists.txt b/src/v2i-hub/RSUHealthMonitorPlugin/CMakeLists.txt new file mode 100755 index 000000000..d863a4bf1 --- /dev/null +++ b/src/v2i-hub/RSUHealthMonitorPlugin/CMakeLists.txt @@ -0,0 +1,26 @@ +PROJECT(RSUHealthMonitorPlugin VERSION 7.5.1 LANGUAGES CXX) + +set(TMX_PLUGIN_NAME "RSU Health Monitor") + +find_library(libasn1c .) + +BuildTmxPlugin() + +TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC tmxutils jsoncpp NemaTode) + +############# +## Testing ## +############# +enable_testing() +include_directories(${PROJECT_SOURCE_DIR}/src) +add_library(${PROJECT_NAME}_lib src/RSUHealthMonitorWorker.cpp) +target_link_libraries(${PROJECT_NAME}_lib PUBLIC + tmxutils + NemaTode + jsoncpp) +set(BINARY ${PROJECT_NAME}_test) +file(GLOB_RECURSE TEST_SOURCES LIST_DIRECTORIES false test/*.h test/*.cpp) +set(SOURCES ${TEST_SOURCES} WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) +add_executable(${BINARY} ${TEST_SOURCES}) +add_test(NAME ${BINARY} COMMAND ${BINARY}) +target_link_libraries(${BINARY} PUBLIC ${PROJECT_NAME}_lib gtest) \ No newline at end of file diff --git a/src/v2i-hub/RSUHealthMonitorPlugin/manifest.json b/src/v2i-hub/RSUHealthMonitorPlugin/manifest.json new file mode 100755 index 000000000..f2ee94443 --- /dev/null +++ b/src/v2i-hub/RSUHealthMonitorPlugin/manifest.json @@ -0,0 +1,51 @@ +{ + "name": "RSUHealthMonitor", + "description": "Monitor RSU health status", + "version": "@PROJECT_VERSION@", + "exeLocation": "/bin/RSUHealthMonitorPlugin", + "coreIpAddr":"127.0.0.1", + "corePort":24601, + "messageTypes": [], + "configuration": [ + { + "key": "LogLevel", + "default": "INFO", + "description": "The log level for this plugin" + }, + { + "key":"Interval", + "default":"1", + "description": "Sending RSU SNMP GET request at every configured interval. Default every 1 second. Unit of measure: second." + }, + { + "key":"RSUIp", + "default":"192.168.XX.XX", + "description":"An IP address of the RSU the V2X hub is connected to." + }, + { + "key":"SNMPPort", + "default":"161", + "description":"The SNMP port for sending message or command." + }, + { + "key":"AuthPassPhrase", + "default":"dummy", + "description":"SNMP v3 authentication passphrase" + }, + { + "key":"SecurityUser", + "default":"authOnlyUser", + "description":"SNMP Security Name" + }, + { + "key":"SecurityLevel", + "default":"authPriv", + "description":"SNMP Security level" + }, + { + "key":"RSUMIBVersion", + "default":"RSU4.1", + "description":"The version of RSU MIB (Management Information Base). E.G. RSU4.1 or RSU1218. Currently only support RSU4.1" + } + ] +} \ No newline at end of file diff --git a/src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorPlugin.cpp b/src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorPlugin.cpp new file mode 100755 index 000000000..8d81a3b6c --- /dev/null +++ b/src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorPlugin.cpp @@ -0,0 +1,90 @@ +#include "RSUHealthMonitorPlugin.h" + +using namespace RSUHealthMonitor; +using namespace tmx::utils; + +namespace RSUHealthMonitor +{ + + RSUHealthMonitorPlugin::RSUHealthMonitorPlugin(const std::string &name) : PluginClient(name) + { + _rsuWorker = std::make_shared(); + _rsuStatusTimer = make_unique(); + UpdateConfigSettings(); + + // Send SNMP call to RSU periodically at configurable interval. + _timerThId = _rsuStatusTimer->AddPeriodicTick([this]() + { + // Periodic SNMP call to get RSU status based on RSU MIB version 4.1 + auto rsuStatusJson = _rsuWorker->getRSUStatus(_rsuMibVersion, _rsuIp, _snmpPort, _securityUser, _authPassPhrase, _securityLevel, SEC_TO_MICRO); + PLOG(logINFO) << "Updating _interval: " << _interval; + //Broadcast RSU status periodically at _interval + BroadcastRSUStatus(rsuStatusJson); }, + std::chrono::milliseconds(_interval * SEC_TO_MILLI)); + _rsuStatusTimer->Start(); + } + + void RSUHealthMonitorPlugin::UpdateConfigSettings() + { + PLOG(logINFO) << "Updating configuration settings."; + + lock_guard lock(_configMutex); + GetConfigValue("Interval", _interval); + GetConfigValue("RSUIp", _rsuIp); + GetConfigValue("SNMPPort", _snmpPort); + GetConfigValue("AuthPassPhrase", _authPassPhrase); + GetConfigValue("SecurityUser", _securityUser); + GetConfigValue("SecurityLevel", _securityLevel); + GetConfigValue("RSUMIBVersion", _rsuMIBVersionStr); + boost::trim_left(_rsuMIBVersionStr); + boost::trim_right(_rsuMIBVersionStr); + // Support RSU MIB version 4.1 + if (boost::iequals(_rsuMIBVersionStr, RSU4_1_str)) + { + _rsuMibVersion = RSUMibVersion::RSUMIB_V_4_1; + } + else + { + _rsuMibVersion = RSUMibVersion::UNKOWN_MIB_V; + PLOG(logERROR) << "Uknown RSU MIB version: " << _rsuMIBVersionStr; + } + + try + { + _rsuStatusTimer->ChangeFrequency(_timerThId, std::chrono::milliseconds(_interval * SEC_TO_MILLI)); + } + catch (const tmx::TmxException &ex) + { + PLOG(logERROR) << ex.what(); + } + } + + void RSUHealthMonitorPlugin::OnConfigChanged(const char *key, const char *value) + { + PluginClient::OnConfigChanged(key, value); + UpdateConfigSettings(); + } + + void RSUHealthMonitorPlugin::BroadcastRSUStatus(const Json::Value &rsuStatusJson) + { + // Broadcast the RSU status info when there are RSU responses. + if (!rsuStatusJson.empty() && _rsuWorker) + { + auto rsuStatusFields = _rsuWorker->getJsonKeys(rsuStatusJson); + auto configTbl = _rsuWorker->GetRSUStatusConfig(_rsuMibVersion); + + // Only broadcast RSU status when all required fields are present. + if (_rsuWorker->validateAllRequiredFieldsPresent(configTbl, rsuStatusFields)) + { + auto sendRsuStatusMsg = _rsuWorker->convertJsonToTMXMsg(rsuStatusJson); + BroadcastMessage(sendRsuStatusMsg, RSUHealthMonitorPlugin::GetName()); + } + } + } + +} // namespace RSUHealthMonitor + +int main(int argc, char *argv[]) +{ + return run_plugin("RSU Health Monitor", argc, argv); +} diff --git a/src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorPlugin.h b/src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorPlugin.h new file mode 100755 index 000000000..47d8ee1bd --- /dev/null +++ b/src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorPlugin.h @@ -0,0 +1,46 @@ + +#pragma once + +#include "PluginClient.h" +#include +#include "RSUStatusMessage.h" +#include "RSUHealthMonitorWorker.h" + +using namespace tmx::utils; +using namespace std; + +namespace RSUHealthMonitor +{ + + class RSUHealthMonitorPlugin : public PluginClient + { + private: + mutex _configMutex; + uint16_t _interval; + string _rsuIp; + uint16_t _snmpPort; + string _authPassPhrase; + string _securityUser; + string _securityLevel; + string _rsuMIBVersionStr; + RSUMibVersion _rsuMibVersion; + const char *RSU4_1_str = "RSU4.1"; + const char *RSU1218_str = "RSU1218"; + shared_ptr _rsuWorker; + unique_ptr _rsuStatusTimer; + uint _timerThId; + const long SEC_TO_MICRO = 1000000; + const long SEC_TO_MILLI= 1000; + /** + * @brief Broadcast RSU status + * @param Json::Value RSU status in JSON format + */ + void BroadcastRSUStatus(const Json::Value& rsuStatusJson); + + public: + explicit RSUHealthMonitorPlugin(const std::string &name); + void UpdateConfigSettings(); + void OnConfigChanged(const char *key, const char *value) override; + }; + +} // namespace RSUHealthMonitorPlugin \ No newline at end of file diff --git a/src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorWorker.cpp b/src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorWorker.cpp new file mode 100644 index 000000000..6de25d57e --- /dev/null +++ b/src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorWorker.cpp @@ -0,0 +1,217 @@ +#include "RSUHealthMonitorWorker.h" + +namespace RSUHealthMonitor +{ + + RSUHealthMonitorWorker::RSUHealthMonitorWorker() + { + _RSUSTATUSConfigMapPtr = make_shared>(); + // Currently only support RSU MIB version 4.1. Other future supported versions will be inserted here. + RSUStatusConfigTable rsuRstatusTable = constructRsuStatusConfigTable(RSUMibVersion::RSUMIB_V_4_1); + _RSUSTATUSConfigMapPtr->insert({RSUMibVersion::RSUMIB_V_4_1, rsuRstatusTable}); + } + + RSUStatusConfigTable RSUHealthMonitorWorker::constructRsuStatusConfigTable(const RSUMibVersion &mibVersion) const + { + RSUStatusConfigTable rsuStatusTbl; + // Populate custom defined RSU Status table with RSU MIB version 4.1. + if (mibVersion == RSUMibVersion::RSUMIB_V_4_1) + { + RSUFieldOIDStruct rsuID = {"rsuID", RSU_ID_OID, true}; + rsuStatusTbl.push_back(rsuID); + + RSUFieldOIDStruct rsuMibVersion = {"rsuMibVersion", RSU_MIB_VERSION, true}; + rsuStatusTbl.push_back(rsuMibVersion); + + RSUFieldOIDStruct rsuFirmwareVersion = {"rsuFirmwareVersion", RSU_FIRMWARE_VERSION, true}; + rsuStatusTbl.push_back(rsuFirmwareVersion); + + RSUFieldOIDStruct rsuManufacturer = {"rsuManufacturer", RSU_MANUFACTURER, true}; + rsuStatusTbl.push_back(rsuManufacturer); + + RSUFieldOIDStruct rsuGpsOutputString = {"rsuGpsOutputString", RSU_GPS_OUTPUT_STRING, true}; + rsuStatusTbl.push_back(rsuGpsOutputString); + + RSUFieldOIDStruct rsuIFMIndex = {"rsuIFMIndex", RSU_IFM_INDEX, false}; + rsuStatusTbl.push_back(rsuIFMIndex); + + RSUFieldOIDStruct rsuIFMPsid = {"rsuIFMPsid", RSU_IFM_PSID, false}; + rsuStatusTbl.push_back(rsuIFMPsid); + + RSUFieldOIDStruct rsuIFMDsrcMsgId = {"rsuIFMDsrcMsgId", RSU_IFM_DSRC_MSG_ID, false}; + rsuStatusTbl.push_back(rsuIFMDsrcMsgId); + + RSUFieldOIDStruct rsuIFMTxMode = {"rsuIFMTxMode", RSU_IFM_INDEX, false}; + rsuStatusTbl.push_back(rsuIFMTxMode); + + RSUFieldOIDStruct rsuIFMTxChannel = {"rsuIFMTxChannel", RSU_IFM_TX_CHANNEL, false}; + rsuStatusTbl.push_back(rsuIFMTxChannel); + + RSUFieldOIDStruct rsuIFMEnable = {"rsuIFMEnable", RSU_IFM_ENABLE, false}; + rsuStatusTbl.push_back(rsuIFMEnable); + + RSUFieldOIDStruct rsuIFMStatus = {"rsuIFMStatus", RSU_IFM_STATUS, false}; + rsuStatusTbl.push_back(rsuIFMStatus); + + RSUFieldOIDStruct rsuMode = {"rsuMode", RSU_MODE, true}; + rsuStatusTbl.push_back(rsuMode); + + RSUFieldOIDStruct rsuChanStatus = {"rsuChanStatus", RSU_CHAN_STATUS, true}; + rsuStatusTbl.push_back(rsuChanStatus); + } + return rsuStatusTbl; + } + + bool RSUHealthMonitorWorker::validateAllRequiredFieldsPresent(const RSUHealthMonitor::RSUStatusConfigTable &configTbl, const vector &fields) const + { + bool isAllPresent = true; + for (const auto &config : configTbl) + { + if (config.required && find(fields.begin(), fields.end(), config.field) == fields.end()) + { + isAllPresent = false; + PLOG(logWARNING) << "No broadcast as required field " << config.field << " is not present!"; + } + } + return isAllPresent; + } + + RSUStatusConfigTable RSUHealthMonitorWorker::GetRSUStatusConfig(const RSUMibVersion &mibVersion) const + { + RSUStatusConfigTable result; + try + { + result = _RSUSTATUSConfigMapPtr->at(mibVersion); + } + catch (const out_of_range &ex) + { + PLOG(logERROR) << "Unknown MIB version! " << ex.what(); + } + return result; + } + + std::map RSUHealthMonitorWorker::ParseRSUGPS(const std::string &gps_nmea_data) const + { + std::map result; + nmea::NMEAParser parser; + nmea::GPSService gps(parser); + try + { + parser.readLine(gps_nmea_data); + std::stringstream ss; + ss << std::setprecision(8) << std::fixed << gps.fix.latitude << std::endl; + auto latitude_str = ss.str(); + std::stringstream sss; + sss << std::setprecision(8) << std::fixed << gps.fix.longitude << std::endl; + auto longitude_str = sss.str(); + result.insert({std::stod(latitude_str), std::stod(longitude_str)}); + PLOG(logDEBUG) << "Parse GPS NMEA string: " << gps_nmea_data << ". Result (Latitude, Longitude): (" << latitude_str << "," << longitude_str << ")"; + } + catch (const nmea::NMEAParseError &e) + { + PLOG(logERROR) << e.message.c_str(); + } + return result; + } + + Json::Value RSUHealthMonitorWorker::getRSUStatus(const RSUMibVersion &mibVersion, const string &_rsuIp, uint16_t &_snmpPort, const string &_securityUser, const string &_authPassPhrase, const string &_securityLevel, long timeout) + { + auto rsuStatusConfigTbl = GetRSUStatusConfig(mibVersion); + if (rsuStatusConfigTbl.size() == 0) + { + PLOG(logERROR) << "RSU status update call failed due to the RSU status config table is empty!"; + return Json::nullValue; + } + try + { + // Create SNMP client and use SNMP V3 protocol + PLOG(logINFO) << "Update SNMP client: RSU IP: " << _rsuIp << ", RSU port: " << _snmpPort << ", User: " << _securityUser << ", auth pass phrase: " << _authPassPhrase << ", security level: " + << _securityLevel; + auto _snmpClientPtr = std::make_unique(_rsuIp, _snmpPort, "", _securityUser, _securityLevel, _authPassPhrase, SNMP_VERSION_3, timeout); + + Json::Value rsuStatuJson; + // Sending RSU SNMP call for each field as each field has its own OID. + for (const auto &config : rsuStatusConfigTbl) + { + PLOG(logINFO) << "SNMP RSU status call for field:" << config.field << ", OID: " << config.oid; + snmp_response_obj responseVal; + if (_snmpClientPtr) + { + auto success = _snmpClientPtr->process_snmp_request(config.oid, request_type::GET, responseVal); + if (!success && config.required) + { + PLOG(logERROR) << "SNMP session stopped as the required field: " << config.field << " failed! Return empty RSU status!"; + return Json::nullValue; + } + else if (success) + { + rsuStatuJson.append(populateJson(config.field, responseVal)); + } + } + } + return rsuStatuJson; + } + catch (tmx::utils::snmp_client_exception &ex) + { + PLOG(logERROR) << ex.what(); + return Json::nullValue; + } + } + + Json::Value RSUHealthMonitorWorker::populateJson(const string &field, const snmp_response_obj &response) const + { + Json::Value rsuStatuJson; + if (response.type == snmp_response_obj::response_type::INTEGER) + { + rsuStatuJson[field] = response.val_int; + } + else if (response.type == snmp_response_obj::response_type::STRING) + { + string response_str(response.val_string.begin(), response.val_string.end()); + // Proess GPS nmea string + if (boost::iequals(field, "rsuGpsOutputString")) + { + auto gps = ParseRSUGPS(response_str); + rsuStatuJson["rsuGpsOutputStringLatitude"] = gps.begin()->first; + rsuStatuJson["rsuGpsOutputStringLongitude"] = gps.begin()->second; + } + rsuStatuJson[field] = response_str; + } + return rsuStatuJson; + } + + RSUStatusMessage RSUHealthMonitorWorker::convertJsonToTMXMsg(const Json::Value &json) const + { + Json::FastWriter fasterWirter; + string json_str = fasterWirter.write(json); + tmx::messages::RSUStatusMessage rsuStatusMsg; + rsuStatusMsg.set_contents(json_str); + return rsuStatusMsg; + } + + vector RSUHealthMonitorWorker::getJsonKeys(const Json::Value &json) const + { + vector keys; + if (json.isArray()) + { + for (auto itr = json.begin(); itr != json.end(); itr++) + { + if (itr->isObject()) + { + for (auto const &field : itr->getMemberNames()) + { + keys.push_back(field); + } + } + } + } + else if (json.isObject()) + { + for (auto const &field : json.getMemberNames()) + { + keys.push_back(field); + } + } + return keys; + } +} \ No newline at end of file diff --git a/src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorWorker.h b/src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorWorker.h new file mode 100644 index 000000000..ef7a429c5 --- /dev/null +++ b/src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorWorker.h @@ -0,0 +1,120 @@ +#pragma once +#include +#include "RSU_MIB_4_1.h" +#include +#include +#include +#include +#include +#include "PluginLog.h" +#include +#include "SNMPClient.h" +#include +#include "RSUStatusMessage.h" + +using namespace std; +using namespace tmx::utils; +using namespace tmx::utils::rsu41::mib::oid; +using namespace tmx::messages; + +namespace RSUHealthMonitor +{ + enum class RSUMibVersion + { + UNKOWN_MIB_V = 0, + RSUMIB_V_4_1 = 1, + RSUMIB_V_1218 = 2 + }; + + struct RSUFieldOIDStruct + { + string field; + string oid; + bool required; // Indicate whether this field is required to before broadcasting the RSU status. + }; + + /** + * RSUStatusTable is custom defined RSU status information. + * The fields are a subset of the fields from the RSU MIB definition used to quantify the health of the RSU. https://github.com/certificationoperatingcouncil/COC_TestSpecs/blob/master/AppNotes/RSU/RSU-MIB.txt + */ + using RSUStatusConfigTable = vector; + + class RSUHealthMonitorWorker + { + private: + // A map of RSU MIB version used and RSUStatusTable + shared_ptr> _RSUSTATUSConfigMapPtr; + + /** + * @brief Poupate the RSU status table with the specified version of OIDs and fields. + * Private: Only allow to initialze the RSU STATUS MAP once + * @param mibVersion specified + * @return RSUStatusTable the self defined RSU status https://usdot-carma.atlassian.net/wiki/spaces/WFD2/pages/2640740360/RSU+Health+Monitor+Plugin+Design + */ + RSUStatusConfigTable constructRsuStatusConfigTable(const RSUMibVersion &mibVersion) const; + + public: + // Populate the RSU Status Table with predefined fields and their mapping OIDs in constructor + RSUHealthMonitorWorker(); + + // Access to the RSU status table based in the RSU MIB version provided + RSUStatusConfigTable GetRSUStatusConfig(const RSUMibVersion &mibVersion) const; + + /** + * @brief determine if all required fields in the RSU config map _RSUSTATUSConfigMapPtr present in the input fields + * Use _RSUSTATUSConfigMapPtr RSU status config map that defines all fields and whether the fields are required. + * @param RSUStatusConfigTable RSU Status configration table to compare with. + * @param vector Input RSU fields to verify + * @return True if all required fields found. Otherwise, false. + */ + bool validateAllRequiredFieldsPresent(const RSUHealthMonitor::RSUStatusConfigTable &configTbl, const vector &fields) const; + + /** + * @brief Parse NMEA GPS sentense and return GPS related data + * @param gps_nmea_data NMEA GPS sentense + * @return map A map of latitude and longitude + */ + std::map ParseRSUGPS(const std::string &gps_nmea_data) const; + + /** + * @brief Sending SNMP V3 requests to get info for each field in the RSUStatusConfigTable, and return the RSU status in JSON + * Use RSU Status configuration table include RSU field, OIDs, and whether fields are required or optional + * @param RSUMibVersion The RSU MIB version used + * @param string RSU IP address + * @param uint16_t SNMP port + * @param string security user used for SNMP authentication + * @param string authentication password + * @param string security level: authPriv or authNoPriv. + * @param long session time out + */ + Json::Value getRSUStatus(const RSUMibVersion &mibVersion, const string &_rsuIp, uint16_t &_snmpPort, const string &_securityUser, const string &_authPassPhrase, const string &_securityLevel, long timeout); + + /*** + *@brief Convert the JSON message into TMX message + @param Json Input Json value + @return RSUStatusMessage TMX message + */ + RSUStatusMessage convertJsonToTMXMsg(const Json::Value &json) const; + + /** + * @brief Populate Json with snmp response object. + * @param string The field that maps to an OID. + * @param snmp_response_obj The response returned by SNMP call for the OID. + * @return Json value populated with response object. + */ + Json::Value populateJson(const string &field, const snmp_response_obj &response) const; + + /** + * @brief List the keys from the input Json values + * @param Json Input JSON values + * @return vector of key strings + */ + vector getJsonKeys(const Json::Value &json) const; + + // Delete move constructor + RSUHealthMonitorWorker(RSUHealthMonitorWorker &&worker) = delete; + + // Delete copy constructor + RSUHealthMonitorWorker(RSUHealthMonitorWorker &worker) = delete; + }; +} // namespace RSUHealthMonitor diff --git a/src/v2i-hub/RSUHealthMonitorPlugin/test/main.cpp b/src/v2i-hub/RSUHealthMonitorPlugin/test/main.cpp new file mode 100644 index 000000000..ba7cd2667 --- /dev/null +++ b/src/v2i-hub/RSUHealthMonitorPlugin/test/main.cpp @@ -0,0 +1,8 @@ + +#include + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/v2i-hub/RSUHealthMonitorPlugin/test/test_RSUHealthMonitorWorker.cpp b/src/v2i-hub/RSUHealthMonitorPlugin/test/test_RSUHealthMonitorWorker.cpp new file mode 100644 index 000000000..9d749f0df --- /dev/null +++ b/src/v2i-hub/RSUHealthMonitorPlugin/test/test_RSUHealthMonitorWorker.cpp @@ -0,0 +1,108 @@ +#include "RSUHealthMonitorWorker.h" +#include + +namespace RSUHealthMonitor +{ + class test_RSUHealthMonitorWorker : public ::testing::Test + { + public: + std::shared_ptr _rsuWorker = std::make_shared(); + }; + + TEST_F(test_RSUHealthMonitorWorker, GetRSUStatusConfig) + { + RSUStatusConfigTable rsuStatusConfigTbl = _rsuWorker->GetRSUStatusConfig(RSUMibVersion::RSUMIB_V_4_1); + ASSERT_EQ(14, rsuStatusConfigTbl.size()); + + rsuStatusConfigTbl = _rsuWorker->GetRSUStatusConfig(RSUMibVersion::UNKOWN_MIB_V); + ASSERT_EQ(0, rsuStatusConfigTbl.size()); + + RSUMibVersion mibVersionDefault; + rsuStatusConfigTbl = _rsuWorker->GetRSUStatusConfig(mibVersionDefault); + ASSERT_EQ(0, rsuStatusConfigTbl.size()); + } + + TEST_F(test_RSUHealthMonitorWorker, validateAllRequiredFieldsPresent) + { + auto config = _rsuWorker->GetRSUStatusConfig(RSUMibVersion::RSUMIB_V_4_1); + vector requiredFields = {"rsuID", "rsuMibVersion", "rsuFirmwareVersion", "rsuManufacturer", "rsuGpsOutputString", "rsuMode", "rsuChanStatus"}; + ASSERT_TRUE(_rsuWorker->validateAllRequiredFieldsPresent(config, requiredFields)); + + requiredFields = {"rsuID", "rsuMibVersion", "rsuFirmwareVersion"}; + ASSERT_FALSE(_rsuWorker->validateAllRequiredFieldsPresent(config, requiredFields)); + } + + TEST_F(test_RSUHealthMonitorWorker, ParseRSUGPS) + { + std::string gps_nmea_data = "$GPGGA,142440.00,3857.3065,N,07708.9734,W,2,18,0.65,86.18,M,-34.722,M,,*62"; + auto gps_map = _rsuWorker->ParseRSUGPS(gps_nmea_data); + ASSERT_EQ(1, gps_map.size()); + double expected_latitude = 38.9551; + double expected_longitude = -77.1496; + for (auto itr = gps_map.begin(); itr != gps_map.end(); itr++) + { + ASSERT_NEAR(expected_latitude, itr->first, 0.001); + ASSERT_NEAR(expected_longitude, itr->second, 0.001); + } + std::string invalid_gps_nmea_data = "$*GPGGA,invalid"; + auto gps_map_invalid = _rsuWorker->ParseRSUGPS(invalid_gps_nmea_data); + ASSERT_EQ(0, gps_map_invalid.size()); + } + + TEST_F(test_RSUHealthMonitorWorker, getRSUStatus) + { + uint16_t port = 161; + auto json = _rsuWorker->getRSUStatus(RSUMibVersion::RSUMIB_V_4_1, "127.0.0.1", port, "test", "testtesttest", "authPriv", 1000); + ASSERT_TRUE(json.empty()); + + json = _rsuWorker->getRSUStatus(RSUMibVersion::RSUMIB_V_4_1, "127.0.0.1", port, "test", "test", "authPriv", 1000); + ASSERT_TRUE(json.empty()); + + json = _rsuWorker->getRSUStatus(RSUMibVersion::RSUMIB_V_1218, "127.0.0.1", port, "test", "test", "authPriv", 1000); + ASSERT_TRUE(json.empty()); + } + + TEST_F(test_RSUHealthMonitorWorker, convertJsonToTMXMsg) + { + Json::Value json; + json["rsuID"] = "RSU4.1"; + json["rsuMode"] = 4; + auto rsuStatusTmxMsg = _rsuWorker->convertJsonToTMXMsg(json); + string expectedStr = "{\"rsuID\":\"RSU4.1\",\"rsuMode\":4}\n"; + ASSERT_EQ(expectedStr, rsuStatusTmxMsg.to_string()); + } + + TEST_F(test_RSUHealthMonitorWorker, populateJson) + { + Json::Value rsuStatusJson; + snmp_response_obj stringObj; + stringObj.type = snmp_response_obj::response_type::STRING; + std::string gps_nmea_data = "$GPGGA,142440.00,3857.3065,N,07708.9734,W,2,18,0.65,86.18,M,-34.722,M,,*62"; + vector rgps_nmea_data_c; + copy(gps_nmea_data.begin(), gps_nmea_data.end(), back_inserter(rgps_nmea_data_c)); + stringObj.val_string = rgps_nmea_data_c; + + auto json = _rsuWorker->populateJson("rsuGpsOutputString", stringObj); + double expected_latitude = 38.9551; + double expected_longitude = -77.1496; + ASSERT_NEAR(expected_latitude, json["rsuGpsOutputStringLatitude"].asDouble(), 0.001); + ASSERT_NEAR(expected_longitude, json["rsuGpsOutputStringLongitude"].asDouble(), 0.001); + rsuStatusJson.append(json); + + snmp_response_obj intObj; + intObj.type = snmp_response_obj::response_type::INTEGER; + intObj.val_int = 4; + + json = _rsuWorker->populateJson("rsuMode", intObj); + ASSERT_EQ(4, json["rsuMode"].asInt64()); + rsuStatusJson.append(json); + + Json::FastWriter fasterWirter; + string json_str = fasterWirter.write(rsuStatusJson); + string expectedStr = "[{\"rsuGpsOutputString\":\"$GPGGA,142440.00,3857.3065,N,07708.9734,W,2,18,0.65,86.18,M,-34.722,M,,*62\",\"rsuGpsOutputStringLatitude\":38.955108330000002,\"rsuGpsOutputStringLongitude\":-77.149556669999996},{\"rsuMode\":4}]\n"; + ASSERT_EQ(expectedStr, json_str); + ASSERT_EQ(4, _rsuWorker->getJsonKeys(rsuStatusJson).size()); + ASSERT_EQ(1, _rsuWorker->getJsonKeys(json).size()); + } + +} \ No newline at end of file From 774b31fced876b48226302ffd093f66484a76a2f Mon Sep 17 00:00:00 2001 From: dan-du-car <62157949+dan-du-car@users.noreply.github.com> Date: Tue, 21 Nov 2023 18:56:15 -0500 Subject: [PATCH 18/28] v2x hub Integration: Implement telematic plugin to subscribe to all TMX messages from V2xHub (#565) # PR Details ## Description In order to publish data from V2xHub to cloud a telematics bridge is needed. This story is the implementation story for the telematic module to collect J2735 data from V2xHub. The design refers to https://usdot-carma.atlassian.net/wiki/spaces/WFD2/pages/2640379977/V2xHub+WFD+Bridge+Design Connect to V2xHub Subscribe all TMX messages from V2xhub. If there are J2735 message payloads in the TMX messages, decode the J2735 into human readable message. Convert TMX messages into messages in JSON format. ## Related Issue NA ## Motivation and Context CDA telematic ## How Has This Been Tested? Unit test Integration test ## Types of changes - [ ] Defect fix (non-breaking change that fixes an issue) - [x] 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. --- .sonarqube/sonar-scanner.properties | 7 +- ext/build.sh | 18 +- scripts/install_dependencies.sh | 3 + .../src/RSUHealthMonitorWorker.cpp | 6 +- .../test/test_RSUHealthMonitorWorker.cpp | 9 +- .../test/testPedestrianDetectionForSPAT.cpp | 2 +- .../TelematicBridgePlugin/CMakeLists.txt | 15 ++ .../TelematicBridgePlugin/manifest.json | 16 ++ .../src/TelematicBridgeException.h | 11 + .../src/TelematicBridgeMsgWorker.h | 247 ++++++++++++++++++ .../src/TelematicBridgePlugin.cpp | 37 +++ .../src/TelematicBridgePlugin.h | 26 ++ .../TelematicBridgePlugin/test/main.cpp | 7 + .../test/test_TelematicMsgWorker.cpp | 106 ++++++++ 14 files changed, 503 insertions(+), 7 deletions(-) create mode 100644 src/v2i-hub/TelematicBridgePlugin/CMakeLists.txt create mode 100644 src/v2i-hub/TelematicBridgePlugin/manifest.json create mode 100644 src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgeException.h create mode 100644 src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgeMsgWorker.h create mode 100644 src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgePlugin.cpp create mode 100644 src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgePlugin.h create mode 100644 src/v2i-hub/TelematicBridgePlugin/test/main.cpp create mode 100644 src/v2i-hub/TelematicBridgePlugin/test/test_TelematicMsgWorker.cpp diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties index d5f118b33..06424d6ef 100644 --- a/.sonarqube/sonar-scanner.properties +++ b/.sonarqube/sonar-scanner.properties @@ -56,7 +56,8 @@ sonar.modules= PedestrianPlugin, \ CARMAStreetsPlugin, \ ERVCloudForwardingPlugin, \ CDASimAdapter, \ - RSUHealthMonitorPlugin + RSUHealthMonitorPlugin, \ + TelematicBridgePlugin @@ -84,6 +85,7 @@ CARMAStreetsPlugin.sonar.projectBaseDir =src/v2i-hub/CARMAStreetsPlugin ERVCloudForwardingPlugin.sonar.projectBaseDir =src/v2i-hub/ERVCloudForwardingPlugin CDASimAdapter.sonar.projectBaseDir =src/v2i-hub/CDASimAdapter RSUHealthMonitorPlugin.sonar.projectBaseDir =src/v2i-hub/RSUHealthMonitorPlugin +TelematicBridgePlugin.sonar.projectBaseDir =src/v2i-hub/TelematicBridgePlugin @@ -121,6 +123,8 @@ CDASimAdapter.sonar.sources =src CDASimAdapter.sonar.exclusions =test/** RSUHealthMonitorPlugin.sonar.sources =src RSUHealthMonitorPlugin.sonar.exclusions =test/** +TelematicBridgePlugin.sonar.sources =src +TelematicBridgePlugin.sonar.exclusions =test/** # Tests # Note: For C++ setting this field does not cause test analysis to occur. It only allows the test source code to be evaluated. @@ -149,3 +153,4 @@ CARMAStreetsPlugin.sonar.tests=test ERVCloudForwardingPlugin.sonar.tests=test CDASimAdapter.sonar.tests=test RSUHealthMonitorPlugin.sonar.tests=test +TelematicBridgePlugin.sonar.tests=test diff --git a/ext/build.sh b/ext/build.sh index 03d2ec4fb..91d59ca8a 100755 --- a/ext/build.sh +++ b/ext/build.sh @@ -50,9 +50,25 @@ popd # GPS Parser pushd /tmp +if [ -d "NemaTode" ]; then + rm -r NemaTode +fi git clone https://github.com/ckgt/NemaTode.git cd NemaTode cmake . make -j${numCPU} make install -popd \ No newline at end of file +popd + +# Nats C API +pushd /tmp +if [ -d "nats.c" ]; then + rm -r nats.c +fi +git clone https://github.com/nats-io/nats.c +cd nats.c +cmake . -DNATS_BUILD_NO_SPIN=ON +make -j${numCPU} +make install +popd + diff --git a/scripts/install_dependencies.sh b/scripts/install_dependencies.sh index 2fa104c86..c4ce8727e 100755 --- a/scripts/install_dependencies.sh +++ b/scripts/install_dependencies.sh @@ -31,6 +31,9 @@ DEPENDENCIES="build-essential \ wget \ zip \ zlib1g \ + rapidjson-dev \ + librapidxml-dev \ + libprotobuf-c-dev \ curl" # STOL library dependencies diff --git a/src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorWorker.cpp b/src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorWorker.cpp index 6de25d57e..6ac62f9fc 100644 --- a/src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorWorker.cpp +++ b/src/v2i-hub/RSUHealthMonitorPlugin/src/RSUHealthMonitorWorker.cpp @@ -145,7 +145,11 @@ namespace RSUHealthMonitor } else if (success) { - rsuStatuJson.append(populateJson(config.field, responseVal)); + auto json = populateJson(config.field, responseVal); + for(const auto &key: json.getMemberNames()) + { + rsuStatuJson[key] = json[key]; + } } } } diff --git a/src/v2i-hub/RSUHealthMonitorPlugin/test/test_RSUHealthMonitorWorker.cpp b/src/v2i-hub/RSUHealthMonitorPlugin/test/test_RSUHealthMonitorWorker.cpp index 9d749f0df..cc61247b5 100644 --- a/src/v2i-hub/RSUHealthMonitorPlugin/test/test_RSUHealthMonitorWorker.cpp +++ b/src/v2i-hub/RSUHealthMonitorPlugin/test/test_RSUHealthMonitorWorker.cpp @@ -87,7 +87,10 @@ namespace RSUHealthMonitor double expected_longitude = -77.1496; ASSERT_NEAR(expected_latitude, json["rsuGpsOutputStringLatitude"].asDouble(), 0.001); ASSERT_NEAR(expected_longitude, json["rsuGpsOutputStringLongitude"].asDouble(), 0.001); - rsuStatusJson.append(json); + for(const auto& key: json.getMemberNames()) + { + rsuStatusJson[key] = json[key]; + } snmp_response_obj intObj; intObj.type = snmp_response_obj::response_type::INTEGER; @@ -95,11 +98,11 @@ namespace RSUHealthMonitor json = _rsuWorker->populateJson("rsuMode", intObj); ASSERT_EQ(4, json["rsuMode"].asInt64()); - rsuStatusJson.append(json); + rsuStatusJson["rsuMode"] = json["rsuMode"]; Json::FastWriter fasterWirter; string json_str = fasterWirter.write(rsuStatusJson); - string expectedStr = "[{\"rsuGpsOutputString\":\"$GPGGA,142440.00,3857.3065,N,07708.9734,W,2,18,0.65,86.18,M,-34.722,M,,*62\",\"rsuGpsOutputStringLatitude\":38.955108330000002,\"rsuGpsOutputStringLongitude\":-77.149556669999996},{\"rsuMode\":4}]\n"; + string expectedStr = "{\"rsuGpsOutputString\":\"$GPGGA,142440.00,3857.3065,N,07708.9734,W,2,18,0.65,86.18,M,-34.722,M,,*62\",\"rsuGpsOutputStringLatitude\":38.955108330000002,\"rsuGpsOutputStringLongitude\":-77.149556669999996,\"rsuMode\":4}\n"; ASSERT_EQ(expectedStr, json_str); ASSERT_EQ(4, _rsuWorker->getJsonKeys(rsuStatusJson).size()); ASSERT_EQ(1, _rsuWorker->getJsonKeys(json).size()); diff --git a/src/v2i-hub/SpatPlugin/test/testPedestrianDetectionForSPAT.cpp b/src/v2i-hub/SpatPlugin/test/testPedestrianDetectionForSPAT.cpp index 8910b8bb0..e404307b2 100644 --- a/src/v2i-hub/SpatPlugin/test/testPedestrianDetectionForSPAT.cpp +++ b/src/v2i-hub/SpatPlugin/test/testPedestrianDetectionForSPAT.cpp @@ -73,7 +73,7 @@ TEST(PedestrianDetectionForSPAT, updateEncodedSpat) EXPECT_EQ(spatPtr->intersections.list.array[0]->states.list.array[0]->signalGroup, 1); EXPECT_EQ(spatPtr->intersections.list.array[0]->states.list.array[1]->signalGroup, 2); ASSERT_NE(spatPtr->intersections.list.array[0]->maneuverAssistList, nullptr); - EXPECT_EQ(spatPtr->intersections.list.array[0]->maneuverAssistList->list.count, 1); + EXPECT_NE(spatPtr->intersections.list.array[0]->maneuverAssistList->list.count, 0); ASSERT_NE(spatPtr->intersections.list.array[0]->maneuverAssistList->list.array[0]->pedBicycleDetect, nullptr); EXPECT_EQ(*(spatPtr->intersections.list.array[0]->maneuverAssistList->list.array[0]->pedBicycleDetect), 1); } diff --git a/src/v2i-hub/TelematicBridgePlugin/CMakeLists.txt b/src/v2i-hub/TelematicBridgePlugin/CMakeLists.txt new file mode 100644 index 000000000..5134ae38b --- /dev/null +++ b/src/v2i-hub/TelematicBridgePlugin/CMakeLists.txt @@ -0,0 +1,15 @@ +PROJECT (TelematicBridgePlugin VERSION 7.5.1 LANGUAGES CXX) + +set (TMX_PLUGIN_NAME "Telematic Bridge") + +BuildTmxPlugin() +TARGET_LINK_LIBRARIES ( ${PROJECT_NAME} tmxutils jsoncpp) + +#################################################### +################## Testing ####################### +#################################################### +enable_testing() +include_directories(${PROJECT_SOURCE_DIR}/src) +file(GLOB_RECURSE TEST_SOURCES LIST_DIRECTORIES false test/*.h test/*.cpp) +add_executable(${PROJECT_NAME}_test ${TEST_SOURCES}) +target_link_libraries(${PROJECT_NAME}_test PRIVATE gtest tmxutils jsoncpp) \ No newline at end of file diff --git a/src/v2i-hub/TelematicBridgePlugin/manifest.json b/src/v2i-hub/TelematicBridgePlugin/manifest.json new file mode 100644 index 000000000..530dbe0eb --- /dev/null +++ b/src/v2i-hub/TelematicBridgePlugin/manifest.json @@ -0,0 +1,16 @@ +{ + "name": "TelematicBridge", + "description": "Plugin that listens for TMX messages and forward them to the Telematic cloud services.", + "version": "@PROJECT_VERSION@", + "exeLocation": "/bin/TelematicBridgePlugin", + "coreIpAddr": "127.0.0.1", + "corePort": 24601, + "messageTypes": [], + "configuration": [ + { + "key": "LogLevel", + "default": "INFO", + "description": "The log level for this plugin" + } + ] +} \ No newline at end of file diff --git a/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgeException.h b/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgeException.h new file mode 100644 index 000000000..5959ca1ff --- /dev/null +++ b/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgeException.h @@ -0,0 +1,11 @@ +#pragma once +#include + +namespace TelematicBridge +{ + class TelematicBridgeException : public tmx::TmxException + { + public: + using TmxException::TmxException; + }; +} \ No newline at end of file diff --git a/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgeMsgWorker.h b/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgeMsgWorker.h new file mode 100644 index 000000000..c31c5cc9c --- /dev/null +++ b/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgeMsgWorker.h @@ -0,0 +1,247 @@ +#pragma once +#include "PluginLog.h" +#include +#include "TelematicBridgeException.h" +#include "jsoncpp/json/json.h" +#include +#include +#include +#include + +using namespace tmx::utils; +using namespace std; +using namespace tmx::messages; +namespace pt = boost::property_tree; + +namespace TelematicBridge +{ + using buffer_structure_t = struct buffer_structure + { + char *buffer; // buffer array + size_t buffer_size; // this is really where we will write next. + size_t allocated_size; // this is the total size of the buffer. + }; + + /** + * @brief Convert hex string into a vector of bytes + * @param string input hex string payload + * @param vector byte buffer to be updated. + * @return bool indicator whether conversion is successful. + */ + bool HexToBytes(const string &hexPaylod, vector &byteBuffer) + { + uint8_t d = 0; + int i = 0; + + for (const char &c : hexPaylod) + { + if (c <= '9' && c >= '0') + { + d = c - '0'; + } + else if (c <= 'F' && c >= 'A') + { + d = c - 55; // c - 'A' + 10 + } + else if (c <= 'f' && c >= 'a') + { + d = c - 87; // c - 'a' + 10; + } + else + { + return false; + } + + if (i % 2) + { + // low order nibble. + byteBuffer.back() |= d; + } + else + { + // high order nibble. + byteBuffer.push_back(d << 4); + } + ++i; + } + return true; + } + + /** + * @brief Decode J2735 message and populate the J2735 data frame + * @param string input hex string payload + * @param MessageFrame_t J2735 struct to be updated + * @return bool indicator whether decoding successful + */ + void DecodeJ2735Msg(const string &hexPaylod, MessageFrame_t *messageFrame) + { + /** + * Decode J2735 message + */ + ostringstream erroross; + vector byte_buffer; + if (!HexToBytes(hexPaylod, byte_buffer)) + { + throw TelematicBridgeException("Failed attempt to decode MessageFrame hex string: cannot convert to bytes."); + } + asn_dec_rval_t decode_rval = asn_decode( + nullptr, + ATS_UNALIGNED_BASIC_PER, + &asn_DEF_MessageFrame, + (void **)&messageFrame, + byte_buffer.data(), + byte_buffer.size()); + + if (decode_rval.code != RC_OK) + { + erroross.str(""); + erroross << "failed ASN.1 binary decoding of element " << asn_DEF_MessageFrame.name << ": bad data. Successfully decoded " << decode_rval.consumed << " bytes."; + throw TelematicBridgeException(erroross.str()); + } + } + + int DynamicBufferAppend(const void *buffer, size_t size, void *app_key) + { + auto *xb = static_cast(app_key); + + while (xb->buffer_size + size + 1 > xb->allocated_size) + { + // increase size of buffer. + size_t new_size = 2 * (xb->allocated_size ? xb->allocated_size : 64); + auto new_buf = static_cast(MALLOC(new_size)); + if (!new_buf) + return -1; + // move old to new. + memcpy(new_buf, xb->buffer, xb->buffer_size); + + FREEMEM(xb->buffer); + xb->buffer = new_buf; + xb->allocated_size = new_size; + } + + memcpy(xb->buffer + xb->buffer_size, buffer, size); + xb->buffer_size += size; + // null terminate the string. + xb->buffer[xb->buffer_size] = '\0'; + return 0; + } + + /** + * @brief Convert the J2735 messageFrame into string in XML format + * @param MessageFrame_t J2735 struct + * @return string XML formatted J2735 message + */ + string ConvertJ2735FrameToXML(const MessageFrame_t *messageFrame) + { + /** + * Convert J2735 message into XML + */ + buffer_structure_t xml_buffer = {nullptr, 0, 0}; + asn_enc_rval_t encode_rval = xer_encode( + &asn_DEF_MessageFrame, + messageFrame, + XER_F_CANONICAL, + DynamicBufferAppend, + static_cast(&xml_buffer)); + if (encode_rval.encoded == -1) + { + throw TelematicBridgeException("Failed to convert message with ID (=" + to_string(messageFrame->messageId) + ") to XML "); + } + return string(xml_buffer.buffer); + } + + /** + * @brief convert JSON value into string + * @param JSON input Json::Value + * @return string + */ + string JsonToString(const Json::Value &json) + { + Json::FastWriter fasterWirter; + string jsonStr = fasterWirter.write(json); + boost::replace_all(jsonStr, "\\n", ""); + boost::replace_all(jsonStr, "\n", ""); + boost::replace_all(jsonStr, "\\t", ""); + boost::replace_all(jsonStr, "\\", ""); + return jsonStr; + } + /** + * @brief convert string into JSON value + * @param string input string + * @return JSON::Value + */ + Json::Value StringToJson(const string &str) + { + Json::Value root; + Json::Reader reader; + bool parsingSuccessful = reader.parse(str, root); + if (!parsingSuccessful) + { + throw TelematicBridgeException("Error parsing the string"); + } + return root; + } + /** + * @brief Convert XML string into JSON string + */ + string xml2Json(const string &xml_str) + { + stringstream xmlss; + xmlss << xml_str; + pt::ptree root; + pt::read_xml(xmlss, root); + stringstream jsonss; + pt::write_json(jsonss, root, false); + return jsonss.str(); + } + /** + * @brief create JSON payload from given IVP message + * @param IVPMessage V2xHub interval exchanged message + * @return JSON value + */ + Json::Value IvpMessageToJson(const IvpMessage *msg) + { + Json::Value json; + if (msg->type) + { + json["type"] = msg->type; + } + + if (msg->subtype) + { + json["subType"] = msg->subtype; + } + + if (msg->dsrcMetadata) + { + json["channel"] = msg->dsrcMetadata->channel; + json["psid"] = msg->dsrcMetadata->psid; + } + + if (msg->encoding) + { + json["encoding"] = msg->encoding; + } + + if (msg->source) + { + json["source"] = msg->source; + } + json["sourceId"] = msg->sourceId; + json["flags"] = msg->flags; + json["timestamp"] = msg->timestamp; + if (msg->payload) + { + if (msg->payload->type == cJSON_Number) + { + json["payload"] = (msg->payload->valueint == 0 ? msg->payload->valuedouble : static_cast(msg->payload->valueint)); + } + else + { + json["payload"] = StringToJson(cJSON_Print(msg->payload)); + } + } + + return json; + } +} // TelematicBridge \ No newline at end of file diff --git a/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgePlugin.cpp b/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgePlugin.cpp new file mode 100644 index 000000000..a316084f4 --- /dev/null +++ b/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgePlugin.cpp @@ -0,0 +1,37 @@ +#include "TelematicBridgePlugin.h" + +namespace TelematicBridge +{ + TelematicBridgePlugin::TelematicBridgePlugin(const string &name) : PluginClient(name) + { + AddMessageFilter("*", "*", IvpMsgFlags_None); + AddMessageFilter("J2735", "*", IvpMsgFlags_RouteDSRC); + SubscribeToMessages(); + } + + void TelematicBridgePlugin::OnMessageReceived(IvpMessage *msg) + { + if (msg && msg->type) + { + auto json = IvpMessageToJson(msg); + // Process J2735 message payload hex string + if (strcasecmp(msg->type, Telematic_MSGTYPE_J2735_STRING) == 0) + { + auto messageFm = (MessageFrame_t *)calloc(1, sizeof(MessageFrame_t)); + DecodeJ2735Msg(msg->payload->valuestring, messageFm); + string xml_payload_str = ConvertJ2735FrameToXML(messageFm); + ASN_STRUCT_FREE(asn_DEF_MessageFrame, messageFm); + json["payload"] = StringToJson(xml2Json(xml_payload_str)); + } + + auto jsonStr = JsonToString(json); + PLOG(logINFO) << jsonStr; + } + } +} + +// The main entry point for this application. +int main(int argc, char *argv[]) +{ + return run_plugin("Telematic Bridge", argc, argv); +} \ No newline at end of file diff --git a/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgePlugin.h b/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgePlugin.h new file mode 100644 index 000000000..c790b6ca4 --- /dev/null +++ b/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgePlugin.h @@ -0,0 +1,26 @@ +#ifndef _TelematicBRIDGEPLUGIN_H_ +#define _TelematicBRIDGEPLUGIN_H_ + +#include "PluginClient.h" +#include "TelematicBridgeMsgWorker.h" + +using namespace tmx::utils; +using namespace std; +namespace TelematicBridge +{ + + + class TelematicBridgePlugin : public tmx::utils::PluginClient + { + private: + static CONSTEXPR const char *Telematic_MSGTYPE_J2735_STRING = "J2735"; + void OnMessageReceived(IvpMessage *msg); + + public: + explicit TelematicBridgePlugin(const string& name); + ~TelematicBridgePlugin() override = default; + }; + +} // namespace TelematicBridge + +#endif \ No newline at end of file diff --git a/src/v2i-hub/TelematicBridgePlugin/test/main.cpp b/src/v2i-hub/TelematicBridgePlugin/test/main.cpp new file mode 100644 index 000000000..d973578fc --- /dev/null +++ b/src/v2i-hub/TelematicBridgePlugin/test/main.cpp @@ -0,0 +1,7 @@ +#include + +int main(int argc, char *argv[]) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/v2i-hub/TelematicBridgePlugin/test/test_TelematicMsgWorker.cpp b/src/v2i-hub/TelematicBridgePlugin/test/test_TelematicMsgWorker.cpp new file mode 100644 index 000000000..9760ec906 --- /dev/null +++ b/src/v2i-hub/TelematicBridgePlugin/test/test_TelematicMsgWorker.cpp @@ -0,0 +1,106 @@ +#include +#include "TelematicBridgeMsgWorker.h" +#include "stdio.h" + +using namespace TelematicBridge; +using namespace std; + +class test_TelematicJ2735MsgWorker : public ::testing::Test +{ +}; + +TEST_F(test_TelematicJ2735MsgWorker, HexToBytes) +{ + vector byteBuff; + string bsmHex = "0014251d59d162dad7de266e9a7d1ea6d4220974ffffffff8ffff080fdfa1fa1007fff0000640fa0"; + auto success = HexToBytes(bsmHex, byteBuff); + ASSERT_TRUE(success); + ASSERT_EQ(bsmHex.size() / 2, byteBuff.size()); +} + +TEST_F(test_TelematicJ2735MsgWorker, DecodeJ2735Msg) +{ + auto messageFrame = (MessageFrame_t *)malloc(sizeof(MessageFrame_t)); + string bsmHex = "0014251d59d162dad7de266e9a7d1ea6d4220974ffffffff8ffff080fdfa1fa1007fff0000640fa0"; + ASSERT_NO_THROW(DecodeJ2735Msg(bsmHex, messageFrame)); + ASSERT_EQ(20, messageFrame->messageId); + ASN_STRUCT_FREE(asn_DEF_MessageFrame, messageFrame); +} + +TEST_F(test_TelematicJ2735MsgWorker, DecodeJ2735MsgFailure) +{ + auto messageFrame = (MessageFrame_t *)malloc(sizeof(MessageFrame_t)); + string badHex = "0014251d59d162dad7de266e9a7d1ea6d4220974ffffffff8ffff080fdfa1fa1007fff0000640fG0"; + ASSERT_THROW(DecodeJ2735Msg(badHex, messageFrame), TelematicBridgeException); + + badHex = "0014251d59d162dad7de266e9a"; + ASSERT_THROW(DecodeJ2735Msg(badHex, messageFrame), TelematicBridgeException); + ASN_STRUCT_FREE(asn_DEF_MessageFrame, messageFrame); +} + +TEST_F(test_TelematicJ2735MsgWorker, ConvertJ2735FrameToXML) +{ + auto messageFrame = (MessageFrame_t *)malloc(sizeof(MessageFrame_t)); + string bsmHex = "0014251d59d162dad7de266e9a7d1ea6d4220974ffffffff8ffff080fdfa1fa1007fff0000640fA0"; + ASSERT_NO_THROW(DecodeJ2735Msg(bsmHex, messageFrame)); + auto xmlStr = ConvertJ2735FrameToXML(messageFrame); + ASN_STRUCT_FREE(asn_DEF_MessageFrame, messageFrame); + string expectedXMLStr = "2011767458B6B24440389565434-7715004757452552556553581912880012720012001-127000000200500"; + ASSERT_EQ(expectedXMLStr, xmlStr); +} + +TEST_F(test_TelematicJ2735MsgWorker, constructTelematicPayload) +{ + IvpMessage msg; + auto payload = cJSON_CreateObject(); + payload->valuedouble = 12; + payload->type = cJSON_Number; + msg.payload = payload; + msg.source = "Plugin"; + msg.encoding = "json"; + msg.type = "Application"; + msg.subtype = "alive"; + msg.sourceId = 123; + msg.flags = 10; + msg.timestamp = 10; + IvpDsrcMetadata metadata; + metadata.channel = 12; + metadata.psid = 120; + msg.dsrcMetadata = &metadata; + auto json = IvpMessageToJson(&msg); + auto str = JsonToString(json); + string expectedStr = "{\"channel\":12,\"encoding\":\"json\",\"flags\":10,\"payload\":12.0,\"psid\":120,\"source\":\"Plugin\",\"sourceId\":123,\"subType\":\"alive\",\"timestamp\":10,\"type\":\"Application\"}"; + ASSERT_EQ(expectedStr, str); + + payload->valueint = 13; + payload->type = cJSON_Number; + msg.payload = payload; + json = IvpMessageToJson(&msg); + str = JsonToString(json); + expectedStr = "{\"channel\":12,\"encoding\":\"json\",\"flags\":10,\"payload\":13.0,\"psid\":120,\"source\":\"Plugin\",\"sourceId\":123,\"subType\":\"alive\",\"timestamp\":10,\"type\":\"Application\"}"; + ASSERT_EQ(expectedStr, str); + + payload->valuestring = "test"; + payload->type = cJSON_String; + msg.payload = payload; + json = IvpMessageToJson(&msg); + str = JsonToString(json); + expectedStr = "{\"channel\":12,\"encoding\":\"json\",\"flags\":10,\"payload\":\"test\",\"psid\":120,\"source\":\"Plugin\",\"sourceId\":123,\"subType\":\"alive\",\"timestamp\":10,\"type\":\"Application\"}"; + ASSERT_EQ(expectedStr, str); + + msg.payload = cJSON_Parse("[{\"test\":12}]"); + json = IvpMessageToJson(&msg); + str = JsonToString(json); + expectedStr = "{\"channel\":12,\"encoding\":\"json\",\"flags\":10,\"payload\":[{\"test\":12}],\"psid\":120,\"source\":\"Plugin\",\"sourceId\":123,\"subType\":\"alive\",\"timestamp\":10,\"type\":\"Application\"}"; + ASSERT_EQ(expectedStr, str); + + json = StringToJson("{\"test\":12}"); + ASSERT_EQ(12, json["test"].asInt64()); +} + +TEST_F(test_TelematicJ2735MsgWorker, xml2json) +{ + string expectedXMLStr = "2011767458B6B24440389565434-7715004757452552556553581912880012720012001-127000000200500"; + string expectedJSONStr = "{\"MessageFrame\":{\"messageId\":\"20\",\"value\":{\"BasicSafetyMessage\":{\"coreData\":{\"msgCnt\":\"117\",\"id\":\"67458B6B\",\"secMark\":\"24440\",\"lat\":\"389565434\",\"long\":\"-771500475\",\"elev\":\"745\",\"accuracy\":{\"semiMajor\":\"255\",\"semiMinor\":\"255\",\"orientation\":\"65535\"},\"transmission\":{\"neutral\":\"\"},\"speed\":\"8191\",\"heading\":\"28800\",\"angle\":\"127\",\"accelSet\":{\"long\":\"2001\",\"lat\":\"2001\",\"vert\":\"-127\",\"yaw\":\"0\"},\"brakes\":{\"wheelBrakes\":\"00000\",\"traction\":{\"unavailable\":\"\"},\"abs\":{\"unavailable\":\"\"},\"scs\":{\"unavailable\":\"\"},\"brakeBoost\":{\"unavailable\":\"\"},\"auxBrakes\":{\"unavailable\":\"\"}},\"size\":{\"width\":\"200\",\"length\":\"500\"}}}}}}\n"; + ASSERT_EQ(expectedJSONStr, xml2Json(expectedXMLStr)); +} \ No newline at end of file From 6b604b73b4b9fa68c70831ccd36d547412c94c9d Mon Sep 17 00:00:00 2001 From: dan-du-car <62157949+dan-du-car@users.noreply.github.com> Date: Thu, 30 Nov 2023 14:36:32 -0500 Subject: [PATCH 19/28] Telematic bridge nats (#567) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # PR Details ## Description This is the implementation story for the telematic bridge to stream J2735 data from V2xHub into the cloud at near real time. The module design refers to https://usdot-carma.atlassian.net/wiki/spaces/WFD2/pages/2640379977/V2xHub+WFD+Bridge+Design The following functionalities should be implemented for this module: Able to forward the JSON message to telematic cloud when the message type is requested by end users through telematic UI. Able to register itself with WFD cloud services. Able to map each TMX message type to each individual topic.   Able to provide available topics to telematic cloud. Able to stream the requested TMX message in JSON format to telematic cloud. ## Related Issue NA ## Motivation and Context Telematic tool data collection ## How Has This Been Tested? Unit test and local integration testing ## Types of changes - [ ] Defect fix (non-breaking change that fixes an issue) - [x] 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. --- .devcontainer/docker-compose-vscode.yml | 3 +- configuration/amd64/docker-compose.yml | 2 + .../src/simulation/SimulationEnvUtils.h | 6 + .../TelematicBridgePlugin/CMakeLists.txt | 6 +- src/v2i-hub/TelematicBridgePlugin/README.md | 96 ++++++ .../TelematicBridgePlugin/manifest.json | 10 + .../src/TelematicBridgePlugin.cpp | 54 ++- .../src/TelematicBridgePlugin.h | 19 +- .../src/TelematicUnit.cpp | 320 ++++++++++++++++++ .../TelematicBridgePlugin/src/TelematicUnit.h | 216 ++++++++++++ .../test/test_TelematicUnit.cpp | 156 +++++++++ 11 files changed, 876 insertions(+), 12 deletions(-) create mode 100644 src/v2i-hub/TelematicBridgePlugin/README.md create mode 100644 src/v2i-hub/TelematicBridgePlugin/src/TelematicUnit.cpp create mode 100644 src/v2i-hub/TelematicBridgePlugin/src/TelematicUnit.h create mode 100644 src/v2i-hub/TelematicBridgePlugin/test/test_TelematicUnit.cpp diff --git a/.devcontainer/docker-compose-vscode.yml b/.devcontainer/docker-compose-vscode.yml index cc113ce62..9035d1f9d 100755 --- a/.devcontainer/docker-compose-vscode.yml +++ b/.devcontainer/docker-compose-vscode.yml @@ -24,7 +24,8 @@ services: - SIM_V2X_PORT=5757 - SIM_INTERACTION_PORT=7576 - V2X_PORT=8686 - - INFRASTRUCTURE_ID=1 + - INFRASTRUCTURE_ID=rsu_ + - INFRASTRUCTURE_NAME= - SENSOR_JSON_FILE_PATH=/var/www/plugins/MAP/sensors.json secrets: - mysql_password diff --git a/configuration/amd64/docker-compose.yml b/configuration/amd64/docker-compose.yml index 6b5188297..0c9d5feb2 100755 --- a/configuration/amd64/docker-compose.yml +++ b/configuration/amd64/docker-compose.yml @@ -38,6 +38,8 @@ services: - db environment: - MYSQL_PASSWORD=/run/secrets/mysql_password + - INFRASTRUCTURE_ID=rsu_ + - INFRASTRUCTURE_NAME= secrets: - mysql_password volumes: diff --git a/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h b/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h index 7498a7922..efffc4ffa 100644 --- a/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h +++ b/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h @@ -66,6 +66,12 @@ namespace tmx::utils::sim{ * for CDASim connection. */ constexpr inline static const char *INFRASTRUCTURE_ID = "INFRASTRUCTURE_ID"; + + /** + * @brief Name of environment variable for storing infrastructure name of v2xhub. Only necessary in SIMULATION MODE + * for CDASim connection. + */ + constexpr inline static const char *INFRASTRUCTURE_NAME = "INFRASTRUCTURE_NAME"; /** * @brief Function to return bool indicating whether V2X-Hub deployment is in SIMULATION MODE or not. * @return true if SIMULATION_MODE is "true" or "TRUE" and false otherwise. diff --git a/src/v2i-hub/TelematicBridgePlugin/CMakeLists.txt b/src/v2i-hub/TelematicBridgePlugin/CMakeLists.txt index 5134ae38b..df7b1c0f7 100644 --- a/src/v2i-hub/TelematicBridgePlugin/CMakeLists.txt +++ b/src/v2i-hub/TelematicBridgePlugin/CMakeLists.txt @@ -3,13 +3,15 @@ PROJECT (TelematicBridgePlugin VERSION 7.5.1 LANGUAGES CXX) set (TMX_PLUGIN_NAME "Telematic Bridge") BuildTmxPlugin() -TARGET_LINK_LIBRARIES ( ${PROJECT_NAME} tmxutils jsoncpp) +TARGET_LINK_LIBRARIES ( ${PROJECT_NAME} tmxutils jsoncpp nats) #################################################### ################## Testing ####################### #################################################### +add_library(${PROJECT_NAME}_lib src/TelematicUnit.cpp) +target_link_libraries(${PROJECT_NAME}_lib PUBLIC tmxutils jsoncpp nats) enable_testing() include_directories(${PROJECT_SOURCE_DIR}/src) file(GLOB_RECURSE TEST_SOURCES LIST_DIRECTORIES false test/*.h test/*.cpp) add_executable(${PROJECT_NAME}_test ${TEST_SOURCES}) -target_link_libraries(${PROJECT_NAME}_test PRIVATE gtest tmxutils jsoncpp) \ No newline at end of file +target_link_libraries(${PROJECT_NAME}_test PRIVATE gtest ${PROJECT_NAME}_lib) \ No newline at end of file diff --git a/src/v2i-hub/TelematicBridgePlugin/README.md b/src/v2i-hub/TelematicBridgePlugin/README.md new file mode 100644 index 000000000..6ed0a0497 --- /dev/null +++ b/src/v2i-hub/TelematicBridgePlugin/README.md @@ -0,0 +1,96 @@ +## Telematic bridge introduction +Telematic bridge is a V2xHub plugin used to collect data stream generated by V2xHub and send the data stream into [telematic tool](https://github.com/usdot-fhwa-stol/cda-telematics) hosted in the AWS cloud. +## NATS Publisher/Subscriber +### NATS Connections and registration +#### Telematic plugin sends registration request to telematic server +##### Subject +NATS subject: *.register_unit +##### Request +``` +{ + "unit_id": "" +} +``` +##### Response +``` +{ + "unit_id": "", + "unit_type": "infrastructure", + "unit_name": "East Intersection", + "timestamp": "1678998191815233965", + "event_name": "wfd_integration_testing", + "location": "TFHRC", + "testing_type": "Integration" +} +``` + +### Available topics +#### Telematic plugin receives request from telematic UI +##### Subject +NATS subject: .available_topics +##### Request +``` +{ + "unit_id": "" +} +``` +##### Reply + +``` + { + "unit_id": "", + "unit_type": "infrastructure", + "unit_name": "East Intersection", + "timestamp": "1678998191815233965", + "event_name": "wfd_integration_testing", + "location": "TFHRC", + "testing_type": "Integration", + "topics": [ + { + "name": "J2735_TMSG03-P_CARMAStreetsPlugin" + }, + { + "name": "" + } + ] +} +``` + +### Selected topics +#### Telematic plugin receives selected topics from telematic UI +##### Subject +NATS subject: .publish_topics +#### Request +``` +{ + "unit_id": "", + "unit_type": "infrastructure", + "timestamp": 1663084528513000400, + "event_name": "wfd_integration_testing", + "location": "TFHRC", + "testing_type": "Integration", + "topics": [ + "", + "" + ] +} +``` +##### Reply +``` +"request received!" +``` + +## Check status +#### Telematic plugin receives live status request from telematic server +##### Subject +NATS subject: .check_status +##### Request +``` +{ + "unit_id": "" +} +``` +##### Reponse +``` +"OK" +``` \ No newline at end of file diff --git a/src/v2i-hub/TelematicBridgePlugin/manifest.json b/src/v2i-hub/TelematicBridgePlugin/manifest.json index 530dbe0eb..d133ceb0f 100644 --- a/src/v2i-hub/TelematicBridgePlugin/manifest.json +++ b/src/v2i-hub/TelematicBridgePlugin/manifest.json @@ -11,6 +11,16 @@ "key": "LogLevel", "default": "INFO", "description": "The log level for this plugin" + }, + { + "key": "NATSUrl", + "default": "nats://127.0.0.1:4222", + "description": "The NATS connection URL" + }, + { + "key": "MessageExclusionList", + "default": "System_KeepAlive_CommandPlugin,System_KeepAlive_CARMAStreetsPlugin,System_KeepAlive_CDASimAdapter,System_KeepAlive_MessageReceiver", + "description": "The list of messages are excluded from the available message list. Message name is a combination of message type, subtype and source separated by underscore. E.G: type_subtype_source" } ] } \ No newline at end of file diff --git a/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgePlugin.cpp b/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgePlugin.cpp index a316084f4..44a6738f2 100644 --- a/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgePlugin.cpp +++ b/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgePlugin.cpp @@ -1,9 +1,16 @@ #include "TelematicBridgePlugin.h" +using namespace tmx::utils; +using namespace std; + namespace TelematicBridge { TelematicBridgePlugin::TelematicBridgePlugin(const string &name) : PluginClient(name) { + _telematicUnitPtr = make_unique(); + _unitId = std::getenv("INFRASTRUCTURE_ID"); + _unitName = std::getenv("INFRASTRUCTURE_NAME"); + UpdateConfigSettings(); AddMessageFilter("*", "*", IvpMsgFlags_None); AddMessageFilter("J2735", "*", IvpMsgFlags_RouteDSRC); SubscribeToMessages(); @@ -20,14 +27,53 @@ namespace TelematicBridge auto messageFm = (MessageFrame_t *)calloc(1, sizeof(MessageFrame_t)); DecodeJ2735Msg(msg->payload->valuestring, messageFm); string xml_payload_str = ConvertJ2735FrameToXML(messageFm); - ASN_STRUCT_FREE(asn_DEF_MessageFrame, messageFm); - json["payload"] = StringToJson(xml2Json(xml_payload_str)); + ASN_STRUCT_FREE(asn_DEF_MessageFrame, messageFm); + string json_payload_str = xml2Json(xml_payload_str.c_str()); + json["payload"] = StringToJson(json_payload_str); } - auto jsonStr = JsonToString(json); - PLOG(logINFO) << jsonStr; + stringstream topic; + topic << (msg->type ? msg->type : "") << "_" << (msg->subtype ? msg->subtype : "") << "_" << (msg->source ? msg->source : ""); + auto topicStr = topic.str(); + _telematicUnitPtr->updateAvailableTopics(topicStr); + if (_telematicUnitPtr->inSelectedTopics(topicStr)) + { + _telematicUnitPtr->publishMessage(topicStr, json); + } + } + } + + void TelematicBridgePlugin::UpdateConfigSettings() + { + lock_guard lock(_configMutex); + GetConfigValue("NATSUrl", _natsURL); + GetConfigValue("MessageExclusionList", _excludedMessages); + unit_st unit = {_unitId, _unitName, UNIT_TYPE_INFRASTRUCTURE}; + if (_telematicUnitPtr) + { + _telematicUnitPtr->setUnit(unit); + _telematicUnitPtr->updateExcludedTopics(_excludedMessages); + } + } + + void TelematicBridgePlugin::OnStateChange(IvpPluginState state) + { + PluginClient::OnStateChange(state); + if (state == IvpPluginState_registered) + { + UpdateConfigSettings(); + if (_telematicUnitPtr) + { + _telematicUnitPtr->connect(_natsURL); + } } } + + void TelematicBridgePlugin::OnConfigChanged(const char *key, const char *value) + { + PluginClient::OnConfigChanged(key, value); + UpdateConfigSettings(); + } } // The main entry point for this application. diff --git a/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgePlugin.h b/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgePlugin.h index c790b6ca4..a75b21bbe 100644 --- a/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgePlugin.h +++ b/src/v2i-hub/TelematicBridgePlugin/src/TelematicBridgePlugin.h @@ -3,21 +3,30 @@ #include "PluginClient.h" #include "TelematicBridgeMsgWorker.h" +#include "TelematicUnit.h" +#include + -using namespace tmx::utils; -using namespace std; namespace TelematicBridge { - - class TelematicBridgePlugin : public tmx::utils::PluginClient { private: static CONSTEXPR const char *Telematic_MSGTYPE_J2735_STRING = "J2735"; + static CONSTEXPR const char *UNIT_TYPE_INFRASTRUCTURE = "Infrastructure"; + std::unique_ptr _telematicUnitPtr; + std::string _unitId; + std::string _unitName; + std::string _natsURL; + std::string _excludedMessages; + std::mutex _configMutex; void OnMessageReceived(IvpMessage *msg); public: - explicit TelematicBridgePlugin(const string& name); + explicit TelematicBridgePlugin(const std::string &name); + void OnConfigChanged(const char *key, const char *value) override; + void OnStateChange(IvpPluginState state) override; + void UpdateConfigSettings(); ~TelematicBridgePlugin() override = default; }; diff --git a/src/v2i-hub/TelematicBridgePlugin/src/TelematicUnit.cpp b/src/v2i-hub/TelematicBridgePlugin/src/TelematicUnit.cpp new file mode 100644 index 000000000..e99029b53 --- /dev/null +++ b/src/v2i-hub/TelematicBridgePlugin/src/TelematicUnit.cpp @@ -0,0 +1,320 @@ +#include "TelematicUnit.h" + +using namespace std; +using namespace tmx::utils; +using namespace std::chrono; + +namespace TelematicBridge +{ + void TelematicUnit::connect(const string &natsURL) + { + auto s = natsConnection_ConnectTo(&_conn, natsURL.c_str()); + PLOG(logINFO) << "NATS connection returned: " << natsStatus_GetText(s); + if (s == NATS_OK) + { + registerUnitRequestor(); + } + else + { + throw TelematicBridgeException(natsStatus_GetText(s)); + } + } + + void TelematicUnit::registerUnitRequestor() + { + // Reset registration status + bool isRegistered = false; + int attempts_count = 0; + + while (!isRegistered && attempts_count < REGISTRATION_MAX_ATTEMPTS) + { + attempts_count++; + PLOG(logDEBUG2) << "Inside register unit requestor"; + natsMsg *reply = nullptr; + string payload = "{\"unit_id\":\"" + _unit.unitId + "\"}"; + auto s = natsConnection_RequestString(&reply, _conn, REGISTER_UNIT_TOPIC, payload.c_str(), TIME_OUT); + if (s == NATS_OK) + { + auto replyStr = natsMsg_GetData(reply); + PLOG(logINFO) << "Received registered reply: " << replyStr; + + // Unit is registered when server responds with event information (location, testing_type, event_name) + isRegistered = validateRegisterStatus(replyStr); + natsMsg_Destroy(reply); + } + else + { + throw TelematicBridgeException(natsStatus_GetText(s)); + } + sleep(1); + } + + if (isRegistered) + { + // Provide below services when the unit is registered + availableTopicsReplier(); + selectedTopicsReplier(); + checkStatusReplier(); + } + } + + bool TelematicUnit::validateRegisterStatus(const string ®isterReply) + { + auto root = parseJson(registerReply); + if (root.isMember(LOCATION_KEY) && root.isMember(TESTING_TYPE_KEY) && root.isMember(EVENT_NAME_KEY)) + { + _eventLocation = root[LOCATION_KEY].asString(); + _testingType = root[TESTING_TYPE_KEY].asString(); + _eventName = root[EVENT_NAME_KEY].asString(); + return true; + } + PLOG(logERROR) << "Failed to register unit as event information (locatoin, testing type and event name) does not exist."; + return false; + } + + void TelematicUnit::availableTopicsReplier() + { + if (!_subAvailableTopic) + { + PLOG(logDEBUG2) << "Inside available topic replier"; + stringstream topic; + topic << _unit.unitId << AVAILABLE_TOPICS; + natsConnection_Subscribe(&_subAvailableTopic, _conn, topic.str().c_str(), onAvailableTopicsCallback, this); + } + } + + void TelematicUnit::selectedTopicsReplier() + { + if (!_subSelectedTopic) + { + PLOG(logDEBUG2) << "Inside selected topic replier"; + stringstream topic; + topic << _unit.unitId << PUBLISH_TOPICS; + natsConnection_Subscribe(&_subSelectedTopic, _conn, topic.str().c_str(), onSelectedTopicsCallback, this); + } + } + + void TelematicUnit::checkStatusReplier() + { + if (!_subCheckStatus) + { + PLOG(logDEBUG2) << "Inside check status replier"; + stringstream topic; + topic << _unit.unitId << CHECK_STATUS; + natsConnection_Subscribe(&_subCheckStatus, _conn, topic.str().c_str(), onCheckStatusCallback, this); + } + } + + void TelematicUnit::publishMessage(const string &topic, const Json::Value &payload) + { + auto pubMsgTopic = "streets." + _unit.unitId + ".data." + topic; + auto jsonStr = constructPublishedDataString(_unit, _eventLocation, _testingType, _eventName, topic, payload); + auto s = natsConnection_PublishString(_conn, pubMsgTopic.c_str(), jsonStr.c_str()); + if (s == NATS_OK) + { + PLOG(logINFO) << "Topic: " << pubMsgTopic << ". Published: " << jsonStr; + } + else + { + throw TelematicBridgeException(natsStatus_GetText(s)); + } + } + + void TelematicUnit::onAvailableTopicsCallback(natsConnection *nc, natsSubscription *sub, natsMsg *msg, void *object) + { + PLOG(logDEBUG3) << "Received available topics: " << natsMsg_GetSubject(msg) << " " << natsMsg_GetData(msg); + // Sends a reply + if (object && natsMsg_GetReply(msg) != nullptr) + { + const auto obj = (TelematicUnit *)object; + auto reply = constructAvailableTopicsReplyString(obj->getUnit(), obj->getEventLocation(), obj->getTestingType(), obj->getEventName(), obj->getAvailableTopics(), obj->getExcludedTopics()); + auto s = natsConnection_PublishString(nc, natsMsg_GetReply(msg), reply.c_str()); + natsMsg_Destroy(msg); + if (s == NATS_OK) + { + PLOG(logDEBUG3) << "Available topics replied: " << reply; + } + } + } + + void TelematicUnit::onSelectedTopicsCallback(natsConnection *nc, natsSubscription *sub, natsMsg *msg, void *object) + { + PLOG(logDEBUG3) << "Received selected topics: " << natsMsg_GetSubject(msg) << " " << natsMsg_GetData(msg); + // Sends a reply + if (natsMsg_GetReply(msg) != nullptr) + { + auto msgStr = natsMsg_GetData(msg); + auto root = parseJson(msgStr); + if (object && root.isMember(TOPICS_KEY) && root[TOPICS_KEY].isArray()) + { + auto obj = (TelematicUnit *)object; + // clear old selected topics + obj->clearSelectedTopics(); + + // update selected topics with selected topics from latest request + for (auto itr = root[TOPICS_KEY].begin(); itr != root[TOPICS_KEY].end(); itr++) + { + obj->addSelectedTopic(itr->asString()); + } + } + string reply = "request received!"; + auto s = natsConnection_PublishString(nc, natsMsg_GetReply(msg), reply.c_str()); + natsMsg_Destroy(msg); + if (s == NATS_OK) + { + PLOG(logDEBUG3) << "Selected topics replied: " << reply; + } + } + } + + void TelematicUnit::onCheckStatusCallback(natsConnection *nc, natsSubscription *sub, natsMsg *msg, void *object) + { + if (natsMsg_GetReply(msg) != nullptr) + { + auto s = natsConnection_PublishString(nc, natsMsg_GetReply(msg), "OK"); + if (s == NATS_OK) + { + PLOG(logDEBUG3) << "Received check status msg: " << natsMsg_GetSubject(msg) << " " << natsMsg_GetData(msg) << ". Replied: OK"; + } + natsMsg_Destroy(msg); + } + } + + string TelematicUnit::constructPublishedDataString(const unit_st &unit, const string &eventLocation, const string &testingType, const string &eventName, const string &topicName, const Json::Value &payload) const + { + Json::Value message; + message[UNIT_ID_KEY] = unit.unitId; + message[UNIT_NAME_KEY] = unit.unitName; + message[UNIT_TYPE_KEY] = unit.unitType; + message[LOCATION_KEY] = eventLocation; + message[TESTING_TYPE_KEY] = testingType; + message[EVENT_NAME_KEY] = eventName; + message[TOPIC_NAME_KEY] = topicName; + message[TIMESTAMP_KEY] = payload.isMember("timestamp") ? payload["timestamp"].asUInt64() * MILLI_TO_MICRO : duration_cast(system_clock::now().time_since_epoch()).count(); + message[PAYLOAD_KEY] = payload; + Json::FastWriter fasterWirter; + string jsonStr = fasterWirter.write(message); + return jsonStr; + } + + Json::Value TelematicUnit::parseJson(const string &jsonStr) + { + Json::Value root; + Json::Reader reader; + bool parsingSuccessful = reader.parse(jsonStr, root); + if (!parsingSuccessful) + { + throw TelematicBridgeException("Error parsing the reply message"); + } + return root; + } + + string TelematicUnit::constructAvailableTopicsReplyString(const unit_st &unit, const string &eventLocation, const string &testingType, const string &eventName, const vector &availableTopicList, const string &excludedTopics) + { + Json::Value message; + message[UNIT_ID_KEY] = unit.unitId; + message[UNIT_NAME_KEY] = unit.unitName; + message[UNIT_TYPE_KEY] = unit.unitType; + message[LOCATION_KEY] = eventLocation; + 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; + for (const auto &topic : availableTopicList) + { + if (!boost::icontains(excludedTopics, topic)) + { + Json::Value topicJson; + topicJson[NAME_KEY] = topic; + topics.append(topicJson); + } + } + message[TOPICS_KEY] = topics; + Json::FastWriter fasterWirter; + string reply = fasterWirter.write(message); + return reply; + } + + void TelematicUnit::updateAvailableTopics(const string &newTopic) + { + if (find(_availableTopics.begin(), _availableTopics.end(), newTopic) == _availableTopics.end()) + { + lock_guard lock(_availableTopicsMutex); + _availableTopics.push_back(newTopic); + PLOG(logINFO) << "Add topic (= " << newTopic << ") to available topics list. Size: " << _availableTopics.size(); + } + } + + void TelematicUnit::updateExcludedTopics(const string &excludedTopics) + { + lock_guard lock(_excludedTopicsMutex); + _excludedTopics = excludedTopics; + } + + bool TelematicUnit::inSelectedTopics(const string &topic) + { + if (find(_selectedTopics.begin(), _selectedTopics.end(), topic) == _selectedTopics.end()) + { + return false; + } + return true; + } + + void TelematicUnit::setUnit(const unit_st &unit) + { + lock_guard lock(_unitMutex); + _unit = unit; + } + + unit_st TelematicUnit::getUnit() const + { + return _unit; + } + + string TelematicUnit::getEventName() const + { + return _eventName; + } + + string TelematicUnit::getEventLocation() const + { + return _eventLocation; + } + + string TelematicUnit::getTestingType() const + { + return _testingType; + } + + vector TelematicUnit::getAvailableTopics() const + { + return _availableTopics; + } + + string TelematicUnit::getExcludedTopics() const + { + return _excludedTopics; + } + + void TelematicUnit::addSelectedTopic(const string &newSelectedTopic) + { + _selectedTopics.push_back(newSelectedTopic); + } + + void TelematicUnit::clearSelectedTopics() + { + _selectedTopics.clear(); + } + + TelematicUnit::~TelematicUnit() + { + natsSubscription_Destroy(_subAvailableTopic); + natsSubscription_Destroy(_subSelectedTopic); + natsSubscription_Destroy(_subCheckStatus); + natsConnection_Destroy(_conn); + _conn = nullptr; + _subAvailableTopic = nullptr; + _subSelectedTopic = nullptr; + _subCheckStatus = nullptr; + } +} \ No newline at end of file diff --git a/src/v2i-hub/TelematicBridgePlugin/src/TelematicUnit.h b/src/v2i-hub/TelematicBridgePlugin/src/TelematicUnit.h new file mode 100644 index 000000000..3a72f080b --- /dev/null +++ b/src/v2i-hub/TelematicBridgePlugin/src/TelematicUnit.h @@ -0,0 +1,216 @@ +#pragma once +#include +#include +#include +#include +#include +#include "PluginLog.h" +#include "ThreadTimer.h" +#include "TelematicBridgeException.h" +#include +#include + + +namespace TelematicBridge +{ + using unit_st = struct unit + { + std::string unitId; // Unique identifier for each unit + std::string unitName; // Descriptive name for each unit + std::string unitType; // Unit categorized base on unit type: platform or infrastructure + }; + + class TelematicUnit + { + private: + std::mutex _unitMutex; + std::mutex _availableTopicsMutex; + std::mutex _excludedTopicsMutex; + unit_st _unit; // Global variable to store the unit information + std::vector _availableTopics; // Global variable to store available topics + std::string _excludedTopics; // Global variable to store topics that are excluded by the users + std::vector _selectedTopics; // Global variable to store selected topics confirmed by users + static CONSTEXPR const char *AVAILABLE_TOPICS = ".available_topics"; // NATS subject to pub/sub available topics + static CONSTEXPR const char *REGISTER_UNIT_TOPIC = "*.register_unit"; // NATS subject to pub/sub registering unit + static CONSTEXPR const char *PUBLISH_TOPICS = ".publish_topics"; // NATS subject to publish data stream + static CONSTEXPR const char *CHECK_STATUS = ".check_status"; // NATS subject to pub/sub checking unit status + natsConnection *_conn = nullptr; // Global NATS connection object + natsSubscription *_subAvailableTopic = nullptr; // Global NATS subscription object + natsSubscription *_subSelectedTopic = nullptr; // Global NATS subscription object + natsSubscription *_subCheckStatus = nullptr; // Global NATS subscription object + int64_t TIME_OUT = 10000; // NATS Connection time out in milliseconds + std::string _eventName; // Testing event the unit is assigned to + std::string _eventLocation; // Testing event location + std::string _testingType; // Testing type + static CONSTEXPR const char *LOCATION_KEY = "location"; // location key used to find location value from JSON + static CONSTEXPR const char *TESTING_TYPE_KEY = "testing_type"; // testing_type key used to find testing_type value from JSON + static CONSTEXPR const char *EVENT_NAME_KEY = "event_name"; // event_name key used to find event_name value from JSON + static CONSTEXPR const char *UNIT_ID_KEY = "unit_id"; // unit_id key used to find unit_id value from JSON + static CONSTEXPR const char *UNIT_NAME_KEY = "unit_name"; // unit_name key used to find unit_name value from JSON + static CONSTEXPR const char *UNIT_TYPE_KEY = "unit_type"; // unit_type key used to find unit_type value from JSON + static CONSTEXPR const char *TOPIC_NAME_KEY = "topic_name"; // topic_name key used to find topic_name value from JSON + static CONSTEXPR const char *TIMESTAMP_KEY = "timestamp"; // timestamp key used to find timestamp value from JSON + static CONSTEXPR const char *PAYLOAD_KEY = "payload"; // payload key used to find payload value from JSON + static CONSTEXPR const char *TOPICS_KEY = "topics"; // topics key used to find topics value from JSON + static CONSTEXPR const char *NAME_KEY = "name"; // topics key used to find topics value from JSON + static const int MILLI_TO_MICRO = 1000; + static const int REGISTRATION_MAX_ATTEMPTS = 30; //The maximum numbers of attempts allowed to register this unit with server + + public: + /** + *@brief Construct telematic unit + */ + explicit TelematicUnit() = default; + /** + * @brief A function for telematic unit to connect to NATS server. Throw exception is connection failed. * + * @param string string NATS server URL + */ + void connect(const std::string &natsURL); + + /** + * @brief A NATS requestor for telematic unit to send register request to NATS server. + * If receives a response, it will update the isRegistered flag to indicate the unit is registered. + * If no response after the specified time out (unit of second) period, it considered register failed. + * */ + void registerUnitRequestor(); + + /** + * @brief A NATS replier to subscribe to NATS server and receive available topics request. + * Publish list of available topics after receiving/processing the request. + */ + void availableTopicsReplier(); + + /** + * @brief A NATS replier to subscribe to NATS server and receive requested topics from telematic server. + * Process the request and update the selectedTopics global variable. + * Respond the telematic server with acknowledgement. + */ + void selectedTopicsReplier(); + + /** + * @brief A NATS replier to subscribe to NATS server and receive request for status check from telematic server. + * Publish unit status upon receiving a request. + */ + void checkStatusReplier(); + + /** + * @brief A function to publish message stream into NATS server + */ + void publishMessage(const std::string &topic, const Json::Value &payload); + + /** + * @brief A function to parse a JSON string and create a JSON object. + * @param string input json string + * @return Json::Value + */ + static Json::Value parseJson(const std::string &jsonStr); + + /** + * @brief construct available topic response + * @param unit_st struct that contains unit related information + * @param vector of available topics + * @param string Excluded topics separated by commas + */ + static std::string constructAvailableTopicsReplyString(const unit_st &unit, const std::string &eventLocation, const std::string &testingType, const std::string &eventName, const std::vector &availableTopicList, const std::string &excludedTopics); + + /** + * @brief A function to update available topics global variables when discovering a new topic. + */ + void updateAvailableTopics(const std::string &newTopic); + + /** + * @brief Update telematic unit registration status when receiving registration reply from NATS server + * @param string Register reply in Json format + * @return True when reply with event information (location, testing type, event name), otherwise false. + */ + bool validateRegisterStatus(const std::string& registerReply); + + /** + * @brief construct Json data string that will be streamed into the cloud by a publisher + * @param unit_st struct that contains unit related information + * @param string Event location + * @param string Testing type + * @param string Event name + * @param string Topic name is a combination of type_subtype_source from TMX IVPMessage + * @param Json::Value Payload is the actual data generated by V2xHub plugin + */ + std::string constructPublishedDataString(const unit_st &unit, const std::string &eventLocation, const std::string &testingType, const std::string &eventName, const std::string &topicName, const Json::Value &payload) const; + + /** + * @brief A function to update global unit variable + * @param unit_st object that has the unit id, type and name information + */ + void setUnit(const unit_st &unit); + + /** + * @brief Return unit structure + */ + unit_st getUnit() const; + + /** + * @brief Return list of available topics + */ + std::vector getAvailableTopics() const; + + /** + * @brief Return excluded topics string. + */ + std::string getExcludedTopics() const; + + /** + * @brief Return event name + */ + std::string getEventName() const; + + /** + * @brief Return event location + */ + std::string getEventLocation() const; + + /** + * @brief Return testing type + */ + std::string getTestingType() const; + + /** + * @brief Add new selected topic into the selected topics list + */ + void addSelectedTopic(const std::string &newSelectedTopic); + + /** + * @brief Clear selected topics list + */ + void clearSelectedTopics(); + + /** + * @brief A function to update excluded topics. + * @param string Excluded topics separated by commas + */ + void updateExcludedTopics(const std::string &excludedTopics); + + /** + * @brief Check if the given topic is inside the selectedTopics list + * @param string A topic to check for existence + * @return boolean indicator whether the input topic eixst. + */ + bool inSelectedTopics(const std::string &topic); + + /** + * @brief A callback function for available topic replier + */ + static void onAvailableTopicsCallback(natsConnection *nc, natsSubscription *sub, natsMsg *msg, void *object); + + /** + * @brief A callback function for selected topic replier + */ + static void onSelectedTopicsCallback(natsConnection *nc, natsSubscription *sub, natsMsg *msg, void *object); + + /** + * @brief A callback function for check status replier + */ + static void onCheckStatusCallback(natsConnection *nc, natsSubscription *sub, natsMsg *msg, void *object); + + ~TelematicUnit(); + }; + +} // namespace TelematicBridge diff --git a/src/v2i-hub/TelematicBridgePlugin/test/test_TelematicUnit.cpp b/src/v2i-hub/TelematicBridgePlugin/test/test_TelematicUnit.cpp new file mode 100644 index 000000000..9a81427b4 --- /dev/null +++ b/src/v2i-hub/TelematicBridgePlugin/test/test_TelematicUnit.cpp @@ -0,0 +1,156 @@ +#include +#include "TelematicUnit.h" +using namespace std; +using namespace tmx::utils; +using namespace std::chrono; + +namespace TelematicBridge +{ + class test_TelematicUnit : public ::testing::Test + { + public: + shared_ptr _telematicUnitPtr = make_shared(); + unit_st unit = + { + "test_id", + "test_name", + "infrastructure"}; + }; + + TEST_F(test_TelematicUnit, setUnit) + { + ASSERT_NO_THROW(_telematicUnitPtr->setUnit(unit)); + ASSERT_EQ(unit.unitId, _telematicUnitPtr->getUnit().unitId); + } + + TEST_F(test_TelematicUnit, updateExcludedTopics) + { + ASSERT_NO_THROW(_telematicUnitPtr->updateExcludedTopics("test_topic")); + } + + TEST_F(test_TelematicUnit, updateAvailableTopics) + { + ASSERT_NO_THROW(_telematicUnitPtr->updateAvailableTopics("test_topic")); + } + + TEST_F(test_TelematicUnit, constructAvailableTopicsReplyString) + { + vector topics = {"test_topic", "excluded_topic"}; + string excluded_topic = "excluded_topic"; + string eventLocation = "location"; + string testingType = "unit_test"; + string eventName = "testing"; + 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()); + } + + TEST_F(test_TelematicUnit, constructPublishedDataString) + { + string eventLocation = "location"; + string testingType = "unit_test"; + string eventName = "testing"; + string topicName = "test_topic"; + Json::Value payload; + payload["timestamp"] = 1701099016033; + + auto reply = _telematicUnitPtr->constructPublishedDataString(unit, eventLocation, testingType, eventName, topicName, payload); + auto json = TelematicUnit::parseJson(reply); + ASSERT_EQ(eventLocation, json["location"].asString()); + ASSERT_EQ(testingType, json["testing_type"].asString()); + ASSERT_EQ(eventName, json["event_name"].asString()); + ASSERT_EQ(1701099016033000, json["timestamp"].asUInt64()); + ASSERT_THROW(TelematicUnit::parseJson("Invalid Json"), TelematicBridgeException); + + Json::Value payload2; + payload2["body"] = "invalid"; + reply = _telematicUnitPtr->constructPublishedDataString(unit, eventLocation, testingType, eventName, topicName, payload2); + json = TelematicUnit::parseJson(reply); + ASSERT_NEAR(duration_cast(system_clock::now().time_since_epoch()).count(), json["timestamp"].asUInt64(), 100); + } + + TEST_F(test_TelematicUnit, onCheckStatusCallback) + { + natsMsg *msg; + string data = "{\"data\":\"test\"}"; + natsMsg_Create(&msg, "test_subject", "Test_reply", data.c_str(), data.size()); + ASSERT_NO_THROW(TelematicUnit::onCheckStatusCallback(nullptr, nullptr, msg, nullptr)); + } + + TEST_F(test_TelematicUnit, onSelectedTopicsCallback) + { + natsMsg *msg; + string data = "{\"topics\":[\"test_topic\"]}"; + natsMsg_Create(&msg, "test_subject", "Test_reply", data.c_str(), data.size()); + ASSERT_NO_THROW(TelematicUnit::onSelectedTopicsCallback(nullptr, nullptr, msg, _telematicUnitPtr.get())); + ASSERT_TRUE(_telematicUnitPtr->inSelectedTopics("test_topic")); + } + + TEST_F(test_TelematicUnit, onAvailableTopicsCallback) + { + natsMsg *msg; + string data = "{\"data\":\"test\"}"; + natsMsg_Create(&msg, "test_subject", "Test_reply", data.c_str(), data.size()); + ASSERT_NO_THROW(TelematicUnit::onAvailableTopicsCallback(nullptr, nullptr, msg, _telematicUnitPtr.get())); + } + + TEST_F(test_TelematicUnit, publishMessage) + { + string topicName = "test_topic"; + Json::Value payload; + payload["body"] = "test_body"; + ASSERT_THROW(_telematicUnitPtr->publishMessage(topicName, payload), TelematicBridgeException); + } + + TEST_F(test_TelematicUnit, checkStatusReplier) + { + _telematicUnitPtr->checkStatusReplier(); + } + + TEST_F(test_TelematicUnit, selectedTopicsReplier) + { + _telematicUnitPtr->selectedTopicsReplier(); + } + + TEST_F(test_TelematicUnit, availableTopicsReplier) + { + _telematicUnitPtr->availableTopicsReplier(); + } + + TEST_F(test_TelematicUnit, registerUnitRequestor) + { + ASSERT_THROW(_telematicUnitPtr->registerUnitRequestor(), TelematicBridgeException); + } + + TEST_F(test_TelematicUnit, connect) + { + ASSERT_THROW(_telematicUnitPtr->connect("nats://127.0.0.1:4222"), TelematicBridgeException); + } + + TEST_F(test_TelematicUnit, getters) + { + ASSERT_EQ(0, _telematicUnitPtr->getAvailableTopics().size()); + ASSERT_EQ("", _telematicUnitPtr->getEventLocation()); + ASSERT_EQ("", _telematicUnitPtr->getEventName()); + ASSERT_EQ("", _telematicUnitPtr->getExcludedTopics()); + ASSERT_EQ("", _telematicUnitPtr->getTestingType()); + } + + TEST_F(test_TelematicUnit, selectedTopics) + { + string selectedTopic = "test_selected_topics"; + _telematicUnitPtr->addSelectedTopic(selectedTopic); + ASSERT_TRUE(_telematicUnitPtr->inSelectedTopics(selectedTopic)); + _telematicUnitPtr->clearSelectedTopics(); + ASSERT_FALSE(_telematicUnitPtr->inSelectedTopics(selectedTopic)); + } + TEST_F(test_TelematicUnit, validateRegisterStatus) + { + string replyStr = "{\"event_name\":\"Test\",\"location\":\"Local\",\"testing_type\":\"Integration\"}"; + _telematicUnitPtr->validateRegisterStatus(replyStr); + ASSERT_EQ("Local", _telematicUnitPtr->getEventLocation()); + ASSERT_EQ("Test", _telematicUnitPtr->getEventName()); + ASSERT_EQ("Integration", _telematicUnitPtr->getTestingType()); + } + +} \ No newline at end of file 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 20/28] 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 21/28] 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 22/28] 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 23/28] 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 24/28] 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(); } From 84251ed418bcce4be4452b3dc1e066b137f584ed Mon Sep 17 00:00:00 2001 From: Saikrishna Bairamoni <84093461+SaikrishnaBairamoni@users.noreply.github.com> Date: Sat, 10 Feb 2024 12:18:50 -0500 Subject: [PATCH 25/28] update dockercompose files to point lavida candidate images --- configuration/amd64/docker-compose.yml | 6 +++--- configuration/arm64/docker-compose.yml | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/configuration/amd64/docker-compose.yml b/configuration/amd64/docker-compose.yml index 0c9d5feb2..032e44eb4 100755 --- a/configuration/amd64/docker-compose.yml +++ b/configuration/amd64/docker-compose.yml @@ -20,7 +20,7 @@ services: - mysql-datavolume:/var/lib/mysql php: - image: usdotfhwaops/php:latest + image: usdotfhwaops/php:lavida container_name: php network_mode: host depends_on: @@ -30,7 +30,7 @@ services: tty: true v2xhub: - image: usdotfhwaops/v2xhubamd:latest + image: usdotfhwaops/v2xhubamd:lavida container_name: v2xhub network_mode: host restart: always @@ -46,7 +46,7 @@ services: - ./logs:/var/log/tmx - ./MAP:/var/www/plugins/MAP port_drayage_webservice: - image: usdotfhwaops/port-drayage-webservice:latest + image: usdotfhwaops/port-drayage-webservice:lavida container_name: port_drayage_webservice network_mode: host secrets: diff --git a/configuration/arm64/docker-compose.yml b/configuration/arm64/docker-compose.yml index a32c31fab..24179d2a2 100644 --- a/configuration/arm64/docker-compose.yml +++ b/configuration/arm64/docker-compose.yml @@ -20,7 +20,7 @@ services: - mysql-datavolume:/var/lib/mysql php: - image: usdotfhwaops/php_arm:latest + image: usdotfhwaops/php_arm:lavida container_name: php network_mode: host depends_on: @@ -30,7 +30,7 @@ services: tty: true v2xhub: - image: usdotfhwaops/v2xhubarm:latest + image: usdotfhwaops/v2xhubarm:lavida container_name: v2xhub network_mode: host restart: always @@ -44,7 +44,7 @@ services: - ./logs:/var/log/tmx - ./MAP:/var/www/plugins/MAP port_drayage_webservice: - image: usdotfhwaops/port-drayage-webservice_arm:latest + image: usdotfhwaops/port-drayage-webservice_arm:lavida container_name: port_drayage_webservice network_mode: host secrets: From 5525e534ac7729cf236c9087fff4fa86683bce36 Mon Sep 17 00:00:00 2001 From: paulbourelly999 <77466294+paulbourelly999@users.noreply.github.com> Date: Thu, 21 Mar 2024 09:22:13 -0400 Subject: [PATCH 26/28] vh-1277: Fix README and install nats.c version (#586) Note: newest version nats 3.8.0 breaks docker build # PR Details ## Description Fix the Docker build by setting nats.c version to install. Also fix the readme. CHanges already merged to develop under https://github.com/usdot-fhwa-OPS/V2X-Hub/pull/584 ## Related Issue VH-1277 ## Motivation and Context Fix CI processes and README ## How Has This Been Tested? CI ## 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. --- README.md | 10 +++++++--- ext/build.sh | 2 +- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index b5782ef46..eb5329ec6 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,11 @@ -| CicleCI Build Status | Sonar Code Quality | +## Develop CI/CD Processes +| V2X-Hub Docker Image Builds (x86 or AMD) | V2X-Hub Docker Image Builds (ARM) | Sonar Code Quality | +|----------------------|---------------------|---------------------| + [![Build Workflows](https://github.com/usdot-fhwa-OPS/V2X-Hub/actions/workflows/build.yml/badge.svg?branch=develop)](https://github.com/usdot-fhwa-OPS/V2X-Hub/actions/workflows/build.yml)|[![CircleCI](https://dl.circleci.com/status-badge/img/gh/usdot-fhwa-OPS/V2X-Hub/tree/develop.svg?style=svg)](https://dl.circleci.com/status-badge/redirect/gh/usdot-fhwa-OPS/V2X-Hub/tree/develop)| [![Quality Gate Status](https://sonarcloud.io/api/project_badges/measure?project=usdot-fhwa-ops_V2X-Hub&metric=alert_status)](https://sonarcloud.io/dashboard?id=usdot-fhwa-ops_V2X-Hub) | + ## Release CI/CD Processes +| V2X-Hub Docker Image Builds (x86 or AMD) | V2X-Hub Docker Image Builds (ARM) | |----------------------|---------------------| -[![CircleCI](https://circleci.com/gh/usdot-fhwa-OPS/V2X-Hub.svg?style=svg)](https://circleci.com/gh/usdot-fhwa-OPS/V2X-Hub) | [![Quality Gate Status](https://sonarcloud.io/api/project_badges/measure?project=usdot-fhwa-ops_V2X-Hub&metric=alert_status)](https://sonarcloud.io/dashboard?id=usdot-fhwa-ops_V2X-Hub) | - + [![Build Workflows](https://github.com/usdot-fhwa-OPS/V2X-Hub/actions/workflows/build.yml/badge.svg?branch=master)](https://github.com/usdot-fhwa-OPS/V2X-Hub/actions/workflows/build.yml)|[![CircleCI](https://dl.circleci.com/status-badge/img/gh/usdot-fhwa-OPS/V2X-Hub/tree/master.svg?style=svg)](https://dl.circleci.com/status-badge/redirect/gh/usdot-fhwa-OPS/V2X-Hub/tree/master)| # Overview In order to bring infrastructure components into the Connected Vehicle architecture, you need software that will facilitate the exchange of data in a format that can be understood by both vehicles and infrastructure devices The V2X Hub, takes in data from vehicles via Basic Safety Messages (BSM) in a Society of Automotive Engineers (SAE) standard format and translates the data to a National Transportation Communications for ITS Protocol (NTCIP) that infrastructure components can understand. And vice versa. It translates Signal Phase and Timing (SPaT) data from NTCIP to SAE and sends it to the Roadside Unit (RSU) for broadcast to mobile devices, including vehicles. diff --git a/ext/build.sh b/ext/build.sh index 91d59ca8a..d5c773685 100755 --- a/ext/build.sh +++ b/ext/build.sh @@ -65,7 +65,7 @@ pushd /tmp if [ -d "nats.c" ]; then rm -r nats.c fi -git clone https://github.com/nats-io/nats.c +git clone https://github.com/nats-io/nats.c --branch v3.7.0 cd nats.c cmake . -DNATS_BUILD_NO_SPIN=ON make -j${numCPU} From 6d60d91d95af9be4532afad1e24a4fc45072cf55 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Thu, 21 Mar 2024 13:29:58 -0400 Subject: [PATCH 27/28] add time sync logs (#583) # PR Details ## Description Adds logs to support https://github.com/usdot-fhwa-stol/carma-analytics-fotda/pull/43 ## Related Issue CDAR-774 ## Motivation and Context Data Analysis ## How Has This Been Tested? ## Types of changes - [ ] 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. - [ ] 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. --- .../CDASimAdapter/src/CDASimAdapter.cpp | 58 +++++++++++-------- 1 file changed, 33 insertions(+), 25 deletions(-) diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp index b9665bcee..1986bb608 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp @@ -1,5 +1,5 @@ #include "include/CDASimAdapter.hpp" - +#include using namespace tmx::utils; @@ -18,12 +18,12 @@ namespace CDASimAdapter{ success = GetConfigValue("X", location.X); success = success && GetConfigValue("Y", location.Y); success = success && GetConfigValue("Z", location.Z); - PLOG(logINFO) << "Location of Simulated V2X-Hub updated to : {" << location.X << ", " + PLOG(logINFO) << "Location of Simulated V2X-Hub updated to : {" << location.X << ", " << location.Y << ", " << location.Z << "}." << std::endl; success = success && GetConfigValue("MaxConnectionAttempts", max_connection_attempts); success = success && GetConfigValue("ConnectionSleepTime", connection_sleep_time); if (connection_sleep_time < 1 ) { - PLOG(logWARNING) << "ConnectionSleepTime of " << connection_sleep_time << " is invalid. Valid values are <= 1." << std::endl; + PLOG(logWARNING) << "ConnectionSleepTime of " << connection_sleep_time << " is invalid. Valid values are <= 1." << std::endl; connection_sleep_time = 1; } if (!success) { @@ -33,7 +33,7 @@ namespace CDASimAdapter{ void CDASimAdapter::OnConfigChanged(const char *key, const char *value) { PluginClient::OnConfigChanged(key, value); - UpdateConfigSettings(); + UpdateConfigSettings(); } void CDASimAdapter::OnStateChange(IvpPluginState state) { @@ -41,7 +41,7 @@ namespace CDASimAdapter{ if (state == IvpPluginState_registered) { UpdateConfigSettings(); - + // While CARMA Simulation connection is down, attempt to reconnect int connection_attempts = 0; while ( (!connection || !connection->is_connected()) && (connection_attempts < max_connection_attempts || max_connection_attempts < 1 ) ) { @@ -66,22 +66,30 @@ namespace CDASimAdapter{ }else { PLOG(logERROR) << "CDASim connection failed!" << std::endl; } - + } } void CDASimAdapter::forward_time_sync_message(tmx::messages::TimeSyncMessage &msg) { + std::string payload =msg.to_string(); - PLOG(logDEBUG1) << "Sending Time Sync Message " << msg << std::endl; + // A script to validate time synchronization of tools in CDASim currently relies on the following + // log line. TODO: This line is meant to be removed in the future upon completion of this work: + // https://github.com/usdot-fhwa-stol/carma-analytics-fotda/pull/43 + auto time_now = std::chrono::system_clock::now(); + auto epoch = time_now.time_since_epoch(); + auto milliseconds = std::chrono::duration_cast(epoch); + PLOG(logDEBUG1) << "Simulation Time: " << msg.get_timestep() << " where current system time is: " << milliseconds.count() << ", where msgs: " << msg << std::endl; + this->BroadcastMessage(msg, _name, 0 , IvpMsgFlags_None); - + } void CDASimAdapter::forward_simulated_detected_message(tmx::messages::simulation::SensorDetectedObject &msg) { PLOG(logDEBUG1) << "Sending Simulated SensorDetectedObject Message " << msg << std::endl; - this->BroadcastMessage(msg, _name, 0 , IvpMsgFlags_None); + this->BroadcastMessage(msg, _name, 0 , IvpMsgFlags_None); } bool CDASimAdapter::connect() { @@ -97,7 +105,7 @@ namespace CDASimAdapter{ std::string infrastructure_id = sim::get_sim_config(sim::INFRASTRUCTURE_ID); std::string sensor_json_file_path = sim::get_sim_config(sim::SENSOR_JSON_FILE_PATH); - PLOG(logINFO) << "CDASim connecting " << simulation_ip << + PLOG(logINFO) << "CDASim connecting " << simulation_ip << "\nUsing Registration Port : " << std::to_string( simulation_registration_port) << " Time Sync Port: " << std::to_string( time_sync_port) << " and V2X Port: " << std::to_string(v2x_port) << std::endl; if ( connection ) { @@ -108,20 +116,20 @@ namespace CDASimAdapter{ connection = std::make_unique(simulation_ip, infrastructure_id, simulation_registration_port, sim_v2x_port, local_ip, time_sync_port, simulated_interaction_port, v2x_port, location, sensor_json_file_path); } - } + } catch (const TmxException &e) { PLOG(logERROR) << "Exception occured attempting to initialize CDASim Connection : " << e.what() << std::endl; return false; } catch (const std::invalid_argument &e ) { // std::stoul throws invalid arguement exception when provided with a string that contains characters that are not numbers. - PLOG(logERROR) << "Exception occured attempting to initialize CDASim Connection : " << e.what() << + PLOG(logERROR) << "Exception occured attempting to initialize CDASim Connection : " << e.what() << ". Check environment variables are set to the correct type!"; return false; - } + } return connection->connect(); } - + void CDASimAdapter::start_immediate_forward_thread() { @@ -130,10 +138,10 @@ namespace CDASimAdapter{ } immediate_forward_tick_id = immediate_forward_timer->AddPeriodicTick([this]() { this->attempt_message_from_v2xhub(); - + } // end of lambda expression , std::chrono::milliseconds(5) ); - + immediate_forward_timer->Start(); } @@ -170,19 +178,19 @@ namespace CDASimAdapter{ void CDASimAdapter::start_sensor_detected_object_detection_thread() { PLOG(logDEBUG) << "Creating Thread Timer for simulated external object" << std::endl; - try + try { if(!external_object_detection_thread_timer) { 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; auto msg = connection->consume_sensor_detected_object_message(); - if ( !msg.is_empty()) { + if ( !msg.is_empty()) { this->forward_simulated_detected_message(msg); } - else + else { PLOG(logDEBUG1) << "CDASim connection has not yet received an simulated sensor detected message!" << std::endl; } @@ -190,7 +198,7 @@ namespace CDASimAdapter{ , std::chrono::milliseconds(5)); external_object_detection_thread_timer->Start(); } - catch ( const UdpServerRuntimeError &e ) + catch ( const UdpServerRuntimeError &e ) { PLOG(logERROR) << "Error occured :" << e.what() << std::endl; } @@ -238,23 +246,23 @@ namespace CDASimAdapter{ } } - + int CDASimAdapter::Main() { - PLOG(logINFO) << "Starting plugin " << _name << std::endl; + PLOG(logINFO) << "Starting plugin " << _name << std::endl; while (_plugin->state != IvpPluginState_error) { if (IsPluginState(IvpPluginState_registered)) { - + } } return EXIT_SUCCESS; } - + } From bce7236e87bbcf0b0bed3c53f158652e7dee1e09 Mon Sep 17 00:00:00 2001 From: paulbourelly999 <77466294+paulbourelly999@users.noreply.github.com> Date: Mon, 8 Apr 2024 14:16:06 -0400 Subject: [PATCH 28/28] Update version to 7.6.0 (#596) # PR Details ## Description Update V2X-hub release version ## Related Issue ## Motivation and Context ## How Has This Been Tested? ## Types of changes - [ ] 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. - [ ] 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. @MishkaMN --- configuration/amd64/docker-compose.yml | 6 +++--- configuration/arm64/docker-compose.yml | 6 +++--- src/v2i-hub/CARMACloudPlugin/CMakeLists.txt | 2 +- src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt | 2 +- src/v2i-hub/CDASimAdapter/CMakeLists.txt | 2 +- src/v2i-hub/CommandPlugin/CMakeLists.txt | 2 +- src/v2i-hub/CswPlugin/CMakeLists.txt | 2 +- src/v2i-hub/DmsPlugin/CMakeLists.txt | 2 +- src/v2i-hub/ERVCloudForwardingPlugin/CMakeLists.txt | 2 +- src/v2i-hub/ImmediateForwardPlugin/CMakeLists.txt | 2 +- src/v2i-hub/LocationPlugin/CMakeLists.txt | 2 +- src/v2i-hub/MapPlugin/CMakeLists.txt | 2 +- src/v2i-hub/MessageLoggerPlugin/CMakeLists.txt | 2 +- src/v2i-hub/MessageReceiverPlugin/CMakeLists.txt | 2 +- src/v2i-hub/ODEForwardPlugin/CMakeLists.txt | 2 +- src/v2i-hub/PedestrianPlugin/CMakeLists.txt | 2 +- src/v2i-hub/PortDrayagePlugin/CMakeLists.txt | 2 +- src/v2i-hub/PreemptionPlugin/CMakeLists.txt | 2 +- src/v2i-hub/RSUHealthMonitorPlugin/CMakeLists.txt | 2 +- src/v2i-hub/RtcmPlugin/CMakeLists.txt | 2 +- src/v2i-hub/SpatPlugin/CMakeLists.txt | 2 +- src/v2i-hub/TelematicBridgePlugin/CMakeLists.txt | 2 +- src/v2i-hub/TimPlugin/CMakeLists.txt | 2 +- web/admin/admin.html | 2 +- 24 files changed, 28 insertions(+), 28 deletions(-) diff --git a/configuration/amd64/docker-compose.yml b/configuration/amd64/docker-compose.yml index 032e44eb4..89a1b00a9 100755 --- a/configuration/amd64/docker-compose.yml +++ b/configuration/amd64/docker-compose.yml @@ -20,7 +20,7 @@ services: - mysql-datavolume:/var/lib/mysql php: - image: usdotfhwaops/php:lavida + image: usdotfhwaops/php:7.6.0 container_name: php network_mode: host depends_on: @@ -30,7 +30,7 @@ services: tty: true v2xhub: - image: usdotfhwaops/v2xhubamd:lavida + image: usdotfhwaops/v2xhubamd:7.6.0 container_name: v2xhub network_mode: host restart: always @@ -46,7 +46,7 @@ services: - ./logs:/var/log/tmx - ./MAP:/var/www/plugins/MAP port_drayage_webservice: - image: usdotfhwaops/port-drayage-webservice:lavida + image: usdotfhwaops/port-drayage-webservice:7.6.0 container_name: port_drayage_webservice network_mode: host secrets: diff --git a/configuration/arm64/docker-compose.yml b/configuration/arm64/docker-compose.yml index 24179d2a2..4e378c4c5 100644 --- a/configuration/arm64/docker-compose.yml +++ b/configuration/arm64/docker-compose.yml @@ -20,7 +20,7 @@ services: - mysql-datavolume:/var/lib/mysql php: - image: usdotfhwaops/php_arm:lavida + image: usdotfhwaops/php_arm:7.6.0 container_name: php network_mode: host depends_on: @@ -30,7 +30,7 @@ services: tty: true v2xhub: - image: usdotfhwaops/v2xhubarm:lavida + image: usdotfhwaops/v2xhubarm:7.6.0 container_name: v2xhub network_mode: host restart: always @@ -44,7 +44,7 @@ services: - ./logs:/var/log/tmx - ./MAP:/var/www/plugins/MAP port_drayage_webservice: - image: usdotfhwaops/port-drayage-webservice_arm:lavida + image: usdotfhwaops/port-drayage-webservice_arm:7.6.0 container_name: port_drayage_webservice network_mode: host secrets: diff --git a/src/v2i-hub/CARMACloudPlugin/CMakeLists.txt b/src/v2i-hub/CARMACloudPlugin/CMakeLists.txt index 6c913e69d..45ac871f1 100644 --- a/src/v2i-hub/CARMACloudPlugin/CMakeLists.txt +++ b/src/v2i-hub/CARMACloudPlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT ( CARMACloudPlugin VERSION 7.5.1 LANGUAGES CXX ) +PROJECT ( CARMACloudPlugin VERSION 7.6.0 LANGUAGES CXX ) SET (TMX_PLUGIN_NAME "CARMACloud") add_compile_options(-fPIC) diff --git a/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt b/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt index 08981e25d..a4f9c63f0 100644 --- a/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt +++ b/src/v2i-hub/CARMAStreetsPlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT ( CARMAStreetsPlugin VERSION 7.5.1 LANGUAGES CXX ) +PROJECT ( CARMAStreetsPlugin VERSION 7.6.0 LANGUAGES CXX ) BuildTmxPlugin ( ) diff --git a/src/v2i-hub/CDASimAdapter/CMakeLists.txt b/src/v2i-hub/CDASimAdapter/CMakeLists.txt index 8fbc4f376..27fdc1093 100755 --- a/src/v2i-hub/CDASimAdapter/CMakeLists.txt +++ b/src/v2i-hub/CDASimAdapter/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT ( CDASimAdapter VERSION 7.5.1 LANGUAGES CXX ) +PROJECT ( CDASimAdapter VERSION 7.6.0 LANGUAGES CXX ) set(CMAKE_CXX_STANDARD 17) FIND_PACKAGE( carma-clock ) diff --git a/src/v2i-hub/CommandPlugin/CMakeLists.txt b/src/v2i-hub/CommandPlugin/CMakeLists.txt index a1543ab51..3e4e8d521 100644 --- a/src/v2i-hub/CommandPlugin/CMakeLists.txt +++ b/src/v2i-hub/CommandPlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT ( CommandPlugin VERSION 7.5.1 LANGUAGES CXX ) +PROJECT ( CommandPlugin VERSION 7.6.0 LANGUAGES CXX ) FIND_PACKAGE (OpenSSL REQUIRED) diff --git a/src/v2i-hub/CswPlugin/CMakeLists.txt b/src/v2i-hub/CswPlugin/CMakeLists.txt index 5e154a2c7..63243d902 100644 --- a/src/v2i-hub/CswPlugin/CMakeLists.txt +++ b/src/v2i-hub/CswPlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT ( CswPlugin VERSION 7.5.1 LANGUAGES CXX ) +PROJECT ( CswPlugin VERSION 7.6.0 LANGUAGES CXX ) SET (TMX_PLUGIN_NAME "CSW") diff --git a/src/v2i-hub/DmsPlugin/CMakeLists.txt b/src/v2i-hub/DmsPlugin/CMakeLists.txt index 08cf09b17..f790c51fe 100644 --- a/src/v2i-hub/DmsPlugin/CMakeLists.txt +++ b/src/v2i-hub/DmsPlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT ( DmsPlugin VERSION 7.5.1 LANGUAGES CXX ) +PROJECT ( DmsPlugin VERSION 7.6.0 LANGUAGES CXX ) SET (TMX_PLUGIN_NAME "Dynamic Message Sign") diff --git a/src/v2i-hub/ERVCloudForwardingPlugin/CMakeLists.txt b/src/v2i-hub/ERVCloudForwardingPlugin/CMakeLists.txt index cc456077a..ab0b8a9fb 100644 --- a/src/v2i-hub/ERVCloudForwardingPlugin/CMakeLists.txt +++ b/src/v2i-hub/ERVCloudForwardingPlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT(ERVCloudForwardingPlugin VERSION 7.5.1 LANGUAGES CXX) +PROJECT(ERVCloudForwardingPlugin VERSION 7.6.0 LANGUAGES CXX) SET(TMX_PLUGIN_NAME "ERVCloudForwarding") add_compile_options(-fPIC) diff --git a/src/v2i-hub/ImmediateForwardPlugin/CMakeLists.txt b/src/v2i-hub/ImmediateForwardPlugin/CMakeLists.txt index 31e28d6ae..752f486c4 100644 --- a/src/v2i-hub/ImmediateForwardPlugin/CMakeLists.txt +++ b/src/v2i-hub/ImmediateForwardPlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT ( ImmediateForwardPlugin VERSION 7.5.1 LANGUAGES CXX ) +PROJECT ( ImmediateForwardPlugin VERSION 7.6.0 LANGUAGES CXX ) SET (TMX_PLUGIN_NAME "Immediate Forward") diff --git a/src/v2i-hub/LocationPlugin/CMakeLists.txt b/src/v2i-hub/LocationPlugin/CMakeLists.txt index 5926227c4..cb6c943f7 100644 --- a/src/v2i-hub/LocationPlugin/CMakeLists.txt +++ b/src/v2i-hub/LocationPlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -project( LocationPlugin VERSION 7.5.1 LANGUAGES CXX ) +project( LocationPlugin VERSION 7.6.0 LANGUAGES CXX ) SET (TMX_PLUGIN_NAME Location) diff --git a/src/v2i-hub/MapPlugin/CMakeLists.txt b/src/v2i-hub/MapPlugin/CMakeLists.txt index 553f1e58a..e40a6c240 100644 --- a/src/v2i-hub/MapPlugin/CMakeLists.txt +++ b/src/v2i-hub/MapPlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT ( MapPlugin VERSION 7.5.1 LANGUAGES CXX ) +PROJECT ( MapPlugin VERSION 7.6.0 LANGUAGES CXX ) SET (TMX_PLUGIN_NAME "MAP") diff --git a/src/v2i-hub/MessageLoggerPlugin/CMakeLists.txt b/src/v2i-hub/MessageLoggerPlugin/CMakeLists.txt index 514b31984..3153bd268 100644 --- a/src/v2i-hub/MessageLoggerPlugin/CMakeLists.txt +++ b/src/v2i-hub/MessageLoggerPlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT ( MessageLoggerPlugin VERSION 7.5.1 LANGUAGES CXX ) +PROJECT ( MessageLoggerPlugin VERSION 7.6.0 LANGUAGES CXX ) SET (TMX_PLUGIN_NAME "MessageLoggerPlugin") diff --git a/src/v2i-hub/MessageReceiverPlugin/CMakeLists.txt b/src/v2i-hub/MessageReceiverPlugin/CMakeLists.txt index 908bc1e90..38d6cd7a0 100644 --- a/src/v2i-hub/MessageReceiverPlugin/CMakeLists.txt +++ b/src/v2i-hub/MessageReceiverPlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT ( MessageReceiverPlugin VERSION 7.5.1 LANGUAGES CXX ) +PROJECT ( MessageReceiverPlugin VERSION 7.6.0 LANGUAGES CXX ) set(CMAKE_CXX_STANDARD 11) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/src/v2i-hub/ODEForwardPlugin/CMakeLists.txt b/src/v2i-hub/ODEForwardPlugin/CMakeLists.txt index 8c594fdbe..c574fc4b6 100644 --- a/src/v2i-hub/ODEForwardPlugin/CMakeLists.txt +++ b/src/v2i-hub/ODEForwardPlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT ( ODEForwardPlugin VERSION 7.5.1 LANGUAGES CXX ) +PROJECT ( ODEForwardPlugin VERSION 7.6.0 LANGUAGES CXX ) SET (TMX_PLUGIN_NAME "ODEForwardPlugin") diff --git a/src/v2i-hub/PedestrianPlugin/CMakeLists.txt b/src/v2i-hub/PedestrianPlugin/CMakeLists.txt index ecb8c8cec..735d84e80 100755 --- a/src/v2i-hub/PedestrianPlugin/CMakeLists.txt +++ b/src/v2i-hub/PedestrianPlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT ( PedestrianPlugin VERSION 7.5.1 LANGUAGES CXX ) +PROJECT ( PedestrianPlugin VERSION 7.6.0 LANGUAGES CXX ) SET (TMX_PLUGIN_NAME "Pedestrian") add_compile_options(-fPIC) diff --git a/src/v2i-hub/PortDrayagePlugin/CMakeLists.txt b/src/v2i-hub/PortDrayagePlugin/CMakeLists.txt index 199be4090..1a7a4b4da 100644 --- a/src/v2i-hub/PortDrayagePlugin/CMakeLists.txt +++ b/src/v2i-hub/PortDrayagePlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT ( PortDrayagePlugin VERSION 7.5.1 LANGUAGES CXX ) +PROJECT ( PortDrayagePlugin VERSION 7.6.0 LANGUAGES CXX ) SET (TMX_PLUGIN_NAME "PortDrayage") set(CMAKE_AUTOMOC ON) diff --git a/src/v2i-hub/PreemptionPlugin/CMakeLists.txt b/src/v2i-hub/PreemptionPlugin/CMakeLists.txt index 1613c3b46..a7c0db86a 100644 --- a/src/v2i-hub/PreemptionPlugin/CMakeLists.txt +++ b/src/v2i-hub/PreemptionPlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT ( PreemptionPlugin VERSION 7.5.1 LANGUAGES CXX ) +PROJECT ( PreemptionPlugin VERSION 7.6.0 LANGUAGES CXX ) SET (TMX_PLUGIN_NAME "Preemption") diff --git a/src/v2i-hub/RSUHealthMonitorPlugin/CMakeLists.txt b/src/v2i-hub/RSUHealthMonitorPlugin/CMakeLists.txt index d863a4bf1..8cbcf79ce 100755 --- a/src/v2i-hub/RSUHealthMonitorPlugin/CMakeLists.txt +++ b/src/v2i-hub/RSUHealthMonitorPlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT(RSUHealthMonitorPlugin VERSION 7.5.1 LANGUAGES CXX) +PROJECT(RSUHealthMonitorPlugin VERSION 7.6.0 LANGUAGES CXX) set(TMX_PLUGIN_NAME "RSU Health Monitor") diff --git a/src/v2i-hub/RtcmPlugin/CMakeLists.txt b/src/v2i-hub/RtcmPlugin/CMakeLists.txt index 694b04ef2..b4a5f0488 100644 --- a/src/v2i-hub/RtcmPlugin/CMakeLists.txt +++ b/src/v2i-hub/RtcmPlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT ( RtcmPlugin VERSION 7.5.1 LANGUAGES CXX ) +PROJECT ( RtcmPlugin VERSION 7.6.0 LANGUAGES CXX ) SET (TMX_PLUGIN_NAME "RTCM") diff --git a/src/v2i-hub/SpatPlugin/CMakeLists.txt b/src/v2i-hub/SpatPlugin/CMakeLists.txt index fb28fcada..bdff7a0b8 100644 --- a/src/v2i-hub/SpatPlugin/CMakeLists.txt +++ b/src/v2i-hub/SpatPlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT ( SpatPlugin VERSION 7.5.1 LANGUAGES CXX ) +PROJECT ( SpatPlugin VERSION 7.6.0 LANGUAGES CXX ) SET (TMX_PLUGIN_NAME "SPAT") diff --git a/src/v2i-hub/TelematicBridgePlugin/CMakeLists.txt b/src/v2i-hub/TelematicBridgePlugin/CMakeLists.txt index df7b1c0f7..59215c46f 100644 --- a/src/v2i-hub/TelematicBridgePlugin/CMakeLists.txt +++ b/src/v2i-hub/TelematicBridgePlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT (TelematicBridgePlugin VERSION 7.5.1 LANGUAGES CXX) +PROJECT (TelematicBridgePlugin VERSION 7.6.0 LANGUAGES CXX) set (TMX_PLUGIN_NAME "Telematic Bridge") diff --git a/src/v2i-hub/TimPlugin/CMakeLists.txt b/src/v2i-hub/TimPlugin/CMakeLists.txt index 3f8c9779e..51ad9797d 100644 --- a/src/v2i-hub/TimPlugin/CMakeLists.txt +++ b/src/v2i-hub/TimPlugin/CMakeLists.txt @@ -1,4 +1,4 @@ -PROJECT ( TimPlugin VERSION 7.5.1 LANGUAGES CXX ) +PROJECT ( TimPlugin VERSION 7.6.0 LANGUAGES CXX ) SET (TMX_PLUGIN_NAME "TIM") add_compile_options(-fPIC) diff --git a/web/admin/admin.html b/web/admin/admin.html index 735c9b63a..2dfdaa0e1 100644 --- a/web/admin/admin.html +++ b/web/admin/admin.html @@ -49,7 +49,7 @@
-
7.4.0
+
7.6.0