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;