Example: Follow Me Mode

This example demonstrates how to use the Follow Me plugin. It shows how to send the drone both the current position of the target (FollowMe::TargetLocation) and the relative position at which it should follow (FollowMe::Config).

Follow Me QGC Screenshot

A real application using this API will get the position information from the underlying operating system. The example uses a fake position source (FakeLocationProvider) to enable it to be run on computers that do not have position information. The FakeLocationProvider emulates the typical usage of common positioning APIs used in Android, Linux and iPhone.

Running the Example

Special notes for this example:

  • Before running this example you will need to install Boost libraries. For Linux this is done as shown below:
    sudo apt-get install libboost-all-dev
    
  • To use QGroundControl with this example you must ensure that GSC Position Streaming is disabled (otherwise QGC and the SDK will both send position updates and they will conflict). To do this use the latest QGC Daily Build and ensure that the Application Setting > General > Miscellaneous > Stream GCS Position is set to Never.

Otherwise the example is built and run in the normal way (as described here).

The example terminal output should be similar to that shown below:

This is from a debug build of the SDK. A release build will omit the "Debug" messages.

$ ./follow_me udp://:14540
Wait for system to connect via heartbeat
[02:47:19|Info ] New device on: 127.0.0.1:14557 (udp_connection.cpp:206)
[02:47:19|Debug] New: System ID: 1 Comp ID: 1 (dronecode_sdk_impl.cpp:310)
[02:47:19|Debug] Component Autopilot added. (mavlink_system.cpp:326)
[02:47:19|Debug] MAVLink: info: [logger] file: rootfs/fs/microsd/log/2018-05-02/0 (mavlink_system.cpp:263)
[02:47:20|Debug] Found 1 component(s). (mavlink_system.cpp:458)
[02:47:20|Debug] Discovered 4294967298 (mavlink_system.cpp:460)
[02:47:20|Info ] FollowMe: Applying default FollowMe configuration FollowMe to the system... (follow_me_impl.cpp:186)
Waiting for system to be ready
System is ready
Armed
[02:47:22|Debug] MAVLink: info: ARMED by arm/disarm component command (mavlink_system.cpp:286)
In Air...
[02:47:23|Debug] MAVLink: info: Using minimum takeoff altitude: 2.50 m (mavlink_system.cpp:286)
[02:47:23|Debug] MAVLink: info: Takeoff detected (mavlink_system.cpp:286)
[02:47:23|Debug] MAVLink: critical: Using minimum takeoff altitude: 2.50 m (mavlink_system.cpp:263)
[FlightMode: Takeoff] Vehicle is at: nan, nan degrees.
[FlightMode: Hold] Vehicle is at: nan, nan degrees.
[02:47:28|Debug] FollowMe: Waiting for the system confirmation of the new configuration.. (follow_me_impl.cpp:98)
[02:47:28|Info ] FollowMe: Configured: Min height: 20 meters, Follow distance: 8 meters, Follow direction: Front right, Responsiveness: 0.5 (follow_me_impl.cpp:101)
[FlightMode: FollowMe] Vehicle is at: nan, nan degrees.
[FlightMode: FollowMe] Vehicle is at: 47.3977, 8.54559 degrees.
[FlightMode: FollowMe] Vehicle is at: 47.3977, 8.54559 degrees.
...
[FlightMode: FollowMe] Vehicle is at: 47.3976, 8.54567 degrees.
[FlightMode: FollowMe] Vehicle is at: 47.3976, 8.5457 degrees.
[FlightMode: FollowMe] Vehicle is at: 47.3976, 8.54573 degrees.
waiting until landed
[12:47:05|Debug] MAVLink: info: Landing at current position (mavlink_system.cpp:286)
waiting until landed
waiting until landed
...
waiting until landed
waiting until landed
[02:48:37|Debug] MAVLink: info: Landing detected (mavlink_system.cpp:263)
waiting until landed
Landed...

How it works

The example registers with FakeLocationProvider for location updates. These are passed to the Follow Me plugin, which in turn sends them to the vehicle.

The operation of the "SDK-specific" part of this code is discussed in the guide: Follow Me.

Source code

The full source code for the example can be found on Github here.

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.12)

project(follow_me)

if (MSVC)
    find_package(Boost 1.64 COMPONENTS system date_time REQUIRED )
    include_directories(${CMAKE_SOURCE_DIR}/../../install/include)
    link_directories(${CMAKE_SOURCE_DIR}/../../install/lib)
    add_definitions("-std=c++11 -WX -W2")
    # This is to avoid auto-linking of libraries that we don't build
    add_definitions("-DBOOST_ALL_NO_LIB")
else()
    find_package(Boost 1.58 COMPONENTS system REQUIRED )
    add_definitions("-std=c++11 -Wall -Wextra -Werror")
endif()

include_directories(${Boost_INCLUDE_DIR})

add_executable(follow_me
    follow_me.cpp
    fake_location_provider.cpp
)

target_link_libraries(follow_me
    LINK_PUBLIC ${Boost_LIBRARIES}
    dronecode_sdk
    dronecode_sdk_action
    dronecode_sdk_follow_me
    dronecode_sdk_telemetry
)

follow_me.cpp

/**
 * @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 <chrono>
#include <dronecode_sdk/action.h>
#include <dronecode_sdk/dronecode_sdk.h>
#include <dronecode_sdk/follow_me.h>
#include <dronecode_sdk/telemetry.h>
#include <iostream>
#include <memory>
#include <thread>

#include "fake_location_provider.h"

using namespace dronecode_sdk;
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(ActionResult result, const std::string &message);
inline void follow_me_error_exit(FollowMe::Result result, const std::string &message);
inline void connection_error_exit(ConnectionResult result, const std::string &message);

void usage(std::string bin_name)
{
    std::cout << NORMAL_CONSOLE_TEXT << "Usage : " << bin_name << " <connection_url>" << std::endl
              << "Connection URL format should be :" << std::endl
              << " For TCP : tcp://[server_host][:server_port]" << std::endl
              << " For UDP : udp://[bind_host][:bind_port]" << std::endl
              << " For Serial : serial:///path/to/serial/dev[:baudrate]" << std::endl
              << "For example, to connect to the simulator use URL: udp://:14540" << std::endl;
}

int main(int argc, char **argv)
{
    DronecodeSDK dc;
    std::string connection_url;
    ConnectionResult connection_result;

    if (argc == 2) {
        connection_url = argv[1];
        connection_result = dc.add_any_connection(connection_url);
    } else {
        usage(argv[0]);
        return 1;
    }

    if (connection_result != ConnectionResult::SUCCESS) {
        std::cout << ERROR_CONSOLE_TEXT
                  << "Connection failed: " << connection_result_str(connection_result)
                  << NORMAL_CONSOLE_TEXT << std::endl;
        return 1;
    }

    // Wait for the system to connect via heartbeat
    while (!dc.is_connected()) {
        std::cout << "Wait for system to connect via heartbeat" << std::endl;
        sleep_for(seconds(1));
    }

    // System got discovered.
    System &system = dc.system();
    auto action = std::make_shared<Action>(system);
    auto follow_me = std::make_shared<FollowMe>(system);
    auto telemetry = std::make_shared<Telemetry>(system);

    while (!telemetry->health_all_ok()) {
        std::cout << "Waiting for system to be ready" << std::endl;
        sleep_for(seconds(1));
    }
    std::cout << "System is ready" << std::endl;

    // Arm
    ActionResult arm_result = 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.
    telemetry->flight_mode_async(std::bind(
        [&](Telemetry::FlightMode flight_mode) {
            const FollowMe::TargetLocation last_location = 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
    ActionResult takeoff_result = 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 = follow_me->set_config(config);

    // Start Follow Me
    follow_me_result = follow_me->start();
    follow_me_error_exit(follow_me_result, "Failed to start FollowMe mode");

    boost::asio::io_service 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([&system, &follow_me](double lat, double lon) {
        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 = follow_me->stop();
    follow_me_error_exit(follow_me_result, "Failed to stop FollowMe mode");

    // Stop flight mode updates.
    telemetry->flight_mode_async(nullptr);

    // Land
    const ActionResult land_result = action->land();
    action_error_exit(land_result, "Landing failed");
    while (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(ActionResult result, const std::string &message)
{
    if (result != ActionResult::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(ConnectionResult result, const std::string &message)
{
    if (result != ConnectionResult::SUCCESS) {
        std::cerr << ERROR_CONSOLE_TEXT << message << connection_result_str(result)
                  << NORMAL_CONSOLE_TEXT << std::endl;
        exit(EXIT_FAILURE);
    }
}

fake_location_provider.h

#pragma once

#include <functional>
/**
  ********************************************************************************************
  ********************************************************************************************
  Important note: Boost isn't a dependency for the Dronecode SDK 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 system.
 */
class FakeLocationProvider {
public:
    typedef std::function<void(double lat, double lon)> location_callback_t;

    FakeLocationProvider(boost::asio::io_service &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;
};

fake_location_provider.cpp

#include "fake_location_provider.h"
#include <chrono> // for seonds()
#include <thread> // for sleep_for()

using std::this_thread::sleep_for;
using std::chrono::seconds;

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_for(seconds(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_for(seconds(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_for(seconds(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_for(seconds(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_for(seconds(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_for(seconds(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;
© Dronecode 2017. License: CC BY 4.0            Updated: 2018-09-20 23:28:43

results matching ""

    No results matching ""