Merge pull request #485 from mzahana/develop

Add Goto Location Action

integration_tests/CMakeLists.txt

@@ -17,6 +17,7 @@ add_executable(integration_tests_runner
     action_hover_sync.cpp
     action_takeoff_and_kill.cpp
     action_transition_multicopter_fixedwing.cpp
+    action_goto.cpp
     calibration.cpp
     camera_mode.cpp
     camera_settings.cpp

integration_tests/action_goto.cpp

@@ -0,0 +1,63 @@
+#include <iostream>
+#include <cmath>
+#include "integration_test_helper.h"
+#include "dronecode_sdk.h"
+#include "plugins/action/action.h"
+#include "plugins/telemetry/telemetry.h"
+
+using namespace dronecode_sdk;
+
+TEST_F(SitlTest, ActionGoto)
+{
+    DronecodeSDK dc;
+
+    ConnectionResult ret = dc.add_udp_connection();
+    ASSERT_EQ(ret, ConnectionResult::SUCCESS);
+
+    // Wait for system to connect via heartbeat.
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+    ASSERT_TRUE(dc.is_connected());
+
+    System &system = dc.system();
+    auto telemetry = std::make_shared<Telemetry>(system);
+
+    int iteration = 0;
+    while (!telemetry->health_all_ok()) {
+        LogInfo() << "waiting for system to be ready";
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+
+        ASSERT_LT(++iteration, 20);
+    }
+
+    auto action = std::make_shared<Action>(system);
+    ActionResult action_ret = action->arm();
+    EXPECT_EQ(action_ret, ActionResult::SUCCESS);
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+
+    action_ret = action->takeoff();
+    EXPECT_EQ(action_ret, ActionResult::SUCCESS);
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    // Go somewhere
+    action->goto_location(47.398000, 8.545592, NAN, NAN);
+    std::this_thread::sleep_for(std::chrono::seconds(10));
+
+    // And back
+    action->goto_location(47.3977233, 8.545592, NAN, NAN);
+    std::this_thread::sleep_for(std::chrono::seconds(10));
+
+    action_ret = action->land();
+    EXPECT_EQ(action_ret, ActionResult::SUCCESS);
+
+    iteration = 0;
+    while (telemetry->in_air()) {
+        LogInfo() << "waiting for system to be landed";
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+
+        // TODO: currently we need to wait a long time until landed is detected.
+        ASSERT_LT(++iteration, 10);
+    }
+
+    action_ret = action->disarm();
+    EXPECT_EQ(action_ret, ActionResult::SUCCESS);
+}

plugins/action/action.cpp

@@ -32,6 +32,14 @@ ActionResult Action::land() const
     return _impl->land();
 }
 
+ActionResult Action::goto_location(double latitude_deg,
+                                   double longitude_deg,
+                                   float altitude_amsl_m,
+                                   float yaw_deg)
+{
+    return _impl->goto_location(latitude_deg, longitude_deg, altitude_amsl_m, yaw_deg);
+}
+
 ActionResult Action::return_to_launch() const
 {
     return _impl->return_to_launch();

plugins/action/action_impl.cpp

@@ -133,6 +133,23 @@ ActionResult ActionImpl::return_to_launch() const
         _parent->set_flight_mode(SystemImpl::FlightMode::RETURN_TO_LAUNCH));
 }
 
+ActionResult ActionImpl::goto_location(double latitude_deg,
+                                       double longitude_deg,
+                                       float altitude_amsl_m,
+                                       float yaw_deg)
+{
+    MAVLinkCommands::CommandInt command{};
+
+    command.command = MAV_CMD_DO_REPOSITION;
+    command.target_component_id = _parent->get_autopilot_id();
+    command.params.param4 = to_rad_from_deg(yaw_deg);
+    command.params.x = (int32_t)(latitude_deg * 1E7);
+    command.params.y = (int32_t)(longitude_deg * 1E7);
+    command.params.z = altitude_amsl_m;
+
+    return action_result_from_command_result(_parent->send_command(command));
+}
+
 ActionResult ActionImpl::transition_to_fixedwing() const
 {
     if (!_vtol_transition_support_known) {

plugins/action/action_impl.h

@@ -26,6 +26,8 @@ class ActionImpl : public PluginImplBase {
     ActionResult takeoff() const;
     ActionResult land() const;
     ActionResult return_to_launch() const;
+    ActionResult
+    goto_location(double latitude_deg, double longitude_deg, float altitude_amsl_m, float yaw_deg);
     ActionResult transition_to_fixedwing() const;
     ActionResult transition_to_multicopter() const;
 

plugins/action/include/plugins/action/action.h

@@ -106,6 +106,20 @@ class Action : public PluginBase {
      */
     ActionResult return_to_launch() const;
 
+    /**
+     * @brief Send command to reposition the vehicle to a specific WGS84 global position
+     *
+     * This sends the vehicle to a specified lattitude/longitude/altitude coordinates.
+     * @param latitude_deg Latitude in degrees
+     * @param longitude_deg Longitude in degrees
+     * @param altitude_amsl_m Altitude AMSL in meters
+     * @param yaw_deg Yaw angle in degrees
+     *
+     * @return ActionResult of request.
+     */
+    ActionResult
+    goto_location(double latitude_deg, double longitude_deg, float altitude_amsl_m, float yaw_deg);
+
     /**
      * @brief Send command to transition the drone to fixedwing.
      *