diff --git a/CMakeLists.txt b/CMakeLists.txt index 2588da6..9f194e6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ INCLUDE(CheckPIESupported) CHECK_PIE_SUPPORTED() SET(CMAKE_POSITION_INDEPENDENT_CODE ON) -SET(MISSION_MODULE_VERSION 1.2.11) +SET(MISSION_MODULE_VERSION 2.0.0) OPTION(BRINGAUTO_INSTALL "Configure install" OFF) OPTION(BRINGAUTO_PACKAGE "Configure package creation" OFF) @@ -54,12 +54,13 @@ IF (NOT BRINGAUTO_SYSTEM_DEP) ENDIF () ENDIF () +FIND_PACKAGE(nlohmann_json 3.10.5 REQUIRED) FIND_PACKAGE(Protobuf 3.21.12 REQUIRED) FIND_PACKAGE(fleet-protocol-interface 2.0.0 REQUIRED) FIND_PACKAGE(fleet-protocol-cxx-helpers-static 1.1.1 REQUIRED) IF (FLEET_PROTOCOL_BUILD_EXTERNAL_SERVER) - FIND_PACKAGE(fleet-http-client-shared 1.5.0 REQUIRED) + FIND_PACKAGE(fleet-http-client-shared 2.0.0 REQUIRED) ### fleet-http-client dependencies FIND_PACKAGE(Boost REQUIRED) FIND_PACKAGE(cpprestsdk REQUIRED) @@ -71,18 +72,18 @@ CMDEF_ADD_LIBRARY( VERSION ${MISSION_MODULE_VERSION} ) -# Protobuf mission payload +# Protobuf, JSON mission payload SET(Protobuf_USE_STATIC_LIBS ON) FILE(GLOB_RECURSE protobuf_mission_cpp_files "lib/protobuf-mission-module/*") -ADD_LIBRARY(protobuf_mission_lib STATIC ${protobuf_mission_cpp_files}) -TARGET_INCLUDE_DIRECTORIES(protobuf_mission_lib PUBLIC "lib/protobuf-mission-module/") -TARGET_LINK_LIBRARIES(protobuf_mission_lib PUBLIC protobuf::libprotobuf) +ADD_LIBRARY(message_handler_lib STATIC ${protobuf_mission_cpp_files}) +TARGET_INCLUDE_DIRECTORIES(message_handler_lib PUBLIC "lib/protobuf-mission-module/") +TARGET_LINK_LIBRARIES(message_handler_lib PUBLIC protobuf::libprotobuf nlohmann_json::nlohmann_json) FILE(GLOB_RECURSE HEADERS "include/*") ADD_LIBRARY(mission_module_hpp INTERFACE ${HEADERS}) TARGET_INCLUDE_DIRECTORIES(mission_module_hpp INTERFACE "include/") -TARGET_LINK_LIBRARIES(mission_module_hpp INTERFACE protobuf_mission_lib) +TARGET_LINK_LIBRARIES(mission_module_hpp INTERFACE message_handler_lib) FILE(GLOB_RECURSE SOURCES "source/bringauto/*") @@ -92,7 +93,7 @@ TARGET_LINK_LIBRARIES(mission_module_sources PUBLIC mission_module_hpp fleet-protocol-interface::common-headers-interface fleet-protocol-cxx-helpers-static::fleet-protocol-cxx-helpers-static - protobuf_mission_lib + message_handler_lib ) IF (FLEET_PROTOCOL_BUILD_MODULE_GATEWAY) diff --git a/cmake/Dependencies.cmake b/cmake/Dependencies.cmake index e594cd1..b3a2802 100644 --- a/cmake/Dependencies.cmake +++ b/cmake/Dependencies.cmake @@ -1,12 +1,13 @@ SET(CMAKE_FIND_USE_CMAKE_SYSTEM_PATH FALSE) -BA_PACKAGE_LIBRARY(fleet-protocol-interface v2.0.0 PLATFORM_STRING_MODE any_machine NO_DEBUG ON) -BA_PACKAGE_LIBRARY(fleet-protocol-cxx-helpers-static v1.1.1) -BA_PACKAGE_LIBRARY(protobuf v4.21.12) -BA_PACKAGE_LIBRARY(zlib v1.2.11) +BA_PACKAGE_LIBRARY(fleet-protocol-interface v2.0.0 PLATFORM_STRING_MODE any_machine NO_DEBUG ON) +BA_PACKAGE_LIBRARY(fleet-protocol-cxx-helpers-static v1.1.1) +BA_PACKAGE_LIBRARY(zlib v1.2.11) +BA_PACKAGE_LIBRARY(nlohmann-json v3.10.5 PLATFORM_STRING_MODE any_machine NO_DEBUG ON) +BA_PACKAGE_LIBRARY(protobuf v4.21.12) IF (FLEET_PROTOCOL_BUILD_EXTERNAL_SERVER) - BA_PACKAGE_LIBRARY(fleet-http-client-shared v1.5.0) + BA_PACKAGE_LIBRARY(fleet-http-client-shared v2.0.0) BA_PACKAGE_LIBRARY(boost v1.86.0) BA_PACKAGE_LIBRARY(cpprestsdk v2.10.20) ENDIF () diff --git a/include/bringauto/ba_json/JsonHelper.hpp b/include/bringauto/ba_json/JsonHelper.hpp new file mode 100644 index 0000000..408ea67 --- /dev/null +++ b/include/bringauto/ba_json/JsonHelper.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include + +#include + +#include + +namespace bringauto::ba_json { + +using json = nlohmann::ordered_json; + +class JsonHelper { +public: + static int bufferToJson(json& json, const buffer& buffer); + + static int jsonToBuffer(buffer* buffer, const json& json); + + static MissionModule::AutonomyStatus_State stringToAutonomyState(const std::string_view &state); + + static std::string autonomyStateToString(MissionModule::AutonomyStatus_State state); + + static std::string autonomyActionToString(MissionModule::AutonomyCommand_Action action); + +}; +} diff --git a/include/bringauto/modules/mission_module/Constants.hpp b/include/bringauto/modules/mission_module/Constants.hpp index 24f2e10..235fbbe 100644 --- a/include/bringauto/modules/mission_module/Constants.hpp +++ b/include/bringauto/modules/mission_module/Constants.hpp @@ -18,7 +18,7 @@ class Constants { /** * @brief Minimum time between two status messages being sent when speed is above the threshold */ - static constexpr std::chrono::milliseconds status_sending_period = std::chrono::milliseconds(2900); + static constexpr auto status_sending_period = std::chrono::milliseconds(2900); }; -} \ No newline at end of file +} diff --git a/include/bringauto/modules/mission_module/Context.hpp b/include/bringauto/modules/mission_module/Context.hpp index 394974b..525429b 100644 --- a/include/bringauto/modules/mission_module/Context.hpp +++ b/include/bringauto/modules/mission_module/Context.hpp @@ -5,14 +5,13 @@ #include #include -#include namespace bringauto::modules::mission_module { struct Context { - std::shared_ptr fleet_api_client; + std::shared_ptr fleet_api_client; std::vector devices; - std::vector> command_vector; + std::vector> command_vector; std::mutex mutex; std::condition_variable con_variable; long last_command_timestamp; diff --git a/include/bringauto/modules/mission_module/MissionModule.hpp b/include/bringauto/modules/mission_module/MissionModule.hpp index 48fc49f..cfe5a6f 100644 --- a/include/bringauto/modules/mission_module/MissionModule.hpp +++ b/include/bringauto/modules/mission_module/MissionModule.hpp @@ -2,7 +2,7 @@ namespace bringauto::modules::mission_module { - static const int MISSION_MODULE_NUMBER = 1; + static constexpr int MISSION_MODULE_NUMBER = 1; - static const int AUTONOMY_DEVICE_TYPE = 1; -} \ No newline at end of file + static constexpr int AUTONOMY_DEVICE_TYPE = 1; +} diff --git a/include/bringauto/modules/mission_module/devices/AutonomyDevice.hpp b/include/bringauto/modules/mission_module/devices/AutonomyDevice.hpp index 749e6a2..730962b 100644 --- a/include/bringauto/modules/mission_module/devices/AutonomyDevice.hpp +++ b/include/bringauto/modules/mission_module/devices/AutonomyDevice.hpp @@ -1,8 +1,8 @@ #pragma once -#include #include +#include #include namespace bringauto::modules::mission_module::devices { @@ -90,13 +90,10 @@ class AutonomyDevice { */ static int command_data_valid(const struct buffer command); private: - static MissionModule::AutonomyCommand generateCommand(std::vector stops, std::string route, - MissionModule::AutonomyCommand::Action action); - /** * @brief Map of last sent status timestamps for each device type */ static std::map last_sent_status_timestamps_; }; -} \ No newline at end of file +} diff --git a/include/bringauto/protobuf/ProtobufHelper.hpp b/include/bringauto/protobuf/ProtobufHelper.hpp index 74405be..4fca370 100644 --- a/include/bringauto/protobuf/ProtobufHelper.hpp +++ b/include/bringauto/protobuf/ProtobufHelper.hpp @@ -8,12 +8,12 @@ class ProtobufHelper { public: static int serializeProtobufMessageToBuffer(struct buffer* message, const google::protobuf::Message &protobufMessage); - static MissionModule::AutonomyStatus parseAutonomyStatus(struct buffer status); + static int validateAutonomyStatus(const std::string &status); - static MissionModule::AutonomyCommand parseAutonomyCommand(struct buffer command); + static int validateAutonomyCommand(const std::string &command); - static MissionModule::AutonomyError parseAutonomyError(struct buffer errorMessage); + static int validateAutonomyError(const std::string &errorMessage); }; -} \ No newline at end of file +} diff --git a/source/bringauto/ba_json/JsonHelper.cpp b/source/bringauto/ba_json/JsonHelper.cpp new file mode 100644 index 0000000..aa6d8ad --- /dev/null +++ b/source/bringauto/ba_json/JsonHelper.cpp @@ -0,0 +1,70 @@ +#include + +#include + + +namespace bringauto::ba_json { + +using json = nlohmann::ordered_json; + +int JsonHelper::bufferToJson(json& json, const buffer& buffer) { + const auto buffer_data = static_cast (buffer.data); + try { + json = json::parse(buffer_data, buffer_data + buffer.size_in_bytes); + } catch (json::parse_error &) { + return NOT_OK; + } + return OK; +} + +int JsonHelper::jsonToBuffer(buffer *buffer, const json& json) { + const std::string tmp = nlohmann::to_string(json); + if (allocate(buffer, tmp.size()) == NOT_OK) { + return NOT_OK; + } + std::memcpy(buffer->data, tmp.c_str(), tmp.size()); + return OK; +} + +MissionModule::AutonomyStatus_State JsonHelper::stringToAutonomyState(const std::string_view &state) { + if (state == "IDLE") { + return MissionModule::AutonomyStatus_State::AutonomyStatus_State_IDLE; + } else if (state == "DRIVE") { + return MissionModule::AutonomyStatus_State::AutonomyStatus_State_DRIVE; + } else if (state == "IN_STOP") { + return MissionModule::AutonomyStatus_State::AutonomyStatus_State_IN_STOP; + } else if (state == "OBSTACLE") { + return MissionModule::AutonomyStatus_State::AutonomyStatus_State_OBSTACLE; + } else if (state == "ERROR") { + return MissionModule::AutonomyStatus_State::AutonomyStatus_State_ERROR; + } + return MissionModule::AutonomyStatus_State::AutonomyStatus_State_ERROR; +} + +std::string JsonHelper::autonomyStateToString(const MissionModule::AutonomyStatus_State state) { + switch (state) { + case MissionModule::AutonomyStatus_State::AutonomyStatus_State_IDLE: + return "IDLE"; + case MissionModule::AutonomyStatus_State::AutonomyStatus_State_DRIVE: + return "DRIVE"; + case MissionModule::AutonomyStatus_State::AutonomyStatus_State_IN_STOP: + return "IN_STOP"; + case MissionModule::AutonomyStatus_State::AutonomyStatus_State_OBSTACLE: + return "OBSTACLE"; + default: + return "ERROR"; + } +} + +std::string JsonHelper::autonomyActionToString(const MissionModule::AutonomyCommand_Action action) { + switch (action) { + case MissionModule::AutonomyCommand_Action::AutonomyCommand_Action_STOP: + return "STOP"; + case MissionModule::AutonomyCommand_Action::AutonomyCommand_Action_START: + return "DRIVE"; + default: + return "NO_ACTION"; + } +} + +} diff --git a/source/bringauto/modules/mission_module/devices/AutonomyDevice.cpp b/source/bringauto/modules/mission_module/devices/AutonomyDevice.cpp index 56b8859..58d6535 100644 --- a/source/bringauto/modules/mission_module/devices/AutonomyDevice.cpp +++ b/source/bringauto/modules/mission_module/devices/AutonomyDevice.cpp @@ -1,30 +1,37 @@ -#include -#include +#include +#include +#include #include #include -#include -#include + +#include namespace bringauto::modules::mission_module::devices { -namespace bamm = bringauto::modules::mission_module; +using json = nlohmann::ordered_json; std::map AutonomyDevice::last_sent_status_timestamps_ {}; int AutonomyDevice::send_status_condition(const struct buffer current_status, const struct buffer new_status, unsigned int device_type) { - auto currentAutonomyStatus = protobuf::ProtobufHelper::parseAutonomyStatus(current_status); - auto newAutonomyStatus = protobuf::ProtobufHelper::parseAutonomyStatus(new_status); + json current_status_json {}; + json new_status_json {}; + if (ba_json::JsonHelper::bufferToJson(current_status_json, current_status) != OK || + ba_json::JsonHelper::bufferToJson(new_status_json, new_status) != OK) { + return NOT_OK; + } - if (currentAutonomyStatus.state() != newAutonomyStatus.state() - || !google::protobuf::util::MessageDifferencer::Equals(currentAutonomyStatus.nextstop(), newAutonomyStatus.nextstop())) { + if (current_status_json.at("state") != new_status_json.at("state") || + current_status_json.at("nextStop") != new_status_json.at("nextStop")) { return OK; - } else if (newAutonomyStatus.telemetry().speed() >= bamm::Constants::status_speed_threshold) { + } + + if (new_status_json.at("telemetry").at("speed") >= Constants::status_speed_threshold) { auto current_time = std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch()); - if (last_sent_status_timestamps_[device_type] + bamm::Constants::status_sending_period < current_time) { + if (last_sent_status_timestamps_[device_type] + Constants::status_sending_period < current_time) { last_sent_status_timestamps_[device_type] = current_time; return OK; } @@ -34,18 +41,23 @@ int AutonomyDevice::send_status_condition(const struct buffer current_status, co int AutonomyDevice::generate_command(struct buffer *generated_command, const struct buffer new_status, const struct buffer current_status, const struct buffer current_command) { - auto currentAutonomyStatus = protobuf::ProtobufHelper::parseAutonomyStatus(current_status); - auto newAutonomyStatus = protobuf::ProtobufHelper::parseAutonomyStatus(new_status); - auto currentAutonomyCommand = protobuf::ProtobufHelper::parseAutonomyCommand(current_command); - - if (currentAutonomyStatus.state() == MissionModule::AutonomyStatus_State_DRIVE && - newAutonomyStatus.state() == MissionModule::AutonomyStatus_State_IN_STOP) { - auto* stations = currentAutonomyCommand.mutable_stops(); - if (stations->size() > 0) { - stations->erase(stations->begin()); - } + json current_status_json {}; + json new_status_json {}; + json current_command_json {}; + if (ba_json::JsonHelper::bufferToJson(current_status_json, current_status) != OK || + ba_json::JsonHelper::bufferToJson(new_status_json, new_status) != OK || + ba_json::JsonHelper::bufferToJson(current_command_json, current_command) != OK) { + return NOT_OK; + } + + if (ba_json::JsonHelper::stringToAutonomyState(std::string(current_status_json.at("state"))) == + MissionModule::AutonomyStatus_State_DRIVE && + ba_json::JsonHelper::stringToAutonomyState(std::string(new_status_json.at("state"))) == + MissionModule::AutonomyStatus_State_IN_STOP && + !current_command_json.at("stops").empty()) { + current_command_json.at("stops").erase(current_command_json.at("stops").begin()); } - return protobuf::ProtobufHelper::serializeProtobufMessageToBuffer(generated_command, currentAutonomyCommand); + return ba_json::JsonHelper::jsonToBuffer(generated_command, current_command_json); } int AutonomyDevice::aggregate_status(struct buffer *aggregated_status, const struct buffer current_status, @@ -59,62 +71,57 @@ int AutonomyDevice::aggregate_status(struct buffer *aggregated_status, const str int AutonomyDevice::aggregate_error(struct buffer *error_message, const struct buffer current_error_message, const struct buffer status) { - auto autonomyError = protobuf::ProtobufHelper::parseAutonomyError(current_error_message); - auto autonomyStatus = protobuf::ProtobufHelper::parseAutonomyStatus(status); - - if (autonomyStatus.state() == MissionModule::AutonomyStatus_State_IN_STOP) { - const auto & finishedStops = autonomyError.finishedstops(); - if (!finishedStops.empty()) { - if (!google::protobuf::util::MessageDifferencer::Equals(finishedStops[finishedStops.size() -1], autonomyStatus.nextstop())) { - autonomyError.add_finishedstops()->CopyFrom(autonomyStatus.nextstop()); + json status_json {}; + json error_json {}; + if (ba_json::JsonHelper::bufferToJson(status_json, status) != OK || + ba_json::JsonHelper::bufferToJson(error_json, current_error_message) != OK) { + return NOT_OK; + } + + if (status_json.at("state") == ba_json::JsonHelper::autonomyStateToString(MissionModule::AutonomyStatus_State_IN_STOP)) { + if (!error_json.at("finishedStops").empty()) { + if (error_json.at("finishedStops")[error_json.at("finishedStops").size() - 1] != status_json.at("nextStop")) { + error_json.at("finishedStops").push_back(status_json.at("nextStop")); } } else { - autonomyError.add_finishedstops()->CopyFrom(autonomyStatus.nextstop()); + error_json.at("finishedStops").push_back(status_json.at("nextStop")); } } - return protobuf::ProtobufHelper::serializeProtobufMessageToBuffer(error_message, autonomyError); + return ba_json::JsonHelper::jsonToBuffer(error_message, error_json); } int AutonomyDevice::generate_first_command(struct buffer *default_command) { - MissionModule::AutonomyCommand command = generateCommand(std::vector(), "", - MissionModule::AutonomyCommand_Action_NO_ACTION); - - if (protobuf::ProtobufHelper::serializeProtobufMessageToBuffer(default_command, command) != OK) { + json command {}; + command["action"] = ba_json::JsonHelper::autonomyActionToString(MissionModule::AutonomyCommand_Action_NO_ACTION); + command["route"] = ""; + command["stops"] = json::array(); + if (ba_json::JsonHelper::jsonToBuffer(default_command, command) != OK) { return NOT_OK; } return OK; } int AutonomyDevice::status_data_valid(const struct buffer status) { - try { - protobuf::ProtobufHelper::parseAutonomyStatus(status); + json status_json {}; + if (ba_json::JsonHelper::bufferToJson(status_json, status) != OK) { + return NOT_OK; } - catch (...) { + if (protobuf::ProtobufHelper::validateAutonomyStatus(nlohmann::to_string(status_json)) != OK) { return NOT_OK; } return OK; } int AutonomyDevice::command_data_valid(const struct buffer command) { - try { - protobuf::ProtobufHelper::parseAutonomyCommand(command); + json command_json {}; + if (ba_json::JsonHelper::bufferToJson(command_json, command) != OK) { + return NOT_OK; } - catch (...) { + if (protobuf::ProtobufHelper::validateAutonomyCommand(nlohmann::to_string(command_json)) != OK) { return NOT_OK; } return OK; } -MissionModule::AutonomyCommand -AutonomyDevice::generateCommand(std::vector stops, std::string route, - MissionModule::AutonomyCommand::Action action) { - MissionModule::AutonomyCommand command; - auto stopsField = command.mutable_stops(); - stopsField->Add(stops.begin(), stops.end()); - command.set_route(route); - command.set_action(action); - return command; } - -} \ No newline at end of file diff --git a/source/bringauto/protobuf/ProtobufHelper.cpp b/source/bringauto/protobuf/ProtobufHelper.cpp index 38028eb..52ac535 100644 --- a/source/bringauto/protobuf/ProtobufHelper.cpp +++ b/source/bringauto/protobuf/ProtobufHelper.cpp @@ -2,31 +2,47 @@ #include +#include + namespace bringauto::protobuf { int ProtobufHelper::serializeProtobufMessageToBuffer(struct buffer* message, const google::protobuf::Message &protobufMessage) { - if ((allocate(message, protobufMessage.ByteSizeLong())) == OK) { - protobufMessage.SerializeToArray(message->data, (int)message->size_in_bytes); + if (allocate(message, protobufMessage.ByteSizeLong()) == OK) { + protobufMessage.SerializeToArray(message->data, static_cast(message->size_in_bytes)); return OK; } return NOT_OK; } -MissionModule::AutonomyStatus ProtobufHelper::parseAutonomyStatus(struct buffer status) { - MissionModule::AutonomyStatus autonomyStatus; - autonomyStatus.ParseFromArray(status.data, status.size_in_bytes); - return autonomyStatus; +int ProtobufHelper::validateAutonomyStatus(const std::string &status) { + MissionModule::AutonomyStatus autonomyStatus {}; + const auto parse_status = google::protobuf::util::JsonStringToMessage( + status, &autonomyStatus + ); + if(!parse_status.ok()) { + return NOT_OK; + } + return OK; } -MissionModule::AutonomyCommand ProtobufHelper::parseAutonomyCommand(struct buffer command) { - MissionModule::AutonomyCommand autonomyCommand; - autonomyCommand.ParseFromArray(command.data, command.size_in_bytes); - return autonomyCommand; +int ProtobufHelper::validateAutonomyCommand(const std::string &command) { + MissionModule::AutonomyCommand autonomyCommand {}; + const auto parse_status = google::protobuf::util::JsonStringToMessage( + command, &autonomyCommand + ); + if(!parse_status.ok()) { + return NOT_OK; + } + return OK; } -MissionModule::AutonomyError ProtobufHelper::parseAutonomyError(struct buffer errorMessage) { - MissionModule::AutonomyError autonomyError; - autonomyError.ParseFromArray(errorMessage.data, errorMessage.size_in_bytes); - return autonomyError; +int ProtobufHelper::validateAutonomyError(const std::string &errorMessage) { + MissionModule::AutonomyError autonomyError {}; + const auto parse_status = google::protobuf::util::JsonStringToMessage( + errorMessage, &autonomyError + ); + if(!parse_status.ok()) { + return NOT_OK; + } + return OK; } } - diff --git a/source/device_management.cpp b/source/device_management.cpp index 718b98a..c591db2 100644 --- a/source/device_management.cpp +++ b/source/device_management.cpp @@ -12,4 +12,3 @@ int is_device_type_supported(unsigned int device_type) { return NOT_OK; } } - diff --git a/source/external_server_api.cpp b/source/external_server_api.cpp index a6cdfaf..a94163a 100644 --- a/source/external_server_api.cpp +++ b/source/external_server_api.cpp @@ -7,12 +7,9 @@ #include #include #include -#include #include #include -#include -#include #include @@ -20,17 +17,17 @@ namespace bamm = bringauto::modules::mission_module; void *init(const config config_data) { auto *context = new bamm::Context {}; - bringauto::fleet_protocol::cxx::KeyValueConfig config(config_data); - std::string api_url; - std::string api_key; - std::string company_name; - std::string car_name; - int max_requests_threshold_count; - int max_requests_threshold_period_ms; - int delay_after_threshold_reached_ms; - int retry_requests_delay_ms; - - for (auto i = config.cbegin(); i != config.cend(); i++) { + const bringauto::fleet_protocol::cxx::KeyValueConfig config(config_data); + std::string api_url {}; + std::string api_key {}; + std::string company_name {}; + std::string car_name {}; + int max_requests_threshold_count {}; + int max_requests_threshold_period_ms {}; + int delay_after_threshold_reached_ms {}; + int retry_requests_delay_ms {}; + + for (auto i = config.cbegin(); i != config.cend(); ++i) { if (i->first == "api_url") { if (!std::regex_match(i->second, std::regex(R"(^(http|https)://([\w-]+\.)?+[\w-]+(:[0-9]+)?(/[\w-]*)?+$)"))) { delete context; @@ -60,45 +57,29 @@ void *init(const config config_data) { car_name = i->second; } else if (i->first == "max_requests_threshold_count") { - try { - max_requests_threshold_count = std::stoi(i->second); - if (max_requests_threshold_count < 0 || i->second.empty()) { - throw std::exception(); - } - } catch (std::exception& e) { + auto result = std::from_chars(i->second.data(), i->second.data() + i->second.size(), max_requests_threshold_count); + if (result.ec != std::errc() || max_requests_threshold_count < 0 || i->second.empty()) { delete context; return nullptr; } } else if (i->first == "max_requests_threshold_period_ms") { - try { - max_requests_threshold_period_ms = std::stoi(i->second); - if (max_requests_threshold_period_ms < 0 || i->second.empty()) { - throw std::exception(); - } - } catch (std::exception& e) { + auto result = std::from_chars(i->second.data(), i->second.data() + i->second.size(), max_requests_threshold_period_ms); + if (result.ec != std::errc() || max_requests_threshold_period_ms < 0 || i->second.empty()) { delete context; return nullptr; } } else if (i->first == "delay_after_threshold_reached_ms") { - try { - delay_after_threshold_reached_ms = std::stoi(i->second); - if (delay_after_threshold_reached_ms < 0 || i->second.empty()) { - throw std::exception(); - } - } catch (std::exception& e) { + auto result = std::from_chars(i->second.data(), i->second.data() + i->second.size(), delay_after_threshold_reached_ms); + if (result.ec != std::errc() || delay_after_threshold_reached_ms < 0 || i->second.empty()) { delete context; return nullptr; } } else if (i->first == "retry_requests_delay_ms") { - try { - retry_requests_delay_ms = std::stoi(i->second); - if (retry_requests_delay_ms < 0 || i->second.empty()) { - throw std::exception(); - } - } catch (std::exception& e) { + auto result = std::from_chars(i->second.data(), i->second.data() + i->second.size(), retry_requests_delay_ms); + if (result.ec != std::errc() || retry_requests_delay_ms < 0 || i->second.empty()) { delete context; return nullptr; } @@ -131,7 +112,7 @@ int destroy(void **context) { if(*context == nullptr){ return NOT_OK; } - auto con = reinterpret_cast (context); + const auto con = reinterpret_cast (context); delete *con; *con = nullptr; @@ -143,17 +124,17 @@ int forward_status(const buffer device_status, const device_identification devic return CONTEXT_INCORRECT; } - auto con = static_cast (context); + const auto con = static_cast (context); if(device.device_type == bamm::AUTONOMY_DEVICE_TYPE) { - std::string device_status_str; - auto device_status_parsed = bringauto::protobuf::ProtobufHelper::parseAutonomyStatus(device_status); - auto protobuf_options = google::protobuf::util::JsonPrintOptions(); - protobuf_options.always_print_primitive_fields = true; - google::protobuf::util::MessageToJsonString(device_status_parsed, &device_status_str, protobuf_options); - - bringauto::fleet_protocol::cxx::BufferAsString device_role(&device.device_role); - bringauto::fleet_protocol::cxx::BufferAsString device_name(&device.device_name); + const bringauto::fleet_protocol::cxx::BufferAsString device_status_bas(&device_status); + const auto device_status_str = std::string(device_status_bas.getStringView()); + if (bringauto::protobuf::ProtobufHelper::validateAutonomyStatus(device_status_str) != OK) { + return NOT_OK; + } + + const bringauto::fleet_protocol::cxx::BufferAsString device_role(&device.device_role); + const bringauto::fleet_protocol::cxx::BufferAsString device_name(&device.device_name); con->fleet_api_client->setDeviceIdentification( bringauto::fleet_protocol::cxx::DeviceID( device.module, @@ -164,9 +145,8 @@ int forward_status(const buffer device_status, const device_identification devic ) ); - try { - con->fleet_api_client->sendStatus(device_status_str); - } catch (std::exception& e) { + auto rc = con->fleet_api_client->sendStatus(device_status_str); + if(rc != bringauto::fleet_protocol::http_client::FleetApiClient::ReturnCode::OK) { return NOT_OK; } @@ -181,17 +161,17 @@ int forward_error_message(const buffer error_msg, const device_identification de return CONTEXT_INCORRECT; } - auto con = static_cast (context); + const auto con = static_cast (context); if(device.device_type == bamm::AUTONOMY_DEVICE_TYPE) { - std::string error_msg_str; - auto error_msg_parsed = bringauto::protobuf::ProtobufHelper::parseAutonomyError(error_msg); - auto protobuf_options = google::protobuf::util::JsonPrintOptions(); - protobuf_options.always_print_primitive_fields = true; - google::protobuf::util::MessageToJsonString(error_msg_parsed, &error_msg_str, protobuf_options); - - bringauto::fleet_protocol::cxx::BufferAsString device_role(&device.device_role); - bringauto::fleet_protocol::cxx::BufferAsString device_name(&device.device_name); + const bringauto::fleet_protocol::cxx::BufferAsString error_msg_bas(&error_msg); + const auto error_msg_str = std::string(error_msg_bas.getStringView()); + if (bringauto::protobuf::ProtobufHelper::validateAutonomyError(error_msg_str) != OK) { + return NOT_OK; + } + + const bringauto::fleet_protocol::cxx::BufferAsString device_role(&device.device_role); + const bringauto::fleet_protocol::cxx::BufferAsString device_name(&device.device_name); con->fleet_api_client->setDeviceIdentification( bringauto::fleet_protocol::cxx::DeviceID( device.module, @@ -202,11 +182,10 @@ int forward_error_message(const buffer error_msg, const device_identification de ) ); - try { - con->fleet_api_client->sendStatus( - error_msg_str, bringauto::fleet_protocol::http_client::FleetApiClient::StatusType::STATUS_ERROR - ); - } catch (std::exception& e) { + auto rc = con->fleet_api_client->sendStatus( + error_msg_str, bringauto::fleet_protocol::http_client::FleetApiClient::StatusType::STATUS_ERROR + ); + if(rc != bringauto::fleet_protocol::http_client::FleetApiClient::ReturnCode::OK) { return NOT_OK; } @@ -221,17 +200,17 @@ int device_disconnected(const int disconnect_type, const device_identification d return CONTEXT_INCORRECT; } - auto con = static_cast (context); + const auto con = static_cast (context); const std::string_view device_device_role(static_cast (device.device_role.data), device.device_role.size_in_bytes); const std::string_view device_device_name(static_cast (device.device_name.data), device.device_name.size_in_bytes); - for(auto it = con->devices.begin(); it != con->devices.end(); it++) { + for(auto it = con->devices.begin(); it != con->devices.end(); ++it) { const std::string_view it_device_role(static_cast (it->device_role.data), it->device_role.size_in_bytes); const std::string_view it_device_name(static_cast (it->device_name.data), it->device_name.size_in_bytes); - bool device_is_present = it->device_type == device.device_type && it_device_role == device_device_role && it_device_name == device_device_name - && it->module == device.module && it->priority == device.priority; + const bool device_is_present = it->device_type == device.device_type && it_device_role == device_device_role && + it_device_name == device_device_name && it->module == device.module && it->priority == device.priority; if(device_is_present) { deallocate(&it->device_role); @@ -249,13 +228,13 @@ int device_connected(const device_identification device, void *context) { return CONTEXT_INCORRECT; } - auto con = static_cast (context); - - device_identification new_device; + const auto con = static_cast (context); - new_device.module = device.module; - new_device.device_type = device.device_type; - new_device.priority = device.priority; + device_identification new_device { + .module = device.module, + .device_type = device.device_type, + .priority = device.priority + }; if(allocate(&new_device.device_role, device.device_role.size_in_bytes) != OK) { return NOT_OK; @@ -276,25 +255,22 @@ int wait_for_command(int timeout_time_in_ms, void *context) { return CONTEXT_INCORRECT; } - auto con = static_cast (context); + const auto con = static_cast (context); std::unique_lock lock(con->mutex); - std::pair>, - bringauto::fleet_protocol::http_client::FleetApiClient::ReturnCode> commands; bool parse_commands = con->last_command_timestamp != 0; - try { - commands = con->fleet_api_client->getCommands(con->last_command_timestamp + 1, true); - } catch (std::exception& e) { + auto [commands, rc] = con->fleet_api_client->getCommands(con->last_command_timestamp + 1, true); + if(rc != bringauto::fleet_protocol::http_client::FleetApiClient::ReturnCode::OK) { return TIMEOUT_OCCURRED; } bool received_no_commands = true; - for(const auto& command : commands.first) { + for(const auto& command : commands) { if(command->getTimestamp() > con->last_command_timestamp) { con->last_command_timestamp = command->getTimestamp(); } - auto received_device_id = command->getDeviceId(); + const auto received_device_id = command->getDeviceId(); if(received_device_id->getModuleId() == bamm::MISSION_MODULE_NUMBER) { received_no_commands = false; } else { @@ -302,17 +278,11 @@ int wait_for_command(int timeout_time_in_ms, void *context) { } if(parse_commands) { - MissionModule::AutonomyCommand proto_command {}; - const auto parse_status = google::protobuf::util::JsonStringToMessage( - command->getPayload()->getData()->getJson().serialize(), &proto_command - ); - if(!parse_status.ok()) { + std::string command_str = command->getPayload()->getData()->getJson().serialize(); + if (bringauto::protobuf::ProtobufHelper::validateAutonomyCommand(command_str) != OK) { return NOT_OK; } - std::string command_str; - proto_command.SerializeToString(&command_str); - con->command_vector.emplace_back(command_str, bringauto::fleet_protocol::cxx::DeviceID( received_device_id->getModuleId(), received_device_id->getType(), @@ -338,12 +308,12 @@ int pop_command(buffer* command, device_identification* device, void *context) { return CONTEXT_INCORRECT; } - auto con = static_cast (context); - auto command_object = std::get<0>(con->command_vector.back()); + const auto con = static_cast (context); + const auto command_object = std::get<0>(con->command_vector.back()); bringauto::fleet_protocol::cxx::StringAsBuffer::createBufferAndCopyData(command, command_object); - auto& device_id = std::get<1>(con->command_vector.back()); + const auto& device_id = std::get<1>(con->command_vector.back()); device->module = device_id.getDeviceId().module; device->device_type = device_id.getDeviceId().device_type; @@ -366,4 +336,4 @@ int pop_command(buffer* command, device_identification* device, void *context) { int command_ack(const buffer command, const device_identification device, void *context) { return OK; -} \ No newline at end of file +} diff --git a/source/module_manager.cpp b/source/module_manager.cpp index 7e335b4..becaa28 100644 --- a/source/module_manager.cpp +++ b/source/module_manager.cpp @@ -70,7 +70,7 @@ aggregate_status(struct buffer *aggregated_status, const struct buffer current_s [[nodiscard]] int command_data_valid(const struct buffer command, unsigned int device_type) { switch (device_type) { case bamm::AUTONOMY_DEVICE_TYPE: - return bamm::devices::AutonomyDevice::status_data_valid(command); + return bamm::devices::AutonomyDevice::command_data_valid(command); default: return NOT_OK; }