Merge pull request #1770 from junwoo091400/pr_followme_updates

Update Follow me code to match upstream changes

examples/follow_me/follow_me.cpp

@@ -129,11 +129,11 @@ int main(int argc, char** argv)
     std::cout << "In Air...\n";
     sleep_for(seconds(5)); // Wait for drone to reach takeoff altitude
 
-    // Configure Min height of the drone to be "20 meters" above home & Follow direction as "Front
-    // right".
+    // Configure Follow height of the drone to be "20 meters"
+    // And Follow direction as "Behind" (180 degrees)
     FollowMe::Config config;
-    config.min_height_m = 10.0;
-    config.follow_direction = FollowMe::Config::FollowDirection::Behind;
+    config.follow_height_m = 10.0;
+    config.follow_angle_deg = 180.0f;
     FollowMe::Result follow_me_result = follow_me.set_config(config);
 
     if (follow_me_result != FollowMe::Result::Success) {

src/integration_tests/follow_me.cpp

@@ -3,22 +3,33 @@
 #include <thread>
 #include <chrono>
 #include <array>
+
 #include "integration_test_helper.h"
+
 #include "mavsdk.h"
 #include "plugins/telemetry/telemetry.h"
 #include "plugins/action/action.h"
 #include "plugins/follow_me/follow_me.h"
+#include "plugins/info/info.h"
 
 using namespace mavsdk;
 using namespace std::chrono;
 using namespace std::this_thread;
 
+/* Updated FollowMe only works from PX4 v1.13, as it went through refactoring */
+const int32_t PX4_SW_MAJOR_MINIMUM = 1;
+const int32_t PX4_SW_MINOR_MINIMUM = 13;
+bool autopilot_sw_ver_minimum_satisfied(const std::shared_ptr<Info> info);
+
+/* Auxilary Functions */
 void print(const FollowMe::Config& config);
+
 void send_location_updates(
     std::shared_ptr<FollowMe> follow_me, size_t count = 25ul, float rate = 2.f);
 
 const size_t N_LOCATIONS = 100ul;
 
+/* Test FollowMe with a stationary target at one location */
 TEST_F(SitlTest, PX4FollowMeOneLocation)
 {
     Mavsdk mavsdk;
@@ -34,6 +45,11 @@ TEST_F(SitlTest, PX4FollowMeOneLocation)
     auto telemetry = std::make_shared<Telemetry>(system);
     auto follow_me = std::make_shared<FollowMe>(system);
     auto action = std::make_shared<Action>(system);
+    auto info = std::make_shared<Info>(system);
+
+    if (!autopilot_sw_ver_minimum_satisfied(info)) {
+        GTEST_SKIP();
+    }
 
     LogInfo() << "Waiting for system to be ready";
     ASSERT_TRUE(poll_condition_with_timeout(
@@ -95,6 +111,7 @@ TEST_F(SitlTest, PX4FollowMeOneLocation)
     }
 }
 
+/* Test FollowMe with a dynamically moving target */
 TEST_F(SitlTest, PX4FollowMeMultiLocationWithConfig)
 {
     Mavsdk mavsdk;
@@ -121,6 +138,11 @@ TEST_F(SitlTest, PX4FollowMeMultiLocationWithConfig)
     auto telemetry = std::make_shared<Telemetry>(system);
     auto follow_me = std::make_shared<FollowMe>(system);
     auto action = std::make_shared<Action>(system);
+    auto info = std::make_shared<Info>(system);
+
+    if (!autopilot_sw_ver_minimum_satisfied(info)) {
+        GTEST_SKIP();
+    }
 
     LogInfo() << "Waiting for system to be ready";
     ASSERT_TRUE(poll_condition_with_timeout(
@@ -148,11 +170,10 @@ TEST_F(SitlTest, PX4FollowMeMultiLocationWithConfig)
 
     // configure follow me behaviour
     FollowMe::Config config;
-    config.min_height_m = 12.f; // increase min height
+    config.follow_height_m = 12.f; // increase min height
     config.follow_distance_m = 20.f; // set distance b/w system and target during FollowMe mode
-    config.responsiveness = 0.2f; // set to higher responsiveness
-    config.follow_direction =
-        FollowMe::Config::FollowDirection::Front; // System follows target from FRONT side
+    config.responsiveness = 0.2f; // Make it less responsive (higher value for the setting)
+    config.follow_angle_deg = 0.0; // System follows target from FRONT side
 
     // Apply configuration
     FollowMe::Result config_result = follow_me->set_config(config);
@@ -184,10 +205,10 @@ void print(const FollowMe::Config& config)
 {
     std::cout << "Current FollowMe configuration of the system" << '\n';
     std::cout << "---------------------------" << '\n';
-    std::cout << "Min Height: " << config.min_height_m << "m" << '\n';
+    std::cout << "Height: " << config.follow_height_m << "m" << '\n';
     std::cout << "Distance: " << config.follow_distance_m << "m" << '\n';
+    std::cout << "Following angle: " << config.follow_angle_deg << "[deg]" << '\n';
     std::cout << "Responsiveness: " << config.responsiveness << '\n';
-    std::cout << "Following from: " << config.follow_direction << '\n';
     std::cout << "---------------------------" << '\n';
 }
 
@@ -203,6 +224,29 @@ FollowMe::TargetLocation create_target_location(double latitude_deg, double long
     return location;
 }
 
+bool autopilot_sw_ver_minimum_satisfied(const std::shared_ptr<Info> info)
+{
+    EXPECT_TRUE(poll_condition_with_timeout(
+        [&]() { return info->get_version().first == Info::Result::Success; },
+        std::chrono::seconds(5)));
+
+    // Check PX4 version running and if too low, skip the test
+    std::pair<Info::Result, Info::Version> version_result = info->get_version();
+    EXPECT_EQ(version_result.first, Info::Result::Success);
+
+    if (version_result.second.flight_sw_major < PX4_SW_MAJOR_MINIMUM) {
+        return false; // Major version not satisfied
+
+    } else if (version_result.second.flight_sw_major > PX4_SW_MAJOR_MINIMUM) {
+        return true; // Major version satisfied
+
+    } else if (version_result.second.flight_sw_minor >= PX4_SW_MINOR_MINIMUM) {
+        return true; // Major version same, minor version satisfied
+    }
+
+    return false;
+}
+
 void send_location_updates(std::shared_ptr<FollowMe> follow_me, size_t count, float rate)
 {
     // TODO: Generate these co-ordinates from an algorithm

src/mavsdk/plugins/follow_me/follow_me.cpp

@@ -57,42 +57,44 @@ FollowMe::Result FollowMe::stop() const
 }
 
 std::ostream&
-operator<<(std::ostream& str, FollowMe::Config::FollowDirection const& follow_direction)
+operator<<(std::ostream& str, FollowMe::Config::FollowAltitudeMode const& follow_altitude_mode)
 {
-    switch (follow_direction) {
-        case FollowMe::Config::FollowDirection::None:
-            return str << "None";
-        case FollowMe::Config::FollowDirection::Behind:
-            return str << "Behind";
-        case FollowMe::Config::FollowDirection::Front:
-            return str << "Front";
-        case FollowMe::Config::FollowDirection::FrontRight:
-            return str << "Front Right";
-        case FollowMe::Config::FollowDirection::FrontLeft:
-            return str << "Front Left";
+    switch (follow_altitude_mode) {
+        case FollowMe::Config::FollowAltitudeMode::Constant:
+            return str << "Constant";
+        case FollowMe::Config::FollowAltitudeMode::Terrain:
+            return str << "Terrain";
+        case FollowMe::Config::FollowAltitudeMode::TargetGps:
+            return str << "Target Gps";
         default:
             return str << "Unknown";
     }
 }
 bool operator==(const FollowMe::Config& lhs, const FollowMe::Config& rhs)
 {
-    return ((std::isnan(rhs.min_height_m) && std::isnan(lhs.min_height_m)) ||
-            rhs.min_height_m == lhs.min_height_m) &&
+    return ((std::isnan(rhs.follow_height_m) && std::isnan(lhs.follow_height_m)) ||
+            rhs.follow_height_m == lhs.follow_height_m) &&
            ((std::isnan(rhs.follow_distance_m) && std::isnan(lhs.follow_distance_m)) ||
             rhs.follow_distance_m == lhs.follow_distance_m) &&
-           (rhs.follow_direction == lhs.follow_direction) &&
            ((std::isnan(rhs.responsiveness) && std::isnan(lhs.responsiveness)) ||
-            rhs.responsiveness == lhs.responsiveness);
+            rhs.responsiveness == lhs.responsiveness) &&
+           (rhs.altitude_mode == lhs.altitude_mode) &&
+           ((std::isnan(rhs.max_tangential_vel_m_s) && std::isnan(lhs.max_tangential_vel_m_s)) ||
+            rhs.max_tangential_vel_m_s == lhs.max_tangential_vel_m_s) &&
+           ((std::isnan(rhs.follow_angle_deg) && std::isnan(lhs.follow_angle_deg)) ||
+            rhs.follow_angle_deg == lhs.follow_angle_deg);
 }
 
 std::ostream& operator<<(std::ostream& str, FollowMe::Config const& config)
 {
     str << std::setprecision(15);
     str << "config:" << '\n' << "{\n";
-    str << "    min_height_m: " << config.min_height_m << '\n';
+    str << "    follow_height_m: " << config.follow_height_m << '\n';
     str << "    follow_distance_m: " << config.follow_distance_m << '\n';
-    str << "    follow_direction: " << config.follow_direction << '\n';
     str << "    responsiveness: " << config.responsiveness << '\n';
+    str << "    altitude_mode: " << config.altitude_mode << '\n';
+    str << "    max_tangential_vel_m_s: " << config.max_tangential_vel_m_s << '\n';
+    str << "    follow_angle_deg: " << config.follow_angle_deg << '\n';
     str << '}';
     return str;
 }

src/mavsdk/plugins/follow_me/follow_me_impl.cpp

@@ -43,37 +43,53 @@ void FollowMeImpl::deinit()
 void FollowMeImpl::enable()
 {
     _parent->get_param_float_async(
-        "NAV_MIN_FT_HT",
+        "FLW_TGT_HT",
         [this](MAVLinkParameters::Result result, float value) {
             if (result == MAVLinkParameters::Result::Success) {
-                _config.min_height_m = value;
+                _config.follow_height_m = value;
             }
         },
         this);
     _parent->get_param_float_async(
-        "NAV_FT_DST",
+        "FLW_TGT_DST",
         [this](MAVLinkParameters::Result result, float value) {
             if (result == MAVLinkParameters::Result::Success) {
                 _config.follow_distance_m = value;
             }
         },
         this);
-    _parent->get_param_int_async(
-        "NAV_FT_FS",
-        [this](MAVLinkParameters::Result result, int32_t value) {
+    _parent->get_param_float_async(
+        "FLW_TGT_FA",
+        [this](MAVLinkParameters::Result result, float value) {
             if (result == MAVLinkParameters::Result::Success) {
-                _config.follow_direction = static_cast<FollowMe::Config::FollowDirection>(value);
+                _config.follow_angle_deg = value;
             }
         },
         this);
     _parent->get_param_float_async(
-        "NAV_FT_RS",
+        "FLW_TGT_RS",
         [this](MAVLinkParameters::Result result, float value) {
             if (result == MAVLinkParameters::Result::Success) {
                 _config.responsiveness = value;
             }
         },
         this);
+    _parent->get_param_int_async(
+        "FLW_TGT_ALT_M",
+        [this](MAVLinkParameters::Result result, int value) {
+            if (result == MAVLinkParameters::Result::Success) {
+                _config.altitude_mode = static_cast<FollowMe::Config::FollowAltitudeMode>(value);
+            }
+        },
+        this);
+    _parent->get_param_float_async(
+        "FLW_TGT_MAX_VEL",
+        [this](MAVLinkParameters::Result result, float value) {
+            if (result == MAVLinkParameters::Result::Success) {
+                _config.max_tangential_vel_m_s = value;
+            }
+        },
+        this);
 }
 
 void FollowMeImpl::disable()
@@ -94,49 +110,70 @@ FollowMe::Result FollowMeImpl::set_config(const FollowMe::Config& config)
         return FollowMe::Result::SetConfigFailed;
     }
 
-    auto height = config.min_height_m;
+    auto height = config.follow_height_m;
     auto distance = config.follow_distance_m;
-    int32_t direction = static_cast<int32_t>(config.follow_direction);
     auto responsiveness = config.responsiveness;
+    auto altitude_mode = config.altitude_mode;
+    auto max_tangential_vel_m_s = config.max_tangential_vel_m_s;
 
     LogDebug() << "Waiting for the system confirmation of the new configuration..";
 
     bool success = true;
 
     // Send configuration to Vehicle
-    if (_config.min_height_m != height) {
-        if (_parent->set_param_float("NAV_MIN_FT_HT", height) ==
-            MAVLinkParameters::Result::Success) {
-            _config.min_height_m = height;
+    if (_config.follow_height_m != height) {
+        if (_parent->set_param_float("FLW_TGT_HT", height) == MAVLinkParameters::Result::Success) {
+            _config.follow_height_m = height;
         } else {
             success = false;
         }
     }
+
     if (_config.follow_distance_m != distance) {
-        if (_parent->set_param_float("NAV_FT_DST", distance) ==
+        if (_parent->set_param_float("FLW_TGT_DST", distance) ==
             MAVLinkParameters::Result::Success) {
             _config.follow_distance_m = distance;
         } else {
             success = false;
         }
     }
-    if (_config.follow_direction != config.follow_direction) {
-        if (_parent->set_param_int("NAV_FT_FS", direction) == MAVLinkParameters::Result::Success) {
-            _config.follow_direction = static_cast<FollowMe::Config::FollowDirection>(direction);
 
+    if (_config.follow_angle_deg != config.follow_angle_deg) {
+        if (_parent->set_param_float("FLW_TGT_FA", config.follow_angle_deg) ==
+            MAVLinkParameters::Result::Success) {
+            _config.follow_angle_deg = config.follow_angle_deg;
         } else {
             success = false;
         }
     }
+
     if (_config.responsiveness != responsiveness) {
-        if (_parent->set_param_float("NAV_FT_RS", responsiveness) ==
+        if (_parent->set_param_float("FLW_TGT_RS", responsiveness) ==
             MAVLinkParameters::Result::Success) {
             _config.responsiveness = responsiveness;
         } else {
             success = false;
         }
     }
 
+    if (_config.altitude_mode != altitude_mode) {
+        if (_parent->set_param_int("FLW_TGT_ALT_M", static_cast<int32_t>(altitude_mode)) ==
+            MAVLinkParameters::Result::Success) {
+            _config.altitude_mode = altitude_mode;
+        } else {
+            success = false;
+        }
+    }
+
+    if (_config.max_tangential_vel_m_s != max_tangential_vel_m_s) {
+        if (_parent->set_param_float("FLW_TGT_MAX_VEL", max_tangential_vel_m_s) ==
+            MAVLinkParameters::Result::Success) {
+            _config.max_tangential_vel_m_s = max_tangential_vel_m_s;
+        } else {
+            success = false;
+        }
+    }
+
     return (success ? FollowMe::Result::Success : FollowMe::Result::SetConfigFailed);
 }
 
@@ -215,18 +252,22 @@ bool FollowMeImpl::is_config_ok(const FollowMe::Config& config) const
 {
     auto config_ok = false;
 
-    if (config.min_height_m < CONFIG_MIN_HEIGHT_M) {
-        LogErr() << debug_str << "Err: Min height must be atleast 8.0 meters";
+    if (config.follow_height_m < CONFIG_MIN_HEIGHT_M) {
+        LogErr() << debug_str << "Err: Min height must be at least " << CONFIG_MIN_HEIGHT_M
+                 << " meters";
     } else if (config.follow_distance_m < CONFIG_MIN_FOLLOW_DIST_M) {
-        LogErr() << debug_str << "Err: Min Follow distance must be atleast 1.0 meter";
-    } else if (
-        config.follow_direction < FollowMe::Config::FollowDirection::None ||
-        config.follow_direction > FollowMe::Config::FollowDirection::FrontLeft) {
-        LogErr() << debug_str << "Err: Invalid Follow direction";
+        LogErr() << debug_str << "Err: Min Follow distance must be at least "
+                 << CONFIG_MIN_FOLLOW_DIST_M << " meters";
     } else if (
         config.responsiveness < CONFIG_MIN_RESPONSIVENESS ||
         config.responsiveness > CONFIG_MAX_RESPONSIVENESS) {
-        LogErr() << debug_str << "Err: Responsiveness must be in range (0.0 to 1.0)";
+        LogErr() << debug_str << "Err: Responsiveness must be in range ("
+                 << CONFIG_MIN_RESPONSIVENESS << " to " << CONFIG_MAX_RESPONSIVENESS << ")";
+    } else if (
+        config.follow_angle_deg < CONFIG_MIN_FOLLOW_ANGLE ||
+        config.follow_angle_deg > CONFIG_MAX_FOLLOW_ANGLE) {
+        LogErr() << debug_str << "Err: Follow Angle must be in range " << CONFIG_MIN_FOLLOW_ANGLE
+                 << " to " << CONFIG_MAX_FOLLOW_ANGLE << " degrees!";
     } else { // Config is OK
         config_ok = true;
     }