WIP: Added first stab at higher level mission protocol implementation

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 <typename BuilderType>
+    class MessageBuilder {
+    protected:
+        mav::Message _message;
+
+        template <typename ARG>
+        BuilderType& _set(const std::string& key, ARG value) {
+            _message[key] = value;
+            return *static_cast<BuilderType*>(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

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 <vector>
+#include <memory>
+#include "MissionMessageBuilders.h"
+#include "ProtocolUtils.h"
+
+
+namespace mav {
+namespace mission {
+    class MissionClient {
+    private:
+        std::shared_ptr<mav::Connection> _connection;
+        const MessageSet& _message_set;
+
+    public:
+        MissionClient(std::shared_ptr<Connection> 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<Message> &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<int>() == mission_type, "Mission type mismatch");
+            throwAssert(count_response["seq"].as<int>() == 0, "Sequence number mismatch");
+
+            int seq = 0;
+            while (seq < static_cast<int>(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<int>(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<int>(mission_messages.size()) - 1) {
+                    // we expect an ack for the last message
+                    throwAssert(item_response["type"].as<int>() == 0, "Mission upload failed");
+                    seq++;
+                } else {
+                    // we expect a request for the next message
+                    throwAssert(item_response["mission_type"].as<int>() == mission_type, "Mission type mismatch");
+                    seq = item_response["seq"];
+                }
+            }
+        }
+
+        std::vector<Message> 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<int>() == 0, "Mission type mismatch");
+
+            int count = request_list_response["count"];
+            std::vector<Message> 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<int>() == 0, "Mission type mismatch");
+                throwAssert(request_response["seq"].as<int>() == seq, "Sequence number mismatch");
+
+                mission_messages.push_back(request_response);
+            }
+            return mission_messages;
+        }
+    };
+};
+}
+
+
+#endif //MAV_MISSIONCLIENT_H

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 <typename T>
+        class MissionItemIntMessage : public MessageBuilder<T> {
+        public:
+            explicit MissionItemIntMessage(const MessageSet &message_set) :
+                    MessageBuilder<T>(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<int32_t>(latitude * 1e7));
+            }
+
+            auto &longitude_deg(double longitude) {
+                return this->_set("y", static_cast<int32_t>(longitude * 1e7));
+            }
+
+            auto &altitude_m(double altitude) {
+                return this->_set("z", altitude);
+            }
+        };
+
+        class TakeoffMessage : public MissionItemIntMessage<TakeoffMessage> {
+        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<LandMessage> {
+        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<WaypointMessage> {
+        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

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 <string>
+#include <stdexcept>
+#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<std::string> &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<mav::Connection> 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<mav::Connection>& 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

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)