From 0963f012d80a2e101608bc166e0cec0d767669fd Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Mon, 17 Jul 2023 23:17:42 +0000 Subject: [PATCH 01/35] init --- .devcontainer/docker-compose-vscode.yml | 1 + configuration/amd64/docker-compose.yml | 11 ++ src/tmx/Messages/include/MessageTypes.h | 2 + .../include/simulation/ExternalObject.h | 145 ++++++++++++++++++ .../include/simulation/ObjectEnumTypes.h | 23 +++ .../simulation/PresenceVectorEnumTypes.h | 27 ++++ .../src/simulation/SimulationEnvUtils.h | 5 + .../src/CARMAStreetsPlugin.cpp | 8 + .../src/CARMAStreetsPlugin.h | 2 + .../scripts/send_sim_external_object_udp.py | 117 ++++++++++++++ .../CDASimAdapter/src/CDASimAdapter.cpp | 41 ++++- .../CDASimAdapter/src/CDASimConnection.cpp | 41 +++-- .../src/include/CDASimAdapter.hpp | 10 ++ .../src/include/CDASimConnection.hpp | 16 +- .../test/TestCARMASimulationConnection.cpp | 11 +- .../test/TestSimulationMessages.cpp | 31 ++++ 16 files changed, 468 insertions(+), 23 deletions(-) create mode 100644 src/tmx/Messages/include/simulation/ExternalObject.h create mode 100644 src/tmx/Messages/include/simulation/ObjectEnumTypes.h create mode 100644 src/tmx/Messages/include/simulation/PresenceVectorEnumTypes.h create mode 100755 src/v2i-hub/CDASimAdapter/scripts/send_sim_external_object_udp.py create mode 100644 src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp diff --git a/.devcontainer/docker-compose-vscode.yml b/.devcontainer/docker-compose-vscode.yml index a34dd25c9..24c56ba10 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_EXTERNAL_OBJECT_PORT=7576 - V2X_PORT=8686 - INFRASTRUCTURE_ID=1 - KAFKA_BROKER_ADDRESS=127.0.0.1:9092 diff --git a/configuration/amd64/docker-compose.yml b/configuration/amd64/docker-compose.yml index 6b5188297..088100a81 100755 --- a/configuration/amd64/docker-compose.yml +++ b/configuration/amd64/docker-compose.yml @@ -38,6 +38,17 @@ services: - db environment: - MYSQL_PASSWORD=/run/secrets/mysql_password + - SIMULATION_MODE=true + - SIMULATION_IP=127.0.0.1 + - SIMULATION_REGISTRATION_PORT=6767 + - LOCAL_IP=127.0.0.1 + - TIME_SYNC_TOPIC=time_sync + - TIME_SYNC_PORT=7575 + - SIM_V2X_PORT=5757 + - SIM_EXTERNAL_OBJECT_PORT=7576 + - V2X_PORT=8686 + - INFRASTRUCTURE_ID=1 + - KAFKA_BROKER_ADDRESS=127.0.0.1:9092 secrets: - mysql_password volumes: diff --git a/src/tmx/Messages/include/MessageTypes.h b/src/tmx/Messages/include/MessageTypes.h index a4a84f6f4..2821d10cc 100644 --- a/src/tmx/Messages/include/MessageTypes.h +++ b/src/tmx/Messages/include/MessageTypes.h @@ -54,6 +54,7 @@ static CONSTEXPR const char *MSGTYPE_VEHICLE_STRING = "Vehicle"; static CONSTEXPR const char *MSGTYPE_PEDESTRIAN_STRING = "Pedestrian"; static CONSTEXPR const char *MSGTYPE_PMM_STRING = "Pmm"; static CONSTEXPR const char *MSGTYPE_RADIO_STRING = "Radio"; +static CONSTEXPR const char *MSGTYPE_DETECTED_STRING = "Simulation"; enum MsgSubType { @@ -91,6 +92,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_EXTERNAL_OBJECT_STRING = "ExternalObject"; } /* End namespace messages */ diff --git a/src/tmx/Messages/include/simulation/ExternalObject.h b/src/tmx/Messages/include/simulation/ExternalObject.h new file mode 100644 index 000000000..a7d8586bf --- /dev/null +++ b/src/tmx/Messages/include/simulation/ExternalObject.h @@ -0,0 +1,145 @@ +#ifndef INCLUDE_SIMULATED_EXTERNALOBJECT_H_ +#define INCLUDE_SIMULATED_EXTERNALOBJECT_H_ + +#include +#include +// #include +#include +#include + +namespace tmx +{ + namespace messages + { + namespace simulation + { + /** + * This ExternalObject 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 ExternalObject : public tmx::message + { + public: + ExternalObject(){}; + ExternalObject(const tmx::message_container_type &contents) : tmx::message(contents) {} + ExternalObject(PRESENCE_VECTOR_TYPES presenceVector, uint32_t id, + double confidence, OBJECT_TYPES objectType, bool dynamticObj) + { + set_PresenceVector(presenceVector); + set_Id(id); + set_Confidence(confidence); + set_ObjectType(objectType); + set_DynamticObj(dynamticObj); + }; + ~ExternalObject(){}; + // Message type fpr routing this message through TMX core + static constexpr const char *MessageType = MSGTYPE_DETECTED_STRING; + + // Message sub type for routing this message through TMX core + static constexpr const char *MessageSubType = MSGSUBTYPE_EXTERNAL_OBJECT_STRING; + + /** + *Header contains the frame rest of the fields will use + */ + // sequence ID: consecutively increasing ID + std_attribute(this->msg, uint32_t, HeaderSeq, 0, ); + // Two-integer timestamp that is expressed as: + // # * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') + // # * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') + // # time-handling sugar is provided by the client library + // The seconds component, valid over all int32 values. + std_attribute(this->msg, uint32_t, HeaderTimeSec, 0, ); + // # The nanoseconds component, valid in the range [0, 10e9). + std_attribute(this->msg, uint32_t, HeaderTimeNanoSec, 0, ); + + // A presence vector, this message is used to describe objects coming from potentially different + // sources. The presence vector is used to determine what items are set by the producer. + std_attribute(this->msg, PRESENCE_VECTOR_TYPES, PresenceVector, PRESENCE_VECTOR_TYPES::OBJECT_TYPE_PRESENCE_VECTOR, ); + + // Object id. Matching ids on a topic should refer to the same object within some time period, expanded + std_attribute(this->msg, uint32_t, Id, 0, ); + + // bsm id is of form [0xff, 0xff, 0xff, 0xff]. It is not required. + // uint8[] bsm_id + struct BSMID + { + uint8_t BsmId = 0; + + BSMID() {} + BSMID(std::uint8_t bsmId) : BsmId(bsmId) {} + + static message_tree_type to_tree(BSMID element) + { + message_tree_type treeElement; + treeElement.put("BsmId", element.BsmId); + return treeElement; + } + + static BSMID from_tree(message_tree_type &treeElement) + { + BSMID element; + element.BsmId = treeElement.get("BsmId"); + return element; + } + }; + array_attribute( BSMID, BsmId); + + // Pose of the object within the frame specified in header + // geometry_msgs/PoseWithCovariance pose + // This represents a pose in free space with uncertainty. + std_attribute(this->msg, double, PosePointX, 0, ); + std_attribute(this->msg, double, PosePointY, 0, ); + std_attribute(this->msg, double, PosePointZ, 0, ); + + // This represents an orientation in free space in quaternion form. + std_attribute(this->msg, double, PoseQuaternionX, 0, ); + std_attribute(this->msg, double, PoseQuaternionY, 0, ); + std_attribute(this->msg, double, PoseQuaternionZ, 0, ); + std_attribute(this->msg, double, PoseQuaternionW, 0, ); + + // Row-major representation of the 6x6 covariance matrix + // # The orientation parameters use a fixed-axis representation. + // # In order, the parameters are: + // # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis) + std_attribute(this->msg, double, Covariance, 0, ); + + // #Average velocity of the object within the frame specified in header + // geometry_msgs/TwistWithCovariance velocity + + // #Instantaneous velocity of an object within the frame specified in header + // geometry_msgs/TwistWithCovariance velocity_inst + + // #The size of the object aligned along the axis of the object described by the orientation in pose + // #Dimensions are specified in meters + /** + * # This represents a vector in free space. + # It is only meant to represent a direction. Therefore, it does not + # make sense to apply a translation to it (e.g., when applying a + # generic rigid transformation to a Vector3, tf2 will only apply the + # rotation). If you want your data to be translatable too, use the + # geometry_msgs/Point message instead. + */ + // geometry_msgs/Vector3 size + std_attribute(this->msg, double, SizeX, 0, ); + std_attribute(this->msg, double, SizeY, 0, ); + std_attribute(this->msg, double, SizeZ, 0, ); + + // #Confidence [0,1] + std_attribute(this->msg, double, Confidence, 0, ); + + // #describes a general object type as defined in this message + std_attribute(this->msg, OBJECT_TYPES, ObjectType, OBJECT_TYPES::UNKNOWN, ); + + // # Binary value to show if the object is static or dynamic (1: dynamic, 0: static) + std_attribute(this->msg, bool, DynamticObj, false, ); + + // Predictions for the object. It is not required. + // carma_perception_msgs/PredictedState[] predictions + }; + + } + } + +}; // namespace tmx +#endif diff --git a/src/tmx/Messages/include/simulation/ObjectEnumTypes.h b/src/tmx/Messages/include/simulation/ObjectEnumTypes.h new file mode 100644 index 000000000..15d443a48 --- /dev/null +++ b/src/tmx/Messages/include/simulation/ObjectEnumTypes.h @@ -0,0 +1,23 @@ +#ifndef INCLUDE_SIMULATED_OBJECT_TYPE_H_ +#define INCLUDE_SIMULATED_OBJECT_TYPE_H_ + +namespace tmx +{ + namespace messages + { + namespace simulation + { + // #used for object type + enum OBJECT_TYPES + { + UNKNOWN = 0, + SMALL_VEHICLE = 1, + LARGE_VEHICLE = 2, + MOTORCYCLE = 3, + PEDESTRIAN = 4 + }; + } + } + +}; // namespace tmx +#endif \ No newline at end of file diff --git a/src/tmx/Messages/include/simulation/PresenceVectorEnumTypes.h b/src/tmx/Messages/include/simulation/PresenceVectorEnumTypes.h new file mode 100644 index 000000000..8515fc728 --- /dev/null +++ b/src/tmx/Messages/include/simulation/PresenceVectorEnumTypes.h @@ -0,0 +1,27 @@ +#ifndef INCLUDE_SIMULATED_PRESENCE_VECTOR_H_ +#define INCLUDE_SIMULATED_PRESENCE_VECTOR_H_ + +namespace tmx +{ + namespace messages + { + namespace simulation + { + enum PRESENCE_VECTOR_TYPES + { + ID_PRESENCE_VECTOR = 1, + POSE_PRESENCE_VECTOR = 2, + VELOCITY_PRESENCE_VECTOR = 4, + VELOCITY_INST_PRESENCE_VECTOR = 8, + SIZE_PRESENCE_VECTOR = 16, + CONFIDENCE_PRESENCE_VECTOR = 32, + OBJECT_TYPE_PRESENCE_VECTOR = 64, + BSM_ID_PRESENCE_VECTOR = 128, + DYNAMIC_OBJ_PRESENCE = 256, + PREDICTION_PRESENCE_VECTOR = 512 + }; + } + } + +}; // namespace tmx +#endif \ No newline at end of file diff --git a/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h b/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h index 1b04ffe11..08b93e49d 100644 --- a/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h +++ b/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h @@ -41,6 +41,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_EXTERNAL_OBJECT_PORT = "SIM_EXTERNAL_OBJECT_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/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index 71703bd59..5ebdeb5e0 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -25,6 +25,7 @@ CARMAStreetsPlugin::CARMAStreetsPlugin(string name) : AddMessageFilter < tsm2Message > (this, &CARMAStreetsPlugin::HandleMobilityPathMessage); AddMessageFilter < MapDataMessage > (this, &CARMAStreetsPlugin::HandleMapMessage); AddMessageFilter < SrmMessage > (this, &CARMAStreetsPlugin::HandleSRMMessage); + AddMessageFilter < simulation::ExternalObject> (this, &CARMAStreetsPlugin::HandleSimulatedExternalMessage ); SubscribeToMessages(); @@ -626,6 +627,13 @@ void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){ } } + +void CARMAStreetsPlugin::HandleSimulatedExternalMessage(simulation::ExternalObject &msg, routeable_message &routeableMsg) +{ + PLOG(logINFO) << "HandleSimulatedExternalMessage called." < #include #include "JsonToJ2735SSMConverter.h" +#include @@ -48,6 +49,7 @@ 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); + void HandleSimulatedExternalMessage(simulation::ExternalObject &msg, routeable_message &routeableMsg); /** * @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/scripts/send_sim_external_object_udp.py b/src/v2i-hub/CDASimAdapter/scripts/send_sim_external_object_udp.py new file mode 100755 index 000000000..e2ec2d4de --- /dev/null +++ b/src/v2i-hub/CDASimAdapter/scripts/send_sim_external_object_udp.py @@ -0,0 +1,117 @@ + +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":{ + "is_simulation": False, + "datum": "", + "proj_string": "", + "sensor_x": 0.0, + "sensor_y": 0.0, + "sensor_z": 0.0, + "infrastructure_id": "", + "sensor_id": "" + }, + "header": { + "seq": 0, + "stamp": { + "secs": 0, + "nsecs": 0 + } + }, + "id": 0, + "pose": { + "pose": { + "position": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 0.0 + } + }, + "covariance": [ + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + ] + }, + "velocity": { + "twist": { + "linear": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "angular": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + } + }, + "covariance": [ + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + ] + }, + "size": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "confidence": 0.0, + "object_type": "", + "dynamic_obj": False + } + + 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 a036ff6f2..0d0bd5269 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp @@ -40,6 +40,7 @@ namespace CDASimAdapter{ start_time_sync_thread_timer(); start_amf_msg_thread(); start_binary_msg_thread(); + start_external_object_detection_thread(); } } @@ -77,6 +78,11 @@ namespace CDASimAdapter{ } + void CDASimAdapter::forward_simulated_external_message(tmx::messages::simulation::ExternalObject &msg) { + PLOG(logDEBUG1) << "Sending Simulated ExternalObject 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); @@ -84,6 +90,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)); + uint external_object_detection_port = std::stoul(sim::get_sim_config(sim::SIM_EXTERNAL_OBJECT_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)); uint infrastructure_id = std::stoul(sim::get_sim_config(sim::INFRASTRUCTURE_ID));; @@ -96,11 +103,11 @@ namespace CDASimAdapter{ } 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, external_object_detection_port, v2x_port, location )); } 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, external_object_detection_port, v2x_port, location); } } catch (const TmxException &e) { @@ -156,6 +163,36 @@ namespace CDASimAdapter{ } } + void CDASimAdapter::start_external_object_detection_thread() { + PLOG(logDEBUG) << "Creating Thread Timer for simulated external object" << std::endl; + try + { + if(!external_bject_detection_thread_timer) + { + external_bject_detection_thread_timer = std::make_unique(); + } + external_bject_detection_thread_timer->AddPeriodicTick([this](){ + PLOG(logDEBUG1) << "Listening for External Object Message from CDASim." << std::endl; + auto msg = connection->consume_external_object_message(); + if ( !msg.is_empty()) { + PLOG(logDEBUG1) << "Consumed External Object Message: " << msg<forward_simulated_external_message(msg); + PLOG(logDEBUG1) << "External Object Message Forwarded to CARMAStreetsPlugin!" << std::endl; + } + else + { + PLOG(logDEBUG1) << "CDASim connection has not yet received an simulated external message!" << std::endl; + } + }//End lambda + , std::chrono::milliseconds(100)); + external_bject_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 7349bda38..7d7722440 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp @@ -5,10 +5,10 @@ using namespace tmx::utils; namespace CDASimAdapter{ CDASimConnection::CDASimConnection(const std::string &simulation_ip, const uint 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 external_object_detection_port, const uint v2x_port, const WGS84Point &location) : _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), + _simulation_v2x_port(sim_v2x_port), _local_ip(local_ip), _time_sync_port(time_sync_port), _external_object_detection_port(external_object_detection_port),_v2x_port(v2x_port), _location(location) { 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, _external_object_detection_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,_external_object_detection_port, _v2x_port, _simulation_v2x_port )) { _connected = false; return _connected; } @@ -33,7 +33,7 @@ namespace CDASimAdapter{ return _connected; } - std::string CDASimConnection::get_handshake_json(const uint infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint v2x_port, + std::string CDASimConnection::get_handshake_json(const uint infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint external_object_detection_port, const uint v2x_port, const WGS84Point &location) const { @@ -44,6 +44,7 @@ namespace CDASimAdapter{ message["infrastructureId"] = infrastructure_id; message["rxMessagePort"] = v2x_port; message["timeSyncPort"] = time_sync_port; + message["ExternalObjectDetectionPort"] = external_object_detection_port; message["location"]["latitude"] = location.Latitude; message["location"]["longitude"] = location.Longitude; message["location"]["elevation"] = location.Elevation; @@ -53,13 +54,13 @@ namespace CDASimAdapter{ } bool CDASimConnection::carma_simulation_handshake(const std::string &simulation_ip, const uint 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 external_object_detection_port, const uint v2x_port, const WGS84Point &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, external_object_detection_port, v2x_port, location); try { @@ -75,7 +76,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 external_object_detection_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 @@ -92,12 +93,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(external_object_detection_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); + external_object_listener = std::make_shared (local_ip, external_object_detection_port); } catch (const UdpClientRuntimeError &e) { PLOG(logERROR) << "Encountered UDPClient Runtime error during UDP connection initialization : " << e.what() << std::endl; @@ -125,6 +128,22 @@ namespace CDASimAdapter{ } + tmx::messages::simulation::ExternalObject CDASimConnection::consume_external_object_message() const + { + tmx::messages::simulation::ExternalObject externalObj; + externalObj.clear(); + if(external_object_listener) + { + std::string str_msg = consume_server_message(external_object_listener); + externalObj.set_contents(str_msg); + } + else + { + throw std::runtime_error("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); diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp index 6de71bcd6..88c5ec032 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp @@ -99,6 +99,11 @@ namespace CDASimAdapter { * @param msg TimeSyncMessage. */ void forward_time_sync_message(tmx::messages::TimeSyncMessage &msg); + /** + * @brief Forward simulated external object message to TMX message bus for other V2X-Hub Plugin + * @param msg simulation::ExternalObject. + */ + void forward_simulated_external_message(tmx::messages::simulation::ExternalObject &msg); /** * @brief Method to start thread timer for regular interval actions lauched on seperate thread. */ @@ -107,6 +112,10 @@ namespace CDASimAdapter { * @brief Method to consume time sychrononization from CDASimConnection and forward to tmx core and CARMA Streets */ void attempt_time_sync(); + /** + * @brief Method to start thread timer for regular interval actions lauched on seperate thread. + */ + void start_external_object_detection_thread(); private: @@ -115,6 +124,7 @@ namespace CDASimAdapter { std::unique_ptr connection; std::mutex _lock; std::unique_ptr thread_timer; + std::unique_ptr external_bject_detection_thread_timer; int time_sync_tick_id; std::unique_ptr amf_thread_timer; std::unique_ptr binary_thread_timer; diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp index ab6771609..fec88be2d 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,7 +34,7 @@ namespace CDASimAdapter { * @param producer Kafka Producer for forwarding time synchronization messages. */ explicit CDASimConnection( const std::string &simulation_ip, const uint 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 external_object_detection_port, const uint v2x_port, const tmx::utils::WGS84Point &location); /** @@ -83,6 +84,11 @@ namespace CDASimAdapter { * @return TimeSyncMessage. */ tmx::messages::TimeSyncMessage consume_time_sync_message() const; + /** + * @brief Method to consume incoming external object message. + * @return simulation::ExternalObject. + */ + tmx::messages::simulation::ExternalObject consume_external_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 uint 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 external_object_detection_port, const uint v2x_port, const tmx::utils::WGS84Point &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 external_object_detection_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,13 +137,14 @@ 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 uint infrastructure_id, const std::string &local_ip, const uint time_sync_port, + std::string get_handshake_json(const uint infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint external_object_detection_port, const uint v2x_port, const tmx::utils::WGS84Point &location) const; std::string _simulation_ip; uint _simulation_registration_port; uint _infrastructure_id; uint _simulation_v2x_port; + uint _external_object_detection_port; std::string _local_ip; uint _time_sync_port; uint _v2x_port; @@ -150,6 +157,7 @@ namespace CDASimAdapter { std::shared_ptr immediate_forward_listener; std::shared_ptr message_receiver_publisher; std::shared_ptr time_sync_listener; + std::shared_ptr external_object_listener; FRIEND_TEST(TestCARMASimulationConnection, get_handshake_json); }; diff --git a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp index 442c83142..eaded6b38 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 and mock kafka producer. WGS84Point 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, 1215, location); } 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) { @@ -87,16 +87,15 @@ namespace CDASimAdapter { location.Elevation = 1000; location.Latitude = 38.955; location.Longitude = -77.149; - - ASSERT_EQ(connection->get_handshake_json(4566, "127.0.0.1", 4567, 4568, location), - "{\n \"infrastructureId\" : 4566,\n \"location\" : {\n \"elevation\" : 1000.0,\n \"latitude\" : 38.954999999999998,\n \"longitude\" : -77.149000000000001\n },\n \"rxMessageIpAddress\" : \"127.0.0.1\",\n \"rxMessagePort\" : 4568,\n \"timeSyncPort\" : 4567\n}\n"); + ASSERT_EQ(connection->get_handshake_json(4566, "127.0.0.1", 4567, 4568, 4569, location), + "{\n \"ExternalObjectDetectionPort\" : 4568,\n \"infrastructureId\" : 4566,\n \"location\" : {\n \"elevation\" : 1000.0,\n \"latitude\" : 38.954999999999998,\n \"longitude\" : -77.149000000000001\n },\n \"rxMessageIpAddress\" : \"127.0.0.1\",\n \"rxMessagePort\" : 4569,\n \"timeSyncPort\" : 4567\n}\n"); } TEST_F( TestCARMASimulationConnection, carma_simulation_handshake) { WGS84Point location; // UDP creation error ASSERT_FALSE(connection->carma_simulation_handshake("", 45, NULL, - "", 45, 45, location)); + "", 45, 45, 45, location)); } TEST_F(TestCARMASimulationConnection, connect) { diff --git a/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp b/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp new file mode 100644 index 000000000..af6ce9f77 --- /dev/null +++ b/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp @@ -0,0 +1,31 @@ +#include +#include +#include +#include + +using namespace std; +using namespace tmx; +using namespace tmx::messages; + +TEST(SimulationMessages, ExternalObjectToRoutableMessage) +{ + simulation::ExternalObject externalObj; + string expectedStr = "{\"metadata\":{\"is_simulation\":false,\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":0.0,\"sensor_y\":0.0,\"sensor_z\":0.0,\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":0,\"stamp\":{\"secs\":0,\"nsecs\":0}},\"id\":0,\"pose\":{\"pose\":{\"position\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"orientation\":{\"x\":0.0,\"y\":0.0,\"z\":0.0,\"w\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"velocity\":{\"twist\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"size\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"confidence\":0.0,\"object_type\":\"\",\"dynamic_obj\":false}"; + externalObj.set_contents(expectedStr); + ASSERT_EQ(expectedStr, externalObj.to_string()); + ASSERT_EQ("ExternalObject", std::string(simulation::ExternalObject::MessageSubType)); + ASSERT_EQ("Simulation", std::string(simulation::ExternalObject::MessageType)); + tmx::routeable_message routeableMsg; + routeableMsg.initialize(externalObj, "CDASimAdapter", 0 , IvpMsgFlags_None); + string output = "{\"metadata\":{\"is_simulation\":\"false\",\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":\"0.0\",\"sensor_y\":\"0.0\",\"sensor_z\":\"0.0\",\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":\"0\",\"stamp\":{\"secs\":\"0\",\"nsecs\":\"0\"}},\"id\":\"0\",\"pose\":{\"pose\":{\"position\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\"},\"orientation\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\",\"w\":\"0.0\"}},\"covariance\":[\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\"]},\"velocity\":{\"twist\":{\"linear\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\"},\"angular\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\"}},\"covariance\":[\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\"]},\"size\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\"},\"confidence\":\"0.0\",\"object_type\":\"\",\"dynamic_obj\":\"false\"}"; + ASSERT_EQ(output, routeableMsg.get_payload_str()); + ASSERT_EQ("json", routeableMsg.get_encoding()); + ASSERT_EQ(0, routeableMsg.get_flags()); + auto current_time_mill = boost::chrono::duration_cast(boost::chrono::system_clock::now().time_since_epoch()).count(); + ASSERT_NEAR(current_time_mill, routeableMsg.get_timestamp(), 1000); + ASSERT_NEAR(current_time_mill, routeableMsg.get_millisecondsSinceEpoch(), 1000); + ASSERT_EQ(0, routeableMsg.get_sourceId()); + ASSERT_EQ("CDASimAdapter", routeableMsg.get_source()); + ASSERT_EQ("ExternalObject",routeableMsg.get_subtype()); + ASSERT_EQ("Detected", routeableMsg.get_type()); +} \ No newline at end of file From c3b7ba84ed9f17e64ddab937f877f1aba91f823f Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Tue, 18 Jul 2023 03:41:59 +0000 Subject: [PATCH 02/35] update externalObject --- .../include/simulation/ExternalObject.h | 81 ++++++++++---- .../src/CARMAStreetsPlugin.cpp | 7 +- .../src/CARMAStreetsPlugin.h | 3 +- .../scripts/send_sim_external_object_udp.py | 101 +++++++----------- .../CDASimAdapter/src/CDASimAdapter.cpp | 4 +- 5 files changed, 103 insertions(+), 93 deletions(-) diff --git a/src/tmx/Messages/include/simulation/ExternalObject.h b/src/tmx/Messages/include/simulation/ExternalObject.h index a7d8586bf..7eac33c0b 100644 --- a/src/tmx/Messages/include/simulation/ExternalObject.h +++ b/src/tmx/Messages/include/simulation/ExternalObject.h @@ -22,23 +22,24 @@ namespace tmx { public: ExternalObject(){}; - ExternalObject(const tmx::message_container_type &contents) : tmx::message(contents) {} - ExternalObject(PRESENCE_VECTOR_TYPES presenceVector, uint32_t id, - double confidence, OBJECT_TYPES objectType, bool dynamticObj) - { - set_PresenceVector(presenceVector); - set_Id(id); - set_Confidence(confidence); - set_ObjectType(objectType); - set_DynamticObj(dynamticObj); - }; + ExternalObject(const tmx::message_container_type &contents) : tmx::message(contents) {}; ~ExternalObject(){}; // Message type fpr routing this message through TMX core static constexpr const char *MessageType = MSGTYPE_DETECTED_STRING; // Message sub type for routing this message through TMX core static constexpr const char *MessageSubType = MSGSUBTYPE_EXTERNAL_OBJECT_STRING; - + /** + * Metadata to describe the external object + */ + std_attribute(this->msg, bool, MetadataIsSimulation, false, ); + std_attribute(this->msg, std::string, MetadataDatum, "", ); + std_attribute(this->msg, std::string, MetadataProjString, "", ); + std_attribute(this->msg, std::string, MetadataSensorX, "", ); + std_attribute(this->msg, std::string, MetadataSensorY, "", ); + std_attribute(this->msg, std::string, MetadataSensorZ, "", ); + std_attribute(this->msg, std::string, MetadataInfrastructureId, "", ); + std_attribute(this->msg, std::string, MetadataSensorId, "", ); /** *Header contains the frame rest of the fields will use */ @@ -49,9 +50,9 @@ namespace tmx // # * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') // # time-handling sugar is provided by the client library // The seconds component, valid over all int32 values. - std_attribute(this->msg, uint32_t, HeaderTimeSec, 0, ); + std_attribute(this->msg, uint32_t, HeaderStampSecs, 0, ); // # The nanoseconds component, valid in the range [0, 10e9). - std_attribute(this->msg, uint32_t, HeaderTimeNanoSec, 0, ); + std_attribute(this->msg, uint32_t, HeaderStampNSecs, 0, ); // A presence vector, this message is used to describe objects coming from potentially different // sources. The presence vector is used to determine what items are set by the producer. @@ -88,27 +89,62 @@ namespace tmx // Pose of the object within the frame specified in header // geometry_msgs/PoseWithCovariance pose // This represents a pose in free space with uncertainty. - std_attribute(this->msg, double, PosePointX, 0, ); - std_attribute(this->msg, double, PosePointY, 0, ); - std_attribute(this->msg, double, PosePointZ, 0, ); + std_attribute(this->msg, double, PosePosePositionX, 0, ); + std_attribute(this->msg, double, PosePosePositionY, 0, ); + std_attribute(this->msg, double, PosePosePositionZ, 0, ); // This represents an orientation in free space in quaternion form. - std_attribute(this->msg, double, PoseQuaternionX, 0, ); - std_attribute(this->msg, double, PoseQuaternionY, 0, ); - std_attribute(this->msg, double, PoseQuaternionZ, 0, ); - std_attribute(this->msg, double, PoseQuaternionW, 0, ); + std_attribute(this->msg, double, PosePoseOrientationX, 0, ); + std_attribute(this->msg, double, PosePoseOrientationY, 0, ); + std_attribute(this->msg, double, PosePoseOrientationZ, 0, ); + std_attribute(this->msg, double, PosePoseOrientationW, 0, ); // Row-major representation of the 6x6 covariance matrix // # The orientation parameters use a fixed-axis representation. // # In order, the parameters are: // # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis) - std_attribute(this->msg, double, Covariance, 0, ); + struct Covariance + { + double covariance = 0; + + Covariance() {} + Covariance(double covariance) : covariance(covariance) {} + + static message_tree_type to_tree(Covariance element) + { + message_tree_type treeElement; + treeElement.put("Covariance", element.covariance); + return treeElement; + } + + static Covariance from_tree(message_tree_type &treeElement) + { + Covariance element; + element.covariance = treeElement.get("Covariance"); + return element; + } + }; + array_attribute( Covariance, PoseCovariance); // #Average velocity of the object within the frame specified in header // geometry_msgs/TwistWithCovariance velocity + std_attribute(this->msg, double, VelocityTwistLinearX, 0, ); + std_attribute(this->msg, double, VelocityTwistLinearY, 0, ); + std_attribute(this->msg, double, VelocityTwistLinearZ, 0, ); + std_attribute(this->msg, double, VelocityTwistAngularX, 0, ); + std_attribute(this->msg, double, VelocityTwistAngularY, 0, ); + std_attribute(this->msg, double, VelocityTwistAngularZ, 0, ); + array_attribute( Covariance, VelocityCovariance); // #Instantaneous velocity of an object within the frame specified in header // geometry_msgs/TwistWithCovariance velocity_inst + std_attribute(this->msg, double, VelocityInstTwistLinearX, 0, ); + std_attribute(this->msg, double, VelocityInstTwistLinearY, 0, ); + std_attribute(this->msg, double, VelocityInstTwistLinearZ, 0, ); + std_attribute(this->msg, double, VelocityInstTwistAngularX, 0, ); + std_attribute(this->msg, double, VelocityInstTwistAngularY, 0, ); + std_attribute(this->msg, double, VelocityInstTwistAngularZ, 0, ); + array_attribute( Covariance, VelocityInstCovariance); // #The size of the object aligned along the axis of the object described by the orientation in pose // #Dimensions are specified in meters @@ -117,8 +153,7 @@ namespace tmx # It is only meant to represent a direction. Therefore, it does not # make sense to apply a translation to it (e.g., when applying a # generic rigid transformation to a Vector3, tf2 will only apply the - # rotation). If you want your data to be translatable too, use the - # geometry_msgs/Point message instead. + # rotation). */ // geometry_msgs/Vector3 size std_attribute(this->msg, double, SizeX, 0, ); diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index 5ebdeb5e0..d4b8c232e 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -25,7 +25,7 @@ CARMAStreetsPlugin::CARMAStreetsPlugin(string name) : AddMessageFilter < tsm2Message > (this, &CARMAStreetsPlugin::HandleMobilityPathMessage); AddMessageFilter < MapDataMessage > (this, &CARMAStreetsPlugin::HandleMapMessage); AddMessageFilter < SrmMessage > (this, &CARMAStreetsPlugin::HandleSRMMessage); - AddMessageFilter < simulation::ExternalObject> (this, &CARMAStreetsPlugin::HandleSimulatedExternalMessage ); + AddMessageFilter < ExternalObject > (this, &CARMAStreetsPlugin::HandleSimulatedExternalMessage ); SubscribeToMessages(); @@ -628,10 +628,11 @@ void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){ } -void CARMAStreetsPlugin::HandleSimulatedExternalMessage(simulation::ExternalObject &msg, routeable_message &routeableMsg) +void CARMAStreetsPlugin::HandleSimulatedExternalMessage(ExternalObject &msg, routeable_message &routeableMsg) { PLOG(logINFO) << "HandleSimulatedExternalMessage called." <AddPeriodicTick([this](){ PLOG(logDEBUG1) << "Listening for External Object Message from CDASim." << std::endl; auto msg = connection->consume_external_object_message(); - if ( !msg.is_empty()) { - PLOG(logDEBUG1) << "Consumed External Object Message: " << msg<forward_simulated_external_message(msg); - PLOG(logDEBUG1) << "External Object Message Forwarded to CARMAStreetsPlugin!" << std::endl; } else { From c98778dedaf23827d25e1bf7f6375f479eb6ddbf Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Tue, 18 Jul 2023 19:42:39 +0000 Subject: [PATCH 03/35] update external object converter --- src/tmx/Messages/include/simulation/BSMID.h | 38 ++ .../Messages/include/simulation/Covariance.h | 41 ++ .../include/simulation/ExternalObject.h | 64 +-- .../simulation/PresenceVectorEnumTypes.h | 1 + src/tmx/TmxUtils/CMakeLists.txt | 3 +- .../SimulationExternalObjectConverter.cpp | 409 ++++++++++++++++++ .../SimulationExternalObjectConverter.h | 30 ++ ...t_simulation_external_object_converter.cpp | 48 ++ src/v2i-hub/CARMAStreetsPlugin/manifest.json | 5 + .../src/CARMAStreetsPlugin.cpp | 10 +- .../src/CARMAStreetsPlugin.h | 2 + .../scripts/send_sim_external_object_udp.py | 122 +++--- .../CDASimAdapter/src/CDASimConnection.cpp | 4 +- .../src/include/CDASimConnection.hpp | 2 + 14 files changed, 668 insertions(+), 111 deletions(-) create mode 100644 src/tmx/Messages/include/simulation/BSMID.h create mode 100644 src/tmx/Messages/include/simulation/Covariance.h create mode 100644 src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp create mode 100644 src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h create mode 100644 src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp diff --git a/src/tmx/Messages/include/simulation/BSMID.h b/src/tmx/Messages/include/simulation/BSMID.h new file mode 100644 index 000000000..23dba4fc2 --- /dev/null +++ b/src/tmx/Messages/include/simulation/BSMID.h @@ -0,0 +1,38 @@ +#ifndef INCLUDE_SIMULATED_BSMID_H_ +#define INCLUDE_SIMULATED_BSMID_H_ + +#include +#include + +namespace tmx +{ + namespace messages + { + namespace simulation + { + + struct BSMID + { + uint8_t BsmId = 0; + + BSMID() {} + BSMID(std::uint8_t bsmId) : BsmId(bsmId) {} + + static message_tree_type to_tree(BSMID element) + { + message_tree_type treeElement; + treeElement.put("BsmId", element.BsmId); + return treeElement; + } + + static BSMID from_tree(message_tree_type &treeElement) + { + BSMID element; + element.BsmId = treeElement.get("BsmId"); + return element; + } + }; + } + } +} +#endif \ No newline at end of file diff --git a/src/tmx/Messages/include/simulation/Covariance.h b/src/tmx/Messages/include/simulation/Covariance.h new file mode 100644 index 000000000..2bd19402f --- /dev/null +++ b/src/tmx/Messages/include/simulation/Covariance.h @@ -0,0 +1,41 @@ +#ifndef INCLUDE_SIMULATED_COVARIANCE_H_ +#define INCLUDE_SIMULATED_COVARIANCE_H_ + +#include +#include + +namespace tmx +{ + namespace messages + { + namespace simulation + { + // Row-major representation of the 6x6 covariance matrix + // # The orientation parameters use a fixed-axis representation. + // # In order, the parameters are: + // # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis) + struct Covariance + { + double covariance = 0; + + Covariance() {} + Covariance(double covariance) : covariance(covariance) {} + + static message_tree_type to_tree(Covariance element) + { + message_tree_type treeElement; + treeElement.put("Covariance", element.covariance); + return treeElement; + } + + static Covariance from_tree(message_tree_type &treeElement) + { + Covariance element; + element.covariance = treeElement.get("Covariance"); + return element; + } + }; + } + } +} +#endif \ No newline at end of file diff --git a/src/tmx/Messages/include/simulation/ExternalObject.h b/src/tmx/Messages/include/simulation/ExternalObject.h index 7eac33c0b..64930fe5f 100644 --- a/src/tmx/Messages/include/simulation/ExternalObject.h +++ b/src/tmx/Messages/include/simulation/ExternalObject.h @@ -3,9 +3,10 @@ #include #include -// #include #include #include +#include +#include namespace tmx { @@ -35,9 +36,9 @@ namespace tmx std_attribute(this->msg, bool, MetadataIsSimulation, false, ); std_attribute(this->msg, std::string, MetadataDatum, "", ); std_attribute(this->msg, std::string, MetadataProjString, "", ); - std_attribute(this->msg, std::string, MetadataSensorX, "", ); - std_attribute(this->msg, std::string, MetadataSensorY, "", ); - std_attribute(this->msg, std::string, MetadataSensorZ, "", ); + std_attribute(this->msg, double, MetadataSensorX, 0, ); + std_attribute(this->msg, double, MetadataSensorY, 0, ); + std_attribute(this->msg, double, MetadataSensorZ, 0, ); std_attribute(this->msg, std::string, MetadataInfrastructureId, "", ); std_attribute(this->msg, std::string, MetadataSensorId, "", ); /** @@ -56,34 +57,12 @@ namespace tmx // A presence vector, this message is used to describe objects coming from potentially different // sources. The presence vector is used to determine what items are set by the producer. - std_attribute(this->msg, PRESENCE_VECTOR_TYPES, PresenceVector, PRESENCE_VECTOR_TYPES::OBJECT_TYPE_PRESENCE_VECTOR, ); + std_attribute(this->msg, PRESENCE_VECTOR_TYPES, PresenceVector, PRESENCE_VECTOR_TYPES::UNAVAILABLE, ); // Object id. Matching ids on a topic should refer to the same object within some time period, expanded std_attribute(this->msg, uint32_t, Id, 0, ); - // bsm id is of form [0xff, 0xff, 0xff, 0xff]. It is not required. - // uint8[] bsm_id - struct BSMID - { - uint8_t BsmId = 0; - - BSMID() {} - BSMID(std::uint8_t bsmId) : BsmId(bsmId) {} - - static message_tree_type to_tree(BSMID element) - { - message_tree_type treeElement; - treeElement.put("BsmId", element.BsmId); - return treeElement; - } - - static BSMID from_tree(message_tree_type &treeElement) - { - BSMID element; - element.BsmId = treeElement.get("BsmId"); - return element; - } - }; + // bsm id is of form [0xff, 0xff, 0xff, 0xff]. It is not required. array_attribute( BSMID, BsmId); // Pose of the object within the frame specified in header @@ -92,38 +71,11 @@ namespace tmx std_attribute(this->msg, double, PosePosePositionX, 0, ); std_attribute(this->msg, double, PosePosePositionY, 0, ); std_attribute(this->msg, double, PosePosePositionZ, 0, ); - // This represents an orientation in free space in quaternion form. std_attribute(this->msg, double, PosePoseOrientationX, 0, ); std_attribute(this->msg, double, PosePoseOrientationY, 0, ); std_attribute(this->msg, double, PosePoseOrientationZ, 0, ); std_attribute(this->msg, double, PosePoseOrientationW, 0, ); - - // Row-major representation of the 6x6 covariance matrix - // # The orientation parameters use a fixed-axis representation. - // # In order, the parameters are: - // # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis) - struct Covariance - { - double covariance = 0; - - Covariance() {} - Covariance(double covariance) : covariance(covariance) {} - - static message_tree_type to_tree(Covariance element) - { - message_tree_type treeElement; - treeElement.put("Covariance", element.covariance); - return treeElement; - } - - static Covariance from_tree(message_tree_type &treeElement) - { - Covariance element; - element.covariance = treeElement.get("Covariance"); - return element; - } - }; array_attribute( Covariance, PoseCovariance); // #Average velocity of the object within the frame specified in header @@ -169,7 +121,7 @@ namespace tmx // # Binary value to show if the object is static or dynamic (1: dynamic, 0: static) std_attribute(this->msg, bool, DynamticObj, false, ); - // Predictions for the object. It is not required. + // Ignored: Predictions for the object. // carma_perception_msgs/PredictedState[] predictions }; diff --git a/src/tmx/Messages/include/simulation/PresenceVectorEnumTypes.h b/src/tmx/Messages/include/simulation/PresenceVectorEnumTypes.h index 8515fc728..94a328620 100644 --- a/src/tmx/Messages/include/simulation/PresenceVectorEnumTypes.h +++ b/src/tmx/Messages/include/simulation/PresenceVectorEnumTypes.h @@ -9,6 +9,7 @@ namespace tmx { enum PRESENCE_VECTOR_TYPES { + UNAVAILABLE = 0, ID_PRESENCE_VECTOR = 1, POSE_PRESENCE_VECTOR = 2, VELOCITY_PRESENCE_VECTOR = 4, diff --git a/src/tmx/TmxUtils/CMakeLists.txt b/src/tmx/TmxUtils/CMakeLists.txt index 30c30fb41..32ac53f27 100644 --- a/src/tmx/TmxUtils/CMakeLists.txt +++ b/src/tmx/TmxUtils/CMakeLists.txt @@ -25,6 +25,7 @@ TARGET_LINK_LIBRARIES (${PROJECT_NAME} PUBLIC rdkafka++ ::carma-clock gmock + jsoncpp pthread m rt) SET (TMXUTILS_LIBRARIES ${PROJECT_NAME} PARENT_SCOPE) @@ -54,4 +55,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} gtest jsoncpp) \ No newline at end of file diff --git a/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp b/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp new file mode 100644 index 000000000..8c065f40f --- /dev/null +++ b/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp @@ -0,0 +1,409 @@ +#include + +namespace tmx::utils::sim +{ + string SimulationExternalObjectConverter::simExternalObjToJsonStr(simulation::ExternalObject &simExternalObj) + { + Json::Value root; + Json::Value metadata; + metadata["is_simulation"] = simExternalObj.get_MetadataIsSimulation(); + metadata["datum"] = simExternalObj.get_MetadataDatum(); + metadata["proj_string"] = simExternalObj.get_MetadataProjString(); + metadata["sensor_x"] = simExternalObj.get_MetadataSensorX(); + metadata["sensor_y"] = simExternalObj.get_MetadataSensorY(); + metadata["sensor_z"] = simExternalObj.get_MetadataSensorZ(); + metadata["infrastructure_id"] = simExternalObj.get_MetadataInfrastructureId(); + metadata["sensor_id"] = simExternalObj.get_MetadataSensorId(); + root["metadata"] = metadata; + + Json::Value header; + header["seq"] = simExternalObj.get_HeaderSeq(); + header["stamp"]["secs"] = simExternalObj.get_HeaderStampSecs(); + header["stamp"]["nsecs"] = simExternalObj.get_HeaderStampNSecs(); + root["header"] = header; + + root["id"] = simExternalObj.get_Id(); + if (simExternalObj.get_PresenceVector() != simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE) + { + root["presence_vector"] = simExternalObj.get_PresenceVector(); + } + + Json::Value pose; + pose["pose"]["position"]["x"] = simExternalObj.get_PosePosePositionX(); + pose["pose"]["position"]["y"] = simExternalObj.get_PosePosePositionY(); + pose["pose"]["position"]["z"] = simExternalObj.get_PosePosePositionZ(); + pose["pose"]["orientation"]["x"] = simExternalObj.get_PosePoseOrientationX(); + pose["pose"]["orientation"]["y"] = simExternalObj.get_PosePoseOrientationY(); + pose["pose"]["orientation"]["z"] = simExternalObj.get_PosePoseOrientationZ(); + pose["pose"]["orientation"]["w"] = simExternalObj.get_PosePoseOrientationW(); + auto pCovarianceV = simExternalObj.get_PoseCovariance(); + for (auto itr = pCovarianceV.begin(); itr != pCovarianceV.end(); itr++) + { + pose["covariance"].append(Json::Value(itr->covariance)); + } + root["pose"] = pose; + + Json::Value velocity; + velocity["twist"]["linear"]["x"] = simExternalObj.get_VelocityTwistLinearX(); + velocity["twist"]["linear"]["y"] = simExternalObj.get_VelocityTwistLinearY(); + velocity["twist"]["linear"]["z"] = simExternalObj.get_VelocityTwistLinearZ(); + velocity["twist"]["angular"]["x"] = simExternalObj.get_VelocityTwistAngularX(); + velocity["twist"]["angular"]["y"] = simExternalObj.get_VelocityTwistAngularY(); + velocity["twist"]["angular"]["z"] = simExternalObj.get_VelocityTwistAngularZ(); + auto vCovarianceV = simExternalObj.get_VelocityCovariance(); + for (auto itr = vCovarianceV.begin(); itr != vCovarianceV.end(); itr++) + { + velocity["covariance"].append(Json::Value(itr->covariance)); + } + root["velocity"] = velocity; + + Json::Value size; + size["x"] = simExternalObj.get_SizeX(); + size["y"] = simExternalObj.get_SizeY(); + size["z"] = simExternalObj.get_SizeZ(); + root["size"] = size; + + root["confidence"] = simExternalObj.get_Confidence(); + root["object_type"] = to_string(simExternalObj.get_ObjectType()); + root["dynamic_obj"] = simExternalObj.get_DynamticObj(); + Json::StreamWriterBuilder builder; + const std::string json_str = Json::writeString(builder, root); + return json_str; + } + + void SimulationExternalObjectConverter::jsonToSimExternalObj(const string &jsonStr, simulation::ExternalObject &simExternalObj) + { + Json::CharReaderBuilder builder; + const std::unique_ptr reader(builder.newCharReader()); + Json::Value root; + JSONCPP_STRING err; + if (!reader->parse(jsonStr.c_str(), jsonStr.c_str() + static_cast(jsonStr.length()), &root, &err)) + { + throw runtime_error("Error parsing external object JSON string."); + } + + /** + * Populate simulation external object metadata + */ + if (root.isMember("metadata") && root["metadata"].isObject()) + { + populateSimExternalObjectMetadata(root["metadata"], simExternalObj); + } + + /** + * Populate simulation external object header + */ + if (root.isMember("header") && root["header"].isObject()) + { + populateSimExternalObjectHeader(root["header"], simExternalObj); + } + + if (root.isMember("presence_vector") && root["presence_vector"].isUInt()) + { + auto presence_vetor = presenceVectorIntToEnum(root["presence_vector"].asUInt()); + simExternalObj.set_PresenceVector(presence_vetor); + } + + // Populate simulation external object id + if (root.isMember("id") && root["id"].isUInt()) + { + simExternalObj.set_Id(root["id"].asUInt()); + } + + /*** + * Populate simulation external object pose + */ + if (root.isMember("pose") && root["pose"].isObject()) + { + populateSimExternalObjectPose(root["pose"], simExternalObj); + } + + /*** + *Populate simulation external object velocity + */ + if (root.isMember("velocity") && root["velocity"].isObject()) + { + populateSimExternalObjectVelocity(root["velocity"], simExternalObj); + } + + /*** + * Populate simulation external object size + */ + if (root.isMember("size") && root["size"].isObject()) + { + populateSimExternalObjectSize(root["size"], simExternalObj); + } + + // Populate simulation external object confidence + if (root.isMember("confidence") && root["confidence"].isDouble()) + { + simExternalObj.set_Confidence(root["confidence"].asDouble()); + } + + // Populate simulation external object object_type + if (root.isMember("object_type") && root["object_type"].isString()) + { + auto object_type = objectTypeStringToEnum(root["object_type"].asString()); + simExternalObj.set_ObjectType(object_type); + } + + // Populate simulation external object dynamic_obj + if (root.isMember("dynamic_obj") && root["dynamic_obj"].isBool()) + { + simExternalObj.set_DynamticObj(root["dynamic_obj"].asBool()); + } + } + + void SimulationExternalObjectConverter::populateSimExternalObjectMetadata(const Json::Value &metadataValue, simulation::ExternalObject &simExternalObj) + { + if (metadataValue.isMember("is_simulation") && metadataValue["is_simulation"].isBool()) + { + simExternalObj.set_MetadataIsSimulation(metadataValue["is_simulation"].asBool()); + } + + if (metadataValue.isMember("datum") && metadataValue["datum"].isString()) + { + simExternalObj.set_MetadataDatum(metadataValue["datum"].asString()); + } + + if (metadataValue.isMember("proj_string") && metadataValue["proj_string"].isString()) + { + simExternalObj.set_MetadataProjString(metadataValue["proj_string"].asString()); + } + + if (metadataValue.isMember("sensor_x") && metadataValue["sensor_x"].isDouble()) + { + simExternalObj.set_MetadataSensorX(metadataValue["sensor_x"].asDouble()); + } + + if (metadataValue.isMember("sensor_y") && metadataValue["sensor_y"].isDouble()) + { + simExternalObj.set_MetadataSensorY(metadataValue["sensor_y"].asDouble()); + } + + if (metadataValue.isMember("sensor_z") && metadataValue["sensor_z"].isDouble()) + { + simExternalObj.set_MetadataSensorZ(metadataValue["sensor_z"].asDouble()); + } + + if (metadataValue.isMember("infrastructure_id") && metadataValue["infrastructure_id"].isString()) + { + simExternalObj.set_MetadataInfrastructureId(metadataValue["infrastructure_id"].asString()); + } + + if (metadataValue.isMember("sensor_id") && metadataValue["sensor_id"].isString()) + { + simExternalObj.set_MetadataSensorId(metadataValue["sensor_id"].asString()); + } + } + + void SimulationExternalObjectConverter::populateSimExternalObjectHeader(const Json::Value &headerValue, simulation::ExternalObject &simExternalObj) + { + if (headerValue.isMember("seq") && headerValue["seq"].isUInt()) + { + simExternalObj.set_HeaderSeq(headerValue["seq"].asUInt()); + } + + if (headerValue.isMember("stamp") && headerValue["stamp"].isMember("secs") && headerValue["stamp"]["secs"].isUInt()) + { + simExternalObj.set_HeaderStampSecs(headerValue["stamp"]["secs"].asUInt()); + } + + if (headerValue.isMember("stamp") && headerValue["stamp"].isMember("nsecs") && headerValue["stamp"]["nsecs"].isUInt()) + { + simExternalObj.set_HeaderStampNSecs(headerValue["stamp"]["nsecs"].asUInt()); + } + } + + void SimulationExternalObjectConverter::populateSimExternalObjectPose(const Json::Value &poseValue, simulation::ExternalObject &simExternalObj) + { + if (poseValue.isMember("pose") && poseValue["pose"].isMember("position") && poseValue["pose"]["position"].isObject()) + { + Json::Value positionValue = poseValue["pose"]["position"]; + if (positionValue.isMember("x")) + { + simExternalObj.set_PosePosePositionX(positionValue["x"].asDouble()); + } + if (positionValue.isMember("y")) + { + simExternalObj.set_PosePosePositionY(positionValue["y"].asDouble()); + } + if (positionValue.isMember("z")) + { + simExternalObj.set_PosePosePositionZ(positionValue["z"].asDouble()); + } + } + + if (poseValue.isMember("pose") && poseValue["pose"].isMember("position") && poseValue["pose"]["orientation"].isObject()) + { + Json::Value orientationValue = poseValue["pose"]["orientation"]; + if (orientationValue.isMember("x")) + { + simExternalObj.set_PosePoseOrientationX(orientationValue["x"].asDouble()); + } + if (orientationValue.isMember("y")) + { + simExternalObj.set_PosePoseOrientationY(orientationValue["y"].asDouble()); + } + if (orientationValue.isMember("z")) + { + simExternalObj.set_PosePoseOrientationZ(orientationValue["z"].asDouble()); + } + if (orientationValue.isMember("w")) + { + simExternalObj.set_PosePoseOrientationW(orientationValue["w"].asDouble()); + } + } + + if (poseValue.isMember("covariance") && poseValue["covariance"].isArray()) + { + Json::Value covarianceArrayValue = poseValue["covariance"]; + std::vector covarianceV; + populateSimCovarianceArray(covarianceArrayValue, covarianceV); + simExternalObj.set_PoseCovariance(covarianceV); + } + } + + void SimulationExternalObjectConverter::populateSimExternalObjectVelocity(const Json::Value &velocityValue, simulation::ExternalObject &simExternalObj) + { + if (velocityValue.isMember("twist") && velocityValue["twist"].isMember("linear") && velocityValue["twist"]["linear"].isObject()) + { + Json::Value linearValue = velocityValue["twist"]["linear"]; + if (linearValue.isMember("x")) + { + simExternalObj.set_VelocityTwistLinearX(linearValue["x"].asDouble()); + } + if (linearValue.isMember("y")) + { + simExternalObj.set_VelocityTwistLinearY(linearValue["y"].asDouble()); + } + if (linearValue.isMember("z")) + { + simExternalObj.set_VelocityTwistLinearZ(linearValue["z"].asDouble()); + } + } + + if (velocityValue.isMember("twist") && velocityValue["twist"].isMember("angular") && velocityValue["twist"]["angular"].isObject()) + { + Json::Value angularValue = velocityValue["twist"]["angular"]; + if (angularValue.isMember("x")) + { + simExternalObj.set_VelocityTwistAngularX(angularValue["x"].asDouble()); + } + if (angularValue.isMember("y")) + { + simExternalObj.set_VelocityTwistAngularY(angularValue["y"].asDouble()); + } + if (angularValue.isMember("z")) + { + simExternalObj.set_VelocityTwistAngularZ(angularValue["z"].asDouble()); + } + if (angularValue.isMember("w")) + { + simExternalObj.set_PosePoseOrientationW(angularValue["w"].asDouble()); + } + } + if (velocityValue.isMember("covariance") && velocityValue["covariance"].isArray()) + { + Json::Value covarianceArrayValue = velocityValue["covariance"]; + std::vector covarianceV; + populateSimCovarianceArray(covarianceArrayValue, covarianceV); + simExternalObj.set_VelocityCovariance(covarianceV); + } + } + + void SimulationExternalObjectConverter::populateSimExternalObjectSize(const Json::Value &sizeValue, simulation::ExternalObject &simExternalObj) + { + if (sizeValue.isMember("x")) + { + simExternalObj.set_SizeX(sizeValue["x"].asDouble()); + } + if (sizeValue.isMember("y")) + { + simExternalObj.set_SizeY(sizeValue["y"].asDouble()); + } + if (sizeValue.isMember("z")) + { + simExternalObj.set_SizeZ(sizeValue["z"].asDouble()); + } + } + + void SimulationExternalObjectConverter::populateSimCovarianceArray(const Json::Value &covarianceArrayValue, std::vector &covarianceV) + { + for (auto itr = covarianceArrayValue.begin(); itr != covarianceArrayValue.end(); itr++) + { + simulation::Covariance covariance(itr->asDouble()); + covarianceV.push_back(covariance); + } + } + + simulation::OBJECT_TYPES SimulationExternalObjectConverter::objectTypeStringToEnum(const string &object_type_str) + { + simulation::OBJECT_TYPES object_type = simulation::OBJECT_TYPES::UNKNOWN; + try + { + int object_type_int = stoi(object_type_str); + switch (object_type_int) + { + case simulation::OBJECT_TYPES::LARGE_VEHICLE: + object_type = simulation::OBJECT_TYPES::LARGE_VEHICLE; + break; + case simulation::OBJECT_TYPES::MOTORCYCLE: + object_type = simulation::OBJECT_TYPES::MOTORCYCLE; + break; + case simulation::OBJECT_TYPES::PEDESTRIAN: + object_type = simulation::OBJECT_TYPES::PEDESTRIAN; + case simulation::OBJECT_TYPES::SMALL_VEHICLE: + object_type = simulation::OBJECT_TYPES::SMALL_VEHICLE; + break; + default: + object_type = simulation::OBJECT_TYPES::UNKNOWN; + break; + } + } + catch (exception &err) + { + object_type = simulation::OBJECT_TYPES::UNKNOWN; + } + return object_type; + } + + simulation::PRESENCE_VECTOR_TYPES SimulationExternalObjectConverter::presenceVectorIntToEnum(uint16_t presence_vector) + { + simulation::PRESENCE_VECTOR_TYPES presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE; + switch (presence_vector) + { + case simulation::PRESENCE_VECTOR_TYPES::BSM_ID_PRESENCE_VECTOR: + presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::BSM_ID_PRESENCE_VECTOR; + break; + case simulation::PRESENCE_VECTOR_TYPES::CONFIDENCE_PRESENCE_VECTOR: + presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::CONFIDENCE_PRESENCE_VECTOR; + break; + case simulation::PRESENCE_VECTOR_TYPES::DYNAMIC_OBJ_PRESENCE: + presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::DYNAMIC_OBJ_PRESENCE; + break; + case simulation::PRESENCE_VECTOR_TYPES::OBJECT_TYPE_PRESENCE_VECTOR: + presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::OBJECT_TYPE_PRESENCE_VECTOR; + break; + case simulation::PRESENCE_VECTOR_TYPES::POSE_PRESENCE_VECTOR: + presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::POSE_PRESENCE_VECTOR; + break; + case simulation::PRESENCE_VECTOR_TYPES::PREDICTION_PRESENCE_VECTOR: + presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::PREDICTION_PRESENCE_VECTOR; + break; + case simulation::PRESENCE_VECTOR_TYPES::SIZE_PRESENCE_VECTOR: + presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::SIZE_PRESENCE_VECTOR; + break; + case simulation::PRESENCE_VECTOR_TYPES::VELOCITY_INST_PRESENCE_VECTOR: + presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::VELOCITY_INST_PRESENCE_VECTOR; + break; + case simulation::PRESENCE_VECTOR_TYPES::VELOCITY_PRESENCE_VECTOR: + presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::VELOCITY_PRESENCE_VECTOR; + break; + default: + presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE; + break; + } + return presence_vector_enum; + } +} \ No newline at end of file diff --git a/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h b/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h new file mode 100644 index 000000000..f4bfc74a1 --- /dev/null +++ b/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h @@ -0,0 +1,30 @@ +#pragma once + +#include +#include +#include + +using namespace tmx::messages; +using namespace std; +namespace tmx::utils::sim +{ + + class SimulationExternalObjectConverter + { + private: + static void populateSimExternalObjectMetadata(const Json::Value &metadataValue, simulation::ExternalObject &simExternalObj); + static void populateSimExternalObjectHeader(const Json::Value &headerValue, simulation::ExternalObject &simExternalObj); + static void populateSimExternalObjectPose(const Json::Value &poseValue, simulation::ExternalObject &simExternalObj); + static void populateSimExternalObjectVelocity(const Json::Value &velocityValue, simulation::ExternalObject &simExternalObj); + static void populateSimExternalObjectSize(const Json::Value &sizeValue, simulation::ExternalObject &simExternalObj); + static void populateSimCovarianceArray(const Json::Value &covarianceArrayValue, std::vector &covarianceV); + static simulation::OBJECT_TYPES objectTypeStringToEnum(const string& object_type_str); + static simulation::PRESENCE_VECTOR_TYPES presenceVectorIntToEnum(uint16_t presence_vector); + + public: + SimulationExternalObjectConverter() = default; + ~SimulationExternalObjectConverter() = default; + static string simExternalObjToJsonStr(simulation::ExternalObject &simExternalObj); + static void jsonToSimExternalObj(const string &jsonStr, simulation::ExternalObject &simExternalObj); + }; +} diff --git a/src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp b/src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp new file mode 100644 index 000000000..73be4b5f9 --- /dev/null +++ b/src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp @@ -0,0 +1,48 @@ +#include +#include +#include + +using namespace tmx::utils::sim; +using namespace tmx::messages; + +TEST(SimulationExternalObjectConverter, jsonToSimExternalObj) +{ + SimulationExternalObjectConverter converter; + simulation::ExternalObject simExternalObj; + std::string json_string = "{\"metadata\":{\"is_simulation\":false,\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":0.0,\"sensor_y\":0.0,\"sensor_z\":0.0,\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":0,\"stamp\":{\"secs\":0,\"nsecs\":0}},\"id\":0,\"pose\":{\"pose\":{\"position\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"orientation\":{\"x\":0.0,\"y\":0.0,\"z\":0.0,\"w\":0.0}},\"covariance\":[12.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"velocity\":{\"twist\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"size\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"confidence\":0.0,\"object_type\":\"\",\"dynamic_obj\":false}"; + converter.jsonToSimExternalObj(json_string, simExternalObj); + ASSERT_EQ(0, simExternalObj.get_Id()); + ASSERT_EQ(false, simExternalObj.get_MetadataIsSimulation()); + ASSERT_EQ("", simExternalObj.get_MetadataDatum()); + ASSERT_EQ("", simExternalObj.get_MetadataSensorId()); + ASSERT_EQ("", simExternalObj.get_MetadataInfrastructureId()); + ASSERT_EQ(0, simExternalObj.get_MetadataSensorX()); + ASSERT_EQ(0, simExternalObj.get_MetadataSensorY()); + ASSERT_EQ(0, simExternalObj.get_MetadataSensorZ()); + ASSERT_EQ(0, simExternalObj.get_HeaderSeq()); + ASSERT_EQ(0, simExternalObj.get_HeaderStampSecs()); + ASSERT_EQ(0, simExternalObj.get_HeaderStampNSecs()); + ASSERT_EQ(0, simExternalObj.get_PosePosePositionX()); + ASSERT_EQ(0, simExternalObj.get_PosePoseOrientationX()); + ASSERT_EQ(36, simExternalObj.get_PoseCovariance().size()); + ASSERT_EQ(0, simExternalObj.get_VelocityInstTwistLinearX()); + ASSERT_EQ(0, simExternalObj.get_VelocityInstTwistAngularX()); + ASSERT_EQ(36, simExternalObj.get_VelocityCovariance().size()); +} + +TEST(SimulationExternalObjectConverter, simExternalObjToJsonStr) +{ + SimulationExternalObjectConverter converter; + simulation::ExternalObject simExternalObj; + std::string json_string = "{\"metadata\":{\"is_simulation\":false,\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":0.0,\"sensor_y\":0.0,\"sensor_z\":0.0,\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":0,\"stamp\":{\"secs\":0,\"nsecs\":0}},\"id\":0,\"pose\":{\"pose\":{\"position\":{\"x\":34.0,\"y\":0.0,\"z\":0.0},\"orientation\":{\"x\":23.0,\"y\":0.0,\"z\":0.0,\"w\":0.0}},\"covariance\":[12.1,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,11.0]},\"velocity\":{\"twist\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"covariance\":[12.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"size\":{\"x\":12.0,\"y\":23.0,\"z\":12.0},\"confidence\":1.0,\"object_type\":\"128\",\"dynamic_obj\":false}"; + converter.jsonToSimExternalObj(json_string, simExternalObj); + std::string output = converter.simExternalObjToJsonStr(simExternalObj); + Json::CharReaderBuilder builder; + const std::unique_ptr reader(builder.newCharReader()); + Json::Value root; + JSONCPP_STRING err; + reader->parse(output.c_str(), output.c_str() + static_cast(output.length()), &root, &err); + ASSERT_FALSE(root["metadata"]["is_simulation"].asBool()); + ASSERT_EQ(12.1, root["pose"]["covariance"].begin()->asDouble()); + ASSERT_EQ(23, root["pose"]["pose"]["orientation"]["x"].asDouble()); +} \ No newline at end of file diff --git a/src/v2i-hub/CARMAStreetsPlugin/manifest.json b/src/v2i-hub/CARMAStreetsPlugin/manifest.json index 790ef1af9..37e705a8d 100644 --- a/src/v2i-hub/CARMAStreetsPlugin/manifest.json +++ b/src/v2i-hub/CARMAStreetsPlugin/manifest.json @@ -116,6 +116,11 @@ "key": "SpatConsumerGroupId", "default": "v2xhub_spat", "description": "Apache Kafka consumer group ID for spat consumer." + }, + { + "key": "SimExternalObjTopic", + "default": "v2xhub_sim_external_object", + "description": "Apache Kafka topic plugin will transmit simulated external object to." } ] diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index d4b8c232e..44529f2c8 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -57,7 +57,8 @@ void CARMAStreetsPlugin::UpdateConfigSettings() { GetConfigValue("MobilityOperationTopic", _transmitMobilityOperationTopic); GetConfigValue("MobilityPathTopic", _transmitMobilityPathTopic); GetConfigValue("MapTopic", _transmitMAPTopic); - GetConfigValue("SRMTopic", _transmitSRMTopic); + GetConfigValue("SRMTopic", _transmitSRMTopic); + GetConfigValue("SimExternalObjTopic", _transmitSimExternalObjTopic); // Populate strategies config string config; GetConfigValue("MobilityOperationStrategies", config); @@ -630,9 +631,10 @@ void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){ void CARMAStreetsPlugin::HandleSimulatedExternalMessage(ExternalObject &msg, routeable_message &routeableMsg) { - PLOG(logINFO) << "HandleSimulatedExternalMessage called." < #include "JsonToJ2735SSMConverter.h" #include +#include @@ -101,6 +102,7 @@ class CARMAStreetsPlugin: public PluginClient { std::string _transmitBSMTopic; std::string _transmitMAPTopic; std::string _transmitSRMTopic; + std::string _transmitSimExternalObjTopic; std::string _kafkaBrokerIp; std::string _kafkaBrokerPort; std::shared_ptr _kafka_producer_ptr; diff --git a/src/v2i-hub/CDASimAdapter/scripts/send_sim_external_object_udp.py b/src/v2i-hub/CDASimAdapter/scripts/send_sim_external_object_udp.py index 2f4bd5d02..42799e984 100755 --- a/src/v2i-hub/CDASimAdapter/scripts/send_sim_external_object_udp.py +++ b/src/v2i-hub/CDASimAdapter/scripts/send_sim_external_object_udp.py @@ -13,55 +13,79 @@ count_num = 0 def generate_sim_external_object(): - jsonResult ={ - "MetadataIsSimulation": False, - "MetadataDatum": "", - "MetadataProjString": "", - "MetadataSensorX": "", - "MetadataSensorY": "", - "MetadataSensorZ": "", - "MetadataInfrastructureId": "", - "MetadataSensorId": "", - "HeaderStampSecs": 1, - "HeaderStampNsecs": 2, - "Id": 10, - "PosePosePositionX": 0.2, - "PosePosePositionY": 0.3, - "PosePosePositionZ": 0.4, - "PosePoseOrientationX": 0.1, - "PosePoseOrientationY": 0.2, - "PosePoseOrientationZ": 0.3, - "PosePoseOrientationW": 0.4, - "BsmId": [12,12,22,11], - "PoseCovariance": [ - 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 - ], - "VelocityTwistLinearX": 0.1, - "VelocityTwistLinearY": 0.1, - "VelocityTwistLinearZ": 0.1, - "VelocityTwistAngularX": 0.1, - "VelocityTwistAngularY": 0.1, - "VelocityTwistAngularZ": 0.1, - "VelocityCovariance": [ - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 - ], - "SizeX": 0.3, - "SizeY": 0.4, - "SizeZ": 0.6, - "Confidence": 0.0, - "ObjectType": "", - "DynamicObj": False - } + jsonResult ={ + "metadata":{ + "is_simulation": True, + "datum": "WGS84", + "proj_string": "epsg:3785", + "sensor_x": 12.212, + "sensor_y": 2.2121212, + "sensor_z": 0.121212212, + "infrastructure_id": "test", + "sensor_id": "test_sensor" + }, + "header": { + "seq": 2, + "stamp": { + "secs": 3, + "nsecs":4 + } + }, + "id": 1212, + "pose": { + "pose": { + "position": { + "x": 12.0, + "y": 23.0, + "z": 56.121212 + }, + "orientation": { + "x": 11.0, + "y": 22.0, + "z": 33.333333, + "w": 44.4444444444 + } + }, + "covariance": [ + 12.12, 12.3233232, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + ] + }, + "velocity": { + "twist": { + "linear": { + "x": 12.1111111111111, + "y": 2.11111112222, + "z": 3.2222222222333 + }, + "angular": { + "x": 2.212222222222, + "y": 2.2333333333333, + "z": 3.3333333333121 + } + }, + "covariance": [ + 12.2222222222, 123.33333333, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0,23.66666666666 + ] + }, + "size": { + "x": 12.0, + "y": 23.0, + "z": 23.0 + }, + "confidence": 12.0, + "object_type": "2", + "dynamic_obj": True + } jsonResult = json.dumps(jsonResult) return jsonResult port = 7576 diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp index 7d7722440..27ddd0ee1 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp @@ -135,7 +135,9 @@ namespace CDASimAdapter{ if(external_object_listener) { std::string str_msg = consume_server_message(external_object_listener); - externalObj.set_contents(str_msg); + tmx::utils::sim::SimulationExternalObjectConverter converter; + //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 + converter.jsonToSimExternalObj(str_msg, externalObj); } else { diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp index fec88be2d..708283488 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp @@ -8,6 +8,7 @@ #include #include #include +#include namespace CDASimAdapter { @@ -86,6 +87,7 @@ namespace CDASimAdapter { 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::ExternalObject. */ tmx::messages::simulation::ExternalObject consume_external_object_message() const; From 1705c98c2db1663457b5204fe47ef1d19cde3ff4 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Tue, 18 Jul 2023 20:05:48 +0000 Subject: [PATCH 04/35] update unit test --- src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp b/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp index af6ce9f77..69a6372a5 100644 --- a/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp +++ b/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp @@ -27,5 +27,5 @@ TEST(SimulationMessages, ExternalObjectToRoutableMessage) ASSERT_EQ(0, routeableMsg.get_sourceId()); ASSERT_EQ("CDASimAdapter", routeableMsg.get_source()); ASSERT_EQ("ExternalObject",routeableMsg.get_subtype()); - ASSERT_EQ("Detected", routeableMsg.get_type()); + ASSERT_EQ("Simulation", routeableMsg.get_type()); } \ No newline at end of file From 4e6aa4335348321894b0f8d2b9d9c06eee6ee979 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Wed, 19 Jul 2023 14:40:21 +0000 Subject: [PATCH 05/35] code smell and coverage --- .../SimulationExternalObjectConverter.cpp | 132 ++++++++++-------- .../SimulationExternalObjectConverter.h | 22 ++- ...t_simulation_external_object_converter.cpp | 107 ++++++++++++-- .../src/CARMAStreetsPlugin.cpp | 3 +- .../CDASimAdapter/src/CDASimConnection.cpp | 3 +- .../test/TestSimulationMessages.cpp | 9 +- 6 files changed, 187 insertions(+), 89 deletions(-) diff --git a/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp b/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp index 8c065f40f..4f15fb4c9 100644 --- a/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp +++ b/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp @@ -2,7 +2,7 @@ namespace tmx::utils::sim { - string SimulationExternalObjectConverter::simExternalObjToJsonStr(simulation::ExternalObject &simExternalObj) + std::string SimulationExternalObjectConverter::simExternalObjToJsonStr(tmx::messages::simulation::ExternalObject &simExternalObj) { Json::Value root; Json::Value metadata; @@ -23,7 +23,7 @@ namespace tmx::utils::sim root["header"] = header; root["id"] = simExternalObj.get_Id(); - if (simExternalObj.get_PresenceVector() != simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE) + if (simExternalObj.get_PresenceVector() != tmx::messages::simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE) { root["presence_vector"] = simExternalObj.get_PresenceVector(); } @@ -37,10 +37,8 @@ namespace tmx::utils::sim pose["pose"]["orientation"]["z"] = simExternalObj.get_PosePoseOrientationZ(); pose["pose"]["orientation"]["w"] = simExternalObj.get_PosePoseOrientationW(); auto pCovarianceV = simExternalObj.get_PoseCovariance(); - for (auto itr = pCovarianceV.begin(); itr != pCovarianceV.end(); itr++) - { - pose["covariance"].append(Json::Value(itr->covariance)); - } + std::for_each(pCovarianceV.begin(), pCovarianceV.end(), [&pose](const auto &item) + { pose["covariance"].append(Json::Value(item.covariance)); }); root["pose"] = pose; Json::Value velocity; @@ -51,10 +49,8 @@ namespace tmx::utils::sim velocity["twist"]["angular"]["y"] = simExternalObj.get_VelocityTwistAngularY(); velocity["twist"]["angular"]["z"] = simExternalObj.get_VelocityTwistAngularZ(); auto vCovarianceV = simExternalObj.get_VelocityCovariance(); - for (auto itr = vCovarianceV.begin(); itr != vCovarianceV.end(); itr++) - { - velocity["covariance"].append(Json::Value(itr->covariance)); - } + std::for_each(vCovarianceV.begin(), vCovarianceV.end(), [&velocity](const auto &item) + { velocity["covariance"].append(Json::Value(item.covariance)); }); root["velocity"] = velocity; Json::Value size; @@ -64,14 +60,14 @@ namespace tmx::utils::sim root["size"] = size; root["confidence"] = simExternalObj.get_Confidence(); - root["object_type"] = to_string(simExternalObj.get_ObjectType()); + root["object_type"] = std::to_string(simExternalObj.get_ObjectType()); root["dynamic_obj"] = simExternalObj.get_DynamticObj(); Json::StreamWriterBuilder builder; const std::string json_str = Json::writeString(builder, root); return json_str; } - void SimulationExternalObjectConverter::jsonToSimExternalObj(const string &jsonStr, simulation::ExternalObject &simExternalObj) + void SimulationExternalObjectConverter::jsonToSimExternalObj(const std::string &jsonStr, tmx::messages::simulation::ExternalObject &simExternalObj) { Json::CharReaderBuilder builder; const std::unique_ptr reader(builder.newCharReader()); @@ -79,7 +75,7 @@ namespace tmx::utils::sim JSONCPP_STRING err; if (!reader->parse(jsonStr.c_str(), jsonStr.c_str() + static_cast(jsonStr.length()), &root, &err)) { - throw runtime_error("Error parsing external object JSON string."); + throw std::runtime_error("Error parsing external object JSON string."); } /** @@ -154,7 +150,7 @@ namespace tmx::utils::sim } } - void SimulationExternalObjectConverter::populateSimExternalObjectMetadata(const Json::Value &metadataValue, simulation::ExternalObject &simExternalObj) + void SimulationExternalObjectConverter::populateSimExternalObjectMetadata(const Json::Value &metadataValue, tmx::messages::simulation::ExternalObject &simExternalObj) { if (metadataValue.isMember("is_simulation") && metadataValue["is_simulation"].isBool()) { @@ -197,7 +193,7 @@ namespace tmx::utils::sim } } - void SimulationExternalObjectConverter::populateSimExternalObjectHeader(const Json::Value &headerValue, simulation::ExternalObject &simExternalObj) + void SimulationExternalObjectConverter::populateSimExternalObjectHeader(const Json::Value &headerValue, tmx::messages::simulation::ExternalObject &simExternalObj) { if (headerValue.isMember("seq") && headerValue["seq"].isUInt()) { @@ -215,7 +211,7 @@ namespace tmx::utils::sim } } - void SimulationExternalObjectConverter::populateSimExternalObjectPose(const Json::Value &poseValue, simulation::ExternalObject &simExternalObj) + void SimulationExternalObjectConverter::populateSimExternalObjectPose(const Json::Value &poseValue, tmx::messages::simulation::ExternalObject &simExternalObj) { if (poseValue.isMember("pose") && poseValue["pose"].isMember("position") && poseValue["pose"]["position"].isObject()) { @@ -258,13 +254,13 @@ namespace tmx::utils::sim if (poseValue.isMember("covariance") && poseValue["covariance"].isArray()) { Json::Value covarianceArrayValue = poseValue["covariance"]; - std::vector covarianceV; + std::vector covarianceV; populateSimCovarianceArray(covarianceArrayValue, covarianceV); simExternalObj.set_PoseCovariance(covarianceV); } } - void SimulationExternalObjectConverter::populateSimExternalObjectVelocity(const Json::Value &velocityValue, simulation::ExternalObject &simExternalObj) + void SimulationExternalObjectConverter::populateSimExternalObjectVelocity(const Json::Value &velocityValue, tmx::messages::simulation::ExternalObject &simExternalObj) { if (velocityValue.isMember("twist") && velocityValue["twist"].isMember("linear") && velocityValue["twist"]["linear"].isObject()) { @@ -306,13 +302,13 @@ namespace tmx::utils::sim if (velocityValue.isMember("covariance") && velocityValue["covariance"].isArray()) { Json::Value covarianceArrayValue = velocityValue["covariance"]; - std::vector covarianceV; + std::vector covarianceV; populateSimCovarianceArray(covarianceArrayValue, covarianceV); simExternalObj.set_VelocityCovariance(covarianceV); } } - void SimulationExternalObjectConverter::populateSimExternalObjectSize(const Json::Value &sizeValue, simulation::ExternalObject &simExternalObj) + void SimulationExternalObjectConverter::populateSimExternalObjectSize(const Json::Value &sizeValue, tmx::messages::simulation::ExternalObject &simExternalObj) { if (sizeValue.isMember("x")) { @@ -328,80 +324,96 @@ namespace tmx::utils::sim } } - void SimulationExternalObjectConverter::populateSimCovarianceArray(const Json::Value &covarianceArrayValue, std::vector &covarianceV) + void SimulationExternalObjectConverter::populateSimCovarianceArray(const Json::Value &covarianceArrayValue, std::vector &covarianceV) { - for (auto itr = covarianceArrayValue.begin(); itr != covarianceArrayValue.end(); itr++) - { - simulation::Covariance covariance(itr->asDouble()); - covarianceV.push_back(covariance); - } + std::for_each(covarianceArrayValue.begin(), covarianceArrayValue.end(), [&covarianceV](const auto &item) + { tmx::messages::simulation::Covariance covariance(item.asDouble()); + covarianceV.push_back(covariance); }); } - simulation::OBJECT_TYPES SimulationExternalObjectConverter::objectTypeStringToEnum(const string &object_type_str) + tmx::messages::simulation::OBJECT_TYPES SimulationExternalObjectConverter::objectTypeStringToEnum(const std::string &object_type_str) { - simulation::OBJECT_TYPES object_type = simulation::OBJECT_TYPES::UNKNOWN; + tmx::messages::simulation::OBJECT_TYPES object_type = tmx::messages::simulation::OBJECT_TYPES::UNKNOWN; try { int object_type_int = stoi(object_type_str); switch (object_type_int) { - case simulation::OBJECT_TYPES::LARGE_VEHICLE: - object_type = simulation::OBJECT_TYPES::LARGE_VEHICLE; + case tmx::messages::simulation::OBJECT_TYPES::LARGE_VEHICLE: + object_type = tmx::messages::simulation::OBJECT_TYPES::LARGE_VEHICLE; break; - case simulation::OBJECT_TYPES::MOTORCYCLE: - object_type = simulation::OBJECT_TYPES::MOTORCYCLE; + + case tmx::messages::simulation::OBJECT_TYPES::MOTORCYCLE: + object_type = tmx::messages::simulation::OBJECT_TYPES::MOTORCYCLE; break; - case simulation::OBJECT_TYPES::PEDESTRIAN: - object_type = simulation::OBJECT_TYPES::PEDESTRIAN; - case simulation::OBJECT_TYPES::SMALL_VEHICLE: - object_type = simulation::OBJECT_TYPES::SMALL_VEHICLE; + + case tmx::messages::simulation::OBJECT_TYPES::PEDESTRIAN: + object_type = tmx::messages::simulation::OBJECT_TYPES::PEDESTRIAN; break; + + case tmx::messages::simulation::OBJECT_TYPES::SMALL_VEHICLE: + object_type = tmx::messages::simulation::OBJECT_TYPES::SMALL_VEHICLE; + break; + default: - object_type = simulation::OBJECT_TYPES::UNKNOWN; + object_type = tmx::messages::simulation::OBJECT_TYPES::UNKNOWN; break; } } - catch (exception &err) + catch (std::exception &err) { - object_type = simulation::OBJECT_TYPES::UNKNOWN; + object_type = tmx::messages::simulation::OBJECT_TYPES::UNKNOWN; } return object_type; } - simulation::PRESENCE_VECTOR_TYPES SimulationExternalObjectConverter::presenceVectorIntToEnum(uint16_t presence_vector) + tmx::messages::simulation::PRESENCE_VECTOR_TYPES SimulationExternalObjectConverter::presenceVectorIntToEnum(uint16_t presence_vector) { - simulation::PRESENCE_VECTOR_TYPES presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE; + tmx::messages::simulation::PRESENCE_VECTOR_TYPES presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE; switch (presence_vector) { - case simulation::PRESENCE_VECTOR_TYPES::BSM_ID_PRESENCE_VECTOR: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::BSM_ID_PRESENCE_VECTOR; + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::BSM_ID_PRESENCE_VECTOR: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::BSM_ID_PRESENCE_VECTOR; break; - case simulation::PRESENCE_VECTOR_TYPES::CONFIDENCE_PRESENCE_VECTOR: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::CONFIDENCE_PRESENCE_VECTOR; + + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::CONFIDENCE_PRESENCE_VECTOR: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::CONFIDENCE_PRESENCE_VECTOR; break; - case simulation::PRESENCE_VECTOR_TYPES::DYNAMIC_OBJ_PRESENCE: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::DYNAMIC_OBJ_PRESENCE; + + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::DYNAMIC_OBJ_PRESENCE: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::DYNAMIC_OBJ_PRESENCE; break; - case simulation::PRESENCE_VECTOR_TYPES::OBJECT_TYPE_PRESENCE_VECTOR: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::OBJECT_TYPE_PRESENCE_VECTOR; + + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::OBJECT_TYPE_PRESENCE_VECTOR: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::OBJECT_TYPE_PRESENCE_VECTOR; + break; + + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::POSE_PRESENCE_VECTOR: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::POSE_PRESENCE_VECTOR; break; - case simulation::PRESENCE_VECTOR_TYPES::POSE_PRESENCE_VECTOR: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::POSE_PRESENCE_VECTOR; + + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::PREDICTION_PRESENCE_VECTOR: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::PREDICTION_PRESENCE_VECTOR; break; - case simulation::PRESENCE_VECTOR_TYPES::PREDICTION_PRESENCE_VECTOR: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::PREDICTION_PRESENCE_VECTOR; + + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::SIZE_PRESENCE_VECTOR: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::SIZE_PRESENCE_VECTOR; break; - case simulation::PRESENCE_VECTOR_TYPES::SIZE_PRESENCE_VECTOR: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::SIZE_PRESENCE_VECTOR; + + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::VELOCITY_INST_PRESENCE_VECTOR: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::VELOCITY_INST_PRESENCE_VECTOR; break; - case simulation::PRESENCE_VECTOR_TYPES::VELOCITY_INST_PRESENCE_VECTOR: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::VELOCITY_INST_PRESENCE_VECTOR; + + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::VELOCITY_PRESENCE_VECTOR: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::VELOCITY_PRESENCE_VECTOR; break; - case simulation::PRESENCE_VECTOR_TYPES::VELOCITY_PRESENCE_VECTOR: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::VELOCITY_PRESENCE_VECTOR; + + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::ID_PRESENCE_VECTOR: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::ID_PRESENCE_VECTOR; break; + default: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE; + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE; break; } return presence_vector_enum; diff --git a/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h b/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h index f4bfc74a1..f1357b10d 100644 --- a/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h +++ b/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h @@ -4,27 +4,25 @@ #include #include -using namespace tmx::messages; -using namespace std; namespace tmx::utils::sim { class SimulationExternalObjectConverter { private: - static void populateSimExternalObjectMetadata(const Json::Value &metadataValue, simulation::ExternalObject &simExternalObj); - static void populateSimExternalObjectHeader(const Json::Value &headerValue, simulation::ExternalObject &simExternalObj); - static void populateSimExternalObjectPose(const Json::Value &poseValue, simulation::ExternalObject &simExternalObj); - static void populateSimExternalObjectVelocity(const Json::Value &velocityValue, simulation::ExternalObject &simExternalObj); - static void populateSimExternalObjectSize(const Json::Value &sizeValue, simulation::ExternalObject &simExternalObj); - static void populateSimCovarianceArray(const Json::Value &covarianceArrayValue, std::vector &covarianceV); - static simulation::OBJECT_TYPES objectTypeStringToEnum(const string& object_type_str); - static simulation::PRESENCE_VECTOR_TYPES presenceVectorIntToEnum(uint16_t presence_vector); + static void populateSimExternalObjectMetadata(const Json::Value &metadataValue, tmx::messages::simulation::ExternalObject &simExternalObj); + static void populateSimExternalObjectHeader(const Json::Value &headerValue, tmx::messages::simulation::ExternalObject &simExternalObj); + static void populateSimExternalObjectPose(const Json::Value &poseValue, tmx::messages::simulation::ExternalObject &simExternalObj); + static void populateSimExternalObjectVelocity(const Json::Value &velocityValue, tmx::messages::simulation::ExternalObject &simExternalObj); + static void populateSimExternalObjectSize(const Json::Value &sizeValue, tmx::messages::simulation::ExternalObject &simExternalObj); + static void populateSimCovarianceArray(const Json::Value &covarianceArrayValue, std::vector &covarianceV); + static tmx::messages::simulation::OBJECT_TYPES objectTypeStringToEnum(const std::string& object_type_str); + static tmx::messages::simulation::PRESENCE_VECTOR_TYPES presenceVectorIntToEnum(uint16_t presence_vector); public: SimulationExternalObjectConverter() = default; ~SimulationExternalObjectConverter() = default; - static string simExternalObjToJsonStr(simulation::ExternalObject &simExternalObj); - static void jsonToSimExternalObj(const string &jsonStr, simulation::ExternalObject &simExternalObj); + static std::string simExternalObjToJsonStr(tmx::messages::simulation::ExternalObject &simExternalObj); + static void jsonToSimExternalObj(const std::string &jsonStr, tmx::messages::simulation::ExternalObject &simExternalObj); }; } diff --git a/src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp b/src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp index 73be4b5f9..1a9bf6587 100644 --- a/src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp +++ b/src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp @@ -5,12 +5,18 @@ using namespace tmx::utils::sim; using namespace tmx::messages; -TEST(SimulationExternalObjectConverter, jsonToSimExternalObj) +TEST(SimulationExternalObjectConverter, jsonToSimExternalObjInvalidJson) { - SimulationExternalObjectConverter converter; simulation::ExternalObject simExternalObj; - std::string json_string = "{\"metadata\":{\"is_simulation\":false,\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":0.0,\"sensor_y\":0.0,\"sensor_z\":0.0,\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":0,\"stamp\":{\"secs\":0,\"nsecs\":0}},\"id\":0,\"pose\":{\"pose\":{\"position\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"orientation\":{\"x\":0.0,\"y\":0.0,\"z\":0.0,\"w\":0.0}},\"covariance\":[12.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"velocity\":{\"twist\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"size\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"confidence\":0.0,\"object_type\":\"\",\"dynamic_obj\":false}"; - converter.jsonToSimExternalObj(json_string, simExternalObj); + std::string invalidJsonStr = "Invalid"; + ASSERT_THROW(SimulationExternalObjectConverter::jsonToSimExternalObj(invalidJsonStr, simExternalObj), std::runtime_error); +} + +TEST(SimulationExternalObjectConverter, jsonToSimExternalObjValidJson) +{ + simulation::ExternalObject simExternalObj; + std::string jsonStr = "{\"metadata\":{\"is_simulation\":false,\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":0.0,\"sensor_y\":0.0,\"sensor_z\":0.0,\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":0,\"stamp\":{\"secs\":0,\"nsecs\":0}},\"id\":0,\"pose\":{\"pose\":{\"position\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"orientation\":{\"x\":0.0,\"y\":0.0,\"z\":0.0,\"w\":0.0}},\"covariance\":[12.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"velocity\":{\"twist\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"size\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"confidence\":0.0,\"object_type\":\"\",\"dynamic_obj\":false}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); ASSERT_EQ(0, simExternalObj.get_Id()); ASSERT_EQ(false, simExternalObj.get_MetadataIsSimulation()); ASSERT_EQ("", simExternalObj.get_MetadataDatum()); @@ -30,13 +36,96 @@ TEST(SimulationExternalObjectConverter, jsonToSimExternalObj) ASSERT_EQ(36, simExternalObj.get_VelocityCovariance().size()); } +TEST(SimulationExternalObjectConverter, jsonToSimExternalObjPresenceVector) +{ + simulation::ExternalObject simExternalObj; + ASSERT_TRUE(simExternalObj.is_empty()); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE, simExternalObj.get_PresenceVector()); + ASSERT_FALSE(simExternalObj.is_empty()); + + std::string jsonStr = "{\"presence_vector\":1}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::ID_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":2}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::POSE_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":4}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::VELOCITY_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":8}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::VELOCITY_INST_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":16}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::SIZE_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":32}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::CONFIDENCE_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":64}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::OBJECT_TYPE_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":128}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::BSM_ID_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":256}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::DYNAMIC_OBJ_PRESENCE, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":512}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::PREDICTION_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":212121}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE, simExternalObj.get_PresenceVector()); +} + +TEST(SimulationExternalObjectConverter, jsonToSimExternalObjObjectTypes) +{ + simulation::ExternalObject simExternalObj; + ASSERT_TRUE(simExternalObj.is_empty()); + ASSERT_EQ(simulation::OBJECT_TYPES::UNKNOWN, simExternalObj.get_ObjectType()); + ASSERT_FALSE(simExternalObj.is_empty()); + + std::string jsonStr = "{\"object_type\":\"1\"}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::OBJECT_TYPES::SMALL_VEHICLE, simExternalObj.get_ObjectType()); + + jsonStr = "{\"object_type\":\"2\"}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::OBJECT_TYPES::LARGE_VEHICLE, simExternalObj.get_ObjectType()); + + jsonStr = "{\"object_type\":\"3\"}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::OBJECT_TYPES::MOTORCYCLE, simExternalObj.get_ObjectType()); + + jsonStr = "{\"object_type\":\"5\"}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::OBJECT_TYPES::UNKNOWN, simExternalObj.get_ObjectType()); + + jsonStr = "{\"object_type\":\"4\"}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::OBJECT_TYPES::PEDESTRIAN, simExternalObj.get_ObjectType()); + + jsonStr = "{\"object_type\":\"invalid\"}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::OBJECT_TYPES::UNKNOWN, simExternalObj.get_ObjectType()); +} + TEST(SimulationExternalObjectConverter, simExternalObjToJsonStr) { - SimulationExternalObjectConverter converter; simulation::ExternalObject simExternalObj; - std::string json_string = "{\"metadata\":{\"is_simulation\":false,\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":0.0,\"sensor_y\":0.0,\"sensor_z\":0.0,\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":0,\"stamp\":{\"secs\":0,\"nsecs\":0}},\"id\":0,\"pose\":{\"pose\":{\"position\":{\"x\":34.0,\"y\":0.0,\"z\":0.0},\"orientation\":{\"x\":23.0,\"y\":0.0,\"z\":0.0,\"w\":0.0}},\"covariance\":[12.1,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,11.0]},\"velocity\":{\"twist\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"covariance\":[12.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"size\":{\"x\":12.0,\"y\":23.0,\"z\":12.0},\"confidence\":1.0,\"object_type\":\"128\",\"dynamic_obj\":false}"; - converter.jsonToSimExternalObj(json_string, simExternalObj); - std::string output = converter.simExternalObjToJsonStr(simExternalObj); + std::string jsonStr = "{\"metadata\":{\"is_simulation\":false,\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":0.0,\"sensor_y\":0.0,\"sensor_z\":0.0,\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":0,\"stamp\":{\"secs\":0,\"nsecs\":0}},\"id\":0,\"pose\":{\"pose\":{\"position\":{\"x\":34.0,\"y\":0.0,\"z\":0.0},\"orientation\":{\"x\":23.0,\"y\":0.0,\"z\":0.0,\"w\":0.0}},\"covariance\":[12.1,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,11.0]},\"velocity\":{\"twist\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"covariance\":[12.0,17.33333333,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,20.3333]},\"size\":{\"x\":12.0,\"y\":23.0,\"z\":12.0},\"confidence\":1.0,\"object_type\":\"128\",\"dynamic_obj\":false}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + std::string output = SimulationExternalObjectConverter::simExternalObjToJsonStr(simExternalObj); Json::CharReaderBuilder builder; const std::unique_ptr reader(builder.newCharReader()); Json::Value root; @@ -45,4 +134,6 @@ TEST(SimulationExternalObjectConverter, simExternalObjToJsonStr) ASSERT_FALSE(root["metadata"]["is_simulation"].asBool()); ASSERT_EQ(12.1, root["pose"]["covariance"].begin()->asDouble()); ASSERT_EQ(23, root["pose"]["pose"]["orientation"]["x"].asDouble()); + ASSERT_EQ(12, root["velocity"]["covariance"].begin()->asDouble()); + ASSERT_EQ(17.33333333, root["velocity"]["covariance"][1].asDouble()); } \ 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 44529f2c8..3809cf6ba 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -631,8 +631,7 @@ void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){ void CARMAStreetsPlugin::HandleSimulatedExternalMessage(ExternalObject &msg, routeable_message &routeableMsg) { - tmx::utils::sim::SimulationExternalObjectConverter converter; - auto json_str = converter.simExternalObjToJsonStr(msg); + auto json_str = tmx::utils::sim::SimulationExternalObjectConverter::simExternalObjToJsonStr(msg); PLOG(logINFO) << "Produce External Object Message in JSON format: " << json_str < #include +#include #include #include @@ -11,14 +12,12 @@ TEST(SimulationMessages, ExternalObjectToRoutableMessage) { simulation::ExternalObject externalObj; string expectedStr = "{\"metadata\":{\"is_simulation\":false,\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":0.0,\"sensor_y\":0.0,\"sensor_z\":0.0,\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":0,\"stamp\":{\"secs\":0,\"nsecs\":0}},\"id\":0,\"pose\":{\"pose\":{\"position\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"orientation\":{\"x\":0.0,\"y\":0.0,\"z\":0.0,\"w\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"velocity\":{\"twist\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"size\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"confidence\":0.0,\"object_type\":\"\",\"dynamic_obj\":false}"; - externalObj.set_contents(expectedStr); - ASSERT_EQ(expectedStr, externalObj.to_string()); + tmx::utils::sim::SimulationExternalObjectConverter converter; + converter.jsonToSimExternalObj(expectedStr, externalObj); ASSERT_EQ("ExternalObject", std::string(simulation::ExternalObject::MessageSubType)); ASSERT_EQ("Simulation", std::string(simulation::ExternalObject::MessageType)); tmx::routeable_message routeableMsg; - routeableMsg.initialize(externalObj, "CDASimAdapter", 0 , IvpMsgFlags_None); - string output = "{\"metadata\":{\"is_simulation\":\"false\",\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":\"0.0\",\"sensor_y\":\"0.0\",\"sensor_z\":\"0.0\",\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":\"0\",\"stamp\":{\"secs\":\"0\",\"nsecs\":\"0\"}},\"id\":\"0\",\"pose\":{\"pose\":{\"position\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\"},\"orientation\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\",\"w\":\"0.0\"}},\"covariance\":[\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\"]},\"velocity\":{\"twist\":{\"linear\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\"},\"angular\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\"}},\"covariance\":[\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\"]},\"size\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\"},\"confidence\":\"0.0\",\"object_type\":\"\",\"dynamic_obj\":\"false\"}"; - ASSERT_EQ(output, routeableMsg.get_payload_str()); + routeableMsg.initialize(externalObj, "CDASimAdapter", 0, IvpMsgFlags_None); ASSERT_EQ("json", routeableMsg.get_encoding()); ASSERT_EQ(0, routeableMsg.get_flags()); auto current_time_mill = boost::chrono::duration_cast(boost::chrono::system_clock::now().time_since_epoch()).count(); From a1faf70f32b62a99b6e301abf200dd1537c78229 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Wed, 19 Jul 2023 14:58:55 +0000 Subject: [PATCH 06/35] add comments --- .../SimulationExternalObjectConverter.cpp | 3 ++ .../SimulationExternalObjectConverter.h | 54 ++++++++++++++++++- 2 files changed, 55 insertions(+), 2 deletions(-) diff --git a/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp b/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp index 4f15fb4c9..29735e287 100644 --- a/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp +++ b/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp @@ -94,6 +94,9 @@ namespace tmx::utils::sim populateSimExternalObjectHeader(root["header"], simExternalObj); } + /** + * Populate simulation external object presence vector + */ if (root.isMember("presence_vector") && root["presence_vector"].isUInt()) { auto presence_vetor = presenceVectorIntToEnum(root["presence_vector"].asUInt()); diff --git a/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h b/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h index f1357b10d..79822e8f0 100644 --- a/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h +++ b/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h @@ -10,19 +10,69 @@ namespace tmx::utils::sim class SimulationExternalObjectConverter { private: + /*** + * @brief Populate simulation external object metadata with metadata information from JSON string. + * @param ExternalObject V2xHub customized simulation external object to populate. + * @param metadataValue Json value that contains the metadata information. + */ static void populateSimExternalObjectMetadata(const Json::Value &metadataValue, tmx::messages::simulation::ExternalObject &simExternalObj); + /*** + * @brief Populate simulation external object header with header information from JSON string. + * @param ExternalObject V2xHub customized simulation external object to populate. + * @param headerValue Json value that contains the header information. + */ static void populateSimExternalObjectHeader(const Json::Value &headerValue, tmx::messages::simulation::ExternalObject &simExternalObj); + /*** + * @brief Populate simulation external object pose with pose information from JSON string. + * @param ExternalObject V2xHub customized simulation external object to populate. + * @param poseValue Json value that contains the pose information. + */ static void populateSimExternalObjectPose(const Json::Value &poseValue, tmx::messages::simulation::ExternalObject &simExternalObj); + /*** + * @brief Populate simulation external object velocity with velocity information from JSON string. + * @param ExternalObject V2xHub customized simulation external object to populate. + * @param velocityValue Json value that contains the velocity information. + */ static void populateSimExternalObjectVelocity(const Json::Value &velocityValue, tmx::messages::simulation::ExternalObject &simExternalObj); + /*** + * @brief Populate simulation external object size with size information from JSON string. + * @param ExternalObject V2xHub customized simulation external object to populate. + * @param sizeValue Json value that contains the size information. + */ static void populateSimExternalObjectSize(const Json::Value &sizeValue, tmx::messages::simulation::ExternalObject &simExternalObj); + /*** + * @brief Populate simulation external object covariance array with covariance array information from JSON string. + * @param ExternalObject V2xHub customized simulation external object to populate. + * @param covarianceArrayValue Json value that contains the covariance array information. + */ static void populateSimCovarianceArray(const Json::Value &covarianceArrayValue, std::vector &covarianceV); + /*** + * @brief Convert the object type in string format to enum. + * @param object_type_str object type in string format. + * @return tmx::messages::simulation::OBJECT_TYPES V2xHub customized simulation object types. + */ static tmx::messages::simulation::OBJECT_TYPES objectTypeStringToEnum(const std::string& object_type_str); + /*** + * @brief Convert the presence vector integer to enum. + * @param presence_vector presence vetor integer. + * @return tmx::messages::simulation::PRESENCE_VECTOR_TYPES V2xHub customized simulation presence vetor types. + */ static tmx::messages::simulation::PRESENCE_VECTOR_TYPES presenceVectorIntToEnum(uint16_t presence_vector); public: - SimulationExternalObjectConverter() = default; - ~SimulationExternalObjectConverter() = default; + SimulationExternalObjectConverter() = delete; + ~SimulationExternalObjectConverter() = delete; + /*** + * @brief Convert simulation external object into JSON string defined by MOSAIC and CARMAStreets integration. + * @param ExternalObject V2xHub customized simulation external object. + * @return EcternalObject string in JSON format defined by MOSAIC and CARMAStreets integration. + */ static std::string simExternalObjToJsonStr(tmx::messages::simulation::ExternalObject &simExternalObj); + /*** + * @brief Populate simulation external object with JSON string defined by MOSAIC and CARMAStreets integration. + * @param ExternalObject V2xHub customized simulation external object to populate. + * @param jsonStr EcternalObject in JSON format defined by MOSAIC and CARMAStreets integration. + */ static void jsonToSimExternalObj(const std::string &jsonStr, tmx::messages::simulation::ExternalObject &simExternalObj); }; } From 9a534b3475188a3cada25207cc4152a7fbd86f47 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Wed, 19 Jul 2023 15:12:29 +0000 Subject: [PATCH 07/35] use static func --- src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp b/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp index 7d6843fca..ede25aa06 100644 --- a/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp +++ b/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp @@ -12,8 +12,7 @@ TEST(SimulationMessages, ExternalObjectToRoutableMessage) { simulation::ExternalObject externalObj; string expectedStr = "{\"metadata\":{\"is_simulation\":false,\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":0.0,\"sensor_y\":0.0,\"sensor_z\":0.0,\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":0,\"stamp\":{\"secs\":0,\"nsecs\":0}},\"id\":0,\"pose\":{\"pose\":{\"position\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"orientation\":{\"x\":0.0,\"y\":0.0,\"z\":0.0,\"w\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"velocity\":{\"twist\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"size\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"confidence\":0.0,\"object_type\":\"\",\"dynamic_obj\":false}"; - tmx::utils::sim::SimulationExternalObjectConverter converter; - converter.jsonToSimExternalObj(expectedStr, externalObj); + tmx::utils::sim::SimulationExternalObjectConverter::jsonToSimExternalObj(expectedStr, externalObj); ASSERT_EQ("ExternalObject", std::string(simulation::ExternalObject::MessageSubType)); ASSERT_EQ("Simulation", std::string(simulation::ExternalObject::MessageType)); tmx::routeable_message routeableMsg; From 7252eb494c0a4bf872cf205e4fe7dfa46d1822d5 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Wed, 19 Jul 2023 15:17:22 +0000 Subject: [PATCH 08/35] address code smell --- src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp | 4 ++-- src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h | 3 +-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index 3809cf6ba..c523f354d 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -25,7 +25,7 @@ CARMAStreetsPlugin::CARMAStreetsPlugin(string name) : AddMessageFilter < tsm2Message > (this, &CARMAStreetsPlugin::HandleMobilityPathMessage); AddMessageFilter < MapDataMessage > (this, &CARMAStreetsPlugin::HandleMapMessage); AddMessageFilter < SrmMessage > (this, &CARMAStreetsPlugin::HandleSRMMessage); - AddMessageFilter < ExternalObject > (this, &CARMAStreetsPlugin::HandleSimulatedExternalMessage ); + AddMessageFilter < simulation::ExternalObject > (this, &CARMAStreetsPlugin::HandleSimulatedExternalMessage ); SubscribeToMessages(); @@ -629,7 +629,7 @@ void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){ } -void CARMAStreetsPlugin::HandleSimulatedExternalMessage(ExternalObject &msg, routeable_message &routeableMsg) +void CARMAStreetsPlugin::HandleSimulatedExternalMessage(simulation::ExternalObject &msg, routeable_message &routeableMsg) { auto json_str = tmx::utils::sim::SimulationExternalObjectConverter::simExternalObjToJsonStr(msg); PLOG(logINFO) << "Produce External Object Message in JSON format: " << json_str < Date: Fri, 21 Jul 2023 14:01:57 +0000 Subject: [PATCH 09/35] resolve merge error --- src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp | 2 -- src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp | 8 +++----- .../CDASimAdapter/src/include/CDASimConnection.hpp | 6 +++--- .../CDASimAdapter/test/TestCARMASimulationConnection.cpp | 8 ++++---- 4 files changed, 10 insertions(+), 14 deletions(-) diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp index 31fe77ec8..ca003d1d5 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp @@ -60,8 +60,6 @@ namespace CDASimAdapter{ if ( connection->is_connected() ) { start_time_sync_thread_timer(); - start_amf_msg_thread(); - start_binary_msg_thread(); start_external_object_detection_thread(); start_immediate_forward_thread(); start_message_receiver_thread(); diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp index 89ca76dbb..ffb27de5b 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp @@ -4,11 +4,9 @@ using namespace tmx::utils; namespace CDASimAdapter{ - CDASimConnection::CDASimConnection(const std::string &simulation_ip, const uint infrastructure_id, const uint simulation_registration_port, const uint sim_v2x_port, + 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 external_object_detection_port, const uint v2x_port, const Point &location) : - - _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), _external_object_detection_port(external_object_detection_port),_v2x_port(v2x_port), _location(location) { @@ -36,7 +34,7 @@ namespace CDASimAdapter{ } - std::string CDASimConnection::get_handshake_json(const uint infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint external_object_detection_port, const uint v2x_port, + std::string CDASimConnection::get_handshake_json(const std::string &infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint external_object_detection_port, const uint v2x_port, const Point &location) const { @@ -56,7 +54,7 @@ namespace CDASimAdapter{ return message_str; } - bool CDASimConnection::carma_simulation_handshake(const std::string &simulation_ip, const uint infrastructure_id, const uint simulation_registration_port, + 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 external_object_detection_port, const uint v2x_port, const Point &location) { diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp index 302fd9e2c..be054e9f6 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp @@ -33,7 +33,7 @@ namespace CDASimAdapter { * @param v2x_port Port on which connecction listens for incoming v2x messages. * @param location Simulationed location of infrastructure. */ - explicit CDASimConnection( const std::string &simulation_ip, const uint infrastructure_id, const uint simulation_registration_port, + 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 external_object_detection_port, const uint v2x_port, const tmx::utils::Point &location); /** @@ -101,7 +101,7 @@ namespace CDASimAdapter { * @param location simulated location of infrastructure hardware. * @return true if handshake successful and false if handshake unsuccessful. */ - bool carma_simulation_handshake(const std::string &simulation_ip, const uint infrastructure_id, const uint simulation_registration_port, + 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 external_object_detection_port, const uint v2x_port, const tmx::utils::Point &location); @@ -137,7 +137,7 @@ 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 uint infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint external_object_detection_port, + std::string get_handshake_json(const std::string &infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint external_object_detection_port, const uint v2x_port, const tmx::utils::Point &location) const; std::string _simulation_ip; uint _simulation_registration_port; diff --git a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp index 6fffad4e2..3f88a8913 100644 --- a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp @@ -23,7 +23,7 @@ namespace CDASimAdapter { void SetUp() override { // Initialize CARMA Simulation connection with (0,0,0) location and mock kafka producer. Point location; - connection = std::make_shared("127.0.0.1", 1212, 4567, 4678, "127.0.0.1", 1213, 1214, 1215, location); + connection = std::make_shared("127.0.0.1", "1212", 4567, 4678, "127.0.0.1", 1213, 1214, 1215, location); } void TearDown() override { @@ -85,14 +85,14 @@ 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, 4569, location), - "{\n \"ExternalObjectDetectionPort\" : 4568,\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 \"timeSyncPort\" : 4567\n}\n"); + ASSERT_EQ(connection->get_handshake_json("4566", "127.0.0.1", 4567, 4568, 4569, location), + "{\n \"ExternalObjectDetectionPort\" : 4568,\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 \"timeSyncPort\" : 4567\n}\n"); } TEST_F( TestCARMASimulationConnection, carma_simulation_handshake) { Point location; // UDP creation error - ASSERT_FALSE(connection->carma_simulation_handshake("", 45, NULL, + ASSERT_FALSE(connection->carma_simulation_handshake("", "45", NULL, "", 45, 45, 45, location)); } From 144f29cf4956e3f747a37210cf3082474bede6f7 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Fri, 21 Jul 2023 14:04:56 +0000 Subject: [PATCH 10/35] remove timer thread --- src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp index 4686b6ee3..a4e6d4e3f 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp @@ -119,7 +119,6 @@ namespace CDASimAdapter std::unique_ptr connection; // Mutex for configuration parameter thread safety std::mutex _lock; - std::unique_ptr thread_timer; std::unique_ptr external_bject_detection_thread_timer; // Time sync thread to forward time sync messages to PluginClientClockAware V2X-Hub plugins. std::unique_ptr time_sync_timer; From f75137d15f637b83fd2e521208ce4b0f37cdb91e Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Mon, 24 Jul 2023 22:16:07 +0000 Subject: [PATCH 11/35] update message definition --- src/tmx/Messages/include/MessageTypes.h | 3 +- src/tmx/Messages/include/simulation/BSMID.h | 38 -- .../include/simulation/ExternalObject.h | 132 ------ .../include/simulation/ObjectEnumTypes.h | 23 - .../simulation/PresenceVectorEnumTypes.h | 28 -- .../include/simulation/SensorDetectedObject.h | 102 +++++ .../SimulationExternalObjectConverter.cpp | 424 ------------------ .../SimulationExternalObjectConverter.h | 78 ---- ...imulationSensorDetectedObjectConverter.cpp | 247 ++++++++++ .../SimulationSensorDetectedObjectConverter.h | 66 +++ ...t_simulation_external_object_converter.cpp | 151 ++----- .../src/CARMAStreetsPlugin.cpp | 6 +- .../src/CARMAStreetsPlugin.h | 6 +- .../scripts/send_sim_detected_object_udp.py | 88 ++++ .../scripts/send_sim_external_object_udp.py | 116 ----- .../CDASimAdapter/src/CDASimAdapter.cpp | 6 +- .../CDASimAdapter/src/CDASimConnection.cpp | 8 +- .../src/include/CDASimAdapter.hpp | 4 +- .../src/include/CDASimConnection.hpp | 8 +- .../test/TestCARMASimulationConnection.cpp | 2 +- .../test/TestSimulationMessages.cpp | 18 +- 21 files changed, 565 insertions(+), 989 deletions(-) delete mode 100644 src/tmx/Messages/include/simulation/BSMID.h delete mode 100644 src/tmx/Messages/include/simulation/ExternalObject.h delete mode 100644 src/tmx/Messages/include/simulation/ObjectEnumTypes.h delete mode 100644 src/tmx/Messages/include/simulation/PresenceVectorEnumTypes.h create mode 100644 src/tmx/Messages/include/simulation/SensorDetectedObject.h delete mode 100644 src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp delete mode 100644 src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h create mode 100644 src/tmx/TmxUtils/src/simulation/SimulationSensorDetectedObjectConverter.cpp create mode 100644 src/tmx/TmxUtils/src/simulation/SimulationSensorDetectedObjectConverter.h create mode 100755 src/v2i-hub/CDASimAdapter/scripts/send_sim_detected_object_udp.py delete mode 100755 src/v2i-hub/CDASimAdapter/scripts/send_sim_external_object_udp.py diff --git a/src/tmx/Messages/include/MessageTypes.h b/src/tmx/Messages/include/MessageTypes.h index 2821d10cc..e0fd09c70 100644 --- a/src/tmx/Messages/include/MessageTypes.h +++ b/src/tmx/Messages/include/MessageTypes.h @@ -54,7 +54,6 @@ static CONSTEXPR const char *MSGTYPE_VEHICLE_STRING = "Vehicle"; static CONSTEXPR const char *MSGTYPE_PEDESTRIAN_STRING = "Pedestrian"; static CONSTEXPR const char *MSGTYPE_PMM_STRING = "Pmm"; static CONSTEXPR const char *MSGTYPE_RADIO_STRING = "Radio"; -static CONSTEXPR const char *MSGTYPE_DETECTED_STRING = "Simulation"; enum MsgSubType { @@ -92,7 +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_EXTERNAL_OBJECT_STRING = "ExternalObject"; +static CONSTEXPR const char *MSGSUBTYPE_SENSOR_DETECTED_OBJECT_STRING = "SensorDetectedObject"; } /* End namespace messages */ diff --git a/src/tmx/Messages/include/simulation/BSMID.h b/src/tmx/Messages/include/simulation/BSMID.h deleted file mode 100644 index 23dba4fc2..000000000 --- a/src/tmx/Messages/include/simulation/BSMID.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef INCLUDE_SIMULATED_BSMID_H_ -#define INCLUDE_SIMULATED_BSMID_H_ - -#include -#include - -namespace tmx -{ - namespace messages - { - namespace simulation - { - - struct BSMID - { - uint8_t BsmId = 0; - - BSMID() {} - BSMID(std::uint8_t bsmId) : BsmId(bsmId) {} - - static message_tree_type to_tree(BSMID element) - { - message_tree_type treeElement; - treeElement.put("BsmId", element.BsmId); - return treeElement; - } - - static BSMID from_tree(message_tree_type &treeElement) - { - BSMID element; - element.BsmId = treeElement.get("BsmId"); - return element; - } - }; - } - } -} -#endif \ No newline at end of file diff --git a/src/tmx/Messages/include/simulation/ExternalObject.h b/src/tmx/Messages/include/simulation/ExternalObject.h deleted file mode 100644 index 64930fe5f..000000000 --- a/src/tmx/Messages/include/simulation/ExternalObject.h +++ /dev/null @@ -1,132 +0,0 @@ -#ifndef INCLUDE_SIMULATED_EXTERNALOBJECT_H_ -#define INCLUDE_SIMULATED_EXTERNALOBJECT_H_ - -#include -#include -#include -#include -#include -#include - -namespace tmx -{ - namespace messages - { - namespace simulation - { - /** - * This ExternalObject 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 ExternalObject : public tmx::message - { - public: - ExternalObject(){}; - ExternalObject(const tmx::message_container_type &contents) : tmx::message(contents) {}; - ~ExternalObject(){}; - // Message type fpr routing this message through TMX core - static constexpr const char *MessageType = MSGTYPE_DETECTED_STRING; - - // Message sub type for routing this message through TMX core - static constexpr const char *MessageSubType = MSGSUBTYPE_EXTERNAL_OBJECT_STRING; - /** - * Metadata to describe the external object - */ - std_attribute(this->msg, bool, MetadataIsSimulation, false, ); - std_attribute(this->msg, std::string, MetadataDatum, "", ); - std_attribute(this->msg, std::string, MetadataProjString, "", ); - std_attribute(this->msg, double, MetadataSensorX, 0, ); - std_attribute(this->msg, double, MetadataSensorY, 0, ); - std_attribute(this->msg, double, MetadataSensorZ, 0, ); - std_attribute(this->msg, std::string, MetadataInfrastructureId, "", ); - std_attribute(this->msg, std::string, MetadataSensorId, "", ); - /** - *Header contains the frame rest of the fields will use - */ - // sequence ID: consecutively increasing ID - std_attribute(this->msg, uint32_t, HeaderSeq, 0, ); - // Two-integer timestamp that is expressed as: - // # * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') - // # * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') - // # time-handling sugar is provided by the client library - // The seconds component, valid over all int32 values. - std_attribute(this->msg, uint32_t, HeaderStampSecs, 0, ); - // # The nanoseconds component, valid in the range [0, 10e9). - std_attribute(this->msg, uint32_t, HeaderStampNSecs, 0, ); - - // A presence vector, this message is used to describe objects coming from potentially different - // sources. The presence vector is used to determine what items are set by the producer. - std_attribute(this->msg, PRESENCE_VECTOR_TYPES, PresenceVector, PRESENCE_VECTOR_TYPES::UNAVAILABLE, ); - - // Object id. Matching ids on a topic should refer to the same object within some time period, expanded - std_attribute(this->msg, uint32_t, Id, 0, ); - - // bsm id is of form [0xff, 0xff, 0xff, 0xff]. It is not required. - array_attribute( BSMID, BsmId); - - // Pose of the object within the frame specified in header - // geometry_msgs/PoseWithCovariance pose - // This represents a pose in free space with uncertainty. - std_attribute(this->msg, double, PosePosePositionX, 0, ); - std_attribute(this->msg, double, PosePosePositionY, 0, ); - std_attribute(this->msg, double, PosePosePositionZ, 0, ); - // This represents an orientation in free space in quaternion form. - std_attribute(this->msg, double, PosePoseOrientationX, 0, ); - std_attribute(this->msg, double, PosePoseOrientationY, 0, ); - std_attribute(this->msg, double, PosePoseOrientationZ, 0, ); - std_attribute(this->msg, double, PosePoseOrientationW, 0, ); - array_attribute( Covariance, PoseCovariance); - - // #Average velocity of the object within the frame specified in header - // geometry_msgs/TwistWithCovariance velocity - std_attribute(this->msg, double, VelocityTwistLinearX, 0, ); - std_attribute(this->msg, double, VelocityTwistLinearY, 0, ); - std_attribute(this->msg, double, VelocityTwistLinearZ, 0, ); - std_attribute(this->msg, double, VelocityTwistAngularX, 0, ); - std_attribute(this->msg, double, VelocityTwistAngularY, 0, ); - std_attribute(this->msg, double, VelocityTwistAngularZ, 0, ); - array_attribute( Covariance, VelocityCovariance); - - // #Instantaneous velocity of an object within the frame specified in header - // geometry_msgs/TwistWithCovariance velocity_inst - std_attribute(this->msg, double, VelocityInstTwistLinearX, 0, ); - std_attribute(this->msg, double, VelocityInstTwistLinearY, 0, ); - std_attribute(this->msg, double, VelocityInstTwistLinearZ, 0, ); - std_attribute(this->msg, double, VelocityInstTwistAngularX, 0, ); - std_attribute(this->msg, double, VelocityInstTwistAngularY, 0, ); - std_attribute(this->msg, double, VelocityInstTwistAngularZ, 0, ); - array_attribute( Covariance, VelocityInstCovariance); - - // #The size of the object aligned along the axis of the object described by the orientation in pose - // #Dimensions are specified in meters - /** - * # This represents a vector in free space. - # It is only meant to represent a direction. Therefore, it does not - # make sense to apply a translation to it (e.g., when applying a - # generic rigid transformation to a Vector3, tf2 will only apply the - # rotation). - */ - // geometry_msgs/Vector3 size - std_attribute(this->msg, double, SizeX, 0, ); - std_attribute(this->msg, double, SizeY, 0, ); - std_attribute(this->msg, double, SizeZ, 0, ); - - // #Confidence [0,1] - std_attribute(this->msg, double, Confidence, 0, ); - - // #describes a general object type as defined in this message - std_attribute(this->msg, OBJECT_TYPES, ObjectType, OBJECT_TYPES::UNKNOWN, ); - - // # Binary value to show if the object is static or dynamic (1: dynamic, 0: static) - std_attribute(this->msg, bool, DynamticObj, false, ); - - // Ignored: Predictions for the object. - // carma_perception_msgs/PredictedState[] predictions - }; - - } - } - -}; // namespace tmx -#endif diff --git a/src/tmx/Messages/include/simulation/ObjectEnumTypes.h b/src/tmx/Messages/include/simulation/ObjectEnumTypes.h deleted file mode 100644 index 15d443a48..000000000 --- a/src/tmx/Messages/include/simulation/ObjectEnumTypes.h +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef INCLUDE_SIMULATED_OBJECT_TYPE_H_ -#define INCLUDE_SIMULATED_OBJECT_TYPE_H_ - -namespace tmx -{ - namespace messages - { - namespace simulation - { - // #used for object type - enum OBJECT_TYPES - { - UNKNOWN = 0, - SMALL_VEHICLE = 1, - LARGE_VEHICLE = 2, - MOTORCYCLE = 3, - PEDESTRIAN = 4 - }; - } - } - -}; // namespace tmx -#endif \ No newline at end of file diff --git a/src/tmx/Messages/include/simulation/PresenceVectorEnumTypes.h b/src/tmx/Messages/include/simulation/PresenceVectorEnumTypes.h deleted file mode 100644 index 94a328620..000000000 --- a/src/tmx/Messages/include/simulation/PresenceVectorEnumTypes.h +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef INCLUDE_SIMULATED_PRESENCE_VECTOR_H_ -#define INCLUDE_SIMULATED_PRESENCE_VECTOR_H_ - -namespace tmx -{ - namespace messages - { - namespace simulation - { - enum PRESENCE_VECTOR_TYPES - { - UNAVAILABLE = 0, - ID_PRESENCE_VECTOR = 1, - POSE_PRESENCE_VECTOR = 2, - VELOCITY_PRESENCE_VECTOR = 4, - VELOCITY_INST_PRESENCE_VECTOR = 8, - SIZE_PRESENCE_VECTOR = 16, - CONFIDENCE_PRESENCE_VECTOR = 32, - OBJECT_TYPE_PRESENCE_VECTOR = 64, - BSM_ID_PRESENCE_VECTOR = 128, - DYNAMIC_OBJ_PRESENCE = 256, - PREDICTION_PRESENCE_VECTOR = 512 - }; - } - } - -}; // namespace tmx -#endif \ No newline at end of file diff --git a/src/tmx/Messages/include/simulation/SensorDetectedObject.h b/src/tmx/Messages/include/simulation/SensorDetectedObject.h new file mode 100644 index 000000000..475005069 --- /dev/null +++ b/src/tmx/Messages/include/simulation/SensorDetectedObject.h @@ -0,0 +1,102 @@ +#ifndef INCLUDE_SIMULATED_SensorDetectedObject_H_ +#define INCLUDE_SIMULATED_SensorDetectedObject_H_ + +#include +#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; + /** + * Metadata to describe the external object + */ + std_attribute(this->msg, bool, MetadataIsSimulation, false, ); + std_attribute(this->msg, uint64_t, MetadataTimestamp, 0, ); + std_attribute(this->msg, std::string, SensorProjString, "", ); + std_attribute(this->msg, std::string, SensorType, "", ); + std_attribute(this->msg, std::string, SensorId, "", ); + std_attribute(this->msg, double, SensorLocationX, 0, ); + std_attribute(this->msg, double, SensorLocationY, 0, ); + std_attribute(this->msg, double, SensorLocationZ, 0, ); + std_attribute(this->msg, std::string, MetadataInfrastructureId, "", ); + std_attribute(this->msg, std::string, MetadataSensorId, "", ); + + // Object id. Matching ids on a topic should refer to the same object within some time period, expanded + std_attribute(this->msg, std::string, Id, "", ); + + // Pose of the object within the frame specified in header + // This represents a pose in free space with uncertainty. + std_attribute(this->msg, double, PositionX, 0, ); + std_attribute(this->msg, double, PositionY, 0, ); + std_attribute(this->msg, double, PositionZ, 0, ); + // This represents an orientation in free space in quaternion form. + std_attribute(this->msg, double, OrientationX, 0, ); + std_attribute(this->msg, double, OrientationY, 0, ); + std_attribute(this->msg, double, OrientationZ, 0, ); + std_attribute(this->msg, double, OrientationW, 0, ); + array_attribute( Covariance, PositionCovariance); + + // #Average velocity of the object within the frame specified in header + std_attribute(this->msg, double, VelocityTwistLinearX, 0, ); + std_attribute(this->msg, double, VelocityTwistLinearY, 0, ); + std_attribute(this->msg, double, VelocityTwistLinearZ, 0, ); + std_attribute(this->msg, double, VelocityTwistAngularX, 0, ); + std_attribute(this->msg, double, VelocityTwistAngularY, 0, ); + std_attribute(this->msg, double, VelocityTwistAngularZ, 0, ); + array_attribute( Covariance, VelocityCovariance); + + // #Instantaneous velocity of an object within the frame specified in header + std_attribute(this->msg, double, VelocityInstTwistLinearX, 0, ); + std_attribute(this->msg, double, VelocityInstTwistLinearY, 0, ); + std_attribute(this->msg, double, VelocityInstTwistLinearZ, 0, ); + std_attribute(this->msg, double, VelocityInstTwistAngularX, 0, ); + std_attribute(this->msg, double, VelocityInstTwistAngularY, 0, ); + std_attribute(this->msg, double, VelocityInstTwistAngularZ, 0, ); + array_attribute( Covariance, VelocityInstCovariance); + + // #The size of the object aligned along the axis of the object described by the orientation in pose + // #Dimensions are specified in meters + /** + * # This represents a vector in free space. + # It is only meant to represent a direction. Therefore, it does not + # make sense to apply a translation to it (e.g., when applying a + # generic rigid transformation to a Vector3, tf2 will only apply the + # rotation). + */ + // geometry_msgs/Vector3 size + std_attribute(this->msg, double, SizeLength, 0, ); + std_attribute(this->msg, double, SizeWidth, 0, ); + std_attribute(this->msg, double, SizeHeight, 0, ); + + // #Confidence [0,1] + std_attribute(this->msg, double, Confidence, 0, ); + + // #describes a general object type as defined in this message + std_attribute(this->msg, std::string, ObjectType, "", ); + }; + + } + } + +}; // namespace tmx +#endif diff --git a/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp b/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp deleted file mode 100644 index 29735e287..000000000 --- a/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp +++ /dev/null @@ -1,424 +0,0 @@ -#include - -namespace tmx::utils::sim -{ - std::string SimulationExternalObjectConverter::simExternalObjToJsonStr(tmx::messages::simulation::ExternalObject &simExternalObj) - { - Json::Value root; - Json::Value metadata; - metadata["is_simulation"] = simExternalObj.get_MetadataIsSimulation(); - metadata["datum"] = simExternalObj.get_MetadataDatum(); - metadata["proj_string"] = simExternalObj.get_MetadataProjString(); - metadata["sensor_x"] = simExternalObj.get_MetadataSensorX(); - metadata["sensor_y"] = simExternalObj.get_MetadataSensorY(); - metadata["sensor_z"] = simExternalObj.get_MetadataSensorZ(); - metadata["infrastructure_id"] = simExternalObj.get_MetadataInfrastructureId(); - metadata["sensor_id"] = simExternalObj.get_MetadataSensorId(); - root["metadata"] = metadata; - - Json::Value header; - header["seq"] = simExternalObj.get_HeaderSeq(); - header["stamp"]["secs"] = simExternalObj.get_HeaderStampSecs(); - header["stamp"]["nsecs"] = simExternalObj.get_HeaderStampNSecs(); - root["header"] = header; - - root["id"] = simExternalObj.get_Id(); - if (simExternalObj.get_PresenceVector() != tmx::messages::simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE) - { - root["presence_vector"] = simExternalObj.get_PresenceVector(); - } - - Json::Value pose; - pose["pose"]["position"]["x"] = simExternalObj.get_PosePosePositionX(); - pose["pose"]["position"]["y"] = simExternalObj.get_PosePosePositionY(); - pose["pose"]["position"]["z"] = simExternalObj.get_PosePosePositionZ(); - pose["pose"]["orientation"]["x"] = simExternalObj.get_PosePoseOrientationX(); - pose["pose"]["orientation"]["y"] = simExternalObj.get_PosePoseOrientationY(); - pose["pose"]["orientation"]["z"] = simExternalObj.get_PosePoseOrientationZ(); - pose["pose"]["orientation"]["w"] = simExternalObj.get_PosePoseOrientationW(); - auto pCovarianceV = simExternalObj.get_PoseCovariance(); - std::for_each(pCovarianceV.begin(), pCovarianceV.end(), [&pose](const auto &item) - { pose["covariance"].append(Json::Value(item.covariance)); }); - root["pose"] = pose; - - Json::Value velocity; - velocity["twist"]["linear"]["x"] = simExternalObj.get_VelocityTwistLinearX(); - velocity["twist"]["linear"]["y"] = simExternalObj.get_VelocityTwistLinearY(); - velocity["twist"]["linear"]["z"] = simExternalObj.get_VelocityTwistLinearZ(); - velocity["twist"]["angular"]["x"] = simExternalObj.get_VelocityTwistAngularX(); - velocity["twist"]["angular"]["y"] = simExternalObj.get_VelocityTwistAngularY(); - velocity["twist"]["angular"]["z"] = simExternalObj.get_VelocityTwistAngularZ(); - auto vCovarianceV = simExternalObj.get_VelocityCovariance(); - std::for_each(vCovarianceV.begin(), vCovarianceV.end(), [&velocity](const auto &item) - { velocity["covariance"].append(Json::Value(item.covariance)); }); - root["velocity"] = velocity; - - Json::Value size; - size["x"] = simExternalObj.get_SizeX(); - size["y"] = simExternalObj.get_SizeY(); - size["z"] = simExternalObj.get_SizeZ(); - root["size"] = size; - - root["confidence"] = simExternalObj.get_Confidence(); - root["object_type"] = std::to_string(simExternalObj.get_ObjectType()); - root["dynamic_obj"] = simExternalObj.get_DynamticObj(); - Json::StreamWriterBuilder builder; - const std::string json_str = Json::writeString(builder, root); - return json_str; - } - - void SimulationExternalObjectConverter::jsonToSimExternalObj(const std::string &jsonStr, tmx::messages::simulation::ExternalObject &simExternalObj) - { - Json::CharReaderBuilder builder; - const std::unique_ptr reader(builder.newCharReader()); - Json::Value root; - JSONCPP_STRING err; - if (!reader->parse(jsonStr.c_str(), jsonStr.c_str() + static_cast(jsonStr.length()), &root, &err)) - { - throw std::runtime_error("Error parsing external object JSON string."); - } - - /** - * Populate simulation external object metadata - */ - if (root.isMember("metadata") && root["metadata"].isObject()) - { - populateSimExternalObjectMetadata(root["metadata"], simExternalObj); - } - - /** - * Populate simulation external object header - */ - if (root.isMember("header") && root["header"].isObject()) - { - populateSimExternalObjectHeader(root["header"], simExternalObj); - } - - /** - * Populate simulation external object presence vector - */ - if (root.isMember("presence_vector") && root["presence_vector"].isUInt()) - { - auto presence_vetor = presenceVectorIntToEnum(root["presence_vector"].asUInt()); - simExternalObj.set_PresenceVector(presence_vetor); - } - - // Populate simulation external object id - if (root.isMember("id") && root["id"].isUInt()) - { - simExternalObj.set_Id(root["id"].asUInt()); - } - - /*** - * Populate simulation external object pose - */ - if (root.isMember("pose") && root["pose"].isObject()) - { - populateSimExternalObjectPose(root["pose"], simExternalObj); - } - - /*** - *Populate simulation external object velocity - */ - if (root.isMember("velocity") && root["velocity"].isObject()) - { - populateSimExternalObjectVelocity(root["velocity"], simExternalObj); - } - - /*** - * Populate simulation external object size - */ - if (root.isMember("size") && root["size"].isObject()) - { - populateSimExternalObjectSize(root["size"], simExternalObj); - } - - // Populate simulation external object confidence - if (root.isMember("confidence") && root["confidence"].isDouble()) - { - simExternalObj.set_Confidence(root["confidence"].asDouble()); - } - - // Populate simulation external object object_type - if (root.isMember("object_type") && root["object_type"].isString()) - { - auto object_type = objectTypeStringToEnum(root["object_type"].asString()); - simExternalObj.set_ObjectType(object_type); - } - - // Populate simulation external object dynamic_obj - if (root.isMember("dynamic_obj") && root["dynamic_obj"].isBool()) - { - simExternalObj.set_DynamticObj(root["dynamic_obj"].asBool()); - } - } - - void SimulationExternalObjectConverter::populateSimExternalObjectMetadata(const Json::Value &metadataValue, tmx::messages::simulation::ExternalObject &simExternalObj) - { - if (metadataValue.isMember("is_simulation") && metadataValue["is_simulation"].isBool()) - { - simExternalObj.set_MetadataIsSimulation(metadataValue["is_simulation"].asBool()); - } - - if (metadataValue.isMember("datum") && metadataValue["datum"].isString()) - { - simExternalObj.set_MetadataDatum(metadataValue["datum"].asString()); - } - - if (metadataValue.isMember("proj_string") && metadataValue["proj_string"].isString()) - { - simExternalObj.set_MetadataProjString(metadataValue["proj_string"].asString()); - } - - if (metadataValue.isMember("sensor_x") && metadataValue["sensor_x"].isDouble()) - { - simExternalObj.set_MetadataSensorX(metadataValue["sensor_x"].asDouble()); - } - - if (metadataValue.isMember("sensor_y") && metadataValue["sensor_y"].isDouble()) - { - simExternalObj.set_MetadataSensorY(metadataValue["sensor_y"].asDouble()); - } - - if (metadataValue.isMember("sensor_z") && metadataValue["sensor_z"].isDouble()) - { - simExternalObj.set_MetadataSensorZ(metadataValue["sensor_z"].asDouble()); - } - - if (metadataValue.isMember("infrastructure_id") && metadataValue["infrastructure_id"].isString()) - { - simExternalObj.set_MetadataInfrastructureId(metadataValue["infrastructure_id"].asString()); - } - - if (metadataValue.isMember("sensor_id") && metadataValue["sensor_id"].isString()) - { - simExternalObj.set_MetadataSensorId(metadataValue["sensor_id"].asString()); - } - } - - void SimulationExternalObjectConverter::populateSimExternalObjectHeader(const Json::Value &headerValue, tmx::messages::simulation::ExternalObject &simExternalObj) - { - if (headerValue.isMember("seq") && headerValue["seq"].isUInt()) - { - simExternalObj.set_HeaderSeq(headerValue["seq"].asUInt()); - } - - if (headerValue.isMember("stamp") && headerValue["stamp"].isMember("secs") && headerValue["stamp"]["secs"].isUInt()) - { - simExternalObj.set_HeaderStampSecs(headerValue["stamp"]["secs"].asUInt()); - } - - if (headerValue.isMember("stamp") && headerValue["stamp"].isMember("nsecs") && headerValue["stamp"]["nsecs"].isUInt()) - { - simExternalObj.set_HeaderStampNSecs(headerValue["stamp"]["nsecs"].asUInt()); - } - } - - void SimulationExternalObjectConverter::populateSimExternalObjectPose(const Json::Value &poseValue, tmx::messages::simulation::ExternalObject &simExternalObj) - { - if (poseValue.isMember("pose") && poseValue["pose"].isMember("position") && poseValue["pose"]["position"].isObject()) - { - Json::Value positionValue = poseValue["pose"]["position"]; - if (positionValue.isMember("x")) - { - simExternalObj.set_PosePosePositionX(positionValue["x"].asDouble()); - } - if (positionValue.isMember("y")) - { - simExternalObj.set_PosePosePositionY(positionValue["y"].asDouble()); - } - if (positionValue.isMember("z")) - { - simExternalObj.set_PosePosePositionZ(positionValue["z"].asDouble()); - } - } - - if (poseValue.isMember("pose") && poseValue["pose"].isMember("position") && poseValue["pose"]["orientation"].isObject()) - { - Json::Value orientationValue = poseValue["pose"]["orientation"]; - if (orientationValue.isMember("x")) - { - simExternalObj.set_PosePoseOrientationX(orientationValue["x"].asDouble()); - } - if (orientationValue.isMember("y")) - { - simExternalObj.set_PosePoseOrientationY(orientationValue["y"].asDouble()); - } - if (orientationValue.isMember("z")) - { - simExternalObj.set_PosePoseOrientationZ(orientationValue["z"].asDouble()); - } - if (orientationValue.isMember("w")) - { - simExternalObj.set_PosePoseOrientationW(orientationValue["w"].asDouble()); - } - } - - if (poseValue.isMember("covariance") && poseValue["covariance"].isArray()) - { - Json::Value covarianceArrayValue = poseValue["covariance"]; - std::vector covarianceV; - populateSimCovarianceArray(covarianceArrayValue, covarianceV); - simExternalObj.set_PoseCovariance(covarianceV); - } - } - - void SimulationExternalObjectConverter::populateSimExternalObjectVelocity(const Json::Value &velocityValue, tmx::messages::simulation::ExternalObject &simExternalObj) - { - if (velocityValue.isMember("twist") && velocityValue["twist"].isMember("linear") && velocityValue["twist"]["linear"].isObject()) - { - Json::Value linearValue = velocityValue["twist"]["linear"]; - if (linearValue.isMember("x")) - { - simExternalObj.set_VelocityTwistLinearX(linearValue["x"].asDouble()); - } - if (linearValue.isMember("y")) - { - simExternalObj.set_VelocityTwistLinearY(linearValue["y"].asDouble()); - } - if (linearValue.isMember("z")) - { - simExternalObj.set_VelocityTwistLinearZ(linearValue["z"].asDouble()); - } - } - - if (velocityValue.isMember("twist") && velocityValue["twist"].isMember("angular") && velocityValue["twist"]["angular"].isObject()) - { - Json::Value angularValue = velocityValue["twist"]["angular"]; - if (angularValue.isMember("x")) - { - simExternalObj.set_VelocityTwistAngularX(angularValue["x"].asDouble()); - } - if (angularValue.isMember("y")) - { - simExternalObj.set_VelocityTwistAngularY(angularValue["y"].asDouble()); - } - if (angularValue.isMember("z")) - { - simExternalObj.set_VelocityTwistAngularZ(angularValue["z"].asDouble()); - } - if (angularValue.isMember("w")) - { - simExternalObj.set_PosePoseOrientationW(angularValue["w"].asDouble()); - } - } - if (velocityValue.isMember("covariance") && velocityValue["covariance"].isArray()) - { - Json::Value covarianceArrayValue = velocityValue["covariance"]; - std::vector covarianceV; - populateSimCovarianceArray(covarianceArrayValue, covarianceV); - simExternalObj.set_VelocityCovariance(covarianceV); - } - } - - void SimulationExternalObjectConverter::populateSimExternalObjectSize(const Json::Value &sizeValue, tmx::messages::simulation::ExternalObject &simExternalObj) - { - if (sizeValue.isMember("x")) - { - simExternalObj.set_SizeX(sizeValue["x"].asDouble()); - } - if (sizeValue.isMember("y")) - { - simExternalObj.set_SizeY(sizeValue["y"].asDouble()); - } - if (sizeValue.isMember("z")) - { - simExternalObj.set_SizeZ(sizeValue["z"].asDouble()); - } - } - - void SimulationExternalObjectConverter::populateSimCovarianceArray(const Json::Value &covarianceArrayValue, std::vector &covarianceV) - { - std::for_each(covarianceArrayValue.begin(), covarianceArrayValue.end(), [&covarianceV](const auto &item) - { tmx::messages::simulation::Covariance covariance(item.asDouble()); - covarianceV.push_back(covariance); }); - } - - tmx::messages::simulation::OBJECT_TYPES SimulationExternalObjectConverter::objectTypeStringToEnum(const std::string &object_type_str) - { - tmx::messages::simulation::OBJECT_TYPES object_type = tmx::messages::simulation::OBJECT_TYPES::UNKNOWN; - try - { - int object_type_int = stoi(object_type_str); - switch (object_type_int) - { - case tmx::messages::simulation::OBJECT_TYPES::LARGE_VEHICLE: - object_type = tmx::messages::simulation::OBJECT_TYPES::LARGE_VEHICLE; - break; - - case tmx::messages::simulation::OBJECT_TYPES::MOTORCYCLE: - object_type = tmx::messages::simulation::OBJECT_TYPES::MOTORCYCLE; - break; - - case tmx::messages::simulation::OBJECT_TYPES::PEDESTRIAN: - object_type = tmx::messages::simulation::OBJECT_TYPES::PEDESTRIAN; - break; - - case tmx::messages::simulation::OBJECT_TYPES::SMALL_VEHICLE: - object_type = tmx::messages::simulation::OBJECT_TYPES::SMALL_VEHICLE; - break; - - default: - object_type = tmx::messages::simulation::OBJECT_TYPES::UNKNOWN; - break; - } - } - catch (std::exception &err) - { - object_type = tmx::messages::simulation::OBJECT_TYPES::UNKNOWN; - } - return object_type; - } - - tmx::messages::simulation::PRESENCE_VECTOR_TYPES SimulationExternalObjectConverter::presenceVectorIntToEnum(uint16_t presence_vector) - { - tmx::messages::simulation::PRESENCE_VECTOR_TYPES presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE; - switch (presence_vector) - { - case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::BSM_ID_PRESENCE_VECTOR: - presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::BSM_ID_PRESENCE_VECTOR; - break; - - case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::CONFIDENCE_PRESENCE_VECTOR: - presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::CONFIDENCE_PRESENCE_VECTOR; - break; - - case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::DYNAMIC_OBJ_PRESENCE: - presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::DYNAMIC_OBJ_PRESENCE; - break; - - case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::OBJECT_TYPE_PRESENCE_VECTOR: - presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::OBJECT_TYPE_PRESENCE_VECTOR; - break; - - case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::POSE_PRESENCE_VECTOR: - presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::POSE_PRESENCE_VECTOR; - break; - - case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::PREDICTION_PRESENCE_VECTOR: - presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::PREDICTION_PRESENCE_VECTOR; - break; - - case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::SIZE_PRESENCE_VECTOR: - presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::SIZE_PRESENCE_VECTOR; - break; - - case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::VELOCITY_INST_PRESENCE_VECTOR: - presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::VELOCITY_INST_PRESENCE_VECTOR; - break; - - case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::VELOCITY_PRESENCE_VECTOR: - presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::VELOCITY_PRESENCE_VECTOR; - break; - - case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::ID_PRESENCE_VECTOR: - presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::ID_PRESENCE_VECTOR; - break; - - default: - presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE; - break; - } - return presence_vector_enum; - } -} \ No newline at end of file diff --git a/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h b/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h deleted file mode 100644 index 79822e8f0..000000000 --- a/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h +++ /dev/null @@ -1,78 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace tmx::utils::sim -{ - - class SimulationExternalObjectConverter - { - private: - /*** - * @brief Populate simulation external object metadata with metadata information from JSON string. - * @param ExternalObject V2xHub customized simulation external object to populate. - * @param metadataValue Json value that contains the metadata information. - */ - static void populateSimExternalObjectMetadata(const Json::Value &metadataValue, tmx::messages::simulation::ExternalObject &simExternalObj); - /*** - * @brief Populate simulation external object header with header information from JSON string. - * @param ExternalObject V2xHub customized simulation external object to populate. - * @param headerValue Json value that contains the header information. - */ - static void populateSimExternalObjectHeader(const Json::Value &headerValue, tmx::messages::simulation::ExternalObject &simExternalObj); - /*** - * @brief Populate simulation external object pose with pose information from JSON string. - * @param ExternalObject V2xHub customized simulation external object to populate. - * @param poseValue Json value that contains the pose information. - */ - static void populateSimExternalObjectPose(const Json::Value &poseValue, tmx::messages::simulation::ExternalObject &simExternalObj); - /*** - * @brief Populate simulation external object velocity with velocity information from JSON string. - * @param ExternalObject V2xHub customized simulation external object to populate. - * @param velocityValue Json value that contains the velocity information. - */ - static void populateSimExternalObjectVelocity(const Json::Value &velocityValue, tmx::messages::simulation::ExternalObject &simExternalObj); - /*** - * @brief Populate simulation external object size with size information from JSON string. - * @param ExternalObject V2xHub customized simulation external object to populate. - * @param sizeValue Json value that contains the size information. - */ - static void populateSimExternalObjectSize(const Json::Value &sizeValue, tmx::messages::simulation::ExternalObject &simExternalObj); - /*** - * @brief Populate simulation external object covariance array with covariance array information from JSON string. - * @param ExternalObject V2xHub customized simulation external object to populate. - * @param covarianceArrayValue Json value that contains the covariance array information. - */ - static void populateSimCovarianceArray(const Json::Value &covarianceArrayValue, std::vector &covarianceV); - /*** - * @brief Convert the object type in string format to enum. - * @param object_type_str object type in string format. - * @return tmx::messages::simulation::OBJECT_TYPES V2xHub customized simulation object types. - */ - static tmx::messages::simulation::OBJECT_TYPES objectTypeStringToEnum(const std::string& object_type_str); - /*** - * @brief Convert the presence vector integer to enum. - * @param presence_vector presence vetor integer. - * @return tmx::messages::simulation::PRESENCE_VECTOR_TYPES V2xHub customized simulation presence vetor types. - */ - static tmx::messages::simulation::PRESENCE_VECTOR_TYPES presenceVectorIntToEnum(uint16_t presence_vector); - - public: - SimulationExternalObjectConverter() = delete; - ~SimulationExternalObjectConverter() = delete; - /*** - * @brief Convert simulation external object into JSON string defined by MOSAIC and CARMAStreets integration. - * @param ExternalObject V2xHub customized simulation external object. - * @return EcternalObject string in JSON format defined by MOSAIC and CARMAStreets integration. - */ - static std::string simExternalObjToJsonStr(tmx::messages::simulation::ExternalObject &simExternalObj); - /*** - * @brief Populate simulation external object with JSON string defined by MOSAIC and CARMAStreets integration. - * @param ExternalObject V2xHub customized simulation external object to populate. - * @param jsonStr EcternalObject in JSON format defined by MOSAIC and CARMAStreets integration. - */ - static void jsonToSimExternalObj(const std::string &jsonStr, tmx::messages::simulation::ExternalObject &simExternalObj); - }; -} diff --git a/src/tmx/TmxUtils/src/simulation/SimulationSensorDetectedObjectConverter.cpp b/src/tmx/TmxUtils/src/simulation/SimulationSensorDetectedObjectConverter.cpp new file mode 100644 index 000000000..f12b9f46d --- /dev/null +++ b/src/tmx/TmxUtils/src/simulation/SimulationSensorDetectedObjectConverter.cpp @@ -0,0 +1,247 @@ +#include + +namespace tmx::utils::sim +{ + std::string SimulationSensorDetectedObjectConverter::simExternalObjToJsonStr(tmx::messages::simulation::SensorDetectedObject &simExternalObj) + { + Json::Value root; + Json::Value rootContent; + Json::Value metadata; + Json::Value sensor; + root["type"] = simExternalObj.MessageType; + root["subtype"] = simExternalObj.MessageSubType; + rootContent["isSimulated"] = simExternalObj.get_MetadataIsSimulation(); + rootContent["timestamp"] = simExternalObj.get_MetadataTimestamp(); + sensor["proj_string"] = simExternalObj.get_SensorProjString(); + sensor["location"]["x"] = simExternalObj.get_SensorLocationX(); + sensor["location"]["y"] = simExternalObj.get_SensorLocationY(); + sensor["location"]["z"] = simExternalObj.get_SensorLocationZ(); + sensor["id"] = simExternalObj.get_MetadataSensorId(); + sensor["type"] = simExternalObj.get_SensorType(); + rootContent["sensor"] = sensor; + + rootContent["objectId"] = simExternalObj.get_Id(); + + Json::Value pose; + pose["x"] = simExternalObj.get_PositionX(); + pose["y"] = simExternalObj.get_PositionY(); + pose["z"] = simExternalObj.get_PositionZ(); + rootContent["position"] = pose; + auto pCovarianceV = simExternalObj.get_PositionCovariance(); + std::for_each(pCovarianceV.begin(), pCovarianceV.end(), [&rootContent](const auto &item) + { rootContent["positionCovariance"].append(Json::Value(item.covariance)); }); + + Json::Value velocity; + velocity["x"] = simExternalObj.get_VelocityTwistLinearX(); + velocity["y"] = simExternalObj.get_VelocityTwistLinearY(); + velocity["z"] = simExternalObj.get_VelocityTwistLinearZ(); + auto vCovarianceV = simExternalObj.get_VelocityCovariance(); + std::for_each(vCovarianceV.begin(), vCovarianceV.end(), [&rootContent](const auto &item) + { rootContent["velocityCovariance"].append(Json::Value(item.covariance)); }); + rootContent["velocity"] = velocity; + + Json::Value size; + size["length"] = simExternalObj.get_SizeLength(); + size["width"] = simExternalObj.get_SizeWidth(); + size["height"] = simExternalObj.get_SizeHeight(); + rootContent["size"] = size; + + rootContent["confidence"] = simExternalObj.get_Confidence(); + rootContent["type"] = simExternalObj.get_ObjectType(); + root["payload"] = rootContent; + Json::StreamWriterBuilder builder; + const std::string json_str = Json::writeString(builder, root); + return json_str; + } + + void SimulationSensorDetectedObjectConverter::jsonToSimExternalObj(const std::string &jsonStr, tmx::messages::simulation::SensorDetectedObject &simExternalObj) + { + Json::CharReaderBuilder builder; + const std::unique_ptr reader(builder.newCharReader()); + Json::Value root; + Json::Value rootContent; + JSONCPP_STRING err; + if (!reader->parse(jsonStr.c_str(), jsonStr.c_str() + static_cast(jsonStr.length()), &root, &err)) + { + throw std::runtime_error("Error parsing external object JSON string."); + } + + if (!root.isMember("content")) + { + throw std::runtime_error("No content from JSON."); + } + rootContent = root["content"]; + /** + * Populate simulation external object metadata + */ + populateSimSensorDetectedObjectMetadata(rootContent, simExternalObj); + + // Populate simulation external object id + if (rootContent.isMember("objectId") && rootContent["objectId"].isString()) + { + simExternalObj.set_Id(rootContent["objectId"].asString()); + } + + if (rootContent.isMember("sensor") && rootContent["sensor"].isObject()) + { + populateSimSensorDetectedObjectSensor(rootContent["sensor"], simExternalObj); + } + + /*** + * Populate simulation external object position + */ + populateSimSensorDetectedObjectPosition(rootContent, simExternalObj); + + /*** + *Populate simulation external object velocity + */ + populateSimSensorDetectedObjectVelocity(rootContent, simExternalObj); + + /*** + * Populate simulation external object size + */ + if (rootContent.isMember("size") && rootContent["size"].isObject()) + { + populateSimSensorDetectedObjectSize(rootContent["size"], simExternalObj); + } + + // Populate simulation external object confidence + if (rootContent.isMember("confidence") && rootContent["confidence"].isDouble()) + { + simExternalObj.set_Confidence(rootContent["confidence"].asDouble()); + } + + // Populate simulation external object type + if (rootContent.isMember("type") && rootContent["type"].isString()) + { + auto object_type = rootContent["type"].asString(); + simExternalObj.set_ObjectType(object_type); + } + } + + void SimulationSensorDetectedObjectConverter::populateSimSensorDetectedObjectMetadata(const Json::Value &metadataValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj) + { + if (metadataValue.isMember("isSimulated") && metadataValue["isSimulated"].isBool()) + { + simExternalObj.set_MetadataIsSimulation(metadataValue["isSimulated"].asBool()); + } + + if (metadataValue.isMember("timestamp") && metadataValue["timestamp"].isUInt64()) + { + simExternalObj.set_MetadataTimestamp(metadataValue["timestamp"].asUInt64()); + } + } + + void SimulationSensorDetectedObjectConverter::populateSimSensorDetectedObjectSensor(const Json::Value &sensorValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj) + { + if (sensorValue.isMember("id") && sensorValue["id"].isString()) + { + simExternalObj.set_SensorId(sensorValue["id"].asString()); + } + + if (sensorValue.isMember("type") && sensorValue["type"].isString()) + { + simExternalObj.set_SensorType(sensorValue["type"].asString()); + } + + if (sensorValue.isMember("location") && sensorValue["location"]["x"].isDouble()) + { + simExternalObj.set_SensorLocationX(sensorValue["location"]["x"].asDouble()); + } + + if (sensorValue.isMember("location") && sensorValue["location"]["y"].isDouble()) + { + simExternalObj.set_SensorLocationY(sensorValue["location"]["y"].asDouble()); + } + + if (sensorValue.isMember("location") && sensorValue["location"]["z"].isDouble()) + { + simExternalObj.set_SensorLocationZ(sensorValue["location"]["z"].asDouble()); + } + + if (sensorValue.isMember("proj_string") && sensorValue["proj_string"].isString()) + { + simExternalObj.set_SensorProjString(sensorValue["proj_string"].asString()); + } + } + + void SimulationSensorDetectedObjectConverter::populateSimSensorDetectedObjectPosition(const Json::Value &contentValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj) + { + if (contentValue.isMember("position") && contentValue["position"].isObject()) + { + Json::Value positionValue = contentValue["position"]; + if (positionValue.isMember("x")) + { + simExternalObj.set_PositionX(positionValue["x"].asDouble()); + } + if (positionValue.isMember("y")) + { + simExternalObj.set_PositionY(positionValue["y"].asDouble()); + } + if (positionValue.isMember("z")) + { + simExternalObj.set_PositionZ(positionValue["z"].asDouble()); + } + } + + if (contentValue.isMember("positionCovariance") && contentValue["positionCovariance"].isArray()) + { + Json::Value covarianceArrayValue = contentValue["positionCovariance"]; + std::vector covarianceV; + populateSimCovarianceArray(covarianceArrayValue, covarianceV); + simExternalObj.set_PositionCovariance(covarianceV); + } + } + + void SimulationSensorDetectedObjectConverter::populateSimSensorDetectedObjectVelocity(const Json::Value &contentValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj) + { + if (contentValue.isMember("velocity") && contentValue["velocity"].isObject()) + { + Json::Value velocityValue = contentValue["velocity"]; + if (velocityValue.isMember("x")) + { + simExternalObj.set_VelocityTwistLinearX(velocityValue["x"].asDouble()); + } + if (velocityValue.isMember("y")) + { + simExternalObj.set_VelocityTwistLinearY(velocityValue["y"].asDouble()); + } + if (velocityValue.isMember("z")) + { + simExternalObj.set_VelocityTwistLinearZ(velocityValue["z"].asDouble()); + } + } + + if (contentValue.isMember("velocityCovariance") && contentValue["velocityCovariance"].isArray()) + { + Json::Value covarianceArrayValue = contentValue["velocityCovariance"]; + std::vector covarianceV; + populateSimCovarianceArray(covarianceArrayValue, covarianceV); + simExternalObj.set_VelocityCovariance(covarianceV); + } + } + + void SimulationSensorDetectedObjectConverter::populateSimSensorDetectedObjectSize(const Json::Value &sizeValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj) + { + if (sizeValue.isMember("length")) + { + simExternalObj.set_SizeLength(sizeValue["length"].asDouble()); + } + if (sizeValue.isMember("width")) + { + simExternalObj.set_SizeWidth(sizeValue["width"].asDouble()); + } + if (sizeValue.isMember("height")) + { + simExternalObj.set_SizeHeight(sizeValue["height"].asDouble()); + } + } + + void SimulationSensorDetectedObjectConverter::populateSimCovarianceArray(const Json::Value &covarianceArrayValue, std::vector &covarianceV) + { + std::for_each(covarianceArrayValue.begin(), covarianceArrayValue.end(), [&covarianceV](const auto &item) + { tmx::messages::simulation::Covariance covariance(item.asDouble()); + covarianceV.push_back(covariance); }); + } + +} \ No newline at end of file diff --git a/src/tmx/TmxUtils/src/simulation/SimulationSensorDetectedObjectConverter.h b/src/tmx/TmxUtils/src/simulation/SimulationSensorDetectedObjectConverter.h new file mode 100644 index 000000000..1ef3b1604 --- /dev/null +++ b/src/tmx/TmxUtils/src/simulation/SimulationSensorDetectedObjectConverter.h @@ -0,0 +1,66 @@ +#pragma once + +#include +#include +#include + +namespace tmx::utils::sim +{ + + class SimulationSensorDetectedObjectConverter + { + private: + /*** + * @brief Populate simulation detected object metadata with metadata information from JSON string. + * @param SensorDetectedObject V2xHub customized simulation detected object to populate. + * @param metadataValue Json value that contains the metadata information. + */ + static void populateSimSensorDetectedObjectMetadata(const Json::Value &metadataValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj); + /*** + * @brief Populate simulation detected object sensor with sensor information from JSON string. + * @param SensorDetectedObject V2xHub customized simulation detected object to populate. + * @param sensorValue Json value that contains the sensor information. + */ + static void populateSimSensorDetectedObjectSensor(const Json::Value &sensorValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj); + /*** + * @brief Populate simulation detected object position with position information from JSON string. + * @param SensorDetectedObject V2xHub customized simulation detected object to populate. + * @param positionValue Json value that contains the position information. + */ + static void populateSimSensorDetectedObjectPosition(const Json::Value &positionValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj); + /*** + * @brief Populate simulation detected object velocity with velocity information from JSON string. + * @param SensorDetectedObject V2xHub customized simulation detected object to populate. + * @param velocityValue Json value that contains the velocity information. + */ + static void populateSimSensorDetectedObjectVelocity(const Json::Value &velocityValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj); + /*** + * @brief Populate simulation detected object size with size information from JSON string. + * @param SensorDetectedObject V2xHub customized simulation detected object to populate. + * @param sizeValue Json value that contains the size information. + */ + static void populateSimSensorDetectedObjectSize(const Json::Value &sizeValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj); + /*** + * @brief Populate simulation detected object covariance array with covariance array information from JSON string. + * @param SensorDetectedObject V2xHub customized simulation detected object to populate. + * @param covarianceArrayValue Json value that contains the covariance array information. + */ + static void populateSimCovarianceArray(const Json::Value &covarianceArrayValue, std::vector &covarianceV); + + public: + SimulationSensorDetectedObjectConverter() = delete; + ~SimulationSensorDetectedObjectConverter() = delete; + /*** + * @brief Convert simulation detected object into JSON string defined by MOSAIC and CARMAStreets integration. + * @param SensorDetectedObject V2xHub customized simulation detected object. + * @return EcternalObject string in JSON format defined by MOSAIC and CARMAStreets integration. + */ + static std::string simExternalObjToJsonStr(tmx::messages::simulation::SensorDetectedObject &simExternalObj); + /*** + * @brief Populate simulation detected object with JSON string defined by MOSAIC and CARMAStreets integration. + * @param SensorDetectedObject V2xHub customized simulation detected object to populate. + * @param jsonStr EcternalObject in JSON format defined by MOSAIC and CARMAStreets integration. + */ + static void jsonToSimExternalObj(const std::string &jsonStr, tmx::messages::simulation::SensorDetectedObject &simExternalObj); + }; +} diff --git a/src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp b/src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp index 1a9bf6587..b42f78101 100644 --- a/src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp +++ b/src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp @@ -1,139 +1,52 @@ -#include +#include #include #include using namespace tmx::utils::sim; using namespace tmx::messages; -TEST(SimulationExternalObjectConverter, jsonToSimExternalObjInvalidJson) +TEST(SimulationSensorDetectedObjectConverter, jsonToSimExternalObjInvalidJson) { - simulation::ExternalObject simExternalObj; + simulation::SensorDetectedObject simExternalObj; std::string invalidJsonStr = "Invalid"; - ASSERT_THROW(SimulationExternalObjectConverter::jsonToSimExternalObj(invalidJsonStr, simExternalObj), std::runtime_error); + ASSERT_THROW(SimulationSensorDetectedObjectConverter::jsonToSimExternalObj(invalidJsonStr, simExternalObj), std::runtime_error); + std::string noContentJsonStr = "Invalid"; + ASSERT_THROW(SimulationSensorDetectedObjectConverter::jsonToSimExternalObj(noContentJsonStr, simExternalObj), std::runtime_error); } -TEST(SimulationExternalObjectConverter, jsonToSimExternalObjValidJson) +TEST(SimulationSensorDetectedObjectConverter, jsonToSimExternalObjValidJson) { - simulation::ExternalObject simExternalObj; - std::string jsonStr = "{\"metadata\":{\"is_simulation\":false,\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":0.0,\"sensor_y\":0.0,\"sensor_z\":0.0,\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":0,\"stamp\":{\"secs\":0,\"nsecs\":0}},\"id\":0,\"pose\":{\"pose\":{\"position\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"orientation\":{\"x\":0.0,\"y\":0.0,\"z\":0.0,\"w\":0.0}},\"covariance\":[12.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"velocity\":{\"twist\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"size\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"confidence\":0.0,\"object_type\":\"\",\"dynamic_obj\":false}"; - SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - ASSERT_EQ(0, simExternalObj.get_Id()); - ASSERT_EQ(false, simExternalObj.get_MetadataIsSimulation()); - ASSERT_EQ("", simExternalObj.get_MetadataDatum()); - ASSERT_EQ("", simExternalObj.get_MetadataSensorId()); - ASSERT_EQ("", simExternalObj.get_MetadataInfrastructureId()); - ASSERT_EQ(0, simExternalObj.get_MetadataSensorX()); - ASSERT_EQ(0, simExternalObj.get_MetadataSensorY()); - ASSERT_EQ(0, simExternalObj.get_MetadataSensorZ()); - ASSERT_EQ(0, simExternalObj.get_HeaderSeq()); - ASSERT_EQ(0, simExternalObj.get_HeaderStampSecs()); - ASSERT_EQ(0, simExternalObj.get_HeaderStampNSecs()); - ASSERT_EQ(0, simExternalObj.get_PosePosePositionX()); - ASSERT_EQ(0, simExternalObj.get_PosePoseOrientationX()); - ASSERT_EQ(36, simExternalObj.get_PoseCovariance().size()); - ASSERT_EQ(0, simExternalObj.get_VelocityInstTwistLinearX()); - ASSERT_EQ(0, simExternalObj.get_VelocityInstTwistAngularX()); - ASSERT_EQ(36, simExternalObj.get_VelocityCovariance().size()); + simulation::SensorDetectedObject simExternalObj; + std::string jsonStr = "{\"type\":\"Application\",\"subtype\":\"SensorDetectedObject\",\"content\":{\"timestamp\":123,\"isSimulated\":true,\"sensor\":{\"id\":\"SomeID\",\"type\":\"SematicLidar\",\"location\":{\"x\":1.0,\"y\":1.0,\"z\":2.0},\"proj_string\":\"+proj=tmerc+lat_0=38.95197911150576+lon_0=-77.14835128349988+k=1+x_0=0+y_0=0+datum=WGS84+units=m+geoidgrids=egm96_15.gtx+vunits=m+no_defs\"},\"type\":\"Car\",\"confidence\":\"0.7\",\"objectId\":\"Object1\",\"position\":{\"x\":1.0,\"y\":2.5,\"z\":1.1},\"positionCovariance\":[12.0,17.33333333,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,20.3333],\"velocity\":{\"x\":1.0,\"y\":2.5,\"z\":1.1},\"velocityCovariance\":[12.0,17.33333333,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,20.3333],\"angularVelocity\":{\"x\":1.0,\"y\":2.5,\"z\":1.1},\"angularVelocityCovariance\":[12.0,17.33333333,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,20.3333],\"size\":{\"length\":0.1,\"width\":0.4,\"height\":1.5}}}"; + SimulationSensorDetectedObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ("Object1", simExternalObj.get_Id()); + ASSERT_EQ(true, simExternalObj.get_MetadataIsSimulation()); + ASSERT_EQ("SomeID", simExternalObj.get_SensorId()); + ASSERT_EQ("SematicLidar", simExternalObj.get_SensorType()); + ASSERT_EQ("+proj=tmerc+lat_0=38.95197911150576+lon_0=-77.14835128349988+k=1+x_0=0+y_0=0+datum=WGS84+units=m+geoidgrids=egm96_15.gtx+vunits=m+no_defs", simExternalObj.get_SensorProjString()); + ASSERT_EQ(1, simExternalObj.get_SensorLocationX()); + ASSERT_EQ(1, simExternalObj.get_SensorLocationY()); + ASSERT_EQ(2, simExternalObj.get_SensorLocationZ()); + ASSERT_EQ(1, simExternalObj.get_PositionX()); + ASSERT_EQ(2.5, simExternalObj.get_PositionY()); + ASSERT_EQ(1.1, simExternalObj.get_PositionZ()); + ASSERT_EQ(36, simExternalObj.get_PositionCovariance().size()); + ASSERT_EQ(36, simExternalObj.get_PositionCovariance().size()); } -TEST(SimulationExternalObjectConverter, jsonToSimExternalObjPresenceVector) +TEST(SimulationSensorDetectedObjectConverter, simExternalObjToJsonStr) { - simulation::ExternalObject simExternalObj; - ASSERT_TRUE(simExternalObj.is_empty()); - ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE, simExternalObj.get_PresenceVector()); - ASSERT_FALSE(simExternalObj.is_empty()); - - std::string jsonStr = "{\"presence_vector\":1}"; - SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::ID_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); - - jsonStr = "{\"presence_vector\":2}"; - SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::POSE_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); - - jsonStr = "{\"presence_vector\":4}"; - SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::VELOCITY_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); - - jsonStr = "{\"presence_vector\":8}"; - SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::VELOCITY_INST_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); - - jsonStr = "{\"presence_vector\":16}"; - SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::SIZE_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); - - jsonStr = "{\"presence_vector\":32}"; - SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::CONFIDENCE_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); - - jsonStr = "{\"presence_vector\":64}"; - SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::OBJECT_TYPE_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); - - jsonStr = "{\"presence_vector\":128}"; - SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::BSM_ID_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); - - jsonStr = "{\"presence_vector\":256}"; - SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::DYNAMIC_OBJ_PRESENCE, simExternalObj.get_PresenceVector()); - - jsonStr = "{\"presence_vector\":512}"; - SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::PREDICTION_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); - - jsonStr = "{\"presence_vector\":212121}"; - SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE, simExternalObj.get_PresenceVector()); -} - -TEST(SimulationExternalObjectConverter, jsonToSimExternalObjObjectTypes) -{ - simulation::ExternalObject simExternalObj; - ASSERT_TRUE(simExternalObj.is_empty()); - ASSERT_EQ(simulation::OBJECT_TYPES::UNKNOWN, simExternalObj.get_ObjectType()); - ASSERT_FALSE(simExternalObj.is_empty()); - - std::string jsonStr = "{\"object_type\":\"1\"}"; - SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - ASSERT_EQ(simulation::OBJECT_TYPES::SMALL_VEHICLE, simExternalObj.get_ObjectType()); - - jsonStr = "{\"object_type\":\"2\"}"; - SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - ASSERT_EQ(simulation::OBJECT_TYPES::LARGE_VEHICLE, simExternalObj.get_ObjectType()); - - jsonStr = "{\"object_type\":\"3\"}"; - SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - ASSERT_EQ(simulation::OBJECT_TYPES::MOTORCYCLE, simExternalObj.get_ObjectType()); - - jsonStr = "{\"object_type\":\"5\"}"; - SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - ASSERT_EQ(simulation::OBJECT_TYPES::UNKNOWN, simExternalObj.get_ObjectType()); - - jsonStr = "{\"object_type\":\"4\"}"; - SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - ASSERT_EQ(simulation::OBJECT_TYPES::PEDESTRIAN, simExternalObj.get_ObjectType()); - - jsonStr = "{\"object_type\":\"invalid\"}"; - SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - ASSERT_EQ(simulation::OBJECT_TYPES::UNKNOWN, simExternalObj.get_ObjectType()); -} - -TEST(SimulationExternalObjectConverter, simExternalObjToJsonStr) -{ - simulation::ExternalObject simExternalObj; - std::string jsonStr = "{\"metadata\":{\"is_simulation\":false,\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":0.0,\"sensor_y\":0.0,\"sensor_z\":0.0,\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":0,\"stamp\":{\"secs\":0,\"nsecs\":0}},\"id\":0,\"pose\":{\"pose\":{\"position\":{\"x\":34.0,\"y\":0.0,\"z\":0.0},\"orientation\":{\"x\":23.0,\"y\":0.0,\"z\":0.0,\"w\":0.0}},\"covariance\":[12.1,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,11.0]},\"velocity\":{\"twist\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"covariance\":[12.0,17.33333333,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,20.3333]},\"size\":{\"x\":12.0,\"y\":23.0,\"z\":12.0},\"confidence\":1.0,\"object_type\":\"128\",\"dynamic_obj\":false}"; - SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - std::string output = SimulationExternalObjectConverter::simExternalObjToJsonStr(simExternalObj); + std::string jsonStr = "{\"type\":\"Application\",\"subtype\":\"SensorDetectedObject\",\"content\":{\"timestamp\":123,\"isSimulated\":true,\"sensor\":{\"id\":\"SomeID\",\"type\":\"SematicLidar\",\"location\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"proj_string\":\"+proj=tmerc+lat_0=38.95197911150576+lon_0=-77.14835128349988+k=1+x_0=0+y_0=0+datum=WGS84+units=m+geoidgrids=egm96_15.gtx+vunits=m+no_defs\"},\"type\":\"Car\",\"confidence\":\"0.7\",\"objectId\":\"Object1\",\"position\":{\"x\":1.0,\"y\":2.5,\"z\":1.1},\"positionCovariance\":[12.0,17.33333333,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,20.3333],\"velocity\":{\"x\":1.0,\"y\":2.5,\"z\":1.1},\"velocityCovariance\":[12.0,17.33333333,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,20.3333],\"angularVelocity\":{\"x\":1.0,\"y\":2.5,\"z\":1.1},\"angularVelocityCovariance\":[12.0,17.33333333,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,20.3333],\"size\":{\"length\":0.1,\"width\":0.4,\"height\":1.5}}}"; + simulation::SensorDetectedObject simExternalObj; + SimulationSensorDetectedObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + std::string output = SimulationSensorDetectedObjectConverter::simExternalObjToJsonStr(simExternalObj); Json::CharReaderBuilder builder; const std::unique_ptr reader(builder.newCharReader()); Json::Value root; JSONCPP_STRING err; reader->parse(output.c_str(), output.c_str() + static_cast(output.length()), &root, &err); - ASSERT_FALSE(root["metadata"]["is_simulation"].asBool()); - ASSERT_EQ(12.1, root["pose"]["covariance"].begin()->asDouble()); - ASSERT_EQ(23, root["pose"]["pose"]["orientation"]["x"].asDouble()); - ASSERT_EQ(12, root["velocity"]["covariance"].begin()->asDouble()); - ASSERT_EQ(17.33333333, root["velocity"]["covariance"][1].asDouble()); + ASSERT_FALSE(root["metadata"]["isSimulation"].asBool()); + ASSERT_EQ(12, root["payload"]["positionCovariance"].begin()->asDouble()); + ASSERT_EQ(12, root["payload"]["velocityCovariance"].begin()->asDouble()); + ASSERT_EQ(17.33333333, root["payload"]["velocityCovariance"][1].asDouble()); } \ 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 79ed7acb6..e163fbeb3 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -25,7 +25,7 @@ CARMAStreetsPlugin::CARMAStreetsPlugin(string name) : AddMessageFilter < tsm2Message > (this, &CARMAStreetsPlugin::HandleMobilityPathMessage); AddMessageFilter < MapDataMessage > (this, &CARMAStreetsPlugin::HandleMapMessage); AddMessageFilter < SrmMessage > (this, &CARMAStreetsPlugin::HandleSRMMessage); - AddMessageFilter < simulation::ExternalObject > (this, &CARMAStreetsPlugin::HandleSimulatedExternalMessage ); + AddMessageFilter < simulation::SensorDetectedObject > (this, &CARMAStreetsPlugin::HandleSimulatedExternalMessage ); SubscribeToMessages(); } @@ -630,9 +630,9 @@ void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){ } -void CARMAStreetsPlugin::HandleSimulatedExternalMessage(simulation::ExternalObject &msg, routeable_message &routeableMsg) +void CARMAStreetsPlugin::HandleSimulatedExternalMessage(simulation::SensorDetectedObject &msg, routeable_message &routeableMsg) { - auto json_str = tmx::utils::sim::SimulationExternalObjectConverter::simExternalObjToJsonStr(msg); + auto json_str = tmx::utils::sim::SimulationSensorDetectedObjectConverter::simExternalObjToJsonStr(msg); PLOG(logINFO) << "Produce External Object Message in JSON format: " << json_str < #include #include "JsonToJ2735SSMConverter.h" -#include -#include +#include +#include #include "PluginClientClockAware.h" @@ -50,7 +50,7 @@ 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); - void HandleSimulatedExternalMessage(simulation::ExternalObject &msg, routeable_message &routeableMsg); + void HandleSimulatedExternalMessage(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. 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..423c000df --- /dev/null +++ b/src/v2i-hub/CDASimAdapter/scripts/send_sim_detected_object_udp.py @@ -0,0 +1,88 @@ + +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 = + { + "type": "Application", + "subtype": "SensorDetectedObject", + "content": { + "timestamp": 123, + "isSimulated": true, + "sensor": { + "id": "SomeID", + "type": "SematicLidar", + "location": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "proj_string": "+proj=tmerc +lat_0=38.95197911150576 +lon_0=-77.14835128349988 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs" + }, + "type": "Car", + "confidence": "0.7", + "objectId": "Object1", + "position": { + "x": 1.0, + "y": 2.5, + "z": 1.1 + }, + "positionCovariance" : ["a11", "a12", "a13", "a21", "a22", "a23", "a31", "a32", "a33"], + "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/scripts/send_sim_external_object_udp.py b/src/v2i-hub/CDASimAdapter/scripts/send_sim_external_object_udp.py deleted file mode 100755 index 42799e984..000000000 --- a/src/v2i-hub/CDASimAdapter/scripts/send_sim_external_object_udp.py +++ /dev/null @@ -1,116 +0,0 @@ - -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":{ - "is_simulation": True, - "datum": "WGS84", - "proj_string": "epsg:3785", - "sensor_x": 12.212, - "sensor_y": 2.2121212, - "sensor_z": 0.121212212, - "infrastructure_id": "test", - "sensor_id": "test_sensor" - }, - "header": { - "seq": 2, - "stamp": { - "secs": 3, - "nsecs":4 - } - }, - "id": 1212, - "pose": { - "pose": { - "position": { - "x": 12.0, - "y": 23.0, - "z": 56.121212 - }, - "orientation": { - "x": 11.0, - "y": 22.0, - "z": 33.333333, - "w": 44.4444444444 - } - }, - "covariance": [ - 12.12, 12.3233232, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 - ] - }, - "velocity": { - "twist": { - "linear": { - "x": 12.1111111111111, - "y": 2.11111112222, - "z": 3.2222222222333 - }, - "angular": { - "x": 2.212222222222, - "y": 2.2333333333333, - "z": 3.3333333333121 - } - }, - "covariance": [ - 12.2222222222, 123.33333333, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0,23.66666666666 - ] - }, - "size": { - "x": 12.0, - "y": 23.0, - "z": 23.0 - }, - "confidence": 12.0, - "object_type": "2", - "dynamic_obj": True - } - 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 ca003d1d5..9f5c52071 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp @@ -79,9 +79,9 @@ namespace CDASimAdapter{ } - void CDASimAdapter::forward_simulated_external_message(tmx::messages::simulation::ExternalObject &msg) { - PLOG(logDEBUG1) << "Sending Simulated ExternalObject Message " << msg << std::endl; - this->BroadcastMessage(msg, _name, 0 , IvpMsgFlags_None); + void CDASimAdapter::forward_simulated_external_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() { diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp index ffb27de5b..b6eee42eb 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp @@ -45,7 +45,7 @@ namespace CDASimAdapter{ message["infrastructureId"] = infrastructure_id; message["rxMessagePort"] = v2x_port; message["timeSyncPort"] = time_sync_port; - message["ExternalObjectDetectionPort"] = external_object_detection_port; + message["SensorDetectedObjectDetectionPort"] = external_object_detection_port; message["location"]["x"] = location.X; message["location"]["y"] = location.Y; message["location"]["z"] = location.Z; @@ -129,15 +129,15 @@ namespace CDASimAdapter{ } - tmx::messages::simulation::ExternalObject CDASimConnection::consume_external_object_message() const + tmx::messages::simulation::SensorDetectedObject CDASimConnection::consume_external_object_message() const { - tmx::messages::simulation::ExternalObject externalObj; + tmx::messages::simulation::SensorDetectedObject externalObj; externalObj.clear(); if(external_object_listener) { std::string str_msg = consume_server_message(external_object_listener); //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 - tmx::utils::sim::SimulationExternalObjectConverter::jsonToSimExternalObj(str_msg, externalObj); + tmx::utils::sim::SimulationSensorDetectedObjectConverter::jsonToSimExternalObj(str_msg, externalObj); } else { diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp index a4e6d4e3f..089929647 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp @@ -91,9 +91,9 @@ namespace CDASimAdapter void forward_time_sync_message(tmx::messages::TimeSyncMessage &msg); /** * @brief Forward simulated external object message to TMX message bus for other V2X-Hub Plugin - * @param msg simulation::ExternalObject. + * @param msg simulation::SensorDetectedObject. */ - void forward_simulated_external_message(tmx::messages::simulation::ExternalObject &msg); + void forward_simulated_external_message(tmx::messages::simulation::SensorDetectedObject &msg); /** * @brief Method to start thread timer for regular interval actions lauched on seperate thread. */ diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp index be054e9f6..560561da7 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp @@ -4,11 +4,11 @@ #include #include #include -#include +#include #include #include #include -#include +#include namespace CDASimAdapter { @@ -86,9 +86,9 @@ namespace CDASimAdapter { /** * @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::ExternalObject. + * @return simulation::SensorDetectedObject. */ - tmx::messages::simulation::ExternalObject consume_external_object_message() const; + tmx::messages::simulation::SensorDetectedObject consume_external_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, diff --git a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp index 3f88a8913..ec2b9f173 100644 --- a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp @@ -86,7 +86,7 @@ namespace CDASimAdapter { location.Y = 38.955; location.Z = -77.149; ASSERT_EQ(connection->get_handshake_json("4566", "127.0.0.1", 4567, 4568, 4569, location), - "{\n \"ExternalObjectDetectionPort\" : 4568,\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 \"timeSyncPort\" : 4567\n}\n"); + "{\n \"SensorDetectedObjectDetectionPort\" : 4568,\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 \"timeSyncPort\" : 4567\n}\n"); } TEST_F( TestCARMASimulationConnection, carma_simulation_handshake) { diff --git a/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp b/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp index ede25aa06..4f119fbe1 100644 --- a/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp +++ b/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp @@ -1,6 +1,6 @@ #include -#include -#include +#include +#include #include #include @@ -8,13 +8,13 @@ using namespace std; using namespace tmx; using namespace tmx::messages; -TEST(SimulationMessages, ExternalObjectToRoutableMessage) +TEST(SimulationMessages, SensorDetectedObjectToRoutableMessage) { - simulation::ExternalObject externalObj; + simulation::SensorDetectedObject externalObj; string expectedStr = "{\"metadata\":{\"is_simulation\":false,\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":0.0,\"sensor_y\":0.0,\"sensor_z\":0.0,\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":0,\"stamp\":{\"secs\":0,\"nsecs\":0}},\"id\":0,\"pose\":{\"pose\":{\"position\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"orientation\":{\"x\":0.0,\"y\":0.0,\"z\":0.0,\"w\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"velocity\":{\"twist\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"size\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"confidence\":0.0,\"object_type\":\"\",\"dynamic_obj\":false}"; - tmx::utils::sim::SimulationExternalObjectConverter::jsonToSimExternalObj(expectedStr, externalObj); - ASSERT_EQ("ExternalObject", std::string(simulation::ExternalObject::MessageSubType)); - ASSERT_EQ("Simulation", std::string(simulation::ExternalObject::MessageType)); + tmx::utils::sim::SimulationSensorDetectedObjectConverter::jsonToSimExternalObj(expectedStr, externalObj); + ASSERT_EQ("SensorDetectedObject", std::string(simulation::SensorDetectedObject::MessageSubType)); + ASSERT_EQ("Application", std::string(simulation::SensorDetectedObject::MessageType)); tmx::routeable_message routeableMsg; routeableMsg.initialize(externalObj, "CDASimAdapter", 0, IvpMsgFlags_None); ASSERT_EQ("json", routeableMsg.get_encoding()); @@ -24,6 +24,6 @@ TEST(SimulationMessages, ExternalObjectToRoutableMessage) ASSERT_NEAR(current_time_mill, routeableMsg.get_millisecondsSinceEpoch(), 1000); ASSERT_EQ(0, routeableMsg.get_sourceId()); ASSERT_EQ("CDASimAdapter", routeableMsg.get_source()); - ASSERT_EQ("ExternalObject",routeableMsg.get_subtype()); - ASSERT_EQ("Simulation", routeableMsg.get_type()); + ASSERT_EQ("SensorDetectedObject",routeableMsg.get_subtype()); + ASSERT_EQ("Application", routeableMsg.get_type()); } \ No newline at end of file From e9e32c71e462f237c105e29ed790d1a51add1bcc Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Mon, 24 Jul 2023 22:25:12 +0000 Subject: [PATCH 12/35] update --- .../src/CARMAStreetsPlugin.cpp | 7 ----- .../src/CARMAStreetsPlugin.h | 2 -- .../CDASimAdapter/src/CDASimConnection.cpp | 3 +- .../src/include/CDASimConnection.hpp | 1 - .../test/TestSimulationMessages.cpp | 29 ------------------- 5 files changed, 1 insertion(+), 41 deletions(-) delete mode 100644 src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index e163fbeb3..46d8c3d55 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -25,7 +25,6 @@ CARMAStreetsPlugin::CARMAStreetsPlugin(string name) : AddMessageFilter < tsm2Message > (this, &CARMAStreetsPlugin::HandleMobilityPathMessage); AddMessageFilter < MapDataMessage > (this, &CARMAStreetsPlugin::HandleMapMessage); AddMessageFilter < SrmMessage > (this, &CARMAStreetsPlugin::HandleSRMMessage); - AddMessageFilter < simulation::SensorDetectedObject > (this, &CARMAStreetsPlugin::HandleSimulatedExternalMessage ); SubscribeToMessages(); } @@ -630,12 +629,6 @@ void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){ } -void CARMAStreetsPlugin::HandleSimulatedExternalMessage(simulation::SensorDetectedObject &msg, routeable_message &routeableMsg) -{ - auto json_str = tmx::utils::sim::SimulationSensorDetectedObjectConverter::simExternalObjToJsonStr(msg); - PLOG(logINFO) << "Produce External Object Message in JSON format: " << json_str < #include "JsonToJ2735SSMConverter.h" #include -#include #include "PluginClientClockAware.h" @@ -50,7 +49,6 @@ 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); - void HandleSimulatedExternalMessage(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. diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp index b6eee42eb..81ce3aa93 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp @@ -136,8 +136,7 @@ namespace CDASimAdapter{ if(external_object_listener) { std::string str_msg = consume_server_message(external_object_listener); - //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 - tmx::utils::sim::SimulationSensorDetectedObjectConverter::jsonToSimExternalObj(str_msg, externalObj); + //ToDo: To populate the simulation detected 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 } else { diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp index 560561da7..4b2ba5201 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp @@ -8,7 +8,6 @@ #include #include #include -#include namespace CDASimAdapter { diff --git a/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp b/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp deleted file mode 100644 index 4f119fbe1..000000000 --- a/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp +++ /dev/null @@ -1,29 +0,0 @@ -#include -#include -#include -#include -#include - -using namespace std; -using namespace tmx; -using namespace tmx::messages; - -TEST(SimulationMessages, SensorDetectedObjectToRoutableMessage) -{ - simulation::SensorDetectedObject externalObj; - string expectedStr = "{\"metadata\":{\"is_simulation\":false,\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":0.0,\"sensor_y\":0.0,\"sensor_z\":0.0,\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":0,\"stamp\":{\"secs\":0,\"nsecs\":0}},\"id\":0,\"pose\":{\"pose\":{\"position\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"orientation\":{\"x\":0.0,\"y\":0.0,\"z\":0.0,\"w\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"velocity\":{\"twist\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"size\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"confidence\":0.0,\"object_type\":\"\",\"dynamic_obj\":false}"; - tmx::utils::sim::SimulationSensorDetectedObjectConverter::jsonToSimExternalObj(expectedStr, externalObj); - ASSERT_EQ("SensorDetectedObject", std::string(simulation::SensorDetectedObject::MessageSubType)); - ASSERT_EQ("Application", std::string(simulation::SensorDetectedObject::MessageType)); - tmx::routeable_message routeableMsg; - routeableMsg.initialize(externalObj, "CDASimAdapter", 0, IvpMsgFlags_None); - ASSERT_EQ("json", routeableMsg.get_encoding()); - ASSERT_EQ(0, routeableMsg.get_flags()); - auto current_time_mill = boost::chrono::duration_cast(boost::chrono::system_clock::now().time_since_epoch()).count(); - ASSERT_NEAR(current_time_mill, routeableMsg.get_timestamp(), 1000); - ASSERT_NEAR(current_time_mill, routeableMsg.get_millisecondsSinceEpoch(), 1000); - ASSERT_EQ(0, routeableMsg.get_sourceId()); - ASSERT_EQ("CDASimAdapter", routeableMsg.get_source()); - ASSERT_EQ("SensorDetectedObject",routeableMsg.get_subtype()); - ASSERT_EQ("Application", routeableMsg.get_type()); -} \ No newline at end of file From 39f5a731771d207f29e9a3feb53b4b19088d37a6 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Mon, 24 Jul 2023 22:26:31 +0000 Subject: [PATCH 13/35] update --- src/tmx/Messages/include/simulation/SensorDetectedObject.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/tmx/Messages/include/simulation/SensorDetectedObject.h b/src/tmx/Messages/include/simulation/SensorDetectedObject.h index 475005069..17eca8f7c 100644 --- a/src/tmx/Messages/include/simulation/SensorDetectedObject.h +++ b/src/tmx/Messages/include/simulation/SensorDetectedObject.h @@ -83,12 +83,11 @@ namespace tmx # generic rigid transformation to a Vector3, tf2 will only apply the # rotation). */ - // geometry_msgs/Vector3 size std_attribute(this->msg, double, SizeLength, 0, ); std_attribute(this->msg, double, SizeWidth, 0, ); std_attribute(this->msg, double, SizeHeight, 0, ); - // #Confidence [0,1] + // #Confidence std_attribute(this->msg, double, Confidence, 0, ); // #describes a general object type as defined in this message From 5b2f6d8f329cbe7472d87a874333a309a7f030f8 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Mon, 24 Jul 2023 22:28:15 +0000 Subject: [PATCH 14/35] update --- src/tmx/TmxUtils/CMakeLists.txt | 3 +- ...imulationSensorDetectedObjectConverter.cpp | 247 ------------------ .../SimulationSensorDetectedObjectConverter.h | 66 ----- 3 files changed, 1 insertion(+), 315 deletions(-) delete mode 100644 src/tmx/TmxUtils/src/simulation/SimulationSensorDetectedObjectConverter.cpp delete mode 100644 src/tmx/TmxUtils/src/simulation/SimulationSensorDetectedObjectConverter.h diff --git a/src/tmx/TmxUtils/CMakeLists.txt b/src/tmx/TmxUtils/CMakeLists.txt index 32ac53f27..30c30fb41 100644 --- a/src/tmx/TmxUtils/CMakeLists.txt +++ b/src/tmx/TmxUtils/CMakeLists.txt @@ -25,7 +25,6 @@ TARGET_LINK_LIBRARIES (${PROJECT_NAME} PUBLIC rdkafka++ ::carma-clock gmock - jsoncpp pthread m rt) SET (TMXUTILS_LIBRARIES ${PROJECT_NAME} PARENT_SCOPE) @@ -55,4 +54,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 jsoncpp) \ No newline at end of file +target_link_libraries(${BINARY} PUBLIC ${PROJECT_NAME} rdkafka++ gmock ${TMXAPI_LIBRARIES} ${ASN_J2735_LIBRARIES} ${UUID_LIBRARY} gtest) \ No newline at end of file diff --git a/src/tmx/TmxUtils/src/simulation/SimulationSensorDetectedObjectConverter.cpp b/src/tmx/TmxUtils/src/simulation/SimulationSensorDetectedObjectConverter.cpp deleted file mode 100644 index f12b9f46d..000000000 --- a/src/tmx/TmxUtils/src/simulation/SimulationSensorDetectedObjectConverter.cpp +++ /dev/null @@ -1,247 +0,0 @@ -#include - -namespace tmx::utils::sim -{ - std::string SimulationSensorDetectedObjectConverter::simExternalObjToJsonStr(tmx::messages::simulation::SensorDetectedObject &simExternalObj) - { - Json::Value root; - Json::Value rootContent; - Json::Value metadata; - Json::Value sensor; - root["type"] = simExternalObj.MessageType; - root["subtype"] = simExternalObj.MessageSubType; - rootContent["isSimulated"] = simExternalObj.get_MetadataIsSimulation(); - rootContent["timestamp"] = simExternalObj.get_MetadataTimestamp(); - sensor["proj_string"] = simExternalObj.get_SensorProjString(); - sensor["location"]["x"] = simExternalObj.get_SensorLocationX(); - sensor["location"]["y"] = simExternalObj.get_SensorLocationY(); - sensor["location"]["z"] = simExternalObj.get_SensorLocationZ(); - sensor["id"] = simExternalObj.get_MetadataSensorId(); - sensor["type"] = simExternalObj.get_SensorType(); - rootContent["sensor"] = sensor; - - rootContent["objectId"] = simExternalObj.get_Id(); - - Json::Value pose; - pose["x"] = simExternalObj.get_PositionX(); - pose["y"] = simExternalObj.get_PositionY(); - pose["z"] = simExternalObj.get_PositionZ(); - rootContent["position"] = pose; - auto pCovarianceV = simExternalObj.get_PositionCovariance(); - std::for_each(pCovarianceV.begin(), pCovarianceV.end(), [&rootContent](const auto &item) - { rootContent["positionCovariance"].append(Json::Value(item.covariance)); }); - - Json::Value velocity; - velocity["x"] = simExternalObj.get_VelocityTwistLinearX(); - velocity["y"] = simExternalObj.get_VelocityTwistLinearY(); - velocity["z"] = simExternalObj.get_VelocityTwistLinearZ(); - auto vCovarianceV = simExternalObj.get_VelocityCovariance(); - std::for_each(vCovarianceV.begin(), vCovarianceV.end(), [&rootContent](const auto &item) - { rootContent["velocityCovariance"].append(Json::Value(item.covariance)); }); - rootContent["velocity"] = velocity; - - Json::Value size; - size["length"] = simExternalObj.get_SizeLength(); - size["width"] = simExternalObj.get_SizeWidth(); - size["height"] = simExternalObj.get_SizeHeight(); - rootContent["size"] = size; - - rootContent["confidence"] = simExternalObj.get_Confidence(); - rootContent["type"] = simExternalObj.get_ObjectType(); - root["payload"] = rootContent; - Json::StreamWriterBuilder builder; - const std::string json_str = Json::writeString(builder, root); - return json_str; - } - - void SimulationSensorDetectedObjectConverter::jsonToSimExternalObj(const std::string &jsonStr, tmx::messages::simulation::SensorDetectedObject &simExternalObj) - { - Json::CharReaderBuilder builder; - const std::unique_ptr reader(builder.newCharReader()); - Json::Value root; - Json::Value rootContent; - JSONCPP_STRING err; - if (!reader->parse(jsonStr.c_str(), jsonStr.c_str() + static_cast(jsonStr.length()), &root, &err)) - { - throw std::runtime_error("Error parsing external object JSON string."); - } - - if (!root.isMember("content")) - { - throw std::runtime_error("No content from JSON."); - } - rootContent = root["content"]; - /** - * Populate simulation external object metadata - */ - populateSimSensorDetectedObjectMetadata(rootContent, simExternalObj); - - // Populate simulation external object id - if (rootContent.isMember("objectId") && rootContent["objectId"].isString()) - { - simExternalObj.set_Id(rootContent["objectId"].asString()); - } - - if (rootContent.isMember("sensor") && rootContent["sensor"].isObject()) - { - populateSimSensorDetectedObjectSensor(rootContent["sensor"], simExternalObj); - } - - /*** - * Populate simulation external object position - */ - populateSimSensorDetectedObjectPosition(rootContent, simExternalObj); - - /*** - *Populate simulation external object velocity - */ - populateSimSensorDetectedObjectVelocity(rootContent, simExternalObj); - - /*** - * Populate simulation external object size - */ - if (rootContent.isMember("size") && rootContent["size"].isObject()) - { - populateSimSensorDetectedObjectSize(rootContent["size"], simExternalObj); - } - - // Populate simulation external object confidence - if (rootContent.isMember("confidence") && rootContent["confidence"].isDouble()) - { - simExternalObj.set_Confidence(rootContent["confidence"].asDouble()); - } - - // Populate simulation external object type - if (rootContent.isMember("type") && rootContent["type"].isString()) - { - auto object_type = rootContent["type"].asString(); - simExternalObj.set_ObjectType(object_type); - } - } - - void SimulationSensorDetectedObjectConverter::populateSimSensorDetectedObjectMetadata(const Json::Value &metadataValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj) - { - if (metadataValue.isMember("isSimulated") && metadataValue["isSimulated"].isBool()) - { - simExternalObj.set_MetadataIsSimulation(metadataValue["isSimulated"].asBool()); - } - - if (metadataValue.isMember("timestamp") && metadataValue["timestamp"].isUInt64()) - { - simExternalObj.set_MetadataTimestamp(metadataValue["timestamp"].asUInt64()); - } - } - - void SimulationSensorDetectedObjectConverter::populateSimSensorDetectedObjectSensor(const Json::Value &sensorValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj) - { - if (sensorValue.isMember("id") && sensorValue["id"].isString()) - { - simExternalObj.set_SensorId(sensorValue["id"].asString()); - } - - if (sensorValue.isMember("type") && sensorValue["type"].isString()) - { - simExternalObj.set_SensorType(sensorValue["type"].asString()); - } - - if (sensorValue.isMember("location") && sensorValue["location"]["x"].isDouble()) - { - simExternalObj.set_SensorLocationX(sensorValue["location"]["x"].asDouble()); - } - - if (sensorValue.isMember("location") && sensorValue["location"]["y"].isDouble()) - { - simExternalObj.set_SensorLocationY(sensorValue["location"]["y"].asDouble()); - } - - if (sensorValue.isMember("location") && sensorValue["location"]["z"].isDouble()) - { - simExternalObj.set_SensorLocationZ(sensorValue["location"]["z"].asDouble()); - } - - if (sensorValue.isMember("proj_string") && sensorValue["proj_string"].isString()) - { - simExternalObj.set_SensorProjString(sensorValue["proj_string"].asString()); - } - } - - void SimulationSensorDetectedObjectConverter::populateSimSensorDetectedObjectPosition(const Json::Value &contentValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj) - { - if (contentValue.isMember("position") && contentValue["position"].isObject()) - { - Json::Value positionValue = contentValue["position"]; - if (positionValue.isMember("x")) - { - simExternalObj.set_PositionX(positionValue["x"].asDouble()); - } - if (positionValue.isMember("y")) - { - simExternalObj.set_PositionY(positionValue["y"].asDouble()); - } - if (positionValue.isMember("z")) - { - simExternalObj.set_PositionZ(positionValue["z"].asDouble()); - } - } - - if (contentValue.isMember("positionCovariance") && contentValue["positionCovariance"].isArray()) - { - Json::Value covarianceArrayValue = contentValue["positionCovariance"]; - std::vector covarianceV; - populateSimCovarianceArray(covarianceArrayValue, covarianceV); - simExternalObj.set_PositionCovariance(covarianceV); - } - } - - void SimulationSensorDetectedObjectConverter::populateSimSensorDetectedObjectVelocity(const Json::Value &contentValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj) - { - if (contentValue.isMember("velocity") && contentValue["velocity"].isObject()) - { - Json::Value velocityValue = contentValue["velocity"]; - if (velocityValue.isMember("x")) - { - simExternalObj.set_VelocityTwistLinearX(velocityValue["x"].asDouble()); - } - if (velocityValue.isMember("y")) - { - simExternalObj.set_VelocityTwistLinearY(velocityValue["y"].asDouble()); - } - if (velocityValue.isMember("z")) - { - simExternalObj.set_VelocityTwistLinearZ(velocityValue["z"].asDouble()); - } - } - - if (contentValue.isMember("velocityCovariance") && contentValue["velocityCovariance"].isArray()) - { - Json::Value covarianceArrayValue = contentValue["velocityCovariance"]; - std::vector covarianceV; - populateSimCovarianceArray(covarianceArrayValue, covarianceV); - simExternalObj.set_VelocityCovariance(covarianceV); - } - } - - void SimulationSensorDetectedObjectConverter::populateSimSensorDetectedObjectSize(const Json::Value &sizeValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj) - { - if (sizeValue.isMember("length")) - { - simExternalObj.set_SizeLength(sizeValue["length"].asDouble()); - } - if (sizeValue.isMember("width")) - { - simExternalObj.set_SizeWidth(sizeValue["width"].asDouble()); - } - if (sizeValue.isMember("height")) - { - simExternalObj.set_SizeHeight(sizeValue["height"].asDouble()); - } - } - - void SimulationSensorDetectedObjectConverter::populateSimCovarianceArray(const Json::Value &covarianceArrayValue, std::vector &covarianceV) - { - std::for_each(covarianceArrayValue.begin(), covarianceArrayValue.end(), [&covarianceV](const auto &item) - { tmx::messages::simulation::Covariance covariance(item.asDouble()); - covarianceV.push_back(covariance); }); - } - -} \ No newline at end of file diff --git a/src/tmx/TmxUtils/src/simulation/SimulationSensorDetectedObjectConverter.h b/src/tmx/TmxUtils/src/simulation/SimulationSensorDetectedObjectConverter.h deleted file mode 100644 index 1ef3b1604..000000000 --- a/src/tmx/TmxUtils/src/simulation/SimulationSensorDetectedObjectConverter.h +++ /dev/null @@ -1,66 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace tmx::utils::sim -{ - - class SimulationSensorDetectedObjectConverter - { - private: - /*** - * @brief Populate simulation detected object metadata with metadata information from JSON string. - * @param SensorDetectedObject V2xHub customized simulation detected object to populate. - * @param metadataValue Json value that contains the metadata information. - */ - static void populateSimSensorDetectedObjectMetadata(const Json::Value &metadataValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj); - /*** - * @brief Populate simulation detected object sensor with sensor information from JSON string. - * @param SensorDetectedObject V2xHub customized simulation detected object to populate. - * @param sensorValue Json value that contains the sensor information. - */ - static void populateSimSensorDetectedObjectSensor(const Json::Value &sensorValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj); - /*** - * @brief Populate simulation detected object position with position information from JSON string. - * @param SensorDetectedObject V2xHub customized simulation detected object to populate. - * @param positionValue Json value that contains the position information. - */ - static void populateSimSensorDetectedObjectPosition(const Json::Value &positionValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj); - /*** - * @brief Populate simulation detected object velocity with velocity information from JSON string. - * @param SensorDetectedObject V2xHub customized simulation detected object to populate. - * @param velocityValue Json value that contains the velocity information. - */ - static void populateSimSensorDetectedObjectVelocity(const Json::Value &velocityValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj); - /*** - * @brief Populate simulation detected object size with size information from JSON string. - * @param SensorDetectedObject V2xHub customized simulation detected object to populate. - * @param sizeValue Json value that contains the size information. - */ - static void populateSimSensorDetectedObjectSize(const Json::Value &sizeValue, tmx::messages::simulation::SensorDetectedObject &simExternalObj); - /*** - * @brief Populate simulation detected object covariance array with covariance array information from JSON string. - * @param SensorDetectedObject V2xHub customized simulation detected object to populate. - * @param covarianceArrayValue Json value that contains the covariance array information. - */ - static void populateSimCovarianceArray(const Json::Value &covarianceArrayValue, std::vector &covarianceV); - - public: - SimulationSensorDetectedObjectConverter() = delete; - ~SimulationSensorDetectedObjectConverter() = delete; - /*** - * @brief Convert simulation detected object into JSON string defined by MOSAIC and CARMAStreets integration. - * @param SensorDetectedObject V2xHub customized simulation detected object. - * @return EcternalObject string in JSON format defined by MOSAIC and CARMAStreets integration. - */ - static std::string simExternalObjToJsonStr(tmx::messages::simulation::SensorDetectedObject &simExternalObj); - /*** - * @brief Populate simulation detected object with JSON string defined by MOSAIC and CARMAStreets integration. - * @param SensorDetectedObject V2xHub customized simulation detected object to populate. - * @param jsonStr EcternalObject in JSON format defined by MOSAIC and CARMAStreets integration. - */ - static void jsonToSimExternalObj(const std::string &jsonStr, tmx::messages::simulation::SensorDetectedObject &simExternalObj); - }; -} From 1d4e003d6ee87ddbae7d09ade3c714ac1fccd43d Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Mon, 24 Jul 2023 22:29:19 +0000 Subject: [PATCH 15/35] update --- ...t_simulation_external_object_converter.cpp | 52 ------------------- 1 file changed, 52 deletions(-) delete mode 100644 src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp diff --git a/src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp b/src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp deleted file mode 100644 index b42f78101..000000000 --- a/src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp +++ /dev/null @@ -1,52 +0,0 @@ -#include -#include -#include - -using namespace tmx::utils::sim; -using namespace tmx::messages; - -TEST(SimulationSensorDetectedObjectConverter, jsonToSimExternalObjInvalidJson) -{ - simulation::SensorDetectedObject simExternalObj; - std::string invalidJsonStr = "Invalid"; - ASSERT_THROW(SimulationSensorDetectedObjectConverter::jsonToSimExternalObj(invalidJsonStr, simExternalObj), std::runtime_error); - std::string noContentJsonStr = "Invalid"; - ASSERT_THROW(SimulationSensorDetectedObjectConverter::jsonToSimExternalObj(noContentJsonStr, simExternalObj), std::runtime_error); -} - -TEST(SimulationSensorDetectedObjectConverter, jsonToSimExternalObjValidJson) -{ - simulation::SensorDetectedObject simExternalObj; - std::string jsonStr = "{\"type\":\"Application\",\"subtype\":\"SensorDetectedObject\",\"content\":{\"timestamp\":123,\"isSimulated\":true,\"sensor\":{\"id\":\"SomeID\",\"type\":\"SematicLidar\",\"location\":{\"x\":1.0,\"y\":1.0,\"z\":2.0},\"proj_string\":\"+proj=tmerc+lat_0=38.95197911150576+lon_0=-77.14835128349988+k=1+x_0=0+y_0=0+datum=WGS84+units=m+geoidgrids=egm96_15.gtx+vunits=m+no_defs\"},\"type\":\"Car\",\"confidence\":\"0.7\",\"objectId\":\"Object1\",\"position\":{\"x\":1.0,\"y\":2.5,\"z\":1.1},\"positionCovariance\":[12.0,17.33333333,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,20.3333],\"velocity\":{\"x\":1.0,\"y\":2.5,\"z\":1.1},\"velocityCovariance\":[12.0,17.33333333,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,20.3333],\"angularVelocity\":{\"x\":1.0,\"y\":2.5,\"z\":1.1},\"angularVelocityCovariance\":[12.0,17.33333333,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,20.3333],\"size\":{\"length\":0.1,\"width\":0.4,\"height\":1.5}}}"; - SimulationSensorDetectedObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - ASSERT_EQ("Object1", simExternalObj.get_Id()); - ASSERT_EQ(true, simExternalObj.get_MetadataIsSimulation()); - ASSERT_EQ("SomeID", simExternalObj.get_SensorId()); - ASSERT_EQ("SematicLidar", simExternalObj.get_SensorType()); - ASSERT_EQ("+proj=tmerc+lat_0=38.95197911150576+lon_0=-77.14835128349988+k=1+x_0=0+y_0=0+datum=WGS84+units=m+geoidgrids=egm96_15.gtx+vunits=m+no_defs", simExternalObj.get_SensorProjString()); - ASSERT_EQ(1, simExternalObj.get_SensorLocationX()); - ASSERT_EQ(1, simExternalObj.get_SensorLocationY()); - ASSERT_EQ(2, simExternalObj.get_SensorLocationZ()); - ASSERT_EQ(1, simExternalObj.get_PositionX()); - ASSERT_EQ(2.5, simExternalObj.get_PositionY()); - ASSERT_EQ(1.1, simExternalObj.get_PositionZ()); - ASSERT_EQ(36, simExternalObj.get_PositionCovariance().size()); - ASSERT_EQ(36, simExternalObj.get_PositionCovariance().size()); -} - -TEST(SimulationSensorDetectedObjectConverter, simExternalObjToJsonStr) -{ - std::string jsonStr = "{\"type\":\"Application\",\"subtype\":\"SensorDetectedObject\",\"content\":{\"timestamp\":123,\"isSimulated\":true,\"sensor\":{\"id\":\"SomeID\",\"type\":\"SematicLidar\",\"location\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"proj_string\":\"+proj=tmerc+lat_0=38.95197911150576+lon_0=-77.14835128349988+k=1+x_0=0+y_0=0+datum=WGS84+units=m+geoidgrids=egm96_15.gtx+vunits=m+no_defs\"},\"type\":\"Car\",\"confidence\":\"0.7\",\"objectId\":\"Object1\",\"position\":{\"x\":1.0,\"y\":2.5,\"z\":1.1},\"positionCovariance\":[12.0,17.33333333,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,20.3333],\"velocity\":{\"x\":1.0,\"y\":2.5,\"z\":1.1},\"velocityCovariance\":[12.0,17.33333333,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,20.3333],\"angularVelocity\":{\"x\":1.0,\"y\":2.5,\"z\":1.1},\"angularVelocityCovariance\":[12.0,17.33333333,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,20.3333],\"size\":{\"length\":0.1,\"width\":0.4,\"height\":1.5}}}"; - simulation::SensorDetectedObject simExternalObj; - SimulationSensorDetectedObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); - std::string output = SimulationSensorDetectedObjectConverter::simExternalObjToJsonStr(simExternalObj); - Json::CharReaderBuilder builder; - const std::unique_ptr reader(builder.newCharReader()); - Json::Value root; - JSONCPP_STRING err; - reader->parse(output.c_str(), output.c_str() + static_cast(output.length()), &root, &err); - ASSERT_FALSE(root["metadata"]["isSimulation"].asBool()); - ASSERT_EQ(12, root["payload"]["positionCovariance"].begin()->asDouble()); - ASSERT_EQ(12, root["payload"]["velocityCovariance"].begin()->asDouble()); - ASSERT_EQ(17.33333333, root["payload"]["velocityCovariance"][1].asDouble()); -} \ No newline at end of file From 05291fd7d51c6eaa57dbff5458dd340a3735dc72 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Mon, 24 Jul 2023 22:32:44 +0000 Subject: [PATCH 16/35] update --- src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp | 1 - src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h | 2 -- 2 files changed, 3 deletions(-) diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index 46d8c3d55..4b5105c26 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -50,7 +50,6 @@ void CARMAStreetsPlugin::UpdateConfigSettings() { GetConfigValue("MobilityPathTopic", _transmitMobilityPathTopic); GetConfigValue("MapTopic", _transmitMAPTopic); GetConfigValue("SRMTopic", _transmitSRMTopic); - GetConfigValue("SimExternalObjTopic", _transmitSimExternalObjTopic); // Populate strategies config string config; GetConfigValue("MobilityOperationStrategies", config); diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h index 53fb22dbc..f7167f269 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h @@ -20,7 +20,6 @@ #include #include #include "JsonToJ2735SSMConverter.h" -#include #include "PluginClientClockAware.h" @@ -105,7 +104,6 @@ class CARMAStreetsPlugin: public PluginClientClockAware { std::string _transmitBSMTopic; std::string _transmitMAPTopic; std::string _transmitSRMTopic; - std::string _transmitSimExternalObjTopic; std::string _kafkaBrokerIp; std::string _kafkaBrokerPort; std::shared_ptr _kafka_producer_ptr; From 48b73eaa74f21a4adfec6b7772ff78cd2a7cd0b9 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Mon, 24 Jul 2023 23:36:03 +0000 Subject: [PATCH 17/35] update --- .../Messages/include/simulation/SensorDetectedObject.h | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/tmx/Messages/include/simulation/SensorDetectedObject.h b/src/tmx/Messages/include/simulation/SensorDetectedObject.h index 17eca8f7c..c1f712064 100644 --- a/src/tmx/Messages/include/simulation/SensorDetectedObject.h +++ b/src/tmx/Messages/include/simulation/SensorDetectedObject.h @@ -65,15 +65,6 @@ namespace tmx std_attribute(this->msg, double, VelocityTwistAngularZ, 0, ); array_attribute( Covariance, VelocityCovariance); - // #Instantaneous velocity of an object within the frame specified in header - std_attribute(this->msg, double, VelocityInstTwistLinearX, 0, ); - std_attribute(this->msg, double, VelocityInstTwistLinearY, 0, ); - std_attribute(this->msg, double, VelocityInstTwistLinearZ, 0, ); - std_attribute(this->msg, double, VelocityInstTwistAngularX, 0, ); - std_attribute(this->msg, double, VelocityInstTwistAngularY, 0, ); - std_attribute(this->msg, double, VelocityInstTwistAngularZ, 0, ); - array_attribute( Covariance, VelocityInstCovariance); - // #The size of the object aligned along the axis of the object described by the orientation in pose // #Dimensions are specified in meters /** From ba1ebbc13b16b46ad46d3f1a43de62bfb1eaf71b Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Tue, 25 Jul 2023 01:40:52 +0000 Subject: [PATCH 18/35] update --- .../include/simulation/SensorDetectedObject.h | 58 +------------------ .../src/CARMAStreetsPlugin.cpp | 7 +++ .../src/CARMAStreetsPlugin.h | 3 + .../scripts/send_sim_detected_object_udp.py | 13 +++-- .../CDASimAdapter/src/CDASimConnection.cpp | 2 +- 5 files changed, 19 insertions(+), 64 deletions(-) diff --git a/src/tmx/Messages/include/simulation/SensorDetectedObject.h b/src/tmx/Messages/include/simulation/SensorDetectedObject.h index c1f712064..54f958e0e 100644 --- a/src/tmx/Messages/include/simulation/SensorDetectedObject.h +++ b/src/tmx/Messages/include/simulation/SensorDetectedObject.h @@ -25,64 +25,8 @@ namespace tmx // 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 + // // Message sub type for routing this message through TMX core static constexpr const char *MessageSubType = MSGSUBTYPE_SENSOR_DETECTED_OBJECT_STRING; - /** - * Metadata to describe the external object - */ - std_attribute(this->msg, bool, MetadataIsSimulation, false, ); - std_attribute(this->msg, uint64_t, MetadataTimestamp, 0, ); - std_attribute(this->msg, std::string, SensorProjString, "", ); - std_attribute(this->msg, std::string, SensorType, "", ); - std_attribute(this->msg, std::string, SensorId, "", ); - std_attribute(this->msg, double, SensorLocationX, 0, ); - std_attribute(this->msg, double, SensorLocationY, 0, ); - std_attribute(this->msg, double, SensorLocationZ, 0, ); - std_attribute(this->msg, std::string, MetadataInfrastructureId, "", ); - std_attribute(this->msg, std::string, MetadataSensorId, "", ); - - // Object id. Matching ids on a topic should refer to the same object within some time period, expanded - std_attribute(this->msg, std::string, Id, "", ); - - // Pose of the object within the frame specified in header - // This represents a pose in free space with uncertainty. - std_attribute(this->msg, double, PositionX, 0, ); - std_attribute(this->msg, double, PositionY, 0, ); - std_attribute(this->msg, double, PositionZ, 0, ); - // This represents an orientation in free space in quaternion form. - std_attribute(this->msg, double, OrientationX, 0, ); - std_attribute(this->msg, double, OrientationY, 0, ); - std_attribute(this->msg, double, OrientationZ, 0, ); - std_attribute(this->msg, double, OrientationW, 0, ); - array_attribute( Covariance, PositionCovariance); - - // #Average velocity of the object within the frame specified in header - std_attribute(this->msg, double, VelocityTwistLinearX, 0, ); - std_attribute(this->msg, double, VelocityTwistLinearY, 0, ); - std_attribute(this->msg, double, VelocityTwistLinearZ, 0, ); - std_attribute(this->msg, double, VelocityTwistAngularX, 0, ); - std_attribute(this->msg, double, VelocityTwistAngularY, 0, ); - std_attribute(this->msg, double, VelocityTwistAngularZ, 0, ); - array_attribute( Covariance, VelocityCovariance); - - // #The size of the object aligned along the axis of the object described by the orientation in pose - // #Dimensions are specified in meters - /** - * # This represents a vector in free space. - # It is only meant to represent a direction. Therefore, it does not - # make sense to apply a translation to it (e.g., when applying a - # generic rigid transformation to a Vector3, tf2 will only apply the - # rotation). - */ - std_attribute(this->msg, double, SizeLength, 0, ); - std_attribute(this->msg, double, SizeWidth, 0, ); - std_attribute(this->msg, double, SizeHeight, 0, ); - - // #Confidence - std_attribute(this->msg, double, Confidence, 0, ); - - // #describes a general object type as defined in this message - std_attribute(this->msg, std::string, ObjectType, "", ); }; } diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index 4b5105c26..bd16592dc 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -25,6 +25,7 @@ CARMAStreetsPlugin::CARMAStreetsPlugin(string name) : AddMessageFilter < tsm2Message > (this, &CARMAStreetsPlugin::HandleMobilityPathMessage); AddMessageFilter < MapDataMessage > (this, &CARMAStreetsPlugin::HandleMapMessage); AddMessageFilter < SrmMessage > (this, &CARMAStreetsPlugin::HandleSRMMessage); + AddMessageFilter < simulation::SensorDetectedObject > (this, &CARMAStreetsPlugin::HandleSimulatedExternalMessage ); SubscribeToMessages(); } @@ -50,6 +51,7 @@ void CARMAStreetsPlugin::UpdateConfigSettings() { GetConfigValue("MobilityPathTopic", _transmitMobilityPathTopic); GetConfigValue("MapTopic", _transmitMAPTopic); GetConfigValue("SRMTopic", _transmitSRMTopic); + GetConfigValue("SimExternalObjTopic", _transmitSimExternalObjTopic); // Populate strategies config string config; GetConfigValue("MobilityOperationStrategies", config); @@ -628,6 +630,11 @@ void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){ } +void CARMAStreetsPlugin::HandleSimulatedExternalMessage(simulation::SensorDetectedObject &msg, routeable_message &routeableMsg) +{ + PLOG(logINFO) << "Produce External Object Message in JSON format: " << msg.to_string() < #include #include "JsonToJ2735SSMConverter.h" +#include #include "PluginClientClockAware.h" @@ -48,6 +49,7 @@ 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); + void HandleSimulatedExternalMessage(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 +106,7 @@ class CARMAStreetsPlugin: public PluginClientClockAware { std::string _transmitBSMTopic; std::string _transmitMAPTopic; std::string _transmitSRMTopic; + std::string _transmitSimExternalObjTopic; 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 index 423c000df..9286236ea 100755 --- a/src/v2i-hub/CDASimAdapter/scripts/send_sim_detected_object_udp.py +++ b/src/v2i-hub/CDASimAdapter/scripts/send_sim_detected_object_udp.py @@ -13,13 +13,14 @@ count_num = 0 def generate_sim_external_object(): - jsonResult = - { - "type": "Application", - "subtype": "SensorDetectedObject", - "content": { + jsonResult = { + "metadata": { + "type": "Application", + "subtype": "SensorDetectedObject", "timestamp": 123, - "isSimulated": true, + "isSimulated": True + }, + "payload": { "sensor": { "id": "SomeID", "type": "SematicLidar", diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp index 81ce3aa93..7a8106afc 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp @@ -136,7 +136,7 @@ namespace CDASimAdapter{ if(external_object_listener) { std::string str_msg = consume_server_message(external_object_listener); - //ToDo: To populate the simulation detected 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 + externalObj.set_contents(str_msg); } else { From a10a2942ff3762016b9531f68aeba0d5c78fdc31 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Tue, 25 Jul 2023 01:42:56 +0000 Subject: [PATCH 19/35] remove definition --- .../Messages/include/simulation/Covariance.h | 41 ------------------- .../include/simulation/SensorDetectedObject.h | 1 - 2 files changed, 42 deletions(-) delete mode 100644 src/tmx/Messages/include/simulation/Covariance.h diff --git a/src/tmx/Messages/include/simulation/Covariance.h b/src/tmx/Messages/include/simulation/Covariance.h deleted file mode 100644 index 2bd19402f..000000000 --- a/src/tmx/Messages/include/simulation/Covariance.h +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef INCLUDE_SIMULATED_COVARIANCE_H_ -#define INCLUDE_SIMULATED_COVARIANCE_H_ - -#include -#include - -namespace tmx -{ - namespace messages - { - namespace simulation - { - // Row-major representation of the 6x6 covariance matrix - // # The orientation parameters use a fixed-axis representation. - // # In order, the parameters are: - // # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis) - struct Covariance - { - double covariance = 0; - - Covariance() {} - Covariance(double covariance) : covariance(covariance) {} - - static message_tree_type to_tree(Covariance element) - { - message_tree_type treeElement; - treeElement.put("Covariance", element.covariance); - return treeElement; - } - - static Covariance from_tree(message_tree_type &treeElement) - { - Covariance element; - element.covariance = treeElement.get("Covariance"); - return element; - } - }; - } - } -} -#endif \ No newline at end of file diff --git a/src/tmx/Messages/include/simulation/SensorDetectedObject.h b/src/tmx/Messages/include/simulation/SensorDetectedObject.h index 54f958e0e..179f48a13 100644 --- a/src/tmx/Messages/include/simulation/SensorDetectedObject.h +++ b/src/tmx/Messages/include/simulation/SensorDetectedObject.h @@ -3,7 +3,6 @@ #include #include -#include namespace tmx { From d1b6086a7c28c63075ece0267e9b6c240986ad18 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Tue, 25 Jul 2023 01:49:29 +0000 Subject: [PATCH 20/35] update name --- src/v2i-hub/CARMAStreetsPlugin/manifest.json | 6 +++--- .../CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp | 10 +++++----- .../CARMAStreetsPlugin/src/CARMAStreetsPlugin.h | 4 ++-- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/v2i-hub/CARMAStreetsPlugin/manifest.json b/src/v2i-hub/CARMAStreetsPlugin/manifest.json index 37e705a8d..8195c49fd 100644 --- a/src/v2i-hub/CARMAStreetsPlugin/manifest.json +++ b/src/v2i-hub/CARMAStreetsPlugin/manifest.json @@ -118,9 +118,9 @@ "description": "Apache Kafka consumer group ID for spat consumer." }, { - "key": "SimExternalObjTopic", - "default": "v2xhub_sim_external_object", - "description": "Apache Kafka topic plugin will transmit simulated external object to." + "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 bd16592dc..7e685a530 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -25,7 +25,7 @@ CARMAStreetsPlugin::CARMAStreetsPlugin(string name) : AddMessageFilter < tsm2Message > (this, &CARMAStreetsPlugin::HandleMobilityPathMessage); AddMessageFilter < MapDataMessage > (this, &CARMAStreetsPlugin::HandleMapMessage); AddMessageFilter < SrmMessage > (this, &CARMAStreetsPlugin::HandleSRMMessage); - AddMessageFilter < simulation::SensorDetectedObject > (this, &CARMAStreetsPlugin::HandleSimulatedExternalMessage ); + AddMessageFilter < simulation::SensorDetectedObject > (this, &CARMAStreetsPlugin::HandleSimulatedSensorDetectedMessage ); SubscribeToMessages(); } @@ -51,7 +51,7 @@ void CARMAStreetsPlugin::UpdateConfigSettings() { GetConfigValue("MobilityPathTopic", _transmitMobilityPathTopic); GetConfigValue("MapTopic", _transmitMAPTopic); GetConfigValue("SRMTopic", _transmitSRMTopic); - GetConfigValue("SimExternalObjTopic", _transmitSimExternalObjTopic); + GetConfigValue("SimSensorDetectedObjTopic", _transmitSimSensorDetectedObjTopic); // Populate strategies config string config; GetConfigValue("MobilityOperationStrategies", config); @@ -630,10 +630,10 @@ void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){ } -void CARMAStreetsPlugin::HandleSimulatedExternalMessage(simulation::SensorDetectedObject &msg, routeable_message &routeableMsg) +void CARMAStreetsPlugin::HandleSimulatedSensorDetectedMessage(simulation::SensorDetectedObject &msg, routeable_message &routeableMsg) { - PLOG(logINFO) << "Produce External Object Message in JSON format: " << msg.to_string() < _kafka_producer_ptr; From dc77fb2d84adabd97a538d21a3bc7b204c5f2efa Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Tue, 25 Jul 2023 02:07:44 +0000 Subject: [PATCH 21/35] update naming convension --- .devcontainer/docker-compose-vscode.yml | 2 +- configuration/amd64/docker-compose.yml | 2 +- .../src/simulation/SimulationEnvUtils.h | 2 +- .../scripts/send_sim_detected_object_udp.py | 2 +- .../CDASimAdapter/src/CDASimAdapter.cpp | 20 ++++++------- .../CDASimAdapter/src/CDASimConnection.cpp | 28 +++++++++---------- .../src/include/CDASimAdapter.hpp | 6 ++-- .../src/include/CDASimConnection.hpp | 14 +++++----- 8 files changed, 38 insertions(+), 38 deletions(-) diff --git a/.devcontainer/docker-compose-vscode.yml b/.devcontainer/docker-compose-vscode.yml index 4f0dac989..8070c30d1 100755 --- a/.devcontainer/docker-compose-vscode.yml +++ b/.devcontainer/docker-compose-vscode.yml @@ -22,7 +22,7 @@ services: - TIME_SYNC_TOPIC=time_sync - TIME_SYNC_PORT=7575 - SIM_V2X_PORT=5757 - - SIM_EXTERNAL_OBJECT_PORT=7576 + - SIM_SENSOR_DETECTED_OBJECT_PORT=7576 - V2X_PORT=8686 - INFRASTRUCTURE_ID=1 secrets: diff --git a/configuration/amd64/docker-compose.yml b/configuration/amd64/docker-compose.yml index 088100a81..ffe16dcb6 100755 --- a/configuration/amd64/docker-compose.yml +++ b/configuration/amd64/docker-compose.yml @@ -45,7 +45,7 @@ services: - TIME_SYNC_TOPIC=time_sync - TIME_SYNC_PORT=7575 - SIM_V2X_PORT=5757 - - SIM_EXTERNAL_OBJECT_PORT=7576 + - SIM_SENSOR_DETECTED_OBJECT_PORT=7576 - V2X_PORT=8686 - INFRASTRUCTURE_ID=1 - KAFKA_BROKER_ADDRESS=127.0.0.1:9092 diff --git a/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h b/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h index 08b93e49d..009df41a2 100644 --- a/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h +++ b/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h @@ -45,7 +45,7 @@ namespace tmx::utils::sim{ * @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_EXTERNAL_OBJECT_PORT = "SIM_EXTERNAL_OBJECT_PORT"; + constexpr inline static const char *SIM_SENSOR_DETECTED_OBJECT_PORT = "SIM_SENSOR_DETECTED_OBJECT_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/CDASimAdapter/scripts/send_sim_detected_object_udp.py b/src/v2i-hub/CDASimAdapter/scripts/send_sim_detected_object_udp.py index 9286236ea..ab7d0cb98 100755 --- a/src/v2i-hub/CDASimAdapter/scripts/send_sim_detected_object_udp.py +++ b/src/v2i-hub/CDASimAdapter/scripts/send_sim_detected_object_udp.py @@ -39,7 +39,7 @@ def generate_sim_external_object(): "y": 2.5, "z": 1.1 }, - "positionCovariance" : ["a11", "a12", "a13", "a21", "a22", "a23", "a31", "a32", "a33"], + "positionCovariance" : [12,12,2, 34, 34, 55], "velocity": { "x": 1.0, "y": 2.5, diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp index 9f5c52071..469a28576 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp @@ -60,7 +60,7 @@ namespace CDASimAdapter{ if ( connection->is_connected() ) { start_time_sync_thread_timer(); - start_external_object_detection_thread(); + start_sensor_detected_object_detection_thread(); start_immediate_forward_thread(); start_message_receiver_thread(); }else { @@ -79,7 +79,7 @@ namespace CDASimAdapter{ } - void CDASimAdapter::forward_simulated_external_message(tmx::messages::simulation::SensorDetectedObject &msg) { + 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); } @@ -91,7 +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)); - uint external_object_detection_port = std::stoul(sim::get_sim_config(sim::SIM_EXTERNAL_OBJECT_PORT)); + uint sensor_detected_object_detection_port = std::stoul(sim::get_sim_config(sim::SIM_sensor_detected_object_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); @@ -101,11 +101,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, external_object_detection_port, v2x_port, location )); + time_sync_port, sensor_detected_object_detection_port, v2x_port, location )); } else { connection = std::make_unique(simulation_ip, infrastructure_id, simulation_registration_port, sim_v2x_port, local_ip, - time_sync_port, external_object_detection_port, v2x_port, location); + time_sync_port, sensor_detected_object_detection_port, v2x_port, location); } } catch (const TmxException &e) { @@ -167,7 +167,7 @@ namespace CDASimAdapter{ } } - void CDASimAdapter::start_external_object_detection_thread() { + void CDASimAdapter::start_sensor_detected_object_detection_thread() { PLOG(logDEBUG) << "Creating Thread Timer for simulated external object" << std::endl; try { @@ -176,14 +176,14 @@ namespace CDASimAdapter{ external_bject_detection_thread_timer = std::make_unique(); } external_bject_detection_thread_timer->AddPeriodicTick([this](){ - PLOG(logDEBUG1) << "Listening for External Object Message from CDASim." << std::endl; - auto msg = connection->consume_external_object_message(); + 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_external_message(msg); + this->forward_simulated_detected_message(msg); } else { - PLOG(logDEBUG1) << "CDASim connection has not yet received an simulated external message!" << std::endl; + PLOG(logDEBUG1) << "CDASim connection has not yet received an simulated sensor detected message!" << std::endl; } }//End lambda , std::chrono::milliseconds(100)); diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp index 7a8106afc..61bc82c7b 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp @@ -5,10 +5,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 external_object_detection_port, const uint v2x_port, + const std::string &local_ip, const uint time_sync_port,const uint sensor_detected_object_detection_port, const uint v2x_port, const Point &location) : _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), _external_object_detection_port(external_object_detection_port),_v2x_port(v2x_port), + _simulation_v2x_port(sim_v2x_port), _local_ip(local_ip), _time_sync_port(time_sync_port), _sensor_detected_object_detection_port(sensor_detected_object_detection_port),_v2x_port(v2x_port), _location(location) { 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, _external_object_detection_port, _v2x_port, _location)) { + if (!carma_simulation_handshake(_simulation_ip, _infrastructure_id, _simulation_registration_port, _local_ip, _time_sync_port, _sensor_detected_object_detection_port, _v2x_port, _location)) { _connected = false; return _connected; } - if (!setup_udp_connection(_simulation_ip, _local_ip, _time_sync_port,_external_object_detection_port, _v2x_port, _simulation_v2x_port )) { + if (!setup_udp_connection(_simulation_ip, _local_ip, _time_sync_port,_sensor_detected_object_detection_port, _v2x_port, _simulation_v2x_port )) { _connected = false; return _connected; } @@ -34,7 +34,7 @@ namespace CDASimAdapter{ } - std::string CDASimConnection::get_handshake_json(const std::string &infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint external_object_detection_port, const uint v2x_port, + std::string CDASimConnection::get_handshake_json(const std::string &infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint sensor_detected_object_detection_port, const uint v2x_port, const Point &location) const { @@ -45,7 +45,7 @@ namespace CDASimAdapter{ message["infrastructureId"] = infrastructure_id; message["rxMessagePort"] = v2x_port; message["timeSyncPort"] = time_sync_port; - message["SensorDetectedObjectDetectionPort"] = external_object_detection_port; + message["SensorDetectedObjectDetectionPort"] = sensor_detected_object_detection_port; message["location"]["x"] = location.X; message["location"]["y"] = location.Y; message["location"]["z"] = location.Z; @@ -55,13 +55,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 external_object_detection_port, const uint v2x_port, + const std::string &local_ip, const uint time_sync_port, const uint sensor_detected_object_detection_port, const uint v2x_port, const Point &location) { // Create JSON message with the content std::string payload = ""; - payload = get_handshake_json(infrastructure_id, local_ip, time_sync_port, external_object_detection_port, v2x_port, location); + payload = get_handshake_json(infrastructure_id, local_ip, time_sync_port, sensor_detected_object_detection_port, v2x_port, location); try { @@ -77,7 +77,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, const uint external_object_detection_port, + bool CDASimConnection::setup_udp_connection(const std::string &simulation_ip, const std::string &local_ip, const uint time_sync_port, const uint sensor_detected_object_detection_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 @@ -95,13 +95,13 @@ namespace CDASimAdapter{ message_receiver_publisher = std::make_shared( local_ip, 8765); // Initialize UDP Server for listening for incoming CARMA-Simulation time synchronization. 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(external_object_detection_port) << "\n" + << "Creating UDPServer for Simulated External Object detection: " << local_ip << ":" << std::to_string(sensor_detected_object_detection_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); - external_object_listener = std::make_shared (local_ip, external_object_detection_port); + sensor_detected_object_listener = std::make_shared (local_ip, sensor_detected_object_detection_port); } catch (const UdpClientRuntimeError &e) { PLOG(logERROR) << "Encountered UDPClient Runtime error during UDP connection initialization : " << e.what() << std::endl; @@ -129,13 +129,13 @@ namespace CDASimAdapter{ } - tmx::messages::simulation::SensorDetectedObject CDASimConnection::consume_external_object_message() const + tmx::messages::simulation::SensorDetectedObject CDASimConnection::consume_sensor_detected_object_message() const { tmx::messages::simulation::SensorDetectedObject externalObj; externalObj.clear(); - if(external_object_listener) + if(sensor_detected_object_listener) { - std::string str_msg = consume_server_message(external_object_listener); + std::string str_msg = consume_server_message(sensor_detected_object_listener); externalObj.set_contents(str_msg); } else diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp index 089929647..97f569db9 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp @@ -90,10 +90,10 @@ namespace CDASimAdapter */ void forward_time_sync_message(tmx::messages::TimeSyncMessage &msg); /** - * @brief Forward simulated external object message to TMX message bus for other V2X-Hub Plugin + * @brief Forward simulated sensor detected object message to TMX message bus for other V2X-Hub Plugin * @param msg simulation::SensorDetectedObject. */ - void forward_simulated_external_message(tmx::messages::simulation::SensorDetectedObject &msg); + void forward_simulated_detected_message(tmx::messages::simulation::SensorDetectedObject &msg); /** * @brief Method to start thread timer for regular interval actions lauched on seperate thread. */ @@ -106,7 +106,7 @@ namespace CDASimAdapter /** * @brief Method to start thread timer for regular interval actions lauched on seperate thread. */ - void start_external_object_detection_thread(); + void start_sensor_detected_object_detection_thread(); private: // Simulated location of RSU diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp index 4b2ba5201..99957c6f9 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp @@ -33,7 +33,7 @@ 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 external_object_detection_port, const uint v2x_port, + const uint sim_v2x_port, const std::string &local_ip, const uint time_sync_port, const uint sensor_detected_object_detection_port, const uint v2x_port, const tmx::utils::Point &location); /** * @brief Method to forward v2x message to CARMA Simulation @@ -87,7 +87,7 @@ namespace CDASimAdapter { * //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_external_object_message() const; + 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, @@ -101,7 +101,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 external_object_detection_port, const uint v2x_port, + const std::string &local_ip, const uint time_sync_port, const uint sensor_detected_object_detection_port, const uint v2x_port, const tmx::utils::Point &location); /** @@ -113,7 +113,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, const uint external_object_detection_port, + bool setup_udp_connection(const std::string &simulation_ip, const std::string &local_ip, const uint time_sync_port, const uint sensor_detected_object_detection_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 @@ -136,13 +136,13 @@ 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 external_object_detection_port, + std::string get_handshake_json(const std::string &infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint sensor_detected_object_detection_port, const uint v2x_port, const tmx::utils::Point &location) const; std::string _simulation_ip; uint _simulation_registration_port; std::string _infrastructure_id; uint _simulation_v2x_port; - uint _external_object_detection_port; + uint _sensor_detected_object_detection_port; std::string _local_ip; uint _time_sync_port; uint _v2x_port; @@ -155,7 +155,7 @@ namespace CDASimAdapter { std::shared_ptr immediate_forward_listener; std::shared_ptr message_receiver_publisher; std::shared_ptr time_sync_listener; - std::shared_ptr external_object_listener; + std::shared_ptr sensor_detected_object_listener; FRIEND_TEST(TestCARMASimulationConnection, get_handshake_json); }; From 78cf596dcf6dd1a3d39fec5aa38e4c39444f1be1 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Tue, 25 Jul 2023 02:14:54 +0000 Subject: [PATCH 22/35] update naming convension --- src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp index 469a28576..2fad976eb 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp @@ -91,7 +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)); - uint sensor_detected_object_detection_port = std::stoul(sim::get_sim_config(sim::SIM_sensor_detected_object_PORT)); + uint sensor_detected_object_detection_port = std::stoul(sim::get_sim_config(sim::SIM_SENSOR_DETECTED_OBJECT_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); From 89fb7349f94ccb82ac2a10dd0286ab477fab467d Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Tue, 25 Jul 2023 21:00:48 +0000 Subject: [PATCH 23/35] update json key --- src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp index 61bc82c7b..e106fb28c 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp @@ -45,7 +45,7 @@ namespace CDASimAdapter{ message["infrastructureId"] = infrastructure_id; message["rxMessagePort"] = v2x_port; message["timeSyncPort"] = time_sync_port; - message["SensorDetectedObjectDetectionPort"] = sensor_detected_object_detection_port; + message["simulatedInteractionPort"] = sensor_detected_object_detection_port; message["location"]["x"] = location.X; message["location"]["y"] = location.Y; message["location"]["z"] = location.Z; From bdd7dcf65dad205044c22816033fee65648ad04b Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Tue, 25 Jul 2023 22:19:21 +0000 Subject: [PATCH 24/35] rename variable --- .devcontainer/docker-compose-vscode.yml | 2 +- configuration/amd64/docker-compose.yml | 2 +- .../src/simulation/SimulationEnvUtils.h | 2 +- .../CDASimAdapter/src/CDASimAdapter.cpp | 6 ++--- .../CDASimAdapter/src/CDASimConnection.cpp | 22 +++++++++---------- .../src/include/CDASimConnection.hpp | 10 ++++----- 6 files changed, 22 insertions(+), 22 deletions(-) diff --git a/.devcontainer/docker-compose-vscode.yml b/.devcontainer/docker-compose-vscode.yml index 8070c30d1..967c33125 100755 --- a/.devcontainer/docker-compose-vscode.yml +++ b/.devcontainer/docker-compose-vscode.yml @@ -22,7 +22,7 @@ services: - TIME_SYNC_TOPIC=time_sync - TIME_SYNC_PORT=7575 - SIM_V2X_PORT=5757 - - SIM_SENSOR_DETECTED_OBJECT_PORT=7576 + - SIM_INTERACTION_PORT=7576 - V2X_PORT=8686 - INFRASTRUCTURE_ID=1 secrets: diff --git a/configuration/amd64/docker-compose.yml b/configuration/amd64/docker-compose.yml index ffe16dcb6..d3ba706d9 100755 --- a/configuration/amd64/docker-compose.yml +++ b/configuration/amd64/docker-compose.yml @@ -45,7 +45,7 @@ services: - TIME_SYNC_TOPIC=time_sync - TIME_SYNC_PORT=7575 - SIM_V2X_PORT=5757 - - SIM_SENSOR_DETECTED_OBJECT_PORT=7576 + - SIM_INTERACTION_PORT=7576 - V2X_PORT=8686 - INFRASTRUCTURE_ID=1 - KAFKA_BROKER_ADDRESS=127.0.0.1:9092 diff --git a/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h b/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h index 009df41a2..e49c7f00a 100644 --- a/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h +++ b/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h @@ -45,7 +45,7 @@ namespace tmx::utils::sim{ * @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_SENSOR_DETECTED_OBJECT_PORT = "SIM_SENSOR_DETECTED_OBJECT_PORT"; + 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/CDASimAdapter/src/CDASimAdapter.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp index 2fad976eb..7c0c6c431 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp @@ -91,7 +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)); - uint sensor_detected_object_detection_port = std::stoul(sim::get_sim_config(sim::SIM_SENSOR_DETECTED_OBJECT_PORT)); + uint simulated_interaction_port = std::stoul(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); @@ -101,11 +101,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, sensor_detected_object_detection_port, v2x_port, location )); + time_sync_port, simulated_interaction_port, v2x_port, location )); } else { connection = std::make_unique(simulation_ip, infrastructure_id, simulation_registration_port, sim_v2x_port, local_ip, - time_sync_port, sensor_detected_object_detection_port, v2x_port, location); + time_sync_port, simulated_interaction_port, v2x_port, location); } } catch (const TmxException &e) { diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp index e106fb28c..288d7839a 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp @@ -5,10 +5,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 sensor_detected_object_detection_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) : _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), _sensor_detected_object_detection_port(sensor_detected_object_detection_port),_v2x_port(v2x_port), + _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) { 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, _sensor_detected_object_detection_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,_sensor_detected_object_detection_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; } @@ -34,7 +34,7 @@ namespace CDASimAdapter{ } - std::string CDASimConnection::get_handshake_json(const std::string &infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint sensor_detected_object_detection_port, const uint v2x_port, + 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 Point &location) const { @@ -45,7 +45,7 @@ namespace CDASimAdapter{ message["infrastructureId"] = infrastructure_id; message["rxMessagePort"] = v2x_port; message["timeSyncPort"] = time_sync_port; - message["simulatedInteractionPort"] = sensor_detected_object_detection_port; + message["simulatedInteractionPort"] = simulated_interaction_port; message["location"]["x"] = location.X; message["location"]["y"] = location.Y; message["location"]["z"] = location.Z; @@ -55,13 +55,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 sensor_detected_object_detection_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) { // Create JSON message with the content std::string payload = ""; - payload = get_handshake_json(infrastructure_id, local_ip, time_sync_port, sensor_detected_object_detection_port, v2x_port, location); + payload = get_handshake_json(infrastructure_id, local_ip, time_sync_port, simulated_interaction_port, v2x_port, location); try { @@ -77,7 +77,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, const uint sensor_detected_object_detection_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 @@ -95,13 +95,13 @@ namespace CDASimAdapter{ message_receiver_publisher = std::make_shared( local_ip, 8765); // Initialize UDP Server for listening for incoming CARMA-Simulation time synchronization. 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(sensor_detected_object_detection_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, sensor_detected_object_detection_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; diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp index 99957c6f9..951f02d76 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp @@ -33,7 +33,7 @@ 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 sensor_detected_object_detection_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); /** * @brief Method to forward v2x message to CARMA Simulation @@ -101,7 +101,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 sensor_detected_object_detection_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); /** @@ -113,7 +113,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, const uint sensor_detected_object_detection_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 @@ -136,13 +136,13 @@ 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 sensor_detected_object_detection_port, + 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; std::string _simulation_ip; uint _simulation_registration_port; std::string _infrastructure_id; uint _simulation_v2x_port; - uint _sensor_detected_object_detection_port; + uint _simulated_interaction_port; std::string _local_ip; uint _time_sync_port; uint _v2x_port; From cba6d6b1550fb0494afa5147756a227ed58477d4 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Wed, 26 Jul 2023 14:18:43 +0000 Subject: [PATCH 25/35] address comments --- .../src/CARMAStreetsPlugin.cpp | 2 +- .../scripts/send_sim_detected_object_udp.py | 17 ++++------------- 2 files changed, 5 insertions(+), 14 deletions(-) diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index 7e685a530..c5bb9a7e6 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -632,7 +632,7 @@ void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){ void CARMAStreetsPlugin::HandleSimulatedSensorDetectedMessage(simulation::SensorDetectedObject &msg, routeable_message &routeableMsg) { - PLOG(logINFO) << "Produce sensor detected message in JSON format: " << msg.to_string() < Date: Wed, 26 Jul 2023 14:20:18 +0000 Subject: [PATCH 26/35] update --- .../CDASimAdapter/scripts/send_sim_detected_object_udp.py | 1 + 1 file changed, 1 insertion(+) 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 index 27e4031e9..e07b0f8a5 100755 --- a/src/v2i-hub/CDASimAdapter/scripts/send_sim_detected_object_udp.py +++ b/src/v2i-hub/CDASimAdapter/scripts/send_sim_detected_object_udp.py @@ -22,6 +22,7 @@ def generate_sim_external_object(): }, "payload": { "sensor_id": "sensor1", + "proj_string": "asdlasdkasd", "type": "Car", "confidence": "0.7", "objectId": "Object1", From aab491106bfc2cc040869ee7e5d7e7d2a528200c Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Wed, 26 Jul 2023 14:23:55 +0000 Subject: [PATCH 27/35] address comms --- configuration/amd64/docker-compose.yml | 11 ----------- .../scripts/send_sim_detected_object_udp.py | 18 +++++++++--------- 2 files changed, 9 insertions(+), 20 deletions(-) diff --git a/configuration/amd64/docker-compose.yml b/configuration/amd64/docker-compose.yml index d3ba706d9..6b5188297 100755 --- a/configuration/amd64/docker-compose.yml +++ b/configuration/amd64/docker-compose.yml @@ -38,17 +38,6 @@ services: - db environment: - MYSQL_PASSWORD=/run/secrets/mysql_password - - SIMULATION_MODE=true - - SIMULATION_IP=127.0.0.1 - - SIMULATION_REGISTRATION_PORT=6767 - - LOCAL_IP=127.0.0.1 - - TIME_SYNC_TOPIC=time_sync - - TIME_SYNC_PORT=7575 - - SIM_V2X_PORT=5757 - - SIM_INTERACTION_PORT=7576 - - V2X_PORT=8686 - - INFRASTRUCTURE_ID=1 - - KAFKA_BROKER_ADDRESS=127.0.0.1:9092 secrets: - mysql_password volumes: 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 index e07b0f8a5..e90eaf9c3 100755 --- a/src/v2i-hub/CDASimAdapter/scripts/send_sim_detected_object_udp.py +++ b/src/v2i-hub/CDASimAdapter/scripts/send_sim_detected_object_udp.py @@ -33,21 +33,21 @@ def generate_sim_external_object(): }, "positionCovariance" : [12,12,2, 34, 34, 55], "velocity": { - "x": 1.0, - "y": 2.5, - "z": 1.1 + "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 + "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 + "length": 0.1, + "width": 0.4, + "height": 1.5 } } } From 1cb1a6ef3e0613ba90002ed1972f3b5fae5d31b4 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Wed, 26 Jul 2023 15:24:32 +0000 Subject: [PATCH 28/35] address comments --- src/v2i-hub/CARMAStreetsPlugin/manifest.json | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/v2i-hub/CARMAStreetsPlugin/manifest.json b/src/v2i-hub/CARMAStreetsPlugin/manifest.json index 8195c49fd..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": [ From 21af9756ff3c3aafb6b5bfc176e10dc3243b01f6 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Wed, 26 Jul 2023 16:02:55 +0000 Subject: [PATCH 29/35] unit test failture --- .../CDASimAdapter/test/TestCARMASimulationConnection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp index ec2b9f173..725203367 100644 --- a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp @@ -86,7 +86,7 @@ namespace CDASimAdapter { location.Y = 38.955; location.Z = -77.149; ASSERT_EQ(connection->get_handshake_json("4566", "127.0.0.1", 4567, 4568, 4569, location), - "{\n \"SensorDetectedObjectDetectionPort\" : 4568,\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 \"timeSyncPort\" : 4567\n}\n"); + "{\n \"simulatedInteractionPort\" : 4568,\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 \"timeSyncPort\" : 4567\n}\n"); } TEST_F( TestCARMASimulationConnection, carma_simulation_handshake) { From ff272a70fecec76e8b6c71735cd41e01546d71f1 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Wed, 26 Jul 2023 16:30:07 +0000 Subject: [PATCH 30/35] unit test failure --- .../CDASimAdapter/test/TestCARMASimulationConnection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp index 725203367..907828fc0 100644 --- a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp @@ -86,7 +86,7 @@ namespace CDASimAdapter { location.Y = 38.955; location.Z = -77.149; ASSERT_EQ(connection->get_handshake_json("4566", "127.0.0.1", 4567, 4568, 4569, location), - "{\n \"simulatedInteractionPort\" : 4568,\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 \"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 \"simulatedInteractionPort\" : 4568,\n \"timeSyncPort\" : 4567\n}\n"); } TEST_F( TestCARMASimulationConnection, carma_simulation_handshake) { From 4aef6b1e73db159ae3ef4bc90a845285ecee2884 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Thu, 27 Jul 2023 19:07:36 +0000 Subject: [PATCH 31/35] fix merge issue --- src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp | 4 ++-- .../CDASimAdapter/src/include/CDASimConnection.hpp | 14 +++++++------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp index fa553b22f..fd41bb548 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp @@ -35,7 +35,7 @@ namespace CDASimAdapter{ 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 Point &location) const + const tmx::utils::Point &location) const { Json::Value message; @@ -65,7 +65,7 @@ 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 simulated_interaction_port, const uint v2x_port, - const Point &location) + const tmx::utils::Point &location) { // Create JSON message with the content std::string payload = ""; diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp index 2509fcb7a..3f53ca368 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp @@ -137,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; From 8aa377851c5a0db212889aa3d92ffab425da21a5 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Thu, 27 Jul 2023 19:10:24 +0000 Subject: [PATCH 32/35] fix unit test --- .../CDASimAdapter/test/TestCARMASimulationConnection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp index 9b4651a04..4f4c4de42 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 \"simulatedInteractionPort\" : 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"); + "{\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"); } } From 4bafa7c75a28f37d763b1b5638f1736adc1bc11c Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Fri, 28 Jul 2023 14:37:50 +0000 Subject: [PATCH 33/35] address comments --- .../CARMAStreetsPlugin/src/CARMAStreetsPlugin.h | 7 ++++++- src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp | 10 +++++----- src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp | 8 ++++---- .../CDASimAdapter/src/include/CDASimAdapter.hpp | 2 +- .../CDASimAdapter/src/include/CDASimConnection.hpp | 2 +- .../test/TestCARMASimulationConnection.cpp | 2 +- 6 files changed, 18 insertions(+), 13 deletions(-) diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h index 7aff1631b..9fdf4e5c7 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h @@ -49,7 +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); - void HandleSimulatedSensorDetectedMessage(simulation::SensorDetectedObject &msg, routeable_message &routeableMsg); + /** + * @brief Handler to be invoked 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. diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp index b5bc7aca2..f89cb264b 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp @@ -91,7 +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)); - uint simulated_interaction_port = std::stoul(sim::get_sim_config(sim::SIM_INTERACTION_PORT)); + uint 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); @@ -172,11 +172,11 @@ namespace CDASimAdapter{ PLOG(logDEBUG) << "Creating Thread Timer for simulated external object" << std::endl; try { - if(!external_bject_detection_thread_timer) + if(!external_object_detection_thread_timer) { - external_bject_detection_thread_timer = std::make_unique(); + external_object_detection_thread_timer = std::make_unique(); } - external_bject_detection_thread_timer->AddPeriodicTick([this](){ + 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()) { @@ -188,7 +188,7 @@ namespace CDASimAdapter{ } }//End lambda , std::chrono::milliseconds(100)); - external_bject_detection_thread_timer->Start(); + external_object_detection_thread_timer->Start(); } catch ( const UdpServerRuntimeError &e ) { diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp index fd41bb548..982ac5f64 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp @@ -132,7 +132,7 @@ 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; @@ -149,7 +149,7 @@ namespace CDASimAdapter{ } else { - throw std::runtime_error("Simulated External Object UDP Server is not initialized."); + throw UdpServerRuntimeError("Simulated External Object UDP Server is not initialized."); } return externalObj; } @@ -200,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 ""; @@ -212,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 97f569db9..529cd9dc9 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp @@ -119,7 +119,7 @@ namespace CDASimAdapter std::unique_ptr connection; // Mutex for configuration parameter thread safety std::mutex _lock; - std::unique_ptr external_bject_detection_thread_timer; + 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 3f53ca368..b6bb55424 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp @@ -85,7 +85,7 @@ namespace CDASimAdapter { 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 + * 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; diff --git a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp index 4f4c4de42..2149ce8b4 100644 --- a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp @@ -22,7 +22,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, 1215, location, sensors_file_path); } From b741588e153c347bc6cd7f173d752a6d8b2c786b Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Fri, 28 Jul 2023 14:42:33 +0000 Subject: [PATCH 34/35] adddress comments --- src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h index 9fdf4e5c7..49fde3432 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h @@ -50,7 +50,7 @@ class CARMAStreetsPlugin: public PluginClientClockAware { void HandleMobilityPathMessage(tsm2Message &msg, routeable_message &routeableMsg); void HandleBasicSafetyMessage(BsmMessage &msg, routeable_message &routeableMsg); /** - * @brief Handler to be invoked when the plugin received detected object, and forward the detected object to Kafka topic. + * @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. */ From bfdce62b04e2633d94d6464ad466de9e03c88946 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Fri, 28 Jul 2023 15:09:47 +0000 Subject: [PATCH 35/35] code smell --- src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp index f89cb264b..c662e6e9c 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp @@ -91,7 +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)); - uint simulated_interaction_port =static_cast(std::stoi(sim::get_sim_config(sim::SIM_INTERACTION_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);