diff --git a/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp b/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp index 8c065f40f..4f15fb4c9 100644 --- a/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp +++ b/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.cpp @@ -2,7 +2,7 @@ namespace tmx::utils::sim { - string SimulationExternalObjectConverter::simExternalObjToJsonStr(simulation::ExternalObject &simExternalObj) + std::string SimulationExternalObjectConverter::simExternalObjToJsonStr(tmx::messages::simulation::ExternalObject &simExternalObj) { Json::Value root; Json::Value metadata; @@ -23,7 +23,7 @@ namespace tmx::utils::sim root["header"] = header; root["id"] = simExternalObj.get_Id(); - if (simExternalObj.get_PresenceVector() != simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE) + if (simExternalObj.get_PresenceVector() != tmx::messages::simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE) { root["presence_vector"] = simExternalObj.get_PresenceVector(); } @@ -37,10 +37,8 @@ namespace tmx::utils::sim pose["pose"]["orientation"]["z"] = simExternalObj.get_PosePoseOrientationZ(); pose["pose"]["orientation"]["w"] = simExternalObj.get_PosePoseOrientationW(); auto pCovarianceV = simExternalObj.get_PoseCovariance(); - for (auto itr = pCovarianceV.begin(); itr != pCovarianceV.end(); itr++) - { - pose["covariance"].append(Json::Value(itr->covariance)); - } + std::for_each(pCovarianceV.begin(), pCovarianceV.end(), [&pose](const auto &item) + { pose["covariance"].append(Json::Value(item.covariance)); }); root["pose"] = pose; Json::Value velocity; @@ -51,10 +49,8 @@ namespace tmx::utils::sim velocity["twist"]["angular"]["y"] = simExternalObj.get_VelocityTwistAngularY(); velocity["twist"]["angular"]["z"] = simExternalObj.get_VelocityTwistAngularZ(); auto vCovarianceV = simExternalObj.get_VelocityCovariance(); - for (auto itr = vCovarianceV.begin(); itr != vCovarianceV.end(); itr++) - { - velocity["covariance"].append(Json::Value(itr->covariance)); - } + std::for_each(vCovarianceV.begin(), vCovarianceV.end(), [&velocity](const auto &item) + { velocity["covariance"].append(Json::Value(item.covariance)); }); root["velocity"] = velocity; Json::Value size; @@ -64,14 +60,14 @@ namespace tmx::utils::sim root["size"] = size; root["confidence"] = simExternalObj.get_Confidence(); - root["object_type"] = to_string(simExternalObj.get_ObjectType()); + root["object_type"] = std::to_string(simExternalObj.get_ObjectType()); root["dynamic_obj"] = simExternalObj.get_DynamticObj(); Json::StreamWriterBuilder builder; const std::string json_str = Json::writeString(builder, root); return json_str; } - void SimulationExternalObjectConverter::jsonToSimExternalObj(const string &jsonStr, simulation::ExternalObject &simExternalObj) + void SimulationExternalObjectConverter::jsonToSimExternalObj(const std::string &jsonStr, tmx::messages::simulation::ExternalObject &simExternalObj) { Json::CharReaderBuilder builder; const std::unique_ptr reader(builder.newCharReader()); @@ -79,7 +75,7 @@ namespace tmx::utils::sim JSONCPP_STRING err; if (!reader->parse(jsonStr.c_str(), jsonStr.c_str() + static_cast(jsonStr.length()), &root, &err)) { - throw runtime_error("Error parsing external object JSON string."); + throw std::runtime_error("Error parsing external object JSON string."); } /** @@ -154,7 +150,7 @@ namespace tmx::utils::sim } } - void SimulationExternalObjectConverter::populateSimExternalObjectMetadata(const Json::Value &metadataValue, simulation::ExternalObject &simExternalObj) + void SimulationExternalObjectConverter::populateSimExternalObjectMetadata(const Json::Value &metadataValue, tmx::messages::simulation::ExternalObject &simExternalObj) { if (metadataValue.isMember("is_simulation") && metadataValue["is_simulation"].isBool()) { @@ -197,7 +193,7 @@ namespace tmx::utils::sim } } - void SimulationExternalObjectConverter::populateSimExternalObjectHeader(const Json::Value &headerValue, simulation::ExternalObject &simExternalObj) + void SimulationExternalObjectConverter::populateSimExternalObjectHeader(const Json::Value &headerValue, tmx::messages::simulation::ExternalObject &simExternalObj) { if (headerValue.isMember("seq") && headerValue["seq"].isUInt()) { @@ -215,7 +211,7 @@ namespace tmx::utils::sim } } - void SimulationExternalObjectConverter::populateSimExternalObjectPose(const Json::Value &poseValue, simulation::ExternalObject &simExternalObj) + void SimulationExternalObjectConverter::populateSimExternalObjectPose(const Json::Value &poseValue, tmx::messages::simulation::ExternalObject &simExternalObj) { if (poseValue.isMember("pose") && poseValue["pose"].isMember("position") && poseValue["pose"]["position"].isObject()) { @@ -258,13 +254,13 @@ namespace tmx::utils::sim if (poseValue.isMember("covariance") && poseValue["covariance"].isArray()) { Json::Value covarianceArrayValue = poseValue["covariance"]; - std::vector covarianceV; + std::vector covarianceV; populateSimCovarianceArray(covarianceArrayValue, covarianceV); simExternalObj.set_PoseCovariance(covarianceV); } } - void SimulationExternalObjectConverter::populateSimExternalObjectVelocity(const Json::Value &velocityValue, simulation::ExternalObject &simExternalObj) + void SimulationExternalObjectConverter::populateSimExternalObjectVelocity(const Json::Value &velocityValue, tmx::messages::simulation::ExternalObject &simExternalObj) { if (velocityValue.isMember("twist") && velocityValue["twist"].isMember("linear") && velocityValue["twist"]["linear"].isObject()) { @@ -306,13 +302,13 @@ namespace tmx::utils::sim if (velocityValue.isMember("covariance") && velocityValue["covariance"].isArray()) { Json::Value covarianceArrayValue = velocityValue["covariance"]; - std::vector covarianceV; + std::vector covarianceV; populateSimCovarianceArray(covarianceArrayValue, covarianceV); simExternalObj.set_VelocityCovariance(covarianceV); } } - void SimulationExternalObjectConverter::populateSimExternalObjectSize(const Json::Value &sizeValue, simulation::ExternalObject &simExternalObj) + void SimulationExternalObjectConverter::populateSimExternalObjectSize(const Json::Value &sizeValue, tmx::messages::simulation::ExternalObject &simExternalObj) { if (sizeValue.isMember("x")) { @@ -328,80 +324,96 @@ namespace tmx::utils::sim } } - void SimulationExternalObjectConverter::populateSimCovarianceArray(const Json::Value &covarianceArrayValue, std::vector &covarianceV) + void SimulationExternalObjectConverter::populateSimCovarianceArray(const Json::Value &covarianceArrayValue, std::vector &covarianceV) { - for (auto itr = covarianceArrayValue.begin(); itr != covarianceArrayValue.end(); itr++) - { - simulation::Covariance covariance(itr->asDouble()); - covarianceV.push_back(covariance); - } + std::for_each(covarianceArrayValue.begin(), covarianceArrayValue.end(), [&covarianceV](const auto &item) + { tmx::messages::simulation::Covariance covariance(item.asDouble()); + covarianceV.push_back(covariance); }); } - simulation::OBJECT_TYPES SimulationExternalObjectConverter::objectTypeStringToEnum(const string &object_type_str) + tmx::messages::simulation::OBJECT_TYPES SimulationExternalObjectConverter::objectTypeStringToEnum(const std::string &object_type_str) { - simulation::OBJECT_TYPES object_type = simulation::OBJECT_TYPES::UNKNOWN; + tmx::messages::simulation::OBJECT_TYPES object_type = tmx::messages::simulation::OBJECT_TYPES::UNKNOWN; try { int object_type_int = stoi(object_type_str); switch (object_type_int) { - case simulation::OBJECT_TYPES::LARGE_VEHICLE: - object_type = simulation::OBJECT_TYPES::LARGE_VEHICLE; + case tmx::messages::simulation::OBJECT_TYPES::LARGE_VEHICLE: + object_type = tmx::messages::simulation::OBJECT_TYPES::LARGE_VEHICLE; break; - case simulation::OBJECT_TYPES::MOTORCYCLE: - object_type = simulation::OBJECT_TYPES::MOTORCYCLE; + + case tmx::messages::simulation::OBJECT_TYPES::MOTORCYCLE: + object_type = tmx::messages::simulation::OBJECT_TYPES::MOTORCYCLE; break; - case simulation::OBJECT_TYPES::PEDESTRIAN: - object_type = simulation::OBJECT_TYPES::PEDESTRIAN; - case simulation::OBJECT_TYPES::SMALL_VEHICLE: - object_type = simulation::OBJECT_TYPES::SMALL_VEHICLE; + + case tmx::messages::simulation::OBJECT_TYPES::PEDESTRIAN: + object_type = tmx::messages::simulation::OBJECT_TYPES::PEDESTRIAN; break; + + case tmx::messages::simulation::OBJECT_TYPES::SMALL_VEHICLE: + object_type = tmx::messages::simulation::OBJECT_TYPES::SMALL_VEHICLE; + break; + default: - object_type = simulation::OBJECT_TYPES::UNKNOWN; + object_type = tmx::messages::simulation::OBJECT_TYPES::UNKNOWN; break; } } - catch (exception &err) + catch (std::exception &err) { - object_type = simulation::OBJECT_TYPES::UNKNOWN; + object_type = tmx::messages::simulation::OBJECT_TYPES::UNKNOWN; } return object_type; } - simulation::PRESENCE_VECTOR_TYPES SimulationExternalObjectConverter::presenceVectorIntToEnum(uint16_t presence_vector) + tmx::messages::simulation::PRESENCE_VECTOR_TYPES SimulationExternalObjectConverter::presenceVectorIntToEnum(uint16_t presence_vector) { - simulation::PRESENCE_VECTOR_TYPES presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE; + tmx::messages::simulation::PRESENCE_VECTOR_TYPES presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE; switch (presence_vector) { - case simulation::PRESENCE_VECTOR_TYPES::BSM_ID_PRESENCE_VECTOR: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::BSM_ID_PRESENCE_VECTOR; + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::BSM_ID_PRESENCE_VECTOR: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::BSM_ID_PRESENCE_VECTOR; break; - case simulation::PRESENCE_VECTOR_TYPES::CONFIDENCE_PRESENCE_VECTOR: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::CONFIDENCE_PRESENCE_VECTOR; + + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::CONFIDENCE_PRESENCE_VECTOR: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::CONFIDENCE_PRESENCE_VECTOR; break; - case simulation::PRESENCE_VECTOR_TYPES::DYNAMIC_OBJ_PRESENCE: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::DYNAMIC_OBJ_PRESENCE; + + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::DYNAMIC_OBJ_PRESENCE: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::DYNAMIC_OBJ_PRESENCE; break; - case simulation::PRESENCE_VECTOR_TYPES::OBJECT_TYPE_PRESENCE_VECTOR: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::OBJECT_TYPE_PRESENCE_VECTOR; + + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::OBJECT_TYPE_PRESENCE_VECTOR: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::OBJECT_TYPE_PRESENCE_VECTOR; + break; + + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::POSE_PRESENCE_VECTOR: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::POSE_PRESENCE_VECTOR; break; - case simulation::PRESENCE_VECTOR_TYPES::POSE_PRESENCE_VECTOR: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::POSE_PRESENCE_VECTOR; + + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::PREDICTION_PRESENCE_VECTOR: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::PREDICTION_PRESENCE_VECTOR; break; - case simulation::PRESENCE_VECTOR_TYPES::PREDICTION_PRESENCE_VECTOR: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::PREDICTION_PRESENCE_VECTOR; + + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::SIZE_PRESENCE_VECTOR: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::SIZE_PRESENCE_VECTOR; break; - case simulation::PRESENCE_VECTOR_TYPES::SIZE_PRESENCE_VECTOR: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::SIZE_PRESENCE_VECTOR; + + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::VELOCITY_INST_PRESENCE_VECTOR: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::VELOCITY_INST_PRESENCE_VECTOR; break; - case simulation::PRESENCE_VECTOR_TYPES::VELOCITY_INST_PRESENCE_VECTOR: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::VELOCITY_INST_PRESENCE_VECTOR; + + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::VELOCITY_PRESENCE_VECTOR: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::VELOCITY_PRESENCE_VECTOR; break; - case simulation::PRESENCE_VECTOR_TYPES::VELOCITY_PRESENCE_VECTOR: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::VELOCITY_PRESENCE_VECTOR; + + case tmx::messages::simulation::PRESENCE_VECTOR_TYPES::ID_PRESENCE_VECTOR: + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::ID_PRESENCE_VECTOR; break; + default: - presence_vector_enum = simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE; + presence_vector_enum = tmx::messages::simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE; break; } return presence_vector_enum; diff --git a/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h b/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h index f4bfc74a1..f1357b10d 100644 --- a/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h +++ b/src/tmx/TmxUtils/src/simulation/SimulationExternalObjectConverter.h @@ -4,27 +4,25 @@ #include #include -using namespace tmx::messages; -using namespace std; namespace tmx::utils::sim { class SimulationExternalObjectConverter { private: - static void populateSimExternalObjectMetadata(const Json::Value &metadataValue, simulation::ExternalObject &simExternalObj); - static void populateSimExternalObjectHeader(const Json::Value &headerValue, simulation::ExternalObject &simExternalObj); - static void populateSimExternalObjectPose(const Json::Value &poseValue, simulation::ExternalObject &simExternalObj); - static void populateSimExternalObjectVelocity(const Json::Value &velocityValue, simulation::ExternalObject &simExternalObj); - static void populateSimExternalObjectSize(const Json::Value &sizeValue, simulation::ExternalObject &simExternalObj); - static void populateSimCovarianceArray(const Json::Value &covarianceArrayValue, std::vector &covarianceV); - static simulation::OBJECT_TYPES objectTypeStringToEnum(const string& object_type_str); - static simulation::PRESENCE_VECTOR_TYPES presenceVectorIntToEnum(uint16_t presence_vector); + static void populateSimExternalObjectMetadata(const Json::Value &metadataValue, tmx::messages::simulation::ExternalObject &simExternalObj); + static void populateSimExternalObjectHeader(const Json::Value &headerValue, tmx::messages::simulation::ExternalObject &simExternalObj); + static void populateSimExternalObjectPose(const Json::Value &poseValue, tmx::messages::simulation::ExternalObject &simExternalObj); + static void populateSimExternalObjectVelocity(const Json::Value &velocityValue, tmx::messages::simulation::ExternalObject &simExternalObj); + static void populateSimExternalObjectSize(const Json::Value &sizeValue, tmx::messages::simulation::ExternalObject &simExternalObj); + static void populateSimCovarianceArray(const Json::Value &covarianceArrayValue, std::vector &covarianceV); + static tmx::messages::simulation::OBJECT_TYPES objectTypeStringToEnum(const std::string& object_type_str); + static tmx::messages::simulation::PRESENCE_VECTOR_TYPES presenceVectorIntToEnum(uint16_t presence_vector); public: SimulationExternalObjectConverter() = default; ~SimulationExternalObjectConverter() = default; - static string simExternalObjToJsonStr(simulation::ExternalObject &simExternalObj); - static void jsonToSimExternalObj(const string &jsonStr, simulation::ExternalObject &simExternalObj); + static std::string simExternalObjToJsonStr(tmx::messages::simulation::ExternalObject &simExternalObj); + static void jsonToSimExternalObj(const std::string &jsonStr, tmx::messages::simulation::ExternalObject &simExternalObj); }; } diff --git a/src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp b/src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp index 73be4b5f9..1a9bf6587 100644 --- a/src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp +++ b/src/tmx/TmxUtils/test/test_simulation_external_object_converter.cpp @@ -5,12 +5,18 @@ using namespace tmx::utils::sim; using namespace tmx::messages; -TEST(SimulationExternalObjectConverter, jsonToSimExternalObj) +TEST(SimulationExternalObjectConverter, jsonToSimExternalObjInvalidJson) { - SimulationExternalObjectConverter converter; simulation::ExternalObject simExternalObj; - std::string json_string = "{\"metadata\":{\"is_simulation\":false,\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":0.0,\"sensor_y\":0.0,\"sensor_z\":0.0,\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":0,\"stamp\":{\"secs\":0,\"nsecs\":0}},\"id\":0,\"pose\":{\"pose\":{\"position\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"orientation\":{\"x\":0.0,\"y\":0.0,\"z\":0.0,\"w\":0.0}},\"covariance\":[12.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"velocity\":{\"twist\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"size\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"confidence\":0.0,\"object_type\":\"\",\"dynamic_obj\":false}"; - converter.jsonToSimExternalObj(json_string, simExternalObj); + std::string invalidJsonStr = "Invalid"; + ASSERT_THROW(SimulationExternalObjectConverter::jsonToSimExternalObj(invalidJsonStr, simExternalObj), std::runtime_error); +} + +TEST(SimulationExternalObjectConverter, jsonToSimExternalObjValidJson) +{ + simulation::ExternalObject simExternalObj; + std::string jsonStr = "{\"metadata\":{\"is_simulation\":false,\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":0.0,\"sensor_y\":0.0,\"sensor_z\":0.0,\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":0,\"stamp\":{\"secs\":0,\"nsecs\":0}},\"id\":0,\"pose\":{\"pose\":{\"position\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"orientation\":{\"x\":0.0,\"y\":0.0,\"z\":0.0,\"w\":0.0}},\"covariance\":[12.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"velocity\":{\"twist\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"size\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"confidence\":0.0,\"object_type\":\"\",\"dynamic_obj\":false}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); ASSERT_EQ(0, simExternalObj.get_Id()); ASSERT_EQ(false, simExternalObj.get_MetadataIsSimulation()); ASSERT_EQ("", simExternalObj.get_MetadataDatum()); @@ -30,13 +36,96 @@ TEST(SimulationExternalObjectConverter, jsonToSimExternalObj) ASSERT_EQ(36, simExternalObj.get_VelocityCovariance().size()); } +TEST(SimulationExternalObjectConverter, jsonToSimExternalObjPresenceVector) +{ + simulation::ExternalObject simExternalObj; + ASSERT_TRUE(simExternalObj.is_empty()); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE, simExternalObj.get_PresenceVector()); + ASSERT_FALSE(simExternalObj.is_empty()); + + std::string jsonStr = "{\"presence_vector\":1}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::ID_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":2}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::POSE_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":4}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::VELOCITY_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":8}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::VELOCITY_INST_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":16}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::SIZE_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":32}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::CONFIDENCE_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":64}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::OBJECT_TYPE_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":128}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::BSM_ID_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":256}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::DYNAMIC_OBJ_PRESENCE, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":512}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::PREDICTION_PRESENCE_VECTOR, simExternalObj.get_PresenceVector()); + + jsonStr = "{\"presence_vector\":212121}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::PRESENCE_VECTOR_TYPES::UNAVAILABLE, simExternalObj.get_PresenceVector()); +} + +TEST(SimulationExternalObjectConverter, jsonToSimExternalObjObjectTypes) +{ + simulation::ExternalObject simExternalObj; + ASSERT_TRUE(simExternalObj.is_empty()); + ASSERT_EQ(simulation::OBJECT_TYPES::UNKNOWN, simExternalObj.get_ObjectType()); + ASSERT_FALSE(simExternalObj.is_empty()); + + std::string jsonStr = "{\"object_type\":\"1\"}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::OBJECT_TYPES::SMALL_VEHICLE, simExternalObj.get_ObjectType()); + + jsonStr = "{\"object_type\":\"2\"}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::OBJECT_TYPES::LARGE_VEHICLE, simExternalObj.get_ObjectType()); + + jsonStr = "{\"object_type\":\"3\"}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::OBJECT_TYPES::MOTORCYCLE, simExternalObj.get_ObjectType()); + + jsonStr = "{\"object_type\":\"5\"}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::OBJECT_TYPES::UNKNOWN, simExternalObj.get_ObjectType()); + + jsonStr = "{\"object_type\":\"4\"}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::OBJECT_TYPES::PEDESTRIAN, simExternalObj.get_ObjectType()); + + jsonStr = "{\"object_type\":\"invalid\"}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + ASSERT_EQ(simulation::OBJECT_TYPES::UNKNOWN, simExternalObj.get_ObjectType()); +} + TEST(SimulationExternalObjectConverter, simExternalObjToJsonStr) { - SimulationExternalObjectConverter converter; simulation::ExternalObject simExternalObj; - std::string json_string = "{\"metadata\":{\"is_simulation\":false,\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":0.0,\"sensor_y\":0.0,\"sensor_z\":0.0,\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":0,\"stamp\":{\"secs\":0,\"nsecs\":0}},\"id\":0,\"pose\":{\"pose\":{\"position\":{\"x\":34.0,\"y\":0.0,\"z\":0.0},\"orientation\":{\"x\":23.0,\"y\":0.0,\"z\":0.0,\"w\":0.0}},\"covariance\":[12.1,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,11.0]},\"velocity\":{\"twist\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"covariance\":[12.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"size\":{\"x\":12.0,\"y\":23.0,\"z\":12.0},\"confidence\":1.0,\"object_type\":\"128\",\"dynamic_obj\":false}"; - converter.jsonToSimExternalObj(json_string, simExternalObj); - std::string output = converter.simExternalObjToJsonStr(simExternalObj); + std::string jsonStr = "{\"metadata\":{\"is_simulation\":false,\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":0.0,\"sensor_y\":0.0,\"sensor_z\":0.0,\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":0,\"stamp\":{\"secs\":0,\"nsecs\":0}},\"id\":0,\"pose\":{\"pose\":{\"position\":{\"x\":34.0,\"y\":0.0,\"z\":0.0},\"orientation\":{\"x\":23.0,\"y\":0.0,\"z\":0.0,\"w\":0.0}},\"covariance\":[12.1,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,11.0]},\"velocity\":{\"twist\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"covariance\":[12.0,17.33333333,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,20.3333]},\"size\":{\"x\":12.0,\"y\":23.0,\"z\":12.0},\"confidence\":1.0,\"object_type\":\"128\",\"dynamic_obj\":false}"; + SimulationExternalObjectConverter::jsonToSimExternalObj(jsonStr, simExternalObj); + std::string output = SimulationExternalObjectConverter::simExternalObjToJsonStr(simExternalObj); Json::CharReaderBuilder builder; const std::unique_ptr reader(builder.newCharReader()); Json::Value root; @@ -45,4 +134,6 @@ TEST(SimulationExternalObjectConverter, simExternalObjToJsonStr) ASSERT_FALSE(root["metadata"]["is_simulation"].asBool()); ASSERT_EQ(12.1, root["pose"]["covariance"].begin()->asDouble()); ASSERT_EQ(23, root["pose"]["pose"]["orientation"]["x"].asDouble()); + ASSERT_EQ(12, root["velocity"]["covariance"].begin()->asDouble()); + ASSERT_EQ(17.33333333, root["velocity"]["covariance"][1].asDouble()); } \ No newline at end of file diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index 44529f2c8..3809cf6ba 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -631,8 +631,7 @@ void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){ void CARMAStreetsPlugin::HandleSimulatedExternalMessage(ExternalObject &msg, routeable_message &routeableMsg) { - tmx::utils::sim::SimulationExternalObjectConverter converter; - auto json_str = converter.simExternalObjToJsonStr(msg); + auto json_str = tmx::utils::sim::SimulationExternalObjectConverter::simExternalObjToJsonStr(msg); PLOG(logINFO) << "Produce External Object Message in JSON format: " << json_str < #include +#include #include #include @@ -11,14 +12,12 @@ TEST(SimulationMessages, ExternalObjectToRoutableMessage) { simulation::ExternalObject externalObj; string expectedStr = "{\"metadata\":{\"is_simulation\":false,\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":0.0,\"sensor_y\":0.0,\"sensor_z\":0.0,\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":0,\"stamp\":{\"secs\":0,\"nsecs\":0}},\"id\":0,\"pose\":{\"pose\":{\"position\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"orientation\":{\"x\":0.0,\"y\":0.0,\"z\":0.0,\"w\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"velocity\":{\"twist\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"size\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"confidence\":0.0,\"object_type\":\"\",\"dynamic_obj\":false}"; - externalObj.set_contents(expectedStr); - ASSERT_EQ(expectedStr, externalObj.to_string()); + tmx::utils::sim::SimulationExternalObjectConverter converter; + converter.jsonToSimExternalObj(expectedStr, externalObj); ASSERT_EQ("ExternalObject", std::string(simulation::ExternalObject::MessageSubType)); ASSERT_EQ("Simulation", std::string(simulation::ExternalObject::MessageType)); tmx::routeable_message routeableMsg; - routeableMsg.initialize(externalObj, "CDASimAdapter", 0 , IvpMsgFlags_None); - string output = "{\"metadata\":{\"is_simulation\":\"false\",\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":\"0.0\",\"sensor_y\":\"0.0\",\"sensor_z\":\"0.0\",\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":\"0\",\"stamp\":{\"secs\":\"0\",\"nsecs\":\"0\"}},\"id\":\"0\",\"pose\":{\"pose\":{\"position\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\"},\"orientation\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\",\"w\":\"0.0\"}},\"covariance\":[\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\"]},\"velocity\":{\"twist\":{\"linear\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\"},\"angular\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\"}},\"covariance\":[\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\"]},\"size\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\"},\"confidence\":\"0.0\",\"object_type\":\"\",\"dynamic_obj\":\"false\"}"; - ASSERT_EQ(output, routeableMsg.get_payload_str()); + routeableMsg.initialize(externalObj, "CDASimAdapter", 0, IvpMsgFlags_None); ASSERT_EQ("json", routeableMsg.get_encoding()); ASSERT_EQ(0, routeableMsg.get_flags()); auto current_time_mill = boost::chrono::duration_cast(boost::chrono::system_clock::now().time_since_epoch()).count();