From b19d41964b856a00a7974d35edb3c1bf89e6d601 Mon Sep 17 00:00:00 2001 From: Thomas Debrunner Date: Fri, 1 Dec 2023 16:43:54 +0100 Subject: [PATCH 1/8] WIP: Added first stab at higher level mission protocol implementation --- .../opinionated_protocols/MessageBuilder.h | 39 ++++ .../mav/opinionated_protocols/MissionClient.h | 119 ++++++++++ .../MissionMessageBuilders.h | 102 ++++++++ .../mav/opinionated_protocols/ProtocolUtils.h | 68 ++++++ tests/CMakeLists.txt | 3 +- tests/opinionated_protocols/MissionClient.cpp | 218 ++++++++++++++++++ 6 files changed, 548 insertions(+), 1 deletion(-) create mode 100644 include/mav/opinionated_protocols/MessageBuilder.h create mode 100644 include/mav/opinionated_protocols/MissionClient.h create mode 100644 include/mav/opinionated_protocols/MissionMessageBuilders.h create mode 100644 include/mav/opinionated_protocols/ProtocolUtils.h create mode 100644 tests/opinionated_protocols/MissionClient.cpp diff --git a/include/mav/opinionated_protocols/MessageBuilder.h b/include/mav/opinionated_protocols/MessageBuilder.h new file mode 100644 index 0000000..635dcc4 --- /dev/null +++ b/include/mav/opinionated_protocols/MessageBuilder.h @@ -0,0 +1,39 @@ +// +// Created by thomas on 20.11.23. +// + +#ifndef MAV_MESSAGEBUILDER_H +#define MAV_MESSAGEBUILDER_H + +#include "mav/MessageSet.h" +#include "mav/Message.h" + +namespace mav { + template + class MessageBuilder { + protected: + mav::Message _message; + + template + BuilderType& _set(const std::string& key, ARG value) { + _message[key] = value; + return *static_cast(this); + } + + public: + MessageBuilder(const MessageSet &message_set, const std::string& message_name) : + _message(message_set.create(message_name)) {} + + [[nodiscard]] mav::Message build() const { + return _message; + } + + operator Message() const { + return _message; + } + }; +} + + + +#endif //MAV_MESSAGEBUILDER_H diff --git a/include/mav/opinionated_protocols/MissionClient.h b/include/mav/opinionated_protocols/MissionClient.h new file mode 100644 index 0000000..deda3f4 --- /dev/null +++ b/include/mav/opinionated_protocols/MissionClient.h @@ -0,0 +1,119 @@ +// +// Created by thomas on 01.12.23. +// + +#ifndef MAV_MISSIONCLIENT_H +#define MAV_MISSIONCLIENT_H + +#include +#include +#include "MissionMessageBuilders.h" +#include "ProtocolUtils.h" + + +namespace mav { +namespace mission { + class MissionClient { + private: + std::shared_ptr _connection; + const MessageSet& _message_set; + + public: + MissionClient(std::shared_ptr connection, const MessageSet& message_set) : + _connection(std::move(connection)), + _message_set(message_set) { + + // Make sure all messages required for the protocol are present in the message set + ensureMessageInMessageSet(_message_set, { + "MISSION_COUNT", + "MISSION_REQUEST_INT", + "MISSION_REQUEST_LIST", + "MISSION_ITEM_INT", + "MISSION_ACK"}); + } + + void upload(const std::vector &mission_messages, Identifier target={1, 1}, + int retry_count=3, int item_timeout=1000) { + + if (mission_messages.empty()) { + // nothing to do + return; + } + // get mission type from first message + int mission_type = mission_messages[0]["mission_type"]; + + // Send mission count + auto mission_count_message = _message_set.create("MISSION_COUNT").set({ + {"target_system", target.system_id}, + {"target_component", target.component_id}, + {"count", mission_messages.size()}, + {"mission_type", mission_type}}); + + auto count_response = exchangeRetry(_connection, mission_count_message, "MISSION_REQUEST_INT", + target.system_id, target.component_id, retry_count, item_timeout); + + throwAssert(count_response["mission_type"].as() == mission_type, "Mission type mismatch"); + throwAssert(count_response["seq"].as() == 0, "Sequence number mismatch"); + + int seq = 0; + while (seq < static_cast(mission_messages.size())) { + auto mission_item_message = mission_messages[seq]; + mission_item_message["target_system"] = target.system_id; + mission_item_message["target_component"] = target.component_id; + mission_item_message["seq"] = seq; + + // we expect an ack for the last message, otherwise a request for the next one + const auto expected_response = seq == static_cast(mission_messages.size()) - 1 ? + "MISSION_ACK" : "MISSION_REQUEST_INT"; + auto item_response = exchangeRetry(_connection, mission_item_message, expected_response, + target.system_id, target.component_id, retry_count, item_timeout); + + if (seq == static_cast(mission_messages.size()) - 1) { + // we expect an ack for the last message + throwAssert(item_response["type"].as() == 0, "Mission upload failed"); + seq++; + } else { + // we expect a request for the next message + throwAssert(item_response["mission_type"].as() == mission_type, "Mission type mismatch"); + seq = item_response["seq"]; + } + } + } + + std::vector download(Identifier target={1, 1}, int retry_count=3, int item_timeout=1000) { + // Send mission request list + auto mission_request_list_message = _message_set.create("MISSION_REQUEST_LIST").set({ + {"target_system", target.system_id}, + {"target_component", target.component_id}, + {"mission_type", 0}}); + + auto request_list_response = exchangeRetry(_connection, mission_request_list_message, "MISSION_COUNT", + target.system_id, target.component_id, retry_count, item_timeout); + + throwAssert(request_list_response["mission_type"].as() == 0, "Mission type mismatch"); + + int count = request_list_response["count"]; + std::vector mission_messages; + for (int seq = 0; seq < count; seq++) { + auto mission_request_message = _message_set.create("MISSION_REQUEST_INT").set({ + {"target_system", target.system_id}, + {"target_component", target.component_id}, + {"seq", seq}, + {"mission_type", 0}}); + + auto request_response = exchangeRetry(_connection, mission_request_message, "MISSION_ITEM_INT", + target.system_id, target.component_id, retry_count, item_timeout); + + throwAssert(request_response["mission_type"].as() == 0, "Mission type mismatch"); + throwAssert(request_response["seq"].as() == seq, "Sequence number mismatch"); + + mission_messages.push_back(request_response); + } + return mission_messages; + } + }; +}; +} + + +#endif //MAV_MISSIONCLIENT_H diff --git a/include/mav/opinionated_protocols/MissionMessageBuilders.h b/include/mav/opinionated_protocols/MissionMessageBuilders.h new file mode 100644 index 0000000..9e8f506 --- /dev/null +++ b/include/mav/opinionated_protocols/MissionMessageBuilders.h @@ -0,0 +1,102 @@ +// +// Created by thomas on 20.11.23. +// + +#ifndef MAV_MISSION_PROTOCOL_MESSAGES_H +#define MAV_MISSION_PROTOCOL_MESSAGES_H + +#include "mav/MessageSet.h" +#include "mav/Message.h" +#include "MessageBuilder.h" + +namespace mav { + namespace mission { + template + class MissionItemIntMessage : public MessageBuilder { + public: + explicit MissionItemIntMessage(const MessageSet &message_set) : + MessageBuilder(message_set, "MISSION_ITEM_INT") { + this->_message.set({ + {"frame", message_set.e("MAV_FRAME_GLOBAL_INT")}, + {"autocontinue", 1}, + {"current", 0}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")} + }); + } + + auto &latitude_deg(double latitude) { + return this->_set("x", static_cast(latitude * 1e7)); + } + + auto &longitude_deg(double longitude) { + return this->_set("y", static_cast(longitude * 1e7)); + } + + auto &altitude_m(double altitude) { + return this->_set("z", altitude); + } + }; + + class TakeoffMessage : public MissionItemIntMessage { + public: + explicit TakeoffMessage(const MessageSet &message_set) : + MissionItemIntMessage(message_set) { + _message["command"] = message_set.e("MAV_CMD_NAV_TAKEOFF"); + } + + auto &pitch_deg(double pitch) { + return _set("param1", pitch); + } + + auto &yaw_deg(double yaw) { + return _set("param4", yaw); + } + }; + + class LandMessage : public MissionItemIntMessage { + public: + explicit LandMessage(const MessageSet &message_set) : + MissionItemIntMessage(message_set) { + _message["command"] = message_set.e("MAV_CMD_NAV_LAND"); + } + + auto &abort_alt_m(double abort_alt) { + return _set("param1", abort_alt); + } + + auto &land_mode(int land_mode) { + return _set("param2", land_mode); + } + + auto &yaw_deg(double yaw) { + return _set("param4", yaw); + } + }; + + class WaypointMessage : public MissionItemIntMessage { + public: + explicit WaypointMessage(const MessageSet &message_set) : + MissionItemIntMessage(message_set) { + _message["command"] = message_set.e("MAV_CMD_NAV_WAYPOINT"); + } + + auto &hold_time_s(double hold_time) { + return _set("param1", hold_time); + } + + auto &acceptance_radius_m(double acceptance_radius) { + return _set("param2", acceptance_radius); + } + + auto &pass_radius_m(double pass_radius) { + return _set("param3", pass_radius); + } + + auto &yaw_deg(double yaw) { + return _set("param4", yaw); + } + }; + } +} + +#endif //MAV_MISSION_MESSAGES_H diff --git a/include/mav/opinionated_protocols/ProtocolUtils.h b/include/mav/opinionated_protocols/ProtocolUtils.h new file mode 100644 index 0000000..19aeb6e --- /dev/null +++ b/include/mav/opinionated_protocols/ProtocolUtils.h @@ -0,0 +1,68 @@ +// +// Created by thomas on 01.12.23. +// + +#ifndef MAV_PROTOCOLUTILS_H +#define MAV_PROTOCOLUTILS_H + +#include +#include +#include "mav/Message.h" +#include "mav/Connection.h" + +namespace mav { + + class ProtocolError : public std::runtime_error { + public: + ProtocolError(const std::string &message) : std::runtime_error(message) {} + }; + + void ensureMessageInMessageSet(const MessageSet &message_set, const std::initializer_list &message_names) { + for (const auto &message_name : message_names) { + if (!message_set.contains(message_name)) { + throw std::runtime_error("Message " + message_name + " not present in message set"); + } + } + } + + void throwAssert(bool condition, const std::string& message) { + if (!condition) { + throw ProtocolError(message); + } + } + + Message exchange( + std::shared_ptr connection, + Message &request, + const std::string &response_message_name, + int source_id = mav::ANY_ID, + int source_component = mav::ANY_ID, + int timeout_ms = 1000) { + auto expectation = connection->expect(response_message_name, source_id, source_component); + connection->send(request); + return connection->receive(expectation, timeout_ms); + } + + Message exchangeRetry( + const std::shared_ptr& connection, + Message &request, + const std::string &response_message_name, + int source_id = mav::ANY_ID, + int source_component = mav::ANY_ID, + int retries = 3, + int timeout_ms = 1000) { + for (int i = 0; i < retries; i++) { + try { + return exchange(connection, request, response_message_name, source_id, source_component, timeout_ms); + } catch (const TimeoutException& e) { + if (i == retries - 1) { + throw e; + } + } + } + throw ProtocolError("Exchange of message " + request.name() + " -> " + response_message_name + + " failed after " + std::to_string(retries) + " retries"); + } +} + +#endif //MAV_PROTOCOLUTILS_H diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index a46f263..fa86646 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -6,7 +6,8 @@ add_executable(tests Network.cpp MessageFieldIterator.cpp UDP.cpp - TCP.cpp) + TCP.cpp + opinionated_protocols/MissionClient.cpp) add_test(NAME tests COMMAND tests) diff --git a/tests/opinionated_protocols/MissionClient.cpp b/tests/opinionated_protocols/MissionClient.cpp new file mode 100644 index 0000000..d31d0cb --- /dev/null +++ b/tests/opinionated_protocols/MissionClient.cpp @@ -0,0 +1,218 @@ +#include "../doctest.h" +#include "mav/opinionated_protocols/MissionClient.h" +#include "mav/TCPClient.h" +#include "mav/TCPServer.h" + +using namespace mav; +#define PORT 13975 + + +TEST_CASE("Mission protocol client") { + MessageSet message_set; + message_set.addFromXMLString(R""""( + + + + + + + Type of mission items being requested/sent in mission protocol. + + Items are mission commands for main mission. + + + + Result of mission operation (in a MISSION_ACK message). + + mission accepted OK + + + Generic error / not accepting mission commands at all right now. + + + Coordinate frame is not supported. + + + Command is not supported. + + + Mission items exceed storage space. + + + One of the parameters has an invalid value. + + + param1 has an invalid value. + + + param2 has an invalid value. + + + param3 has an invalid value. + + + param4 has an invalid value. + + + x / param5 has an invalid value. + + + y / param6 has an invalid value. + + + z / param7 has an invalid value. + + + Mission item received out of sequence + + + Not accepting any mission commands from this communication partner. + + + Current mission operation cancelled (e.g. mission upload, mission download). + + + + + Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. NaN and INT32_MAX may be used in float/integer params (respectively) to indicate optional/default values (e.g. to use the component's current yaw or latitude rather than a specific value). See https://mavlink.io/en/guide/xml_schema.html#MAV_CMD for information about the structure of the MAV_CMD entries + + Navigate to waypoint. + Hold time. (ignored by fixed wing, time to stay at waypoint for rotary wing) + Acceptance radius (if the sphere with this radius is hit, the waypoint counts as reached) + 0 to pass through the WP, if > 0 radius to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. + Desired yaw angle at waypoint (rotary wing). NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). + Latitude + Longitude + Altitude + + + Return to launch location + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Land at location. + Minimum target altitude if landing is aborted (0 = undefined/use system default). + Precision land mode. + Empty. + Desired yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). + Latitude. + Longitude. + Landing altitude (ground level in current frame). + + + Takeoff from ground / hand. Vehicles that support multiple takeoff modes (e.g. VTOL quadplane) should take off using the currently configured mode. + Minimum pitch (if airspeed sensor present), desired pitch without sensor + Empty + Empty + Yaw angle (if magnetometer present), ignored without magnetometer. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). + Latitude + Longitude + Altitude + + + + + + Request the overall list of mission items from the system/component. + System ID + Component ID + + Mission type. + + + This message is emitted as response to MISSION_REQUEST_LIST by the MAV and to initiate a write transaction. The GCS can then request the individual mission item based on the knowledge of the total number of waypoints. + System ID + Component ID + Number of mission items in the sequence + + Mission type. + + + Request the information of the mission item with the sequence number seq. The response of the system to this message should be a MISSION_ITEM_INT message. https://mavlink.io/en/services/mission.html + System ID + Component ID + Sequence + + Mission type. + + + Message encoding a mission item. This message is emitted to announce + the presence of a mission item and to set a mission item on the system. The mission item can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed (NED), global frame is Z-up, right handed (ENU). NaN or INT32_MAX may be used in float/integer params (respectively) to indicate optional/default values (e.g. to use the component's current latitude, yaw rather than a specific value). See also https://mavlink.io/en/services/mission.html. + System ID + Component ID + Waypoint ID (sequence number). Starts at zero. Increases monotonically for each waypoint, no gaps in the sequence (0,1,2,3,4). + The coordinate system of the waypoint. + The scheduled action for the waypoint. + false:0, true:1 + Autocontinue to next waypoint. 0: false, 1: true. Set false to pause mission after the item completes. + PARAM1, see MAV_CMD enum + PARAM2, see MAV_CMD enum + PARAM3, see MAV_CMD enum + PARAM4, see MAV_CMD enum + PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 + PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + + Mission type. + + + Acknowledgment message during waypoint handling. The type field states if this message is a positive ack (type=0) or if an error happened (type=non-zero). + System ID + Component ID + Mission result. + + Mission type. + + + +)""""); + + mav::TCPServer server_physical(PORT); + mav::NetworkRuntime server_runtime(message_set, server_physical); + + std::promise connection_called_promise; + auto connection_called_future = connection_called_promise.get_future(); + server_runtime.onConnection([&connection_called_promise](const std::shared_ptr&) { + connection_called_promise.set_value(); + }); + + // setup client + auto heartbeat = message_set.create("HEARTBEAT")({ + {"type", 1}, + {"autopilot", 2}, + {"base_mode", 3}, + {"custom_mode", 4}, + {"system_status", 5}, + {"mavlink_version", 6}}); + + mav::TCPClient client_physical("localhost", PORT); + mav::NetworkRuntime client_runtime(message_set, heartbeat, client_physical); + + CHECK((connection_called_future.wait_for(std::chrono::seconds(2)) != std::future_status::timeout)); + + auto server_connection = server_runtime.awaitConnection(100); + server_connection->send(heartbeat); + + auto client_connection = client_runtime.awaitConnection(100); + + + SUBCASE("Can upload mission") { + std::vector mission { + mav::mission::TakeoffMessage(message_set).altitude_m(10), + mav::mission::WaypointMessage(message_set).latitude_deg(47.397742).longitude_deg(8.545594).altitude_m(10), + mav::mission::LandMessage(message_set).altitude_m(10) + }; + + mav::mission::MissionClient mission_client(client_connection, message_set); + mission_client.upload(mission); + + auto downloaded_mission = mission_client.download(); + } +} + From 1329d02e2e5f2d59c2504e00fbeef9ecf27016c3 Mon Sep 17 00:00:00 2001 From: Thomas Debrunner Date: Mon, 4 Dec 2023 11:13:32 +0100 Subject: [PATCH 2/8] protocols: First stab at param protocol --- .../mav/opinionated_protocols/ParamClient.h | 123 ++++++++++++++++++ tests/CMakeLists.txt | 3 +- tests/opinionated_protocols/ParamClient.cpp | 5 + 3 files changed, 130 insertions(+), 1 deletion(-) create mode 100644 include/mav/opinionated_protocols/ParamClient.h create mode 100644 tests/opinionated_protocols/ParamClient.cpp diff --git a/include/mav/opinionated_protocols/ParamClient.h b/include/mav/opinionated_protocols/ParamClient.h new file mode 100644 index 0000000..18152f2 --- /dev/null +++ b/include/mav/opinionated_protocols/ParamClient.h @@ -0,0 +1,123 @@ +// +// Created by thomas on 04.12.23. +// + +#ifndef MAV_PARAMCLIENT_H +#define MAV_PARAMCLIENT_H + +#include +#include +#include +#include "mav/Connection.h" +#include "ProtocolUtils.h" + + +namespace mav { +namespace param { + class ParamClient { + private: + std::shared_ptr _connection; + const MessageSet& _message_set; + + std::variant _unpack(const mav::Message &message) { + if (message["param_type"].as() == _message_set.e("MAV_PARAM_TYPE_REAL32")) { + return message["param_value"].as(); + } else { + return message["param_value"].floatUnpack(); + } + } + + public: + ParamClient(std::shared_ptr connection, const MessageSet& message_set) : + _connection(std::move(connection)), + _message_set(message_set) { + ensureMessageInMessageSet(message_set, {"PARAM_REQUEST_LIST", "PARAM_REQUEST_READ", "PARAM_SET", "PARAM_VALUE"}); + } + + mav::Message readRaw(mav::Message &message, int target_system = mav::ANY_ID, int target_component = mav::ANY_ID, int retry_count=3, int item_timeout=1000) { + throwAssert(message.name() == "PARAM_REQUEST_READ", "Message must be of type PARAM_REQUEST_READ"); + auto response = exchangeRetry(_connection, message, "PARAM_VALUE", target_system, target_component, retry_count, item_timeout); + } + + std::variant read(const std::string ¶m_id, int target_system = mav::ANY_ID, int target_component = mav::ANY_ID, int retry_count=3, int item_timeout=1000) { + auto message = _message_set.create("PARAM_REQUEST_READ").set({ + {"target_system", target_system}, + {"target_component", target_component}, + {"param_id", param_id}, + {"param_index", -1}}); + auto response = readRaw(message, target_system, target_component, retry_count, item_timeout); + return _unpack(response); + } + + mav::Message writeRaw(mav::Message &message, int target_system = mav::ANY_ID, int target_component = mav::ANY_ID, int retry_count=3, int item_timeout=1000) { + throwAssert(message.name() == "PARAM_SET", "Message must be of type PARAM_SET"); + auto response = exchangeRetry(_connection, message, "PARAM_VALUE", target_system, target_component, retry_count, item_timeout); + throwAssert(response["param_id"].as() == message["param_id"].as(), "Parameter ID mismatch"); + throwAssert(response["param_type"].as() == message["param_type"].as(), "Parameter type mismatch"); + } + + void write(const std::string ¶m_id, std::variant value, int target_system = mav::ANY_ID, int target_component = mav::ANY_ID, int retry_count=3, int item_timeout=1000) { + auto message = _message_set.create("PARAM_SET").set({ + {"target_system", target_system}, + {"target_component", target_component}, + {"param_id", param_id}}); + + if (std::holds_alternative(value)) { + message["param_value"].floatPack(std::get(value)); + message["param_type"] = _message_set.e("MAV_PARAM_TYPE_INT32"); + } else { + message["param_value"] = std::get(value); + message["param_type"] = _message_set.e("MAV_PARAM_TYPE_REAL32"); + } + + writeRaw(message, target_system, target_component, retry_count, item_timeout); + } + + std::vector> listRaw(int target_system = mav::ANY_ID, int target_component = mav::ANY_ID, int retry_count=3, int item_timeout=1000) { + std::vector> result; + + auto list_message = _message_set.create("PARAM_REQUEST_LIST").set({ + {"target_system", target_system}, + {"target_component", target_component}}); + _connection->send(list_message); + while (true) { + try { + auto message = _connection->receive("PARAM_VALUE", item_timeout); + if (static_cast(result.size()) < message["param_count"].as()) { + result.resize(message["param_count"].as()); + } + throwAssert(message["param_index"].as() < static_cast(result.size()), "Index out of bounds"); + result[message["param_index"].as()] = message; + } catch (const TimeoutException& e) { + // by the mavlink spec, the end of a param list transaction is detected by a timeout + break; + } + } + + // re-request missing parameters + for (int i = 0; i < static_cast(result.size()); i++) { + if (!result[i].has_value()) { + result[i] = readRaw(_message_set.create("PARAM_REQUEST_READ").set({ + {"target_system", target_system}, + {"target_component", target_component}, + {"param_index", i}}), target_system, target_component); + } + } + return result; + } + + std::map> list(int target_system = mav::ANY_ID, int target_component = mav::ANY_ID, int retry_count=3, int item_timeout=1000) { + std::map> result; + auto raw_result = listRaw(target_system, target_component, retry_count, item_timeout); + for (auto &message : raw_result) { + if (!message.has_value()) { + continue; + } + result[message.value()["param_id"].as()] = _unpack(message.value()); + } + return result; + } + }; +} +} +#endif //MAV_PARAMCLIENT_H diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index fa86646..631c3f8 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -7,7 +7,8 @@ add_executable(tests MessageFieldIterator.cpp UDP.cpp TCP.cpp - opinionated_protocols/MissionClient.cpp) + opinionated_protocols/MissionClient.cpp + opinionated_protocols/ParamClient.cpp) add_test(NAME tests COMMAND tests) diff --git a/tests/opinionated_protocols/ParamClient.cpp b/tests/opinionated_protocols/ParamClient.cpp new file mode 100644 index 0000000..2ffe4c7 --- /dev/null +++ b/tests/opinionated_protocols/ParamClient.cpp @@ -0,0 +1,5 @@ +// +// Created by thomas on 04.12.23. +// +#include "mav/opinionated_protocols/ParamClient.h" +#include "mav/opinionated_protocols/ParamMessageBuilders.h" From 56a201dd6f5d80b6d0806bb7dd10c312a446323d Mon Sep 17 00:00:00 2001 From: Thomas Debrunner Date: Tue, 5 Dec 2023 11:23:35 +0100 Subject: [PATCH 3/8] protocols: initial tests for mission upload / download working --- .../mav/opinionated_protocols/MissionClient.h | 16 ++- .../mav/opinionated_protocols/ParamClient.h | 4 +- .../mav/opinionated_protocols/ProtocolUtils.h | 8 +- tests/opinionated_protocols/MissionClient.cpp | 135 +++++++++++++++++- tests/opinionated_protocols/ParamClient.cpp | 1 - .../ProtocolTestSequencer.h | 97 +++++++++++++ 6 files changed, 249 insertions(+), 12 deletions(-) create mode 100644 tests/opinionated_protocols/ProtocolTestSequencer.h diff --git a/include/mav/opinionated_protocols/MissionClient.h b/include/mav/opinionated_protocols/MissionClient.h index deda3f4..f671bfd 100644 --- a/include/mav/opinionated_protocols/MissionClient.h +++ b/include/mav/opinionated_protocols/MissionClient.h @@ -13,6 +13,7 @@ namespace mav { namespace mission { + class MissionClient { private: std::shared_ptr _connection; @@ -70,7 +71,7 @@ namespace mission { if (seq == static_cast(mission_messages.size()) - 1) { // we expect an ack for the last message - throwAssert(item_response["type"].as() == 0, "Mission upload failed"); + throwAssert(item_response["type"].as() == _message_set.e("MAV_MISSION_ACCEPTED"), "Mission upload failed"); seq++; } else { // we expect a request for the next message @@ -80,17 +81,17 @@ namespace mission { } } - std::vector download(Identifier target={1, 1}, int retry_count=3, int item_timeout=1000) { + std::vector download(Identifier target={1, 1}, int mission_type=0, int retry_count=3, int item_timeout=1000) { // Send mission request list auto mission_request_list_message = _message_set.create("MISSION_REQUEST_LIST").set({ {"target_system", target.system_id}, {"target_component", target.component_id}, - {"mission_type", 0}}); + {"mission_type", mission_type}}); auto request_list_response = exchangeRetry(_connection, mission_request_list_message, "MISSION_COUNT", target.system_id, target.component_id, retry_count, item_timeout); - throwAssert(request_list_response["mission_type"].as() == 0, "Mission type mismatch"); + throwAssert(request_list_response["mission_type"].as() == mission_type, "Mission type mismatch"); int count = request_list_response["count"]; std::vector mission_messages; @@ -109,6 +110,13 @@ namespace mission { mission_messages.push_back(request_response); } + auto ack_message = _message_set.create("MISSION_ACK").set({ + {"target_system", target.system_id}, + {"target_component", target.component_id}, + {"type", _message_set.e("MAV_MISSION_ACCEPTED")}, + {"mission_type", mission_type}}); + _connection->send(ack_message); + return mission_messages; } }; diff --git a/include/mav/opinionated_protocols/ParamClient.h b/include/mav/opinionated_protocols/ParamClient.h index 18152f2..ee9daba 100644 --- a/include/mav/opinionated_protocols/ParamClient.h +++ b/include/mav/opinionated_protocols/ParamClient.h @@ -37,6 +37,7 @@ namespace param { mav::Message readRaw(mav::Message &message, int target_system = mav::ANY_ID, int target_component = mav::ANY_ID, int retry_count=3, int item_timeout=1000) { throwAssert(message.name() == "PARAM_REQUEST_READ", "Message must be of type PARAM_REQUEST_READ"); auto response = exchangeRetry(_connection, message, "PARAM_VALUE", target_system, target_component, retry_count, item_timeout); + return response; } std::variant read(const std::string ¶m_id, int target_system = mav::ANY_ID, int target_component = mav::ANY_ID, int retry_count=3, int item_timeout=1000) { @@ -54,6 +55,7 @@ namespace param { auto response = exchangeRetry(_connection, message, "PARAM_VALUE", target_system, target_component, retry_count, item_timeout); throwAssert(response["param_id"].as() == message["param_id"].as(), "Parameter ID mismatch"); throwAssert(response["param_type"].as() == message["param_type"].as(), "Parameter type mismatch"); + return response; } void write(const std::string ¶m_id, std::variant value, int target_system = mav::ANY_ID, int target_component = mav::ANY_ID, int retry_count=3, int item_timeout=1000) { @@ -100,7 +102,7 @@ namespace param { result[i] = readRaw(_message_set.create("PARAM_REQUEST_READ").set({ {"target_system", target_system}, {"target_component", target_component}, - {"param_index", i}}), target_system, target_component); + {"param_index", i}}), target_system, target_component, retry_count, item_timeout); } } return result; diff --git a/include/mav/opinionated_protocols/ProtocolUtils.h b/include/mav/opinionated_protocols/ProtocolUtils.h index 19aeb6e..ce30b8c 100644 --- a/include/mav/opinionated_protocols/ProtocolUtils.h +++ b/include/mav/opinionated_protocols/ProtocolUtils.h @@ -17,7 +17,7 @@ namespace mav { ProtocolError(const std::string &message) : std::runtime_error(message) {} }; - void ensureMessageInMessageSet(const MessageSet &message_set, const std::initializer_list &message_names) { + inline void ensureMessageInMessageSet(const MessageSet &message_set, const std::initializer_list &message_names) { for (const auto &message_name : message_names) { if (!message_set.contains(message_name)) { throw std::runtime_error("Message " + message_name + " not present in message set"); @@ -25,13 +25,13 @@ namespace mav { } } - void throwAssert(bool condition, const std::string& message) { + inline void throwAssert(bool condition, const std::string& message) { if (!condition) { throw ProtocolError(message); } } - Message exchange( + inline Message exchange( std::shared_ptr connection, Message &request, const std::string &response_message_name, @@ -43,7 +43,7 @@ namespace mav { return connection->receive(expectation, timeout_ms); } - Message exchangeRetry( + inline Message exchangeRetry( const std::shared_ptr& connection, Message &request, const std::string &response_message_name, diff --git a/tests/opinionated_protocols/MissionClient.cpp b/tests/opinionated_protocols/MissionClient.cpp index d31d0cb..d92e847 100644 --- a/tests/opinionated_protocols/MissionClient.cpp +++ b/tests/opinionated_protocols/MissionClient.cpp @@ -2,6 +2,7 @@ #include "mav/opinionated_protocols/MissionClient.h" #include "mav/TCPClient.h" #include "mav/TCPServer.h" +#include "ProtocolTestSequencer.h" using namespace mav; #define PORT 13975 @@ -118,6 +119,14 @@ TEST_CASE("Mission protocol client") { + + Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + Autopilot type / class. defined in MAV_AUTOPILOT ENUM + System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h + A bitfield for use for autopilot-specific flags. + System status flag, see MAV_STATE ENUM + MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version + Request the overall list of mission items from the system/component. System ID @@ -174,7 +183,7 @@ TEST_CASE("Mission protocol client") { )""""); mav::TCPServer server_physical(PORT); - mav::NetworkRuntime server_runtime(message_set, server_physical); + mav::NetworkRuntime server_runtime({1, 1}, message_set, server_physical); std::promise connection_called_promise; auto connection_called_future = connection_called_promise.get_future(); @@ -203,16 +212,138 @@ TEST_CASE("Mission protocol client") { SUBCASE("Can upload mission") { + + // mock the server response + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("MISSION_COUNT") + .out(message_set.create("MISSION_REQUEST_INT").set({ + {"seq", 0}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })) + .in("MISSION_ITEM_INT") + .out(message_set.create("MISSION_REQUEST_INT").set({ + {"seq", 1}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })) + .in("MISSION_ITEM_INT") + .out(message_set.create("MISSION_REQUEST_INT").set({ + {"seq", 2}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })) + .in("MISSION_ITEM_INT") + .out(message_set.create("MISSION_ACK").set({ + {"type", message_set.e("MAV_MISSION_ACCEPTED")}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })); + std::vector mission { mav::mission::TakeoffMessage(message_set).altitude_m(10), mav::mission::WaypointMessage(message_set).latitude_deg(47.397742).longitude_deg(8.545594).altitude_m(10), mav::mission::LandMessage(message_set).altitude_m(10) }; + server_sequence.start(); mav::mission::MissionClient mission_client(client_connection, message_set); mission_client.upload(mission); + server_sequence.finish(); + } + + SUBCASE("Can download mission") { + + // mock the server response + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("MISSION_REQUEST_LIST") + .out(message_set.create("MISSION_COUNT").set({ + {"count", 3}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })) + .in("MISSION_REQUEST_INT", [](auto &msg) { + CHECK_EQ(msg.template get("seq"), 0); + }) + .out(message_set.create("MISSION_ITEM_INT").set({ + {"seq", 0}, + {"frame", message_set.e("MAV_FRAME_GLOBAL_INT")}, + {"command", message_set.e("MAV_CMD_NAV_TAKEOFF")}, + {"current", 0}, + {"autocontinue", 1}, + {"param1", 0}, + {"param2", 0}, + {"param3", 0}, + {"param4", 0}, + {"x", 0}, + {"y", 0}, + {"z", 10}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })) + .in("MISSION_REQUEST_INT", [](auto &msg) { + CHECK_EQ(msg.template get("seq"), 1); + }) + .out(message_set.create("MISSION_ITEM_INT").set({ + {"seq", 1}, + {"frame", message_set.e("MAV_FRAME_GLOBAL_INT")}, + {"command", message_set.e("MAV_CMD_NAV_WAYPOINT")}, + {"current", 0}, + {"autocontinue", 1}, + {"param1", 0}, + {"param2", 0}, + {"param3", 0}, + {"param4", 0}, + {"x", 47.397742 * 1e7}, + {"y", 8.545594 * 1e7}, + {"z", 10}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })) + .in("MISSION_REQUEST_INT", [](auto &msg) { + CHECK_EQ(msg.template get("seq"), 2); + }) + .out(message_set.create("MISSION_ITEM_INT").set({ + {"seq", 2}, + {"frame", message_set.e("MAV_FRAME_GLOBAL_INT")}, + {"command", message_set.e("MAV_CMD_NAV_LAND")}, + {"current", 0}, + {"autocontinue", 1}, + {"param1", 0}, + {"param2", 0}, + {"param3", 0}, + {"param4", 0}, + {"x", 0}, + {"y", 0}, + {"z", 10}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })).in("MISSION_ACK", [&message_set](auto &msg) { + CHECK_EQ(msg.template get("type"), message_set.e("MAV_MISSION_ACCEPTED")); + }); + + mav::mission::MissionClient mission_client(client_connection, message_set); + server_sequence.start(); + auto mission = mission_client.download(); + server_sequence.finish(); - auto downloaded_mission = mission_client.download(); + CHECK_EQ(mission.size(), 3); + CHECK_EQ(mission[0].name(), "MISSION_ITEM_INT"); + CHECK_EQ(mission[0]["command"].as(), message_set.e("MAV_CMD_NAV_TAKEOFF")); + CHECK_EQ(mission[1].name(), "MISSION_ITEM_INT"); + CHECK_EQ(mission[1]["command"].as(), message_set.e("MAV_CMD_NAV_WAYPOINT")); + CHECK_EQ(mission[2].name(), "MISSION_ITEM_INT"); + CHECK_EQ(mission[2]["command"].as(), message_set.e("MAV_CMD_NAV_LAND")); } } diff --git a/tests/opinionated_protocols/ParamClient.cpp b/tests/opinionated_protocols/ParamClient.cpp index 2ffe4c7..3b3e6d0 100644 --- a/tests/opinionated_protocols/ParamClient.cpp +++ b/tests/opinionated_protocols/ParamClient.cpp @@ -2,4 +2,3 @@ // Created by thomas on 04.12.23. // #include "mav/opinionated_protocols/ParamClient.h" -#include "mav/opinionated_protocols/ParamMessageBuilders.h" diff --git a/tests/opinionated_protocols/ProtocolTestSequencer.h b/tests/opinionated_protocols/ProtocolTestSequencer.h new file mode 100644 index 0000000..4c062ff --- /dev/null +++ b/tests/opinionated_protocols/ProtocolTestSequencer.h @@ -0,0 +1,97 @@ +// +// Created by thomas on 04.12.23. +// + +#ifndef MAV_PROTOCOLTESTSEQUENCER_H +#define MAV_PROTOCOLTESTSEQUENCER_H + +#include "mav/MessageSet.h" +#include "mav/Message.h" + +class ProtocolTestSequencer { + +public: + explicit ProtocolTestSequencer(std::shared_ptr connection, bool debug = false) : + _connection(std::move(connection)), _debug(debug) { + } + + struct ReceiveItem { + std::string name; + std::function verification; + }; + + using SendItem = mav::Message; + using SequenceItem = std::variant; + + std::vector sequence; + + ProtocolTestSequencer& out(const SendItem& item) { + sequence.emplace_back(item); + return *this; + } + + ProtocolTestSequencer& in(const std::string& name, const std::function & verification) { + sequence.emplace_back(ReceiveItem{name, verification}); + return *this; + } + + ProtocolTestSequencer& in(const std::string &name) { + sequence.emplace_back(ReceiveItem{name, [](const mav::Message&){}}); + return *this; + } + + void start() { + _thread = std::make_unique(&ProtocolTestSequencer::_run, this); + } + + void finish() { + stop(); + if (_exception) { + std::rethrow_exception(_exception); + } + } + + ~ProtocolTestSequencer() { + stop(); + } + +private: + void stop() { + if (_thread) { + _thread->join(); + _thread = nullptr; + } + } + + void _run() { + for (auto& item : sequence) { + try { + if (std::holds_alternative(item)) { + _connection->send(std::get(item)); + if (_debug) { + std::cout << "SENT: " << std::get(item).name() << std::endl; + } + } else { + // RECEIVE case always receive with 5s timeout + ReceiveItem rec_item = std::get(item); + auto message = _connection->receive(rec_item.name, 5000); + rec_item.verification(message); + if (_debug) { + std::cout << "RECEIVED: " << message.name() << std::endl; + } + } + } catch(...) { + _exception = std::current_exception(); + return; + } + } + } + + bool _debug; + std::shared_ptr _connection; + std::unique_ptr _thread; + + std::exception_ptr _exception; +}; + +#endif //MAV_PROTOCOLTESTSEQUENCER_H From f967b4c4f852af246687196fa4a63ef8723eb361 Mon Sep 17 00:00:00 2001 From: Thomas Debrunner Date: Wed, 6 Dec 2023 14:40:56 +0100 Subject: [PATCH 4/8] mission protocol: added lots of unit testing and protocol hardeing --- .../mav/opinionated_protocols/MissionClient.h | 64 ++-- .../mav/opinionated_protocols/ProtocolUtils.h | 74 +++- tests/opinionated_protocols/MissionClient.cpp | 350 +++++++++++++++++- 3 files changed, 436 insertions(+), 52 deletions(-) diff --git a/include/mav/opinionated_protocols/MissionClient.h b/include/mav/opinionated_protocols/MissionClient.h index f671bfd..ae0db9c 100644 --- a/include/mav/opinionated_protocols/MissionClient.h +++ b/include/mav/opinionated_protocols/MissionClient.h @@ -19,6 +19,14 @@ namespace mission { std::shared_ptr _connection; const MessageSet& _message_set; + void _assertNotNack(const Message& message) { + if (message.id() == _message_set.idForMessage("MISSION_ACK")) { + if (message["type"].as() != _message_set.e("MAV_MISSION_ACCEPTED")) { + throw mav::ProtocolException("Received NACK from server. Mission transaction failed."); + } + } + } + public: MissionClient(std::shared_ptr connection, const MessageSet& message_set) : _connection(std::move(connection)), @@ -44,15 +52,17 @@ namespace mission { int mission_type = mission_messages[0]["mission_type"]; // Send mission count - auto mission_count_message = _message_set.create("MISSION_COUNT").set({ + auto mission_count_message = _message_set.create("MISSION_COUNT")({ {"target_system", target.system_id}, {"target_component", target.component_id}, {"count", mission_messages.size()}, {"mission_type", mission_type}}); - auto count_response = exchangeRetry(_connection, mission_count_message, "MISSION_REQUEST_INT", - target.system_id, target.component_id, retry_count, item_timeout); - + Message count_response = exchangeRetryAnyResponse(_connection, _message_set, mission_count_message, + {"MISSION_ACK", "MISSION_REQUEST_INT"}, + target.system_id, target.component_id, + retry_count, item_timeout); + _assertNotNack(count_response); throwAssert(count_response["mission_type"].as() == mission_type, "Mission type mismatch"); throwAssert(count_response["seq"].as() == 0, "Sequence number mismatch"); @@ -63,54 +73,65 @@ namespace mission { mission_item_message["target_component"] = target.component_id; mission_item_message["seq"] = seq; - // we expect an ack for the last message, otherwise a request for the next one - const auto expected_response = seq == static_cast(mission_messages.size()) - 1 ? - "MISSION_ACK" : "MISSION_REQUEST_INT"; - auto item_response = exchangeRetry(_connection, mission_item_message, expected_response, + auto item_response = exchangeRetryAnyResponse(_connection, _message_set, mission_item_message, + {"MISSION_ACK", "MISSION_REQUEST_INT"}, target.system_id, target.component_id, retry_count, item_timeout); + // NACK is always bad, throw if we receive NACK + _assertNotNack(item_response); if (seq == static_cast(mission_messages.size()) - 1) { - // we expect an ack for the last message - throwAssert(item_response["type"].as() == _message_set.e("MAV_MISSION_ACCEPTED"), "Mission upload failed"); - seq++; - } else { - // we expect a request for the next message - throwAssert(item_response["mission_type"].as() == mission_type, "Mission type mismatch"); - seq = item_response["seq"]; + // we're okay with an ack, only when we're at the last message + if (item_response.id() == _message_set.idForMessage("MISSION_ACK")) { + break; + } } + // in general, we need a mission request int + throwAssert(item_response.id() == _message_set.idForMessage("MISSION_REQUEST_INT"), "Unexpected message"); + // we expect a request for the next message + throwAssert(item_response["mission_type"].as() == mission_type, "Mission type mismatch"); + int response_seq = item_response["seq"]; + // we allow requests for the next message or the current message + throwAssert((response_seq == seq + 1 || response_seq == seq) + && response_seq < static_cast(mission_messages.size()), "Sequence number mismatch"); + seq = response_seq; } } std::vector download(Identifier target={1, 1}, int mission_type=0, int retry_count=3, int item_timeout=1000) { // Send mission request list - auto mission_request_list_message = _message_set.create("MISSION_REQUEST_LIST").set({ + auto mission_request_list_message = _message_set.create("MISSION_REQUEST_LIST")({ {"target_system", target.system_id}, {"target_component", target.component_id}, {"mission_type", mission_type}}); - auto request_list_response = exchangeRetry(_connection, mission_request_list_message, "MISSION_COUNT", + auto request_list_response = exchangeRetryAnyResponse(_connection, _message_set, + mission_request_list_message, {"MISSION_COUNT", "MISSION_ACK"}, target.system_id, target.component_id, retry_count, item_timeout); - + _assertNotNack(request_list_response); + throwAssert(request_list_response.id() == _message_set.idForMessage("MISSION_COUNT"), "Unexpected message"); throwAssert(request_list_response["mission_type"].as() == mission_type, "Mission type mismatch"); int count = request_list_response["count"]; std::vector mission_messages; for (int seq = 0; seq < count; seq++) { - auto mission_request_message = _message_set.create("MISSION_REQUEST_INT").set({ + auto mission_request_message = _message_set.create("MISSION_REQUEST_INT")({ {"target_system", target.system_id}, {"target_component", target.component_id}, {"seq", seq}, {"mission_type", 0}}); - auto request_response = exchangeRetry(_connection, mission_request_message, "MISSION_ITEM_INT", + auto request_response = exchangeRetryAnyResponse(_connection, _message_set, mission_request_message, + {"MISSION_ITEM_INT", "MISSION_ACK"}, target.system_id, target.component_id, retry_count, item_timeout); + _assertNotNack(request_response); + throwAssert(request_response.id() == _message_set.idForMessage("MISSION_ITEM_INT"), "Unexpected message"); throwAssert(request_response["mission_type"].as() == 0, "Mission type mismatch"); throwAssert(request_response["seq"].as() == seq, "Sequence number mismatch"); mission_messages.push_back(request_response); } - auto ack_message = _message_set.create("MISSION_ACK").set({ + auto ack_message = _message_set.create("MISSION_ACK")({ {"target_system", target.system_id}, {"target_component", target.component_id}, {"type", _message_set.e("MAV_MISSION_ACCEPTED")}, @@ -123,5 +144,4 @@ namespace mission { }; } - #endif //MAV_MISSIONCLIENT_H diff --git a/include/mav/opinionated_protocols/ProtocolUtils.h b/include/mav/opinionated_protocols/ProtocolUtils.h index ce30b8c..107715b 100644 --- a/include/mav/opinionated_protocols/ProtocolUtils.h +++ b/include/mav/opinionated_protocols/ProtocolUtils.h @@ -12,9 +12,9 @@ namespace mav { - class ProtocolError : public std::runtime_error { + class ProtocolException : public std::runtime_error { public: - ProtocolError(const std::string &message) : std::runtime_error(message) {} + ProtocolException(const std::string &message) : std::runtime_error(message) {} }; inline void ensureMessageInMessageSet(const MessageSet &message_set, const std::initializer_list &message_names) { @@ -27,12 +27,30 @@ namespace mav { inline void throwAssert(bool condition, const std::string& message) { if (!condition) { - throw ProtocolError(message); + throw ProtocolException(message); } } + inline Connection::Expectation expectAny(const std::shared_ptr& connection, + const std::vector &message_ids, int source_id=mav::ANY_ID, int component_id=mav::ANY_ID) { + return connection->expect([message_ids, source_id, component_id](const Message& message) { + return std::find(message_ids.begin(), message_ids.end(), message.id()) != message_ids.end() && + (source_id == mav::ANY_ID || message.header().systemId() == source_id) && + (component_id == mav::ANY_ID || message.header().componentId() == component_id); + }); + } + + inline Connection::Expectation expectAny(const std::shared_ptr& connection, const MessageSet &message_set, + const std::vector &message_names, int source_id=mav::ANY_ID, int component_id=mav::ANY_ID) { + std::vector message_ids; + for (const auto &message_name : message_names) { + message_ids.push_back(message_set.idForMessage(message_name)); + } + return expectAny(connection, message_ids, source_id, component_id); + } + inline Message exchange( - std::shared_ptr connection, + const std::shared_ptr &connection, Message &request, const std::string &response_message_name, int source_id = mav::ANY_ID, @@ -43,25 +61,59 @@ namespace mav { return connection->receive(expectation, timeout_ms); } - inline Message exchangeRetry( - const std::shared_ptr& connection, + inline Message exchangeAnyResponse( + const std::shared_ptr &connection, + const MessageSet &message_set, Message &request, - const std::string &response_message_name, + const std::vector &response_message_names, int source_id = mav::ANY_ID, int source_component = mav::ANY_ID, - int retries = 3, int timeout_ms = 1000) { + auto expectation = expectAny(connection, message_set, response_message_names, source_id, source_component); + connection->send(request); + return connection->receive(expectation, timeout_ms); + } + + template + inline Ret _retry(int retries, Ret (*func)(Arg...), Arg... args) { for (int i = 0; i < retries; i++) { try { - return exchange(connection, request, response_message_name, source_id, source_component, timeout_ms); + return func(args...); } catch (const TimeoutException& e) { if (i == retries - 1) { throw e; } } } - throw ProtocolError("Exchange of message " + request.name() + " -> " + response_message_name + - " failed after " + std::to_string(retries) + " retries"); + throw ProtocolException("Function failed after " + std::to_string(retries) + " retries"); + } + + inline Message exchangeRetry( + const std::shared_ptr &connection, + Message &request, + const std::string &response_message_name, + int source_id = mav::ANY_ID, + int source_component = mav::ANY_ID, + int retries = 3, + int timeout_ms = 1000) { + return _retry&, Message&, const std::string&, int, int, int> + (retries, exchange, connection, request, response_message_name, source_id, + source_component, timeout_ms); + } + + inline Message exchangeRetryAnyResponse( + const std::shared_ptr &connection, + const MessageSet &message_set, + Message &request, + const std::vector &response_message_names, + int source_id = mav::ANY_ID, + int source_component = mav::ANY_ID, + int retries = 3, + int timeout_ms = 1000) { + return _retry&, const MessageSet&, Message&, const std::vector& + , int, int, int> + (retries, exchangeAnyResponse, connection, message_set, request, response_message_names, source_id, + source_component, timeout_ms); } } diff --git a/tests/opinionated_protocols/MissionClient.cpp b/tests/opinionated_protocols/MissionClient.cpp index d92e847..fc4f71d 100644 --- a/tests/opinionated_protocols/MissionClient.cpp +++ b/tests/opinionated_protocols/MissionClient.cpp @@ -210,50 +210,67 @@ TEST_CASE("Mission protocol client") { auto client_connection = client_runtime.awaitConnection(100); + std::vector dummy_mission_short { + mav::mission::TakeoffMessage(message_set).altitude_m(10), + }; + + std::vector dummy_mission_long { + mav::mission::TakeoffMessage(message_set).altitude_m(10), + mav::mission::WaypointMessage(message_set).latitude_deg(47.397742).longitude_deg(8.545594).altitude_m(10), + mav::mission::LandMessage(message_set).altitude_m(10) + }; + + + SUBCASE("Can upload mission") { // mock the server response ProtocolTestSequencer server_sequence(server_connection); server_sequence - .in("MISSION_COUNT") - .out(message_set.create("MISSION_REQUEST_INT").set({ + .in("MISSION_COUNT", [](const mav::Message &msg) { + CHECK_EQ(msg["count"].as(), 3); + }) + .out(message_set.create("MISSION_REQUEST_INT")({ {"seq", 0}, {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, {"target_system", LIBMAV_DEFAULT_ID}, {"target_component", LIBMAV_DEFAULT_ID} })) - .in("MISSION_ITEM_INT") - .out(message_set.create("MISSION_REQUEST_INT").set({ + .in("MISSION_ITEM_INT", [](const mav::Message &msg) { + CHECK_EQ(msg["seq"].as(), 0); + CHECK_EQ(msg["mission_type"].as(), 0); + }) + .out(message_set.create("MISSION_REQUEST_INT")({ {"seq", 1}, {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, {"target_system", LIBMAV_DEFAULT_ID}, {"target_component", LIBMAV_DEFAULT_ID} })) - .in("MISSION_ITEM_INT") - .out(message_set.create("MISSION_REQUEST_INT").set({ + .in("MISSION_ITEM_INT", [](const mav::Message &msg) { + CHECK_EQ(msg["seq"].as(), 1); + CHECK_EQ(msg["mission_type"].as(), 0); + }) + .out(message_set.create("MISSION_REQUEST_INT")({ {"seq", 2}, {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, {"target_system", LIBMAV_DEFAULT_ID}, {"target_component", LIBMAV_DEFAULT_ID} })) - .in("MISSION_ITEM_INT") - .out(message_set.create("MISSION_ACK").set({ + .in("MISSION_ITEM_INT", [](const mav::Message &msg) { + CHECK_EQ(msg["seq"].as(), 2); + CHECK_EQ(msg["mission_type"].as(), 0); + }) + .out(message_set.create("MISSION_ACK")({ {"type", message_set.e("MAV_MISSION_ACCEPTED")}, {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, {"target_system", LIBMAV_DEFAULT_ID}, {"target_component", LIBMAV_DEFAULT_ID} })); - std::vector mission { - mav::mission::TakeoffMessage(message_set).altitude_m(10), - mav::mission::WaypointMessage(message_set).latitude_deg(47.397742).longitude_deg(8.545594).altitude_m(10), - mav::mission::LandMessage(message_set).altitude_m(10) - }; - server_sequence.start(); mav::mission::MissionClient mission_client(client_connection, message_set); - mission_client.upload(mission); + mission_client.upload(dummy_mission_long); server_sequence.finish(); } @@ -263,7 +280,7 @@ TEST_CASE("Mission protocol client") { ProtocolTestSequencer server_sequence(server_connection); server_sequence .in("MISSION_REQUEST_LIST") - .out(message_set.create("MISSION_COUNT").set({ + .out(message_set.create("MISSION_COUNT")({ {"count", 3}, {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, {"target_system", LIBMAV_DEFAULT_ID}, @@ -272,7 +289,7 @@ TEST_CASE("Mission protocol client") { .in("MISSION_REQUEST_INT", [](auto &msg) { CHECK_EQ(msg.template get("seq"), 0); }) - .out(message_set.create("MISSION_ITEM_INT").set({ + .out(message_set.create("MISSION_ITEM_INT")({ {"seq", 0}, {"frame", message_set.e("MAV_FRAME_GLOBAL_INT")}, {"command", message_set.e("MAV_CMD_NAV_TAKEOFF")}, @@ -292,7 +309,7 @@ TEST_CASE("Mission protocol client") { .in("MISSION_REQUEST_INT", [](auto &msg) { CHECK_EQ(msg.template get("seq"), 1); }) - .out(message_set.create("MISSION_ITEM_INT").set({ + .out(message_set.create("MISSION_ITEM_INT")({ {"seq", 1}, {"frame", message_set.e("MAV_FRAME_GLOBAL_INT")}, {"command", message_set.e("MAV_CMD_NAV_WAYPOINT")}, @@ -312,7 +329,7 @@ TEST_CASE("Mission protocol client") { .in("MISSION_REQUEST_INT", [](auto &msg) { CHECK_EQ(msg.template get("seq"), 2); }) - .out(message_set.create("MISSION_ITEM_INT").set({ + .out(message_set.create("MISSION_ITEM_INT")({ {"seq", 2}, {"frame", message_set.e("MAV_FRAME_GLOBAL_INT")}, {"command", message_set.e("MAV_CMD_NAV_LAND")}, @@ -345,5 +362,300 @@ TEST_CASE("Mission protocol client") { CHECK_EQ(mission[2].name(), "MISSION_ITEM_INT"); CHECK_EQ(mission[2]["command"].as(), message_set.e("MAV_CMD_NAV_LAND")); } + + SUBCASE("Throws on receiving NACK on upload MISSION_COUNT") { + ProtocolTestSequencer server_sequence(server_connection); + server_sequence.in("MISSION_COUNT", [](auto &msg) { + CHECK_EQ(msg.template get("count"), 3); + }).out(message_set.create("MISSION_ACK")({ + {"type", message_set.e("MAV_MISSION_ERROR")}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })); + + mav::mission::MissionClient mission_client(client_connection, message_set); + server_sequence.start(); + CHECK_THROWS_AS(mission_client.upload(dummy_mission_long), mav::ProtocolException); + server_sequence.finish(); + } + + SUBCASE("Throws on receiving NACK on upload MISSION_ITEM") { + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("MISSION_COUNT", [](auto &msg) { + CHECK_EQ(msg.template get("count"), 3); + }) + .out(message_set.create("MISSION_REQUEST_INT")({ + {"seq", 0}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })) + .in("MISSION_ITEM_INT", [](auto &msg) { + CHECK_EQ(msg.template get("seq"), 0); + }) + .out(message_set.create("MISSION_ACK")({ + {"type", message_set.e("MAV_MISSION_ERROR")}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })); + + mav::mission::MissionClient mission_client(client_connection, message_set); + server_sequence.start(); + CHECK_THROWS_AS(mission_client.upload(dummy_mission_long), mav::ProtocolException); + server_sequence.finish(); + } + + SUBCASE("Throws on a timeout after not receiving responses on upload") { + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("MISSION_COUNT", [](auto &msg) { + CHECK_EQ(msg.template get("count"), 3); + }) + .out(message_set.create("MISSION_REQUEST_INT")({ + {"seq", 0}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })) + .in("MISSION_ITEM_INT", [](auto &msg) { + CHECK_EQ(msg.template get("seq"), 0); + }); + + mav::mission::MissionClient mission_client(client_connection, message_set); + server_sequence.start(); + CHECK_THROWS_AS(mission_client.upload(dummy_mission_long), mav::TimeoutException); + server_sequence.finish(); + } + + SUBCASE("Re-tries 3 times when no response from server") { + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("MISSION_COUNT") + .in("MISSION_COUNT") + .in("MISSION_COUNT") + .out(message_set.create("MISSION_REQUEST_INT")({ + {"seq", 0}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })) + .in("MISSION_ITEM_INT") + .in("MISSION_ITEM_INT") + .in("MISSION_ITEM_INT") + .out(message_set.create("MISSION_ACK")({ + {"type", message_set.e("MAV_MISSION_ACCEPTED")}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })); + + mav::mission::MissionClient mission_client(client_connection, message_set); + server_sequence.start(); + mission_client.upload(dummy_mission_short); + server_sequence.finish(); + } + + SUBCASE("Throws on early-accept of mission 1") { + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("MISSION_COUNT", [](auto &msg) { + CHECK_EQ(msg.template get("count"), 3); + }) + .out(message_set.create("MISSION_ACK")({ + {"type", message_set.e("MAV_MISSION_ACCEPTED")}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })); + + mav::mission::MissionClient mission_client(client_connection, message_set); + server_sequence.start(); + CHECK_THROWS_AS(mission_client.upload(dummy_mission_long), mav::ProtocolException); + server_sequence.finish(); + } + + SUBCASE("Throws on early-accept of mission 2") { + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("MISSION_COUNT", [](auto &msg) { + CHECK_EQ(msg.template get("count"), 3); + }) + .out(message_set.create("MISSION_REQUEST_INT")({ + {"seq", 0}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })) + .in("MISSION_ITEM_INT", [](auto &msg) { + CHECK_EQ(msg.template get("seq"), 0); + }) + .out(message_set.create("MISSION_ACK")({ + {"type", message_set.e("MAV_MISSION_ACCEPTED")}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })); + + mav::mission::MissionClient mission_client(client_connection, message_set); + server_sequence.start(); + CHECK_THROWS_AS(mission_client.upload(dummy_mission_long), mav::ProtocolException); + server_sequence.finish(); + } + + SUBCASE("Happy to re-upload missed items") { + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("MISSION_COUNT", [](auto &msg) { + CHECK_EQ(msg.template get("count"), 3); + }) + .out(message_set.create("MISSION_REQUEST_INT")({ + {"seq", 0}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })) + .in("MISSION_ITEM_INT", [](auto &msg) { + CHECK_EQ(msg.template get("seq"), 0); + }) + .out(message_set.create("MISSION_REQUEST_INT")({ + {"seq", 0}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })) + .in("MISSION_ITEM_INT", [](auto &msg) { + CHECK_EQ(msg.template get("seq"), 0); + }).out(message_set.create("MISSION_REQUEST_INT")({ + {"seq", 1}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })).in("MISSION_ITEM_INT", [](auto &msg) { + CHECK_EQ(msg.template get("seq"), 1); + }).out(message_set.create("MISSION_REQUEST_INT")({ + {"seq", 2}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })).in("MISSION_ITEM_INT", [](auto &msg) { + CHECK_EQ(msg.template get("seq"), 2); + }).out(message_set.create("MISSION_REQUEST_INT")({ + {"seq", 2}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })).in("MISSION_ITEM_INT", [](auto &msg) { + CHECK_EQ(msg.template get("seq"), 2); + }).out(message_set.create("MISSION_ACK")({ + {"type", message_set.e("MAV_MISSION_ACCEPTED")}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })); + + mav::mission::MissionClient mission_client(client_connection, message_set); + server_sequence.start(); + mission_client.upload(dummy_mission_long); + server_sequence.finish(); + } + + SUBCASE("Throws on NACK on download mission count") { + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("MISSION_REQUEST_LIST") + .out(message_set.create("MISSION_ACK")({ + {"type", message_set.e("MAV_MISSION_ERROR")}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })); + + mav::mission::MissionClient mission_client(client_connection, message_set); + server_sequence.start(); + CHECK_THROWS_AS(mission_client.download(), mav::ProtocolException); + server_sequence.finish(); + } + + SUBCASE("Throws on NACK on download mission request item") { + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("MISSION_REQUEST_LIST") + .out(message_set.create("MISSION_COUNT")({ + {"count", 3}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })) + .in("MISSION_REQUEST_INT", [](auto &msg) { + CHECK_EQ(msg.template get("seq"), 0); + }) + .out(message_set.create("MISSION_ACK")({ + {"type", message_set.e("MAV_MISSION_ERROR")}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })); + + mav::mission::MissionClient mission_client(client_connection, message_set); + server_sequence.start(); + CHECK_THROWS_AS(mission_client.download(), mav::ProtocolException); + server_sequence.finish(); + } + + + SUBCASE("Download times out if no response") { + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("MISSION_REQUEST_LIST") + .out(message_set.create("MISSION_COUNT")({ + {"count", 3}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })) + .in("MISSION_REQUEST_INT", [](auto &msg) { + CHECK_EQ(msg.template get("seq"), 0); + }); + + mav::mission::MissionClient mission_client(client_connection, message_set); + server_sequence.start(); + CHECK_THROWS_AS(mission_client.download(), mav::TimeoutException); + server_sequence.finish(); + } + + SUBCASE("Download tries three times to download an item before giving up") { + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("MISSION_REQUEST_LIST") + .out(message_set.create("MISSION_COUNT")({ + {"count", 1}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })) + .in("MISSION_REQUEST_INT") + .in("MISSION_REQUEST_INT") + .in("MISSION_REQUEST_INT") + .out(message_set.create("MISSION_ITEM_INT")({ + {"seq", 0}, + {"frame", message_set.e("MAV_FRAME_GLOBAL_INT")}, + {"command", message_set.e("MAV_CMD_NAV_WAYPOINT")}, + {"autocontinue", 1}, + {"mission_type", message_set.e("MAV_MISSION_TYPE_MISSION")}, + {"target_system", LIBMAV_DEFAULT_ID}, + {"target_component", LIBMAV_DEFAULT_ID} + })) + .in("MISSION_ACK"); + + mav::mission::MissionClient mission_client(client_connection, message_set); + server_sequence.start(); + auto mission = mission_client.download(); + server_sequence.finish(); + CHECK_EQ(mission.size(), 1); + } + } From 7f8b61c20af96e1761e75011bc908fae37f23028 Mon Sep 17 00:00:00 2001 From: Thomas Debrunner Date: Wed, 6 Dec 2023 15:13:48 +0100 Subject: [PATCH 5/8] protocols: add license headers --- .../opinionated_protocols/MessageBuilder.h | 36 ++++++++++++++++-- .../mav/opinionated_protocols/MissionClient.h | 36 ++++++++++++++++-- .../MissionMessageBuilders.h | 36 ++++++++++++++++-- .../mav/opinionated_protocols/ParamClient.h | 36 ++++++++++++++++-- .../mav/opinionated_protocols/ProtocolUtils.h | 38 +++++++++++++++++-- tests/opinionated_protocols/MissionClient.cpp | 34 +++++++++++++++++ tests/opinionated_protocols/ParamClient.cpp | 36 ++++++++++++++++-- .../ProtocolTestSequencer.h | 36 ++++++++++++++++-- 8 files changed, 266 insertions(+), 22 deletions(-) diff --git a/include/mav/opinionated_protocols/MessageBuilder.h b/include/mav/opinionated_protocols/MessageBuilder.h index 635dcc4..d54bfcd 100644 --- a/include/mav/opinionated_protocols/MessageBuilder.h +++ b/include/mav/opinionated_protocols/MessageBuilder.h @@ -1,6 +1,36 @@ -// -// Created by thomas on 20.11.23. -// +/**************************************************************************** + * + * Copyright (c) 2023, libmav development team + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name libmav nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ #ifndef MAV_MESSAGEBUILDER_H #define MAV_MESSAGEBUILDER_H diff --git a/include/mav/opinionated_protocols/MissionClient.h b/include/mav/opinionated_protocols/MissionClient.h index ae0db9c..59cdae4 100644 --- a/include/mav/opinionated_protocols/MissionClient.h +++ b/include/mav/opinionated_protocols/MissionClient.h @@ -1,6 +1,36 @@ -// -// Created by thomas on 01.12.23. -// +/**************************************************************************** + * + * Copyright (c) 2023, libmav development team + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name libmav nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ #ifndef MAV_MISSIONCLIENT_H #define MAV_MISSIONCLIENT_H diff --git a/include/mav/opinionated_protocols/MissionMessageBuilders.h b/include/mav/opinionated_protocols/MissionMessageBuilders.h index 9e8f506..f857a24 100644 --- a/include/mav/opinionated_protocols/MissionMessageBuilders.h +++ b/include/mav/opinionated_protocols/MissionMessageBuilders.h @@ -1,6 +1,36 @@ -// -// Created by thomas on 20.11.23. -// +/**************************************************************************** + * + * Copyright (c) 2023, libmav development team + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name libmav nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ #ifndef MAV_MISSION_PROTOCOL_MESSAGES_H #define MAV_MISSION_PROTOCOL_MESSAGES_H diff --git a/include/mav/opinionated_protocols/ParamClient.h b/include/mav/opinionated_protocols/ParamClient.h index ee9daba..fe6ab68 100644 --- a/include/mav/opinionated_protocols/ParamClient.h +++ b/include/mav/opinionated_protocols/ParamClient.h @@ -1,6 +1,36 @@ -// -// Created by thomas on 04.12.23. -// +/**************************************************************************** + * + * Copyright (c) 2023, libmav development team + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name libmav nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ #ifndef MAV_PARAMCLIENT_H #define MAV_PARAMCLIENT_H diff --git a/include/mav/opinionated_protocols/ProtocolUtils.h b/include/mav/opinionated_protocols/ProtocolUtils.h index 107715b..124a4e6 100644 --- a/include/mav/opinionated_protocols/ProtocolUtils.h +++ b/include/mav/opinionated_protocols/ProtocolUtils.h @@ -1,6 +1,36 @@ -// -// Created by thomas on 01.12.23. -// +/**************************************************************************** + * + * Copyright (c) 2023, libmav development team + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name libmav nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ #ifndef MAV_PROTOCOLUTILS_H #define MAV_PROTOCOLUTILS_H @@ -85,7 +115,7 @@ namespace mav { } } } - throw ProtocolException("Function failed after " + std::to_string(retries) + " retries"); + throw TimeoutException(std::string{"Function failed after " + std::to_string(retries) + " retries"}.c_str()); } inline Message exchangeRetry( diff --git a/tests/opinionated_protocols/MissionClient.cpp b/tests/opinionated_protocols/MissionClient.cpp index fc4f71d..51541b7 100644 --- a/tests/opinionated_protocols/MissionClient.cpp +++ b/tests/opinionated_protocols/MissionClient.cpp @@ -1,3 +1,37 @@ +/**************************************************************************** + * + * Copyright (c) 2023, libmav development team + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name libmav nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + #include "../doctest.h" #include "mav/opinionated_protocols/MissionClient.h" #include "mav/TCPClient.h" diff --git a/tests/opinionated_protocols/ParamClient.cpp b/tests/opinionated_protocols/ParamClient.cpp index 3b3e6d0..3287bb0 100644 --- a/tests/opinionated_protocols/ParamClient.cpp +++ b/tests/opinionated_protocols/ParamClient.cpp @@ -1,4 +1,34 @@ -// -// Created by thomas on 04.12.23. -// +/**************************************************************************** + * + * Copyright (c) 2023, libmav development team + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name libmav nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ #include "mav/opinionated_protocols/ParamClient.h" diff --git a/tests/opinionated_protocols/ProtocolTestSequencer.h b/tests/opinionated_protocols/ProtocolTestSequencer.h index 4c062ff..34b8825 100644 --- a/tests/opinionated_protocols/ProtocolTestSequencer.h +++ b/tests/opinionated_protocols/ProtocolTestSequencer.h @@ -1,6 +1,36 @@ -// -// Created by thomas on 04.12.23. -// +/**************************************************************************** + * + * Copyright (c) 2023, libmav development team + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name libmav nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ #ifndef MAV_PROTOCOLTESTSEQUENCER_H #define MAV_PROTOCOLTESTSEQUENCER_H From a701a334b52ac774325c41da5035e252dfce8176 Mon Sep 17 00:00:00 2001 From: Thomas Debrunner Date: Mon, 22 Jan 2024 16:51:44 +0100 Subject: [PATCH 6/8] mission-upload: better handling preliminary ack --- .../mav/opinionated_protocols/MissionClient.h | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/include/mav/opinionated_protocols/MissionClient.h b/include/mav/opinionated_protocols/MissionClient.h index 59cdae4..75e5a2e 100644 --- a/include/mav/opinionated_protocols/MissionClient.h +++ b/include/mav/opinionated_protocols/MissionClient.h @@ -78,6 +78,13 @@ namespace mission { // nothing to do return; } + + // assert all messages are MISSION_ITEM_INT + for (const auto& message : mission_messages) { + throwAssert(message.id() == _message_set.idForMessage("MISSION_ITEM_INT"), + "Mission upload only supports MISSION_ITEM_INT messages"); + } + // get mission type from first message int mission_type = mission_messages[0]["mission_type"]; @@ -93,6 +100,8 @@ namespace mission { target.system_id, target.component_id, retry_count, item_timeout); _assertNotNack(count_response); + // also an ack is always bad here, as we have at least one item to upload + throwAssert(count_response.id() == _message_set.idForMessage("MISSION_REQUEST_INT"), "Unexpected message"); throwAssert(count_response["mission_type"].as() == mission_type, "Mission type mismatch"); throwAssert(count_response["seq"].as() == 0, "Sequence number mismatch"); @@ -109,12 +118,15 @@ namespace mission { // NACK is always bad, throw if we receive NACK _assertNotNack(item_response); - if (seq == static_cast(mission_messages.size()) - 1) { + if (item_response.id() == _message_set.idForMessage("MISSION_ACK")) { // we're okay with an ack, only when we're at the last message - if (item_response.id() == _message_set.idForMessage("MISSION_ACK")) { + if (seq == static_cast(mission_messages.size()) - 1) { break; + } else { + throw mav::ProtocolException("Received preliminary ACK from server."); } } + // in general, we need a mission request int throwAssert(item_response.id() == _message_set.idForMessage("MISSION_REQUEST_INT"), "Unexpected message"); // we expect a request for the next message From 152d0fea7e23ed3c025361cac7a412657a8212e7 Mon Sep 17 00:00:00 2001 From: Thomas Debrunner Date: Mon, 26 Feb 2024 17:05:13 +0100 Subject: [PATCH 7/8] protocols: added tests for param protocol --- .../mav/opinionated_protocols/ParamClient.h | 5 +- tests/opinionated_protocols/ParamClient.cpp | 289 ++++++++++++++++++ 2 files changed, 293 insertions(+), 1 deletion(-) diff --git a/include/mav/opinionated_protocols/ParamClient.h b/include/mav/opinionated_protocols/ParamClient.h index fe6ab68..a6a59ea 100644 --- a/include/mav/opinionated_protocols/ParamClient.h +++ b/include/mav/opinionated_protocols/ParamClient.h @@ -67,6 +67,7 @@ namespace param { mav::Message readRaw(mav::Message &message, int target_system = mav::ANY_ID, int target_component = mav::ANY_ID, int retry_count=3, int item_timeout=1000) { throwAssert(message.name() == "PARAM_REQUEST_READ", "Message must be of type PARAM_REQUEST_READ"); auto response = exchangeRetry(_connection, message, "PARAM_VALUE", target_system, target_component, retry_count, item_timeout); + throwAssert(response["param_id"].as() == message["param_id"].as(), "Parameter ID mismatch"); return response; } @@ -84,7 +85,9 @@ namespace param { throwAssert(message.name() == "PARAM_SET", "Message must be of type PARAM_SET"); auto response = exchangeRetry(_connection, message, "PARAM_VALUE", target_system, target_component, retry_count, item_timeout); throwAssert(response["param_id"].as() == message["param_id"].as(), "Parameter ID mismatch"); - throwAssert(response["param_type"].as() == message["param_type"].as(), "Parameter type mismatch"); + throwAssert(response["param_type"].as() == message["param_type"].as(), "Parameter type mismatch"); + // compare as floats, as that's the native type for both types + throwAssert(response["param_value"].as() == message["param_value"].as(), "Parameter value mismatch. Setting param failed."); return response; } diff --git a/tests/opinionated_protocols/ParamClient.cpp b/tests/opinionated_protocols/ParamClient.cpp index 3287bb0..82b2c16 100644 --- a/tests/opinionated_protocols/ParamClient.cpp +++ b/tests/opinionated_protocols/ParamClient.cpp @@ -31,4 +31,293 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ + +#include "../doctest.h" #include "mav/opinionated_protocols/ParamClient.h" +#include "mav/TCPClient.h" +#include "mav/TCPServer.h" +#include "ProtocolTestSequencer.h" + +using namespace mav; +#define PORT 13975 + + +TEST_CASE("Param client") { + MessageSet message_set; + message_set.addFromXMLString(R""""( + + + + Specifies the datatype of a MAVLink parameter. + + 8-bit unsigned integer + + + 8-bit signed integer + + + 16-bit unsigned integer + + + 16-bit signed integer + + + 32-bit unsigned integer + + + 32-bit signed integer + + + 64-bit unsigned integer + + + 64-bit signed integer + + + 32-bit floating-point + + + 64-bit floating-point + + + + + + Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + Autopilot type / class. defined in MAV_AUTOPILOT ENUM + System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h + A bitfield for use for autopilot-specific flags. + System status flag, see MAV_STATE ENUM + MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version + + + Request to read the onboard parameter with the param_id string id. Onboard parameters are stored as key[const char*] -> value[float]. This allows to send a parameter to any other component (such as the GCS) without the need of previous knowledge of possible parameter names. Thus the same GCS can store different parameters for different autopilots. See also https://mavlink.io/en/services/parameter.html for a full documentation of QGroundControl and IMU code. + System ID + Component ID + Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) + + + Request all parameters of this component. After this request, all parameters are emitted. The parameter microservice is documented at https://mavlink.io/en/services/parameter.html + System ID + Component ID + + + Emit the value of a onboard parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows him to re-request missing parameters after a loss or timeout. The parameter microservice is documented at https://mavlink.io/en/services/parameter.html + Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Onboard parameter value + Onboard parameter type. + Total number of onboard parameters + Index of this onboard parameter + + + Set a parameter value (write new value to permanent storage). + The receiving component should acknowledge the new parameter value by broadcasting a PARAM_VALUE message (broadcasting ensures that multiple GCS all have an up-to-date list of all parameters). If the sending GCS did not receive a PARAM_VALUE within its timeout time, it should re-send the PARAM_SET message. The parameter microservice is documented at https://mavlink.io/en/services/parameter.html. + PARAM_SET may also be called within the context of a transaction (started with MAV_CMD_PARAM_TRANSACTION). Within a transaction the receiving component should respond with PARAM_ACK_TRANSACTION to the setter component (instead of broadcasting PARAM_VALUE), and PARAM_SET should be re-sent if this is ACK not received. + System ID + Component ID + Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Onboard parameter value + Onboard parameter type. + + + +)""""); + + mav::TCPServer server_physical(PORT); + mav::NetworkRuntime server_runtime({1, 1}, message_set, server_physical); + + std::promise connection_called_promise; + auto connection_called_future = connection_called_promise.get_future(); + server_runtime.onConnection([&connection_called_promise](const std::shared_ptr&) { + connection_called_promise.set_value(); + }); + + // setup client + auto heartbeat = message_set.create("HEARTBEAT")({ + {"type", 1}, + {"autopilot", 2}, + {"base_mode", 3}, + {"custom_mode", 4}, + {"system_status", 5}, + {"mavlink_version", 6}}); + + mav::TCPClient client_physical("localhost", PORT); + mav::NetworkRuntime client_runtime(message_set, heartbeat, client_physical); + + CHECK((connection_called_future.wait_for(std::chrono::seconds(2)) != std::future_status::timeout)); + + auto server_connection = server_runtime.awaitConnection(100); + server_connection->send(heartbeat); + + auto client_connection = client_runtime.awaitConnection(100); + + + SUBCASE("Can read float param") { + + // mock the server response + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("PARAM_REQUEST_READ", [](const mav::Message &msg) { + CHECK_EQ(msg["param_index"].as(), -1); + CHECK_EQ(msg["param_id"].as(), "DUMMY_PARAM"); + }) + .out(message_set.create("PARAM_VALUE")({ + {"param_id", "DUMMY_PARAM"}, + {"param_value", 3.14f}, + {"param_type", message_set.e("MAV_PARAM_TYPE_REAL32")}, + {"param_count", 1}, + {"param_index", 0} + })); + + server_sequence.start(); + mav::param::ParamClient param_client(client_connection, message_set); + auto param = param_client.read("DUMMY_PARAM"); + server_sequence.finish(); + CHECK_EQ(std::holds_alternative(param), true); + CHECK_EQ(std::get(param), 3.14f); + } + + + SUBCASE("Can read int param") { + // mock the server response + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("PARAM_REQUEST_READ", [](const mav::Message &msg) { + CHECK_EQ(msg["param_index"].as(), -1); + CHECK_EQ(msg["param_id"].as(), "DUMMY_PARAM"); + }) + .out(message_set.create("PARAM_VALUE")({ + {"param_id", "DUMMY_PARAM"}, + {"param_value", mav::floatPack(42)}, + {"param_type", message_set.e("MAV_PARAM_TYPE_INT32")}, + {"param_count", 1}, + {"param_index", 0} + })); + + server_sequence.start(); + mav::param::ParamClient param_client(client_connection, message_set); + auto param = param_client.read("DUMMY_PARAM"); + server_sequence.finish(); + CHECK_EQ(std::holds_alternative(param), true); + CHECK_EQ(std::get(param), 42); + } + + + SUBCASE("Throws on receiving wrong param") { + // mock the server response + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("PARAM_REQUEST_READ", [](const mav::Message &msg) { + CHECK_EQ(msg["param_index"].as(), -1); + CHECK_EQ(msg["param_id"].as(), "DUMMY_PARAM"); + }) + .out(message_set.create("PARAM_VALUE")({ + {"param_id", "WRONG_PARAM"}, + {"param_value", 3.14f}, + {"param_type", message_set.e("MAV_PARAM_TYPE_REAL32")}, + {"param_count", 1}, + {"param_index", 0} + })); + + server_sequence.start(); + mav::param::ParamClient param_client(client_connection, message_set); + CHECK_THROWS_AS(param_client.read("DUMMY_PARAM"), mav::ProtocolException); + server_sequence.finish(); + } + + SUBCASE("Read tries 3 times to obtain the parameter") { + // mock the server response + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("PARAM_REQUEST_READ", [](const mav::Message &msg) { + CHECK_EQ(msg["param_index"].as(), -1); + CHECK_EQ(msg["param_id"].as(), "DUMMY_PARAM"); + }) + .in("PARAM_REQUEST_READ", [](const mav::Message &msg) { + CHECK_EQ(msg["param_index"].as(), -1); + CHECK_EQ(msg["param_id"].as(), "DUMMY_PARAM"); + }) + .in("PARAM_REQUEST_READ", [](const mav::Message &msg) { + CHECK_EQ(msg["param_index"].as(), -1); + CHECK_EQ(msg["param_id"].as(), "DUMMY_PARAM"); + }) + .out(message_set.create("PARAM_VALUE")({ + {"param_id", "DUMMY_PARAM"}, + {"param_value", 3.14f}, + {"param_type", message_set.e("MAV_PARAM_TYPE_REAL32")}, + {"param_count", 1}, + {"param_index", 0} + })); + + server_sequence.start(); + mav::param::ParamClient param_client(client_connection, message_set); + auto param = param_client.read("DUMMY_PARAM"); + server_sequence.finish(); + CHECK_EQ(std::holds_alternative(param), true); + CHECK_EQ(std::get(param), 3.14f); + } + + SUBCASE("Read times out if no response") { + // mock the server response + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("PARAM_REQUEST_READ", [](const mav::Message &msg) { + CHECK_EQ(msg["param_index"].as(), -1); + CHECK_EQ(msg["param_id"].as(), "DUMMY_PARAM"); + }); + + server_sequence.start(); + mav::param::ParamClient param_client(client_connection, message_set); + CHECK_THROWS_AS(param_client.read("DUMMY_PARAM"), mav::TimeoutException); + server_sequence.finish(); + } + + + SUBCASE("Can write float param") { + // mock the server response + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("PARAM_SET", [&message_set](const mav::Message &msg) { + CHECK_EQ(msg["param_id"].as(), "DUMMY_PARAM"); + CHECK_EQ(msg["param_type"].as(), message_set.e("MAV_PARAM_TYPE_REAL32")); + CHECK_EQ(msg["param_value"].as(), 3.14f); + }) + .out(message_set.create("PARAM_VALUE")({ + {"param_id", "DUMMY_PARAM"}, + {"param_value", 3.14f}, + {"param_type", message_set.e("MAV_PARAM_TYPE_REAL32")}, + {"param_count", 1}, + {"param_index", 0} + })); + + server_sequence.start(); + mav::param::ParamClient param_client(client_connection, message_set); + param_client.write("DUMMY_PARAM", 3.14f); + server_sequence.finish(); + } + + SUBCASE("Can write int param") { + // mock the server response + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("PARAM_SET", [&message_set](const mav::Message &msg) { + CHECK_EQ(msg["param_id"].as(), "DUMMY_PARAM"); + CHECK_EQ(msg["param_type"].as(), message_set.e("MAV_PARAM_TYPE_INT32")); + CHECK_EQ(msg["param_value"].floatUnpack(), 42); + }) + .out(message_set.create("PARAM_VALUE")({ + {"param_id", "DUMMY_PARAM"}, + {"param_value", mav::floatPack(42)}, + {"param_type", message_set.e("MAV_PARAM_TYPE_INT32")}, + {"param_count", 1}, + {"param_index", 0} + })); + + server_sequence.start(); + mav::param::ParamClient param_client(client_connection, message_set); + param_client.write("DUMMY_PARAM", 42); + server_sequence.finish(); } +} + + From d891d1a826aa720ab84e262830cc3eb28402f557 Mon Sep 17 00:00:00 2001 From: Thomas Debrunner Date: Tue, 27 Feb 2024 09:18:11 +0100 Subject: [PATCH 8/8] protocols: do not copy on exception rethrow --- include/mav/opinionated_protocols/ProtocolUtils.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/mav/opinionated_protocols/ProtocolUtils.h b/include/mav/opinionated_protocols/ProtocolUtils.h index 124a4e6..8126aec 100644 --- a/include/mav/opinionated_protocols/ProtocolUtils.h +++ b/include/mav/opinionated_protocols/ProtocolUtils.h @@ -111,7 +111,7 @@ namespace mav { return func(args...); } catch (const TimeoutException& e) { if (i == retries - 1) { - throw e; + throw; } } }