From c1691abd85cc33cdfe0a2b59ee6247c06106c759 Mon Sep 17 00:00:00 2001 From: Thomas Debrunner Date: Fri, 1 Dec 2023 16:43:54 +0100 Subject: [PATCH] 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(); + } +} +