Add Follow me example

example/follow_me/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 2.8.12)
+
+project(follow_me)
+
+find_package(Boost 1.66 COMPONENTS REQUIRED system)
+include_directories(${Boost_INCLUDE_DIR})
+
+if(NOT MSVC)
+    add_definitions("-std=c++11 -Wall -Wextra -Werror")
+else()
+    add_definitions("-std=c++11 -WX -W2")
+    include_directories(${CMAKE_SOURCE_DIR}/../../install/include)
+    link_directories(${CMAKE_SOURCE_DIR}/../../install/lib)
+endif()
+
+add_executable(follow_me
+    follow_me.cpp
+    fake_location_provider.cpp
+)
+
+target_link_libraries(follow_me
+    LINK_PUBLIC ${Boost_LIBRARIES}
+    dronecore
+    pthread
+)

example/follow_me/fake_location_provider.cpp

@@ -0,0 +1,60 @@
+
+#include "fake_location_provider.h"
+
+void FakeLocationProvider::request_location_updates(location_callback_t callback)
+{
+    location_callback_ = callback;
+    timer_.async_wait(std::bind(&FakeLocationProvider::compute_next_location, this));
+}
+
+// Rudimentary location provider whose successive lat, lon combination
+// makes Drone revolve in a semi-circular path.
+void FakeLocationProvider::compute_next_location()
+{
+    if (count_++ < 10) {
+        location_callback_(latitude_deg_, longitude_deg_);
+        latitude_deg_ -= LATITUDE_DEG_PER_METER * 4;
+        timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
+        timer_.async_wait(std::bind(&FakeLocationProvider::compute_next_location, this));
+        sleep(1);
+    }
+    if (count_++ < 20) {
+        location_callback_(latitude_deg_, longitude_deg_);
+        longitude_deg_ += LONGITUDE_DEG_PER_METER * 4;
+        timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
+        timer_.async_wait(std::bind(&FakeLocationProvider::compute_next_location, this));
+        sleep(1);
+    }
+    if (count_++ < 30) {
+        location_callback_(latitude_deg_, longitude_deg_);
+        latitude_deg_ += LATITUDE_DEG_PER_METER * 4;
+        timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
+        timer_.async_wait(std::bind(&FakeLocationProvider::compute_next_location, this));
+        sleep(1);
+    }
+    if (count_++ < 40) {
+        location_callback_(latitude_deg_, longitude_deg_);
+        longitude_deg_ -= LONGITUDE_DEG_PER_METER * 4;
+        timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
+        timer_.async_wait(std::bind(&FakeLocationProvider::compute_next_location, this));
+        sleep(1);
+    }
+    if (count_++ < 50) {
+        location_callback_(latitude_deg_, longitude_deg_);
+        latitude_deg_ -= LATITUDE_DEG_PER_METER * 3;
+        timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
+        timer_.async_wait(std::bind(&FakeLocationProvider::compute_next_location, this));
+        sleep(1);
+    }
+    if (count_++ < MAX_LOCATIONS) {
+        location_callback_(latitude_deg_, longitude_deg_);
+        longitude_deg_ += LONGITUDE_DEG_PER_METER * 3;
+        timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
+        timer_.async_wait(std::bind(&FakeLocationProvider::compute_next_location, this));
+        sleep(1);
+    }
+}
+
+const size_t FakeLocationProvider::MAX_LOCATIONS = 60u;
+const double FakeLocationProvider::LATITUDE_DEG_PER_METER = 0.000009044;
+const double FakeLocationProvider::LONGITUDE_DEG_PER_METER = 0.000008985;

example/follow_me/fake_location_provider.h

@@ -0,0 +1,48 @@
+#pragma once
+
+#include <functional>
+/**
+  ********************************************************************************************
+  ********************************************************************************************
+  Important note: Boost isn't a dependency for DroneCore library.
+  We're using Boost::Asio in this example ONLY to simulate asynchronous Fake location provider.
+  Applications on platforms Android, Windows, Apple, etc should make use of their platform-specific
+  Location Provider in place of FakeLocationProvider.
+  ********************************************************************************************
+  ********************************************************************************************
+  */
+#include <boost/asio.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+
+/**
+ * @brief The FakeLocationProvider class
+ * This class provides periodic reports on the fake location of the device.
+ */
+class FakeLocationProvider
+{
+public:
+    typedef std::function<void(double lat, double lon)> location_callback_t;
+
+    FakeLocationProvider(boost::asio::io_context &io)
+        : timer_(io, boost::posix_time::seconds(1))
+    {}
+
+    ~FakeLocationProvider()
+    {}
+
+    void request_location_updates(location_callback_t callback);
+
+private:
+
+    void compute_next_location();
+
+    boost::asio::deadline_timer timer_;
+    location_callback_t location_callback_ = nullptr;
+    double latitude_deg_ = 47.3977419;
+    double longitude_deg_ = 8.5455938;
+    size_t count_ = 0u;
+
+    static const size_t MAX_LOCATIONS;
+    static const double LATITUDE_DEG_PER_METER;
+    static const double LONGITUDE_DEG_PER_METER;
+};

example/follow_me/follow_me.cpp

@@ -0,0 +1,138 @@
+/**
+* @file follow_me.cpp
+*
+* @brief Example that demonstrates the usage of Follow Me plugin.
+* The example registers with FakeLocationProvider for location updates
+* and sends them to the Follow Me plugin which are sent to drone. You can observe
+* drone following you. We print last location of the drone in flight mode callback.
+*
+* @author Shakthi Prashanth <shakthi.prashanth.m@intel.com>
+* @date 2018-01-03
+*/
+
+#include <dronecore/dronecore.h>
+#include <iostream>
+#include <thread>
+#include <memory>
+#include <chrono>
+#include "fake_location_provider.h"
+
+using namespace dronecore;
+using namespace std::placeholders; // for `_1`
+using namespace std::chrono; // for seconds(), milliseconds(), etc
+using namespace std::this_thread;  // for sleep_for()
+
+// For coloring output
+#define ERROR_CONSOLE_TEXT "\033[31m" //Turn text on console red
+#define TELEMETRY_CONSOLE_TEXT "\033[34m" //Turn text on console blue
+#define NORMAL_CONSOLE_TEXT "\033[0m"  //Restore normal console colour
+
+inline void action_error_exit(Action::Result result, const std::string &message);
+inline void follow_me_error_exit(FollowMe::Result result, const std::string &message);
+inline void connection_error_exit(DroneCore::ConnectionResult result, const std::string &message);
+
+int main(int, char **)
+{
+    DroneCore dc;
+
+    DroneCore::ConnectionResult conn_result = dc.add_udp_connection();
+    connection_error_exit(conn_result, "Connection failed");
+
+    // Wait for the device to connect via heartbeat
+    while (!dc.is_connected()) {
+        std::cout << "Wait for device to connect via heartbeat" << std::endl;
+        sleep_for(seconds(1));
+    }
+
+    // Device got discovered.
+    Device &device = dc.device();
+    while (!device.telemetry().health_all_ok()) {
+        std::cout << "Waiting for device to be ready" << std::endl;
+        sleep_for(seconds(1));
+    }
+    std::cout << "Device is ready" << std::endl;
+
+    // Arm
+    Action::Result arm_result = device.action().arm();
+    action_error_exit(arm_result, "Arming failed");
+    std::cout << "Armed" << std::endl;
+
+    // Subscribe to receive updates on flight mode. You can find out whether FollowMe is active.
+    device.telemetry().flight_mode_async(
+    std::bind([&](Telemetry::FlightMode flight_mode) {
+        const FollowMe::TargetLocation last_location = device.follow_me().get_last_location();
+        std::cout << "[FlightMode: " << Telemetry::flight_mode_str(flight_mode)
+                  << "] Vehicle is at: " << last_location.latitude_deg << ", "
+                  << last_location.longitude_deg << " degrees." << std::endl;
+    }, std::placeholders::_1));
+
+    // Takeoff
+    Action::Result takeoff_result = device.action().takeoff();
+    action_error_exit(takeoff_result, "Takeoff failed");
+    std::cout << "In Air..." << std::endl;
+    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".
+    FollowMe::Config config;
+    config.min_height_m = 20.0;
+    config.follow_direction = FollowMe::Config::FollowDirection::FRONT_RIGHT;
+    FollowMe::Result follow_me_result = device.follow_me().set_config(config);
+
+    // Start Follow Me
+    follow_me_result = device.follow_me().start();
+    follow_me_error_exit(follow_me_result, "Failed to start FollowMe mode");
+
+    boost::asio::io_context io; // for event loop
+    std::unique_ptr<FakeLocationProvider> location_provider(new FakeLocationProvider(io));
+    // Register for platform-specific Location provider. We're using FakeLocationProvider for the example.
+    location_provider->request_location_updates([&device](double lat, double lon) {
+        device.follow_me().set_target_location({lat, lon, 0.0, 0.f, 0.f, 0.f});
+    });
+    io.run(); // will run as long as location updates continue to happen.
+
+    // Stop Follow Me
+    follow_me_result = device.follow_me().stop();
+    follow_me_error_exit(follow_me_result, "Failed to stop FollowMe mode");
+
+    // Stop flight mode updates.
+    device.telemetry().flight_mode_async(nullptr);
+
+    // Land
+    const Action::Result land_result = device.action().land();
+    action_error_exit(land_result, "Landing failed");
+    while (device.telemetry().in_air()) {
+        std::cout << "waiting until landed" << std::endl;
+        sleep_for(seconds(1));
+    }
+    std::cout << "Landed..." << std::endl;
+    return 0;
+}
+
+// Handles Action's result
+inline void action_error_exit(Action::Result result, const std::string &message)
+{
+    if (result != Action::Result::SUCCESS) {
+        std::cerr << ERROR_CONSOLE_TEXT << message << Action::result_str(
+                      result) << NORMAL_CONSOLE_TEXT << std::endl;
+        exit(EXIT_FAILURE);
+    }
+}
+// Handles FollowMe's result
+inline void follow_me_error_exit(FollowMe::Result result, const std::string &message)
+{
+    if (result != FollowMe::Result::SUCCESS) {
+        std::cerr << ERROR_CONSOLE_TEXT << message << FollowMe::result_str(
+                      result) << NORMAL_CONSOLE_TEXT << std::endl;
+        exit(EXIT_FAILURE);
+    }
+}
+// Handles connection result
+inline void connection_error_exit(DroneCore::ConnectionResult result, const std::string &message)
+{
+    if (result != DroneCore::ConnectionResult::SUCCESS) {
+        std::cerr << ERROR_CONSOLE_TEXT << message
+                  << DroneCore::connection_result_str(result)
+                  << NORMAL_CONSOLE_TEXT << std::endl;
+        exit(EXIT_FAILURE);
+    }
+}

integration_tests/follow_me.cpp

@@ -111,7 +111,7 @@ TEST_F(SitlTest, FollowMeMultiLocationWithConfig)
     // configure follow me behaviour
     FollowMe::Config config;
     config.min_height_m = 12.f; // increase min height
-    config.follow_dist_m = 20.f; // set distance b/w device and target during FollowMe mode
+    config.follow_distance_m = 20.f; // set distance b/w device and target during FollowMe mode
     config.responsiveness = 0.2f; // set to higher responsiveness
     config.follow_direction =
         FollowMe::Config::FollowDirection::FRONT; // Device follows target from FRONT side
@@ -120,12 +120,6 @@ TEST_F(SitlTest, FollowMeMultiLocationWithConfig)
     FollowMe::Result config_result = device.follow_me().set_config(config);
     ASSERT_EQ(FollowMe::Result::SUCCESS, config_result);
 
-    sleep_for(seconds(2)); // until config is applied
-
-    // Verify your configuration
-    auto curr_config = device.follow_me().get_config();
-    print(curr_config);
-
     // Start following
     FollowMe::Result follow_me_result = device.follow_me().start();
     ASSERT_EQ(FollowMe::Result::SUCCESS, follow_me_result);
@@ -148,7 +142,7 @@ void print(const FollowMe::Config &config)
     std::cout << "Current FollowMe configuration of the device" << std::endl;
     std::cout << "---------------------------" << std::endl;
     std::cout << "Min Height: " << config.min_height_m << "m" << std::endl;
-    std::cout << "Distance: " << config.follow_dist_m << "m" << std::endl;
+    std::cout << "Distance: " << config.follow_distance_m << "m" << std::endl;
     std::cout << "Responsiveness: " << config.responsiveness << std::endl;
     std::cout << "Following from: " << FollowMe::Config::to_str(config.follow_direction) << std::endl;
     std::cout << "---------------------------" << std::endl;

plugins/follow_me/follow_me.h

@@ -70,7 +70,7 @@ class FollowMe
         constexpr static const float MAX_RESPONSIVENESS = 1.0f; /**< @brief Max responsiveness. */
 
         float min_height_m = 8.0f; /**< @brief Min follow height, in meters. */
-        float follow_dist_m = 8.0f; /**< @brief Horizontal follow distance to target, in meters. */
+        float follow_distance_m = 8.0f; /**< @brief Horizontal follow distance to target, in meters. */
         FollowDirection follow_direction =
             FollowDirection::BEHIND; /**< @brief Relative position of the following vehicle. */
         float responsiveness = 0.5f; /**< @brief Responsiveness, Range (0.0-1.0) */