From f75137d15f637b83fd2e521208ce4b0f37cdb91e Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Mon, 24 Jul 2023 22:16:07 +0000 Subject: [PATCH] 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