Merge pull request #818 from 3drobotics/douglas/clear-mission

Add clear mission support
mavlink · Aug 5, 2019 · 844e0e7 · 844e0e7
2 parents 3755853 + 7c33a11
commit 844e0e7
Show file tree

Hide file tree

Showing 7 changed files with 109 additions and 2 deletions.
diff --git a/src/backend/src/plugins/mission/mission_service_impl.h b/src/backend/src/plugins/mission/mission_service_impl.h
@@ -134,6 +134,28 @@ class MissionServiceImpl final : public mavsdk::rpc::mission::MissionService::Se
         return grpc::Status::OK;
     }
 
+    grpc::Status ClearMission(
+        grpc::ServerContext* /* context */,
+        const rpc::mission::ClearMissionRequest* /* request */,
+        rpc::mission::ClearMissionResponse* response) override
+    {
+        std::promise<void> result_promise;
+        const auto result_future = result_promise.get_future();
+
+        _mission.clear_mission_async(
+            [this, response, &result_promise](const mavsdk::Mission::Result result) {
+                if (response != nullptr) {
+                    auto rpc_mission_result = generateRPCMissionResult(result);
+                    response->set_allocated_mission_result(rpc_mission_result);
+                }
+
+                result_promise.set_value();
+            });
+
+        result_future.wait();
+        return grpc::Status::OK;
+    }
+
     grpc::Status SetCurrentMissionItemIndex(
         grpc::ServerContext* /* context */,
         const rpc::mission::SetCurrentMissionItemIndexRequest* request,

diff --git a/src/integration_tests/mission.cpp b/src/integration_tests/mission.cpp
@@ -249,7 +249,22 @@ void test_mission(
         // Wait until we're done.
         std::this_thread::sleep_for(std::chrono::seconds(1));
     }
-    LogInfo() << "Disarmed, exiting.";
+    LogInfo() << "Disarmed.";
+
+    {
+        LogInfo() << "Clearing mission.";
+        auto prom = std::make_shared<std::promise<void>>();
+        auto future_result = prom->get_future();
+        mission->clear_mission_async([prom](Mission::Result result) {
+            ASSERT_EQ(result, Mission::Result::SUCCESS);
+            prom->set_value();
+            LogInfo() << "Cleared mission, exiting.";
+        });
+
+        auto status = future_result.wait_for(std::chrono::seconds(2));
+        ASSERT_EQ(status, std::future_status::ready);
+        future_result.get();
+    }
 }
 
 std::shared_ptr<MissionItem> add_mission_item(

diff --git a/src/plugins/mission/include/plugins/mission/mission.h b/src/plugins/mission/include/plugins/mission/mission.h
@@ -182,6 +182,13 @@ class Mission : public PluginBase {
      */
     void pause_mission_async(result_callback_t callback);
 
+    /**
+     * @brief Clears the mission saved on the vehicle (asynchronous).
+     *
+     * @param callback Callback to receive result of this request.
+     */
+    void clear_mission_async(result_callback_t callback);
+
     /**
      * @brief Sets the mission item index to go to (asynchronous).
      *

diff --git a/src/plugins/mission/mission.cpp b/src/plugins/mission/mission.cpp
@@ -52,6 +52,11 @@ void Mission::pause_mission_async(result_callback_t callback)
     _impl->pause_mission_async(callback);
 }
 
+void Mission::clear_mission_async(result_callback_t callback)
+{
+    _impl->clear_mission_async(callback);
+}
+
 void Mission::set_current_mission_item_async(int current, result_callback_t callback)
 {
     _impl->set_current_mission_item_async(current, callback);

diff --git a/src/plugins/mission/mission_impl.cpp b/src/plugins/mission/mission_impl.cpp
@@ -1063,6 +1063,59 @@ void MissionImpl::pause_mission_async(const Mission::result_callback_t& callback
         std::bind(&MissionImpl::receive_command_result, this, std::placeholders::_1, callback));
 }
 
+void MissionImpl::clear_mission_async(const Mission::result_callback_t& callback)
+{
+    bool should_report_mission_result = false;
+    {
+        std::lock_guard<std::mutex> lock(_activity.mutex);
+        if (_activity.state == Activity::State::ABORTED) {
+            _activity.state = Activity::State::NONE;
+        }
+
+        if (_activity.state != Activity::State::NONE) {
+            should_report_mission_result = true;
+        }
+    }
+
+    if (should_report_mission_result) {
+        report_mission_result(callback, Mission::Result::BUSY);
+        return;
+    }
+
+    _parent->register_timeout_handler(
+        std::bind(&MissionImpl::process_timeout, this), RETRY_TIMEOUT_S, &_timeout_cookie);
+
+    {
+        std::lock_guard<std::mutex> lock(_activity.mutex);
+        _activity.state = Activity::State::MISSION_CLEAR;
+    }
+    {
+        std::lock_guard<std::recursive_mutex> lock(_mission_data.mutex);
+        _mission_data.result_callback = callback;
+        _mission_data.retries = 0;
+    }
+
+    clear_mission();
+}
+
+void MissionImpl::clear_mission()
+{
+    mavlink_message_t message;
+    mavlink_msg_mission_clear_all_pack(
+        _parent->get_own_system_id(),
+        _parent->get_own_component_id(),
+        &message,
+        _parent->get_system_id(),
+        _parent->get_autopilot_id(),
+        MAV_MISSION_TYPE_MISSION);
+
+    if (!_parent->send_message(message)) {
+        std::lock_guard<std::recursive_mutex> lock(_mission_data.mutex);
+        report_mission_result(_mission_data.result_callback, Mission::Result::ERROR);
+        return;
+    }
+}
+
 void MissionImpl::set_current_mission_item_async(int current, Mission::result_callback_t& callback)
 {
     bool should_report_mission_result = false;

diff --git a/src/plugins/mission/mission_impl.h b/src/plugins/mission/mission_impl.h
@@ -38,6 +38,9 @@ class MissionImpl : public PluginImplBase {
 
     void start_mission_async(const Mission::result_callback_t& callback);
     void pause_mission_async(const Mission::result_callback_t& callback);
+    void clear_mission_async(const Mission::result_callback_t& callback);
+
+    void clear_mission();
 
     void set_current_mission_item_async(int current, Mission::result_callback_t& callback);
 
@@ -105,7 +108,8 @@ class MissionImpl : public PluginImplBase {
             GET_MISSION_LIST,
             GET_MISSION_REQUEST,
             ABORTED,
-            SEND_COMMAND
+            SEND_COMMAND,
+            MISSION_CLEAR
         } state{Activity::State::NONE};
     } _activity{};
 

diff --git a/src/plugins/mission/mocks/mission_mock.h b/src/plugins/mission/mocks/mission_mock.h
@@ -19,6 +19,7 @@ class MockMission {
     MOCK_CONST_METHOD0(download_mission_cancel, void()){};
     MOCK_CONST_METHOD1(start_mission_async, void(Mission::result_callback_t)){};
     MOCK_CONST_METHOD1(pause_mission_async, void(Mission::result_callback_t)){};
+    MOCK_CONST_METHOD1(clear_mission_async, void(Mission::result_callback_t)){};
     MOCK_CONST_METHOD2(
         set_current_mission_item_async, void(int current, Mission::result_callback_t)){};
     MOCK_CONST_METHOD0(current_mission_item, int()){};