vpRobotMavsdk.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See https://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Interface to mavlink compatible controller using mavsdk 3rd party
 *
 *****************************************************************************/
 
#include <visp3/core/vpConfig.h>
 
// Check if std:c++17 or higher
#if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) \
  && defined(VISP_HAVE_THREADS)
 
#include <iostream>
#include <math.h>
#include <thread>
 
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/calibration/calibration.h>
#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
#include <mavsdk/plugins/mocap/mocap.h>
#include <mavsdk/plugins/offboard/offboard.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
 
#include <visp3/core/vpExponentialMap.h> // For velocity computation
#include <visp3/robot/vpRobotMavsdk.h>
 
using std::chrono::milliseconds;
using std::chrono::seconds;
using std::this_thread::sleep_for;
using namespace std::chrono_literals;
 
BEGIN_VISP_NAMESPACE
#ifndef DOXYGEN_SHOULD_SKIP_THIS
class vpRobotMavsdk::vpRobotMavsdkImpl
{
public:
  vpRobotMavsdkImpl() : m_takeoffAlt(1.0) { }
  vpRobotMavsdkImpl(const std::string &connection_info) : m_takeoffAlt(1.0) { connect(connection_info); }
 
  virtual ~vpRobotMavsdkImpl()
  {
    if (m_has_flying_capability && m_auto_land) {
      land();
    }
  }
 
private:
  std::shared_ptr<mavsdk::System> getSystem(mavsdk::Mavsdk &mavsdk)
  {
    std::cout << "Waiting to discover system..." << std::endl;
    auto prom = std::promise<std::shared_ptr<mavsdk::System> > {};
    auto fut = prom.get_future();
 
    // We wait for new systems to be discovered, once we find one that has an
    // autopilot, we decide to use it.
#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
    mavsdk::Mavsdk::NewSystemHandle handle = mavsdk.subscribe_on_new_system([&mavsdk, &prom, &handle]() {
#else
    mavsdk.subscribe_on_new_system([&mavsdk, &prom]() {
#endif
      auto system = mavsdk.systems().back();
 
      if (system->has_autopilot()) {
        std::cout << "Discovered autopilot" << std::endl;
 
        // Unsubscribe again as we only want to find one system.
#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
        mavsdk.unsubscribe_on_new_system(handle);
#else
        mavsdk.subscribe_on_new_system(nullptr);
#endif
        prom.set_value(system);
      }
    });
 
    // We usually receive heartbeats at 1Hz, therefore we should find a
    // system after around 3 seconds max, surely.
    if (fut.wait_for(seconds(3)) == std::future_status::timeout) {
      std::cerr << "No autopilot found." << std::endl;
      return {};
    }
 
    // Get discovered system now.
    return fut.get();
  }
 
  MAV_TYPE getVehicleType()
  {
    auto passthrough = mavsdk::MavlinkPassthrough { m_system };
 
    auto prom = std::promise<MAV_TYPE> {};
    auto fut = prom.get_future();
#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
    mavsdk::MavlinkPassthrough::MessageHandle handle = passthrough.subscribe_message(
        MAVLINK_MSG_ID_HEARTBEAT, [&passthrough, &prom, &handle](const mavlink_message_t &message) {
#else
    passthrough.subscribe_message_async(MAVLINK_MSG_ID_HEARTBEAT,
                                        [&passthrough, &prom](const mavlink_message_t &message) {
#endif
          // Process only Heartbeat coming from the autopilot
                                          if (message.compid != MAV_COMP_ID_AUTOPILOT1) {
                                            return;
                                          }
 
                                          mavlink_heartbeat_t heartbeat;
                                          mavlink_msg_heartbeat_decode(&message, &heartbeat);
 
                                      // Unsubscribe again as we only want to find one system.
#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
                                          passthrough.unsubscribe_message(handle);
#else
                                          passthrough.subscribe_message_async(MAVLINK_MSG_ID_HEARTBEAT, nullptr);
#endif
 
                                          prom.set_value(static_cast<MAV_TYPE>(heartbeat.type));
        });
 
    // We usually receive heartbeats at 1Hz, therefore we should find a
    // system after around 3 seconds max, surely.
    if (fut.wait_for(seconds(3)) == std::future_status::timeout) {
      std::cerr << "No heartbeat received to get vehicle type." << std::endl;
      return {};
    }
 
    // Get discovered system now.
    return fut.get();
  }
 
  void calibrate_accelerometer(mavsdk::Calibration &calibration)
  {
    std::cout << "Calibrating accelerometer..." << std::endl;
 
    std::promise<void> calibration_promise;
    auto calibration_future = calibration_promise.get_future();
 
    calibration.calibrate_accelerometer_async(create_calibration_callback(calibration_promise));
 
    calibration_future.wait();
  }
 
  std::function<void(mavsdk::Calibration::Result, mavsdk::Calibration::ProgressData)>
    create_calibration_callback(std::promise<void> &calibration_promise)
  {
    return [&calibration_promise](const mavsdk::Calibration::Result result,
                                  const mavsdk::Calibration::ProgressData progress_data) {
                                    switch (result) {
                                    case mavsdk::Calibration::Result::Success:
                                      std::cout << "--- Calibration succeeded!" << std::endl;
                                      calibration_promise.set_value();
                                      break;
                                    case mavsdk::Calibration::Result::Next:
                                      if (progress_data.has_progress) {
                                        std::cout << "    Progress: " << progress_data.progress << std::endl;
                                      }
                                      if (progress_data.has_status_text) {
                                        std::cout << "    Instruction: " << progress_data.status_text << std::endl;
                                      }
                                      break;
                                    default:
                                      std::cout << "--- Calibration failed with message: " << result << std::endl;
                                      calibration_promise.set_value();
                                      break;
                                    }
      };
  }
 
  void calibrate_gyro(mavsdk::Calibration &calibration)
  {
    std::cout << "Calibrating gyro..." << std::endl;
 
    std::promise<void> calibration_promise;
    auto calibration_future = calibration_promise.get_future();
 
    calibration.calibrate_gyro_async(create_calibration_callback(calibration_promise));
 
    calibration_future.wait();
  }
 
public:
  void connect(const std::string &connectionInfo)
  {
    m_address = connectionInfo;
    mavsdk::ConnectionResult connection_result = m_mavsdk.add_any_connection(connectionInfo);
 
    if (connection_result != mavsdk::ConnectionResult::Success) {
      std::cerr << "Connection failed: " << connection_result << std::endl;
      return;
    }
 
    m_system = getSystem(m_mavsdk);
 
    if (!m_system) {
      throw vpException(vpException::fatalError, "Unable to connect to: %s", connectionInfo.c_str());
    }
 
    m_mav_type = getVehicleType();
 
    m_has_flying_capability = hasFlyingCapability(m_mav_type);
 
    std::cout << (m_has_flying_capability ? "Connected to a flying vehicle" : "Connected to a non flying vehicle")
      << std::endl;
 
    m_action = std::make_shared<mavsdk::Action>(m_system);
    m_telemetry = std::make_shared<mavsdk::Telemetry>(m_system);
    m_offboard = std::make_shared<mavsdk::Offboard>(m_system);
  }
 
  bool hasFlyingCapability(MAV_TYPE mav_type)
  {
    switch (mav_type) {
    case MAV_TYPE::MAV_TYPE_GROUND_ROVER:
    case MAV_TYPE::MAV_TYPE_SURFACE_BOAT:
    case MAV_TYPE::MAV_TYPE_SUBMARINE:
      return false;
    default:
      return true;
    }
  }
 
  bool isRunning() const
  {
    if (m_system == nullptr) {
      return false;
    }
    else {
      return true;
    }
  }
 
  std::string getAddress() const
  {
    std::string sequence;
    std::stringstream ss(m_address);
    std::string actual_address;
    std::getline(ss, sequence, ':');
    if (sequence == "serial" || sequence == "udp" || sequence == "tcp") {
      getline(ss, sequence, ':');
      for (const char &c : sequence) {
        if (c != '/') {
          actual_address.append(1, c);
        }
      }
      return actual_address;
    }
    else {
      std::cout << "ERROR : The address parameter must start with \"serial:\" or \"udp:\" or \"tcp:\"." << std::endl;
      return std::string();
    }
  }
 
  float getBatteryLevel() const
  {
    mavsdk::Telemetry::Battery battery = m_telemetry.get()->battery();
    return battery.voltage_v;
  }
 
  void getPosition(vpHomogeneousMatrix &ned_M_frd) const
  {
    auto quat = m_telemetry.get()->attitude_quaternion();
    auto posvel = m_telemetry.get()->position_velocity_ned();
    vpQuaternionVector q { quat.x, quat.y, quat.z, quat.w };
    vpTranslationVector t { posvel.position.north_m, posvel.position.east_m, posvel.position.down_m };
    ned_M_frd.buildFrom(t, q);
  }
 
  void getPosition(float &ned_north, float &ned_east, float &ned_down, float &ned_yaw) const
  {
    auto odom = m_telemetry.get()->odometry();
    auto angles = m_telemetry.get()->attitude_euler();
    ned_north = odom.position_body.x_m;
    ned_east = odom.position_body.y_m;
    ned_down = odom.position_body.z_m;
    ned_yaw = vpMath::rad(angles.yaw_deg);
  }
 
  std::tuple<float, float> getHome() const
  {
    auto position = m_telemetry.get()->home();
    return { float(position.latitude_deg), float(position.longitude_deg) };
  }
 
  bool sendMocapData(const vpHomogeneousMatrix &enu_M_flu, int display_fps)
  {
    static double time_prev = vpTime::measureTimeMs();
 
    // We suppose here that the body frame which pose is given by the MoCap is FLU (Front Left Up).
    // Thus we need to transform this frame to FRD (Front Right Down).
    vpHomogeneousMatrix flu_M_frd;
    flu_M_frd.eye();
    flu_M_frd[1][1] = -1;
    flu_M_frd[2][2] = -1;
 
    vpHomogeneousMatrix enu_M_frd = enu_M_flu * flu_M_frd;
    auto mocap = mavsdk::Mocap { m_system };
    mavsdk::Mocap::VisionPositionEstimate pose_estimate;
 
    vpHomogeneousMatrix ned_M_frd = vpMath::enu2ned(enu_M_frd);
    vpRxyzVector ned_rxyz_frd = vpRxyzVector(ned_M_frd.getRotationMatrix());
    pose_estimate.angle_body.roll_rad = ned_rxyz_frd[0];
    pose_estimate.angle_body.pitch_rad = ned_rxyz_frd[1];
    pose_estimate.angle_body.yaw_rad = ned_rxyz_frd[2];
 
    vpTranslationVector ned_t_frd = ned_M_frd.getTranslationVector();
    pose_estimate.position_body.x_m = ned_t_frd[0];
    pose_estimate.position_body.y_m = ned_t_frd[1];
    pose_estimate.position_body.z_m = ned_t_frd[2];
 
    pose_estimate.pose_covariance.covariance_matrix.push_back(NAN);
    pose_estimate.time_usec = 0; // We are using the back end timestamp
 
    const mavsdk::Mocap::Result set_position_result = mocap.set_vision_position_estimate(pose_estimate);
    if (set_position_result != mavsdk::Mocap::Result::Success) {
      std::cerr << "Set position failed: " << set_position_result << '\n';
      return false;
    }
    else {
      if (display_fps > 0) {
        double display_time_ms = 1000. / display_fps;
        if (vpTime::measureTimeMs() - time_prev > display_time_ms) {
          time_prev = vpTime::measureTimeMs();
          std::cout << "Send ned_M_frd MoCap data: " << std::endl;
          std::cout << "Translation [m]: " << pose_estimate.position_body.x_m << " , "
            << pose_estimate.position_body.y_m << " , " << pose_estimate.position_body.z_m << std::endl;
          std::cout << "Roll [rad]: " << pose_estimate.angle_body.roll_rad
            << " , Pitch [rad]: " << pose_estimate.angle_body.pitch_rad
            << " , Yaw [rad]: " << pose_estimate.angle_body.yaw_rad << " ." << std::endl;
        }
      }
      return true;
    }
  }
 
  void setTakeOffAlt(double altitude)
  {
    if (altitude > 0) {
      m_takeoffAlt = altitude;
    }
    else {
      std::cerr << "ERROR : The take off altitude must be positive." << std::endl;
    }
  }
 
  void doFlatTrim()
  {
    // Instantiate plugin.
    auto calibration = mavsdk::Calibration(m_system);
 
    // Run calibrations
    calibrate_accelerometer(calibration);
    calibrate_gyro(calibration);
  }
 
  bool arm()
  {
    // Arm vehicle
    std::cout << "Arming...\n";
    const mavsdk::Action::Result arm_result = m_action.get()->arm();
 
    if (arm_result != mavsdk::Action::Result::Success) {
      std::cerr << "Arming failed: " << arm_result << std::endl;
      return false;
    }
    return true;
  }
 
  bool disarm()
  {
    // Arm vehicle
    std::cout << "Disarming...\n";
    const mavsdk::Action::Result arm_result = m_action.get()->disarm();
 
    if (arm_result != mavsdk::Action::Result::Success) {
      std::cerr << "Disarming failed: " << arm_result << std::endl;
      return false;
    }
    return true;
  }
 
  bool setGPSGlobalOrigin(double latitude, double longitude, double altitude)
  {
    auto passthrough = mavsdk::MavlinkPassthrough { m_system };
    mavlink_set_gps_global_origin_t gps_global_origin;
    gps_global_origin.latitude = latitude * 10000000;
    gps_global_origin.longitude = longitude * 10000000;
    gps_global_origin.altitude = altitude * 1000; // in mm
    gps_global_origin.target_system = m_system->get_system_id();
    mavlink_message_t msg;
    mavlink_msg_set_gps_global_origin_encode(passthrough.get_our_sysid(), passthrough.get_our_compid(), &msg,
                                             &gps_global_origin);
    auto resp = passthrough.send_message(msg);
    if (resp != mavsdk::MavlinkPassthrough::Result::Success) {
      std::cerr << "Set GPS global position failed: " << resp << std::endl;
      return false;
    }
    return true;
  }
 
  bool takeControl()
  {
    if (m_verbose) {
      std::cout << "Starting offboard mode..." << std::endl;
    }
 
    if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
      const mavsdk::Offboard::VelocityBodyYawspeed stay {};
      m_offboard.get()->set_velocity_body(stay);
 
      mavsdk::Offboard::Result offboard_result = m_offboard.get()->start();
      if (offboard_result != mavsdk::Offboard::Result::Success) {
        std::cerr << "Offboard mode failed: " << offboard_result << std::endl;
        return false;
      }
    }
    else if (m_verbose) {
      std::cout << "Already in offboard mode" << std::endl;
    }
 
    // Wait to ensure offboard mode active in telemetry
    double t = vpTime::measureTimeMs();
    while (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
      if (vpTime::measureTimeMs() - t > 3. * 1000.) {
        std::cout << "Time out received in takeControl()" << std::endl;
        break;
      }
    };
 
    if (m_verbose) {
      std::cout << "Offboard mode started" << std::endl;
    }
    return true;
  }
 
  void setPositioningIncertitude(float position_incertitude, float yaw_incertitude)
  {
    m_position_incertitude = position_incertitude;
    m_yaw_incertitude = yaw_incertitude;
  }
 
  bool takeOff(bool interactive, int timeout_sec, bool use_gps)
  {
    if (!m_has_flying_capability) {
      std::cerr << "Warning: Cannot takeoff this non flying vehicle" << std::endl;
      return true;
    }
 
    bool authorize_takeoff = false;
 
    if (!interactive) {
      authorize_takeoff = true;
    }
    else {
      if (m_telemetry.get()->flight_mode() == mavsdk::Telemetry::FlightMode::Offboard) {
        authorize_takeoff = true;
      }
      else {
        std::string answer;
        while (answer != "Y" && answer != "y" && answer != "N" && answer != "n") {
          std::cout << "Current flight mode is not the offboard mode. Do you "
            "want to force offboard mode ? (y/n)"
            << std::endl;
          std::cin >> answer;
          if (answer == "Y" || answer == "y") {
            authorize_takeoff = true;
          }
        }
      }
    }
 
    if (m_telemetry.get()->in_air()) {
      std::cerr << "Cannot take off as the robot is already flying." << std::endl;
      return true;
    }
    else if (authorize_takeoff) {
   // Arm vehicle
      if (!arm()) {
        return false;
      }
 
      vpTime::wait(2000);
 
      if (interactive) {
        std::string answer;
        while (answer != "Y" && answer != "y" && answer != "N" && answer != "n") {
          std::cout << "If vehicle armed ? (y/n)" << std::endl;
          std::cin >> answer;
          if (answer == "N" || answer == "n") {
            disarm();
            kill();
            return false;
          }
        }
      }
 
      // Takeoff
      if (m_telemetry.get()->gps_info().fix_type == mavsdk::Telemetry::FixType::NoGps || !use_gps) {
        // No GPS connected.
        // When using odometry from MoCap, Action::takeoff() behavior is to takeoff at 0,0,0,alt
        // that is weird when the drone is not placed at 0,0,0.
        // That's why here use set_position_ned() to takeoff
 
        // Start off-board or guided mode
        takeControl();
 
        auto in_air_promise = std::promise<void> {};
        auto in_air_future = in_air_promise.get_future();
 
        mavsdk::Telemetry::Odometry odom = m_telemetry.get()->odometry();
        vpQuaternionVector q { odom.q.x, odom.q.y, odom.q.z, odom.q.w };
        vpRotationMatrix R(q);
        vpRxyzVector rxyz(R);
 
        double X_init = odom.position_body.x_m;
        double Y_init = odom.position_body.y_m;
        double Z_init = odom.position_body.z_m;
        double yaw_init = vpMath::deg(rxyz[2]);
 
        std::cout << "Takeoff using position NED." << std::endl;
 
        mavsdk::Offboard::PositionNedYaw takeoff {};
        takeoff.north_m = X_init;
        takeoff.east_m = Y_init;
        takeoff.down_m = Z_init - m_takeoffAlt;
        takeoff.yaw_deg = yaw_init;
        m_offboard.get()->set_position_ned(takeoff);
        // Possibility is to use set_position_velocity_ned(); to speed up takeoff
 
#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
        Telemetry::LandedStateHandle handle = m_telemetry.get()->subscribe_landed_state(
            [this, &in_air_promise, &handle](mavsdk::Telemetry::LandedState state) {
              if (state == mavsdk::Telemetry::LandedState::InAir) {
                std::cout << "Drone is taking off\n.";
                m_telemetry.get()->unsubscribe_landed_state(handle);
                in_air_promise.set_value();
              }
            });
#else
        m_telemetry.get()->subscribe_landed_state([this, &in_air_promise](mavsdk::Telemetry::LandedState state) {
          if (state == mavsdk::Telemetry::LandedState::InAir) {
            std::cout << "Drone is taking off\n.";
            m_telemetry.get()->subscribe_landed_state(nullptr);
            in_air_promise.set_value();
          }
          std::cout << "state: " << state << std::endl;
        });
#endif
        if (in_air_future.wait_for(seconds(timeout_sec)) == std::future_status::timeout) {
          std::cerr << "Takeoff failed: drone not in air.\n";
#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
          m_telemetry.get()->unsubscribe_landed_state(handle);
#else
          m_telemetry.get()->subscribe_landed_state(nullptr);
#endif
          return false;
        }
        // Add check with Altitude
        auto takeoff_finished_promise = std::promise<void> {};
        auto takeoff_finished_future = takeoff_finished_promise.get_future();
 
#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
        auto handle_odom = m_telemetry.get()->subscribe_odometry(
            [this, &takeoff_finished_promise, &handle, &Z_init](mavsdk::Telemetry::Odometry odom) {
              if (odom.position_body.z_m < 0.90 * (Z_init - m_takeoffAlt) + m_position_incertitude) {
                std::cout << "Takeoff altitude reached\n.";
                m_telemetry.get()->unsubscribe_odometry(handle_odom);
                takeoff_finished_promise.set_value();
              }
            });
#else
        m_telemetry.get()->subscribe_odometry(
            [this, &takeoff_finished_promise, &Z_init](mavsdk::Telemetry::Odometry odom) {
              if (odom.position_body.z_m < 0.90 * (Z_init - m_takeoffAlt) + m_position_incertitude) {
                std::cout << "Takeoff altitude reached\n.";
                m_telemetry.get()->subscribe_odometry(nullptr);
                takeoff_finished_promise.set_value();
              }
            });
#endif
        if (takeoff_finished_future.wait_for(seconds(timeout_sec)) == std::future_status::timeout) {
          std::cerr << "Takeoff failed:  altitude not reached.\n";
#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
          m_telemetry.get()->unsubscribe_odometry(handle);
#else
          m_telemetry.get()->subscribe_odometry(nullptr);
#endif
          return false;
        }
      }
      else {
     // GPS connected, we use Action::takeoff()
        std::cout << "---- DEBUG: GPS detected: use action::takeoff()" << std::endl;
 
        mavsdk::Telemetry::Odometry odom = m_telemetry.get()->odometry();
        double Z_init = odom.position_body.z_m;
 
        m_action.get()->set_takeoff_altitude(m_takeoffAlt);
        const auto takeoff_result = m_action.get()->takeoff();
        if (takeoff_result != mavsdk::Action::Result::Success) {
          std::cerr << "Takeoff failed: " << takeoff_result << '\n';
          return false;
        }
 
        auto in_air_promise = std::promise<void> {};
        auto in_air_future = in_air_promise.get_future();
#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
        Telemetry::LandedStateHandle handle = m_telemetry.get()->subscribe_landed_state(
            [this, &in_air_promise, &handle](mavsdk::Telemetry::LandedState state) {
              if (state == mavsdk::Telemetry::LandedState::InAir) {
                std::cout << "Taking off has finished\n.";
                m_telemetry.get()->unsubscribe_landed_state(handle);
                in_air_promise.set_value();
              }
            });
#else
        m_telemetry.get()->subscribe_landed_state([this, &in_air_promise](mavsdk::Telemetry::LandedState state) {
          if (state == mavsdk::Telemetry::LandedState::InAir) {
            std::cout << "Taking off has finished\n.";
            m_telemetry.get()->subscribe_landed_state(nullptr);
            in_air_promise.set_value();
          }
          std::cout << "state: " << state << std::endl;
        });
#endif
        if (in_air_future.wait_for(seconds(timeout_sec)) == std::future_status::timeout) {
          // Add check with Altitude
          std::cerr << "Takeoff timed out.\n";
#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
          m_telemetry.get()->unsubscribe_landed_state(handle);
#else
          m_telemetry.get()->subscribe_landed_state(nullptr);
#endif
        }
        // Add check with Altitude
        auto takeoff_finished_promise = std::promise<void> {};
        auto takeoff_finished_future = takeoff_finished_promise.get_future();
 
#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
        auto handle_odom = m_telemetry.get()->subscribe_odometry(
            [this, &takeoff_finished_promise, &handle, &Z_init](mavsdk::Telemetry::Odometry odom) {
              if (odom.position_body.z_m < 0.90 * (Z_init - m_takeoffAlt) + m_position_incertitude) {
                std::cout << "Takeoff altitude reached\n.";
                m_telemetry.get()->unsubscribe_odometry(handle_odom);
                takeoff_finished_promise.set_value();
              }
            });
#else
        m_telemetry.get()->subscribe_odometry(
            [this, &takeoff_finished_promise, &Z_init](mavsdk::Telemetry::Odometry odom) {
              if (odom.position_body.z_m < 0.90 * (Z_init - m_takeoffAlt) + m_position_incertitude) {
                std::cout << "Takeoff altitude reached\n.";
                m_telemetry.get()->subscribe_odometry(nullptr);
                takeoff_finished_promise.set_value();
              }
            });
#endif
        if (takeoff_finished_future.wait_for(seconds(timeout_sec)) == std::future_status::timeout) {
          std::cerr << "Takeoff failed:  altitude not reached.\n";
#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
          m_telemetry.get()->unsubscribe_odometry(handle);
#else
          m_telemetry.get()->subscribe_odometry(nullptr);
#endif
          return false;
        }
        // Start off-board or guided mode
        takeControl();
      }
    }
    return true;
  }
 
  bool land(bool use_buildin = false)
  {
    if (!m_has_flying_capability) {
      std::cerr << "Warning: Cannot land this non flying vehicle" << std::endl;
      return true;
    }
    // Takeoff
    if (!use_buildin) {
      // No GPS connected.
      // When using odometry from MoCap, Action::takeoff() behavior is to
      // takeoff at 0,0,0,alt that is weird when the drone is not placed at
      // 0,0,0. That's why here use set_position_ned() to takeoff
 
      // Start off-board or guided mode
      takeControl();
 
      mavsdk::Telemetry::Odometry odom = m_telemetry.get()->odometry();
      vpQuaternionVector q { odom.q.x, odom.q.y, odom.q.z, odom.q.w };
      vpRotationMatrix R(q);
      vpRxyzVector rxyz(R);
 
      double X_init = odom.position_body.x_m;
      double Y_init = odom.position_body.y_m;
      double yaw_init = vpMath::deg(rxyz[2]);
 
      std::cout << "Landing using position NED." << std::endl;
 
      mavsdk::Offboard::PositionNedYaw landing {};
      landing.north_m = X_init;
      landing.east_m = Y_init;
      landing.down_m = 0.;
      landing.yaw_deg = yaw_init;
      m_offboard.get()->set_position_ned(landing);
      // Possibility is to use set_position_velocity_ned(); to speed up
      bool success = false;
 
      // Add check with Altitude
      auto landing_finished_promise = std::promise<void> {};
      auto landing_finished_future = landing_finished_promise.get_future();
 
#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
      auto handle_odom = m_telemetry.get()->subscribe_odometry(
          [this, &landing_finished_promise, &success, &handle](mavsdk::Telemetry::Odometry odom) {
            if (odom.position_body.z_m > -0.15) {
              std::cout << "Landing altitude reached \n.";
 
              success = true;
              m_telemetry.get()->unsubscribe_odometry(handle_odom);
              landing_finished_promise.set_value();
            }
          });
#else
      m_telemetry.get()->subscribe_odometry(
          [this, &landing_finished_promise, &success](mavsdk::Telemetry::Odometry odom) {
            if (odom.position_body.z_m > -0.15) {
              std::cout << "Landing altitude reached\n.";
 
              success = true;
              m_telemetry.get()->subscribe_odometry(nullptr);
              landing_finished_promise.set_value();
            }
          });
#endif
      if (landing_finished_future.wait_for(seconds(10)) == std::future_status::timeout) {
        std::cerr << "failed:  altitude not reached.\n";
        success = true; // go to automatic landing
      }
 
      while (!success) {
        std::cout << "Descending\n.";
        sleep_for(100ms);
      }
    }
 
    if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Land) {
      std::cout << "Landing...\n";
      const mavsdk::Action::Result land_result = m_action.get()->land();
      if (land_result != mavsdk::Action::Result::Success) {
        std::cerr << "Land failed: " << land_result << std::endl;
        return false;
      }
 
      // Check if vehicle is still in air
      while (m_telemetry.get()->in_air()) {
        std::cout << "Vehicle is landing..." << std::endl;
        sleep_for(seconds(1));
      }
    }
 
    std::cout << "Landed!" << std::endl;
    // We are relying on auto-disarming but let's keep watching the telemetry
    // for a bit longer.
    sleep_for(seconds(5));
    std::cout << "Finished..." << std::endl;
    return true;
  }
 
  bool setPosition(float ned_north, float ned_east, float ned_down, float ned_yaw, bool blocking, int timeout_sec)
  {
    mavsdk::Offboard::PositionNedYaw position_target {};
 
    position_target.north_m = ned_north;
    position_target.east_m = ned_east;
    position_target.down_m = ned_down;
    position_target.yaw_deg = vpMath::deg(ned_yaw);
 
    std::cout << "NED Pos to reach: " << position_target.north_m << " " << position_target.east_m << " "
      << position_target.down_m << " " << position_target.yaw_deg << std::endl;
    m_offboard.get()->set_position_ned(position_target);
 
    if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
      if (m_verbose) {
        std::cout << "Cannot set vehicle position: offboard mode not started" << std::endl;
      }
      return false;
    }
 
    if (blocking) {
      // Add check with Altitude
      auto position_reached_promise = std::promise<void> {};
      auto position_reached_future = position_reached_promise.get_future();
 
#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
      auto handle_odom = m_telemetry.get()->subscribe_odometry(
          [this, &position_reached_promise, &handle, &position_target](mavsdk::Telemetry::Odometry odom) {
            vpQuaternionVector q { odom.q.x, odom.q.y, odom.q.z, odom.q.w };
            vpRotationMatrix R(q);
            vpRxyzVector rxyz(R);
            double odom_yaw = vpMath::deg(rxyz[2]);
            double distance_to_target = std::sqrt(vpMath::sqr(odom.position_body.x_m - position_target.north_m) +
                                                  vpMath::sqr(odom.position_body.y_m - position_target.east_m) +
                                                  vpMath::sqr(odom.position_body.z_m - position_target.down_m));
            if (distance_to_target < m_position_incertitude &&
                std::fabs(odom_yaw - position_target.yaw_deg) < m_yaw_incertitude) {
              std::cout << "Position reached\n.";
              m_telemetry.get()->unsubscribe_odometry(handle_odom);
              position_reached_promise.set_value();
            }
          });
#else
      m_telemetry.get()->subscribe_odometry(
          [this, &position_reached_promise, &position_target](mavsdk::Telemetry::Odometry odom) {
            vpQuaternionVector q { odom.q.x, odom.q.y, odom.q.z, odom.q.w };
            vpRotationMatrix R(q);
            vpRxyzVector rxyz(R);
            double odom_yaw = vpMath::deg(rxyz[2]);
            double distance_to_target = std::sqrt(vpMath::sqr(odom.position_body.x_m - position_target.north_m) +
                                                  vpMath::sqr(odom.position_body.y_m - position_target.east_m) +
                                                  vpMath::sqr(odom.position_body.z_m - position_target.down_m));
            if (distance_to_target < m_position_incertitude &&
                std::fabs(odom_yaw - position_target.yaw_deg) < m_yaw_incertitude) {
              std::cout << "Position reached\n.";
              m_telemetry.get()->subscribe_odometry(nullptr);
              position_reached_promise.set_value();
            }
          });
#endif
      if (position_reached_future.wait_for(seconds(timeout_sec)) == std::future_status::timeout) {
        std::cerr << "Positioning failed: position not reached.\n";
        return false;
      }
    }
 
    std::cout << "---- DEBUG timeout: " << timeout_sec << std::endl;
    return true;
  }
 
  bool setPositionRelative(float ned_delta_north, float ned_delta_east, float ned_delta_down, float ned_delta_yaw,
                           bool blocking, int timeout_sec)
  {
    mavsdk::Telemetry::Odometry odom;
    mavsdk::Telemetry::EulerAngle angles;
    mavsdk::Offboard::PositionNedYaw position_target {};
 
    position_target.north_m = ned_delta_north;
    position_target.east_m = ned_delta_east;
    position_target.down_m = ned_delta_down;
    position_target.yaw_deg = vpMath::deg(ned_delta_yaw);
 
    // Set a relative position
    odom = m_telemetry.get()->odometry();
    angles = m_telemetry.get()->attitude_euler();
 
    position_target.north_m += odom.position_body.x_m;
    position_target.east_m += odom.position_body.y_m;
    position_target.down_m += odom.position_body.z_m;
    position_target.yaw_deg += angles.yaw_deg;
 
    return setPosition(position_target.north_m, position_target.east_m, position_target.down_m,
                       vpMath::rad(position_target.yaw_deg), blocking, timeout_sec);
  }
 
  bool setPosition(const vpHomogeneousMatrix &M, bool absolute, int timeout_sec)
  {
    auto XYZvec = vpRxyzVector(M.getRotationMatrix());
    if (XYZvec[0] != 0.0) {
      std::cerr << "ERROR : Can't move, rotation around X axis should be 0." << std::endl;
      return false;
    }
    if (XYZvec[1] != 0.0) {
      std::cerr << "ERROR : Can't move, rotation around Y axis should be 0." << std::endl;
      return false;
    }
    return setPosition(M.getTranslationVector()[0], M.getTranslationVector()[1], M.getTranslationVector()[2], XYZvec[2],
                       absolute, timeout_sec);
  }
 
  bool setPositionRelative(const vpHomogeneousMatrix &M, bool blocking, int timeout_sec)
  {
    auto XYZvec = vpRxyzVector(M.getRotationMatrix());
    if (XYZvec[0] != 0.0) {
      std::cerr << "ERROR : Can't move, rotation around X axis should be 0." << std::endl;
      return false;
    }
    if (XYZvec[1] != 0.0) {
      std::cerr << "ERROR : Can't move, rotation around Y axis should be 0." << std::endl;
      return false;
    }
    return setPositionRelative(M.getTranslationVector()[0], M.getTranslationVector()[1], M.getTranslationVector()[2],
                               XYZvec[2], blocking, timeout_sec);
  }
 
  bool setVelocity(const vpColVector &frd_vel_cmd)
  {
    if (frd_vel_cmd.size() != 4) {
      throw(vpException(vpException::dimensionError,
                        "ERROR : Can't set velocity, dimension of the velocity vector %d should be equal to 4.",
                        frd_vel_cmd.size()));
    }
 
    if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
      if (m_verbose) {
        std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl;
      }
      return false;
    }
    mavsdk::Offboard::VelocityBodyYawspeed velocity_comm {};
    velocity_comm.forward_m_s = frd_vel_cmd[0];
    velocity_comm.right_m_s = frd_vel_cmd[1];
    velocity_comm.down_m_s = frd_vel_cmd[2];
    velocity_comm.yawspeed_deg_s = vpMath::deg(frd_vel_cmd[3]);
    m_offboard.get()->set_velocity_body(velocity_comm);
 
    return true;
  }
 
  bool kill()
  {
    const mavsdk::Action::Result kill_result = m_action.get()->kill();
    if (kill_result != mavsdk::Action::Result::Success) {
      std::cerr << "Kill failed: " << kill_result << std::endl;
      return false;
    }
    return true;
  }
 
  bool holdPosition()
  {
    if (m_telemetry.get()->in_air()) {
      if (m_telemetry.get()->gps_info().fix_type != mavsdk::Telemetry::FixType::NoGps) {
        // Action::hold() doesn't work with PX4 when in offboard mode
        const mavsdk::Action::Result hold_result = m_action.get()->hold();
        if (hold_result != mavsdk::Action::Result::Success) {
          std::cerr << "Hold failed: " << hold_result << std::endl;
          return false;
        }
      }
      else {
        if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
          if (m_verbose) {
            std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl;
          }
          return false;
        }
 
        setPositionRelative(0., 0., 0., 0., false, 10.); // timeout not used
      }
    }
    return true;
  }
 
  bool stopMoving()
  {
    if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
      if (m_verbose) {
        std::cout << "Cannot stop moving: offboard mode not started" << std::endl;
      }
      return false;
    }
 
    const mavsdk::Offboard::VelocityBodyYawspeed stay {};
    m_offboard.get()->set_velocity_body(stay);
 
    return true;
  }
 
  bool releaseControl()
  {
    auto offboard_result = m_offboard.get()->stop();
    if (offboard_result != mavsdk::Offboard::Result::Success) {
      std::cerr << "Offboard stop failed: " << offboard_result << '\n';
      return false;
    }
    std::cout << "Offboard stopped\n";
    return true;
  }
 
  void setAutoLand(bool auto_land) { m_auto_land = auto_land; }
 
  bool setYawSpeed(double body_frd_wz)
  {
    if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
      if (m_verbose) {
        std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl;
      }
      return false;
    }
    mavsdk::Offboard::VelocityBodyYawspeed velocity_comm {};
    velocity_comm.forward_m_s = 0.0;
    velocity_comm.right_m_s = 0.0;
    velocity_comm.down_m_s = 0.0;
    velocity_comm.yawspeed_deg_s = vpMath::deg(body_frd_wz);
    m_offboard.get()->set_velocity_body(velocity_comm);
 
    return true;
  }
 
  bool setForwardSpeed(double body_frd_vx)
  {
    if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
      if (m_verbose) {
        std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl;
      }
      return false;
    }
 
    mavsdk::Offboard::VelocityBodyYawspeed velocity_comm {};
    velocity_comm.forward_m_s = body_frd_vx;
    velocity_comm.right_m_s = 0.0;
    velocity_comm.down_m_s = 0.0;
    velocity_comm.yawspeed_deg_s = 0.0;
    m_offboard.get()->set_velocity_body(velocity_comm);
 
    return true;
  }
 
  bool setLateralSpeed(double body_frd_vy)
  {
    if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
      if (m_verbose) {
        std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl;
      }
      return false;
    }
    mavsdk::Offboard::VelocityBodyYawspeed velocity_comm {};
    velocity_comm.forward_m_s = 0.0;
    velocity_comm.right_m_s = body_frd_vy;
    velocity_comm.down_m_s = 0.0;
    velocity_comm.yawspeed_deg_s = 0.0;
    m_offboard.get()->set_velocity_body(velocity_comm);
 
    return true;
  }
 
  bool setVerticalSpeed(double body_frd_vz)
  {
    if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
      if (m_verbose) {
        std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl;
      }
      return false;
    }
    mavsdk::Offboard::VelocityBodyYawspeed velocity_comm {};
    velocity_comm.forward_m_s = 0.0;
    velocity_comm.right_m_s = 0.0;
    velocity_comm.down_m_s = body_frd_vz;
    velocity_comm.yawspeed_deg_s = 0.0;
    m_offboard.get()->set_velocity_body(velocity_comm);
 
    return true;
  }
 
  bool getFlyingCapability() { return m_has_flying_capability; }
 
  void setVerbose(bool verbose) { m_verbose = verbose; }
 
private:
  //*** Attributes ***//
  std::string m_address {}; 
  mavsdk::Mavsdk m_mavsdk {};
  std::shared_ptr<mavsdk::System> m_system;
  std::shared_ptr<mavsdk::Action> m_action;
  std::shared_ptr<mavsdk::Telemetry> m_telemetry;
  std::shared_ptr<mavsdk::Offboard> m_offboard;
 
  double m_takeoffAlt { 1.0 }; 
 
  MAV_TYPE m_mav_type {}; // Vehicle type
  bool m_has_flying_capability { false };
 
  float m_position_incertitude { 0.05 };
  float m_yaw_incertitude { 0.09 }; // 5 deg
  bool m_verbose { false };
  bool m_auto_land { true };
  };
#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
 
vpRobotMavsdk::vpRobotMavsdk(const std::string &connection_info) : m_impl(new vpRobotMavsdkImpl(connection_info))
{
  m_impl->setPositioningIncertitude(0.05, vpMath::rad(5));
}
 
vpRobotMavsdk::vpRobotMavsdk() : m_impl(new vpRobotMavsdkImpl())
{
  m_impl->setPositioningIncertitude(0.05, vpMath::rad(5));
}
 
vpRobotMavsdk::~vpRobotMavsdk() { delete m_impl; }
 
void vpRobotMavsdk::connect(const std::string &connection_info) { m_impl->connect(connection_info); }
 
bool vpRobotMavsdk::isRunning() const { return m_impl->isRunning(); }
 
bool vpRobotMavsdk::sendMocapData(const vpHomogeneousMatrix &enu_M_flu, int display_fps)
{
  return m_impl->sendMocapData(enu_M_flu, display_fps);
}
 
std::string vpRobotMavsdk::getAddress() const { return m_impl->getAddress(); }
 
float vpRobotMavsdk::getBatteryLevel() const { return m_impl->getBatteryLevel(); }
 
void vpRobotMavsdk::getPosition(vpHomogeneousMatrix &ned_M_frd) const { m_impl->getPosition(ned_M_frd); }
 
void vpRobotMavsdk::getPosition(float &ned_north, float &ned_east, float &ned_down, float &ned_yaw) const
{
  m_impl->getPosition(ned_north, ned_east, ned_down, ned_yaw);
}
 
std::tuple<float, float> vpRobotMavsdk::getHome() const { return m_impl->getHome(); }
 
void vpRobotMavsdk::doFlatTrim() { }
 
void vpRobotMavsdk::setTakeOffAlt(double altitude) { m_impl->setTakeOffAlt(altitude); }
 
bool vpRobotMavsdk::arm() { return m_impl->arm(); }
 
bool vpRobotMavsdk::disarm() { return m_impl->disarm(); }
 
bool vpRobotMavsdk::takeOff(bool interactive, int timeout_sec, bool use_gps)
{
  return m_impl->takeOff(interactive, timeout_sec, use_gps);
}
 
bool vpRobotMavsdk::takeOff(bool interactive, double takeoff_altitude, int timeout_sec, bool use_gps)
{
  m_impl->setTakeOffAlt(takeoff_altitude);
  return m_impl->takeOff(interactive, timeout_sec, use_gps);
}
 
bool vpRobotMavsdk::holdPosition() { return m_impl->holdPosition(); };
 
bool vpRobotMavsdk::stopMoving() { return m_impl->stopMoving(); };
 
bool vpRobotMavsdk::land() { return m_impl->land(); }
 
bool vpRobotMavsdk::setPosition(float ned_north, float ned_east, float ned_down, float ned_yaw, bool blocking,
                                int timeout_sec)
{
  return m_impl->setPosition(ned_north, ned_east, ned_down, ned_yaw, blocking, timeout_sec);
}
 
bool vpRobotMavsdk::setPosition(const vpHomogeneousMatrix &ned_M_frd, bool blocking, int timeout_sec)
{
  return m_impl->setPosition(ned_M_frd, blocking, timeout_sec);
}
 
bool vpRobotMavsdk::setPositionRelative(float ned_delta_north, float ned_delta_east, float ned_delta_down,
                                        float ned_delta_yaw, bool blocking, int timeout_sec)
{
  return m_impl->setPositionRelative(ned_delta_north, ned_delta_east, ned_delta_down, ned_delta_yaw, blocking,
                                     timeout_sec);
}
 
bool vpRobotMavsdk::setPositionRelative(const vpHomogeneousMatrix &delta_frd_M_frd, bool blocking, int timeout_sec)
{
  return m_impl->setPositionRelative(delta_frd_M_frd, blocking, timeout_sec);
}
 
bool vpRobotMavsdk::setVelocity(const vpColVector &frd_vel_cmd) { return m_impl->setVelocity(frd_vel_cmd); }
 
bool vpRobotMavsdk::kill() { return m_impl->kill(); }
 
bool vpRobotMavsdk::setYawSpeed(double body_frd_wz) { return m_impl->setYawSpeed(body_frd_wz); }
 
bool vpRobotMavsdk::setForwardSpeed(double body_frd_vx) { return m_impl->setForwardSpeed(body_frd_vx); }
 
bool vpRobotMavsdk::setLateralSpeed(double body_frd_vy) { return m_impl->setLateralSpeed(body_frd_vy); }
 
bool vpRobotMavsdk::setGPSGlobalOrigin(double latitude, double longitude, double altitude)
{
  return m_impl->setGPSGlobalOrigin(latitude, longitude, altitude);
}
 
bool vpRobotMavsdk::takeControl() { return m_impl->takeControl(); }
 
bool vpRobotMavsdk::releaseControl() { return m_impl->releaseControl(); }
 
void vpRobotMavsdk::setAutoLand(bool auto_land) { m_impl->setAutoLand(auto_land); }
 
void vpRobotMavsdk::setPositioningIncertitude(float position_incertitude, float yaw_incertitude)
{
  m_impl->setPositioningIncertitude(position_incertitude, yaw_incertitude);
}
 
bool vpRobotMavsdk::setVerticalSpeed(double body_frd_vz) { return m_impl->setVerticalSpeed(body_frd_vz); }
 
void vpRobotMavsdk::setVerbose(bool verbose) { m_impl->setVerbose(verbose); }
 
bool vpRobotMavsdk::hasFlyingCapability() { return m_impl->getFlyingCapability(); }
#ifdef ENABLE_VISP_NAMESPACE
  }
#endif
#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work around to avoid warning: libvisp_robot.a(vpRobotMavsdk.cpp.o) has no symbols
void dummy_vpRobotMavsdk() { };
#endif