From 4f6a6886c058bf6233767fb7d8d763b280d43b71 Mon Sep 17 00:00:00 2001 From: Thomas Debrunner Date: Wed, 6 Dec 2023 14:40:56 +0100 Subject: [PATCH] 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); + } + }