From 14ac04be3dbb69a6a500a318abc083288d1dbf9b Mon Sep 17 00:00:00 2001 From: Antonio Date: Tue, 4 Apr 2023 17:45:57 +0200 Subject: [PATCH 1/6] fixed timeout and call takeControl in takeoff with GPS --- .../src/real-robot/mavsdk/vpRobotMavsdk.cpp | 1101 +++++++++-------- 1 file changed, 614 insertions(+), 487 deletions(-) diff --git a/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp b/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp index 7c026ae12c..438c769412 100644 --- a/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp +++ b/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp @@ -57,14 +57,14 @@ using std::chrono::seconds; using std::this_thread::sleep_for; #ifndef DOXYGEN_SHOULD_SKIP_THIS -class vpRobotMavsdk::vpRobotMavsdkImpl -{ +class vpRobotMavsdk::vpRobotMavsdkImpl { public: vpRobotMavsdkImpl() : m_takeoffAlt(1.0) {} - vpRobotMavsdkImpl(const std::string &connection_info) : m_takeoffAlt(1.0) { connect(connection_info); } + vpRobotMavsdkImpl(const std::string &connection_info) : m_takeoffAlt(1.0) { + connect(connection_info); + } - virtual ~vpRobotMavsdkImpl() - { + virtual ~vpRobotMavsdkImpl() { if (m_has_flying_capability && m_auto_land) { land(); } @@ -72,37 +72,37 @@ class vpRobotMavsdk::vpRobotMavsdkImpl private: /*! - * Connects a first time to the robot in order to get the system running on it. - * \param[in] mavsdk : the Mavsdk object we will use to subscribe to the system. - * \returns Returns a shared pointer to the system. + * Connects a first time to the robot in order to get the system running on + * it. \param[in] mavsdk : the Mavsdk object we will use to subscribe to the + * system. \returns Returns a shared pointer to the system. */ - std::shared_ptr getSystem(mavsdk::Mavsdk &mavsdk) - { + std::shared_ptr getSystem(mavsdk::Mavsdk &mavsdk) { std::cout << "Waiting to discover system..." << std::endl; - auto prom = std::promise >{}; + auto prom = std::promise>{}; 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]() { + 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(); + auto system = mavsdk.systems().back(); - if (system->has_autopilot()) { - std::cout << "Discovered autopilot" << std::endl; + 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); + mavsdk.unsubscribe_on_new_system(handle); #else mavsdk.subscribe_on_new_system(nullptr); #endif - prom.set_value(system); - } - }); + prom.set_value(system); + } + }); // We usually receive heartbeats at 1Hz, therefore we should find a // system after around 3 seconds max, surely. @@ -115,36 +115,39 @@ class vpRobotMavsdk::vpRobotMavsdkImpl return fut.get(); } - MAV_TYPE getVehicleType() - { + MAV_TYPE getVehicleType() { auto passthrough = mavsdk::MavlinkPassthrough{m_system}; auto prom = std::promise{}; 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) { + 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) { + 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; - } + // 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); + 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); + passthrough.unsubscribe_message(handle); #else - passthrough.subscribe_message_async(MAVLINK_MSG_ID_HEARTBEAT, nullptr); + passthrough.subscribe_message_async(MAVLINK_MSG_ID_HEARTBEAT, + nullptr); #endif - prom.set_value(static_cast(heartbeat.type)); - }); + prom.set_value(static_cast(heartbeat.type)); + }); // We usually receive heartbeats at 1Hz, therefore we should find a // system after around 3 seconds max, surely. @@ -157,23 +160,24 @@ class vpRobotMavsdk::vpRobotMavsdkImpl return fut.get(); } - void calibrate_accelerometer(mavsdk::Calibration &calibration) - { + void calibrate_accelerometer(mavsdk::Calibration &calibration) { std::cout << "Calibrating accelerometer..." << std::endl; std::promise calibration_promise; auto calibration_future = calibration_promise.get_future(); - calibration.calibrate_accelerometer_async(create_calibration_callback(calibration_promise)); + calibration.calibrate_accelerometer_async( + create_calibration_callback(calibration_promise)); calibration_future.wait(); } - std::function - create_calibration_callback(std::promise &calibration_promise) - { - return [&calibration_promise](const mavsdk::Calibration::Result result, - const mavsdk::Calibration::ProgressData progress_data) { + std::function + create_calibration_callback(std::promise &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; @@ -184,34 +188,36 @@ class vpRobotMavsdk::vpRobotMavsdkImpl std::cout << " Progress: " << progress_data.progress << std::endl; } if (progress_data.has_status_text) { - std::cout << " Instruction: " << progress_data.status_text << std::endl; + std::cout << " Instruction: " << progress_data.status_text + << std::endl; } break; default: - std::cout << "--- Calibration failed with message: " << result << std::endl; + std::cout << "--- Calibration failed with message: " << result + << std::endl; calibration_promise.set_value(); break; } }; } - void calibrate_gyro(mavsdk::Calibration &calibration) - { + void calibrate_gyro(mavsdk::Calibration &calibration) { std::cout << "Calibrating gyro..." << std::endl; std::promise calibration_promise; auto calibration_future = calibration_promise.get_future(); - calibration.calibrate_gyro_async(create_calibration_callback(calibration_promise)); + calibration.calibrate_gyro_async( + create_calibration_callback(calibration_promise)); calibration_future.wait(); } public: - void connect(const std::string &connectionInfo) - { + void connect(const std::string &connectionInfo) { m_address = connectionInfo; - mavsdk::ConnectionResult connection_result = m_mavsdk.add_any_connection(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; @@ -221,25 +227,24 @@ class vpRobotMavsdk::vpRobotMavsdkImpl m_system = getSystem(m_mavsdk); if (!m_system) { - throw vpException(vpException::fatalError, "Unable to connect to: %s", connectionInfo.c_str()); + 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::cout << (m_has_flying_capability ? "Connected to a flying vehicle" + : "Connected to a non flying vehicle") << std::endl; m_action = std::make_shared(m_system); m_telemetry = std::make_shared(m_system); m_offboard = std::make_shared(m_system); - - } - bool hasFlyingCapability(MAV_TYPE mav_type) - { + bool hasFlyingCapability(MAV_TYPE mav_type) { switch (mav_type) { case MAV_TYPE::MAV_TYPE_GROUND_ROVER: case MAV_TYPE::MAV_TYPE_SURFACE_BOAT: @@ -250,8 +255,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl } } - bool isRunning() const - { + bool isRunning() const { if (m_system == NULL) { return false; } else { @@ -259,8 +263,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl } } - std::string getAddress() const - { + std::string getAddress() const { std::string sequence; std::stringstream ss(m_address); std::string actual_address; @@ -274,28 +277,29 @@ class vpRobotMavsdk::vpRobotMavsdkImpl } return actual_address; } else { - std::cout << "ERROR : The address parameter must start with \"serial:\" or \"udp:\" or \"tcp:\"." << std::endl; + std::cout << "ERROR : The address parameter must start with \"serial:\" " + "or \"udp:\" or \"tcp:\"." + << std::endl; return std::string(); } } - float getBatteryLevel() const - { + float getBatteryLevel() const { mavsdk::Telemetry::Battery battery = m_telemetry.get()->battery(); return battery.voltage_v; } - void getPosition(vpHomogeneousMatrix &ned_M_frd) const - { + 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}; + 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 - { + 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; @@ -304,23 +308,22 @@ class vpRobotMavsdk::vpRobotMavsdkImpl ned_yaw = vpMath::rad(angles.yaw_deg); } - std::tuple getHome() const - { + std::tuple 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) - { + 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). + // 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; @@ -339,37 +342,40 @@ class vpRobotMavsdk::vpRobotMavsdkImpl 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); + 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; + 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; + 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) - { + void setTakeOffAlt(double altitude) { if (altitude > 0) { m_takeoffAlt = altitude; } else { - std::cerr << "ERROR : The take off altitude must be positive." << std::endl; + std::cerr << "ERROR : The take off altitude must be positive." + << std::endl; } } - void doFlatTrim() - { + void doFlatTrim() { // Instantiate plugin. auto calibration = mavsdk::Calibration(m_system); @@ -378,8 +384,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl calibrate_gyro(calibration); } - bool arm() - { + bool arm() { // Arm vehicle std::cout << "Arming...\n"; const mavsdk::Action::Result arm_result = m_action.get()->arm(); @@ -391,8 +396,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl return true; } - bool disarm() - { + bool disarm() { // Arm vehicle std::cout << "Disarming...\n"; const mavsdk::Action::Result arm_result = m_action.get()->disarm(); @@ -404,8 +408,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl return true; } - bool setGPSGlobalOrigin(double latitude, double longitude, double altitude) - { + 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; @@ -413,7 +416,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl 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, + 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) { @@ -423,13 +427,13 @@ class vpRobotMavsdk::vpRobotMavsdkImpl return true; } - bool takeControl() - { + bool takeControl() { if (m_verbose) { std::cout << "Starting offboard mode..." << std::endl; } - if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) { + if (m_telemetry.get()->flight_mode() != + mavsdk::Telemetry::FlightMode::Offboard) { const mavsdk::Offboard::VelocityBodyYawspeed stay{}; m_offboard.get()->set_velocity_body(stay); @@ -438,14 +442,14 @@ class vpRobotMavsdk::vpRobotMavsdkImpl std::cerr << "Offboard mode failed: " << offboard_result << std::endl; return false; } - } - else if (m_verbose) { + } 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) { + 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; @@ -458,16 +462,16 @@ class vpRobotMavsdk::vpRobotMavsdkImpl return true; } - void setPositioningIncertitude(float position_incertitude, float yaw_incertitude) - { + 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 takeOff(bool interactive, int timeout_sec) { if (!m_has_flying_capability) { - std::cerr << "Warning: Cannot takeoff this non flying vehicle" << std::endl; + std::cerr << "Warning: Cannot takeoff this non flying vehicle" + << std::endl; return true; } @@ -476,12 +480,15 @@ class vpRobotMavsdk::vpRobotMavsdkImpl if (!interactive) { authorize_takeoff = true; } else { - if (m_telemetry.get()->flight_mode() == mavsdk::Telemetry::FlightMode::Offboard) { + 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)" + 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") { @@ -492,11 +499,12 @@ class vpRobotMavsdk::vpRobotMavsdkImpl } if (m_telemetry.get()->in_air()) { - std::cerr << "Cannot take off as the robot is already flying." << std::endl; + std::cerr << "Cannot take off as the robot is already flying." + << std::endl; return true; } else if (authorize_takeoff) { // Arm vehicle - if (! arm()) { + if (!arm()) { return false; } @@ -504,7 +512,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl if (interactive) { std::string answer; - while (answer != "Y" && answer != "y" && answer != "N" && answer != "n") { + 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") { @@ -516,11 +525,12 @@ class vpRobotMavsdk::vpRobotMavsdkImpl } // Takeoff - if (m_telemetry.get()->gps_info().fix_type == mavsdk::Telemetry::FixType::NoGps) { - // 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 + if (m_telemetry.get()->gps_info().fix_type == + mavsdk::Telemetry::FixType::NoGps) { + // 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(); @@ -546,71 +556,82 @@ class vpRobotMavsdk::vpRobotMavsdkImpl 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) { + // 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) +#if (VISP_HAVE_MAVSDK_VERSION > 0x010412) m_telemetry.get()->unsubscribe_landed_state(handle); - #else +#else m_telemetry.get()->subscribe_landed_state(nullptr); - #endif +#endif return false; } // Add check with Altitude auto takeoff_finished_promise = std::promise{}; auto takeoff_finished_future = takeoff_finished_promise.get_future(); - #if (VISP_HAVE_MAVSDK_VERSION > 0x010412) +#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) { + [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) +#if (VISP_HAVE_MAVSDK_VERSION > 0x010412) m_telemetry.get()->unsubscribe_odometry(handle); - #else +#else m_telemetry.get()->subscribe_odometry(nullptr); - #endif +#endif return false; } - } - else { + } else { // GPS connected, we use Action::takeoff() - std::cout << "---- DEBUG: GPS detected: use action::takeoff()" << std::endl; + 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; @@ -624,58 +645,68 @@ class vpRobotMavsdk::vpRobotMavsdkImpl auto in_air_promise = std::promise{}; 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(3)) == std::future_status::timeout) { +#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) +#if (VISP_HAVE_MAVSDK_VERSION > 0x010412) m_telemetry.get()->unsubscribe_landed_state(handle); - #else +#else m_telemetry.get()->subscribe_landed_state(nullptr); - #endif - //return false; +#endif + // return false; } // Add check with Altitude auto takeoff_finished_promise = std::promise{}; auto takeoff_finished_future = takeoff_finished_promise.get_future(); - #if (VISP_HAVE_MAVSDK_VERSION > 0x010412) +#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) { + [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); @@ -684,19 +715,21 @@ class vpRobotMavsdk::vpRobotMavsdkImpl #endif return false; } - } + // Start off-board or guided mode + takeControl(); + } } return true; } - bool land() - { + bool land() { if (!m_has_flying_capability) { std::cerr << "Warning: Cannot land this non flying vehicle" << std::endl; return true; } - if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Land) { + 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) { @@ -712,14 +745,15 @@ class vpRobotMavsdk::vpRobotMavsdkImpl } std::cout << "Landed!" << std::endl; - // We are relying on auto-disarming but let's keep watching the telemetry for a bit longer. + // 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) - { + 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; @@ -727,12 +761,16 @@ class vpRobotMavsdk::vpRobotMavsdkImpl 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; + 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_telemetry.get()->flight_mode() != + mavsdk::Telemetry::FlightMode::Offboard) { if (m_verbose) { - std::cout << "Cannot set vehicle position: offboard mode not started" << std::endl; + std::cout << "Cannot set vehicle position: offboard mode not started" + << std::endl; } return false; } @@ -742,52 +780,61 @@ class vpRobotMavsdk::vpRobotMavsdkImpl auto position_reached_promise = std::promise{}; auto position_reached_future = position_reached_promise.get_future(); - #if (VISP_HAVE_MAVSDK_VERSION > 0x010412) +#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) { + [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; + 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) - { + 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{}; @@ -806,48 +853,60 @@ std::cout << "---- DEBUG timeout: " << timeout_sec << std::endl; 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); + 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) - { + 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; + 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; + 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); + 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) - { + 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; + 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; + 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); + return setPositionRelative( + M.getTranslationVector()[0], M.getTranslationVector()[1], + M.getTranslationVector()[2], XYZvec[2], blocking, timeout_sec); } - bool setVelocity(const vpColVector &frd_vel_cmd) - { + 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.", + "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_telemetry.get()->flight_mode() != + mavsdk::Telemetry::FlightMode::Offboard) { if (m_verbose) { - std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl; + std::cout << "Cannot set vehicle velocity: offboard mode not started" + << std::endl; } return false; } @@ -861,8 +920,7 @@ std::cout << "---- DEBUG timeout: " << timeout_sec << std::endl; return true; } - bool kill() - { + 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; @@ -871,10 +929,10 @@ std::cout << "---- DEBUG timeout: " << timeout_sec << std::endl; return true; } - bool holdPosition() - { + bool holdPosition() { if (m_telemetry.get()->in_air()) { - if (m_telemetry.get()->gps_info().fix_type != mavsdk::Telemetry::FixType::NoGps) { + 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) { @@ -882,9 +940,12 @@ std::cout << "---- DEBUG timeout: " << timeout_sec << std::endl; return false; } } else { - if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) { + 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; + std::cout + << "Cannot set vehicle velocity: offboard mode not started" + << std::endl; } return false; } @@ -895,11 +956,12 @@ std::cout << "---- DEBUG timeout: " << timeout_sec << std::endl; return true; } - bool stopMoving() - { - if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) { + 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; + std::cout << "Cannot stop moving: offboard mode not started" + << std::endl; } return false; } @@ -910,8 +972,7 @@ std::cout << "---- DEBUG timeout: " << timeout_sec << std::endl; return true; } - bool releaseControl() - { + bool releaseControl() { auto offboard_result = m_offboard.get()->stop(); if (offboard_result != mavsdk::Offboard::Result::Success) { std::cerr << "Offboard stop failed: " << offboard_result << '\n'; @@ -921,16 +982,14 @@ std::cout << "---- DEBUG timeout: " << timeout_sec << std::endl; return true; } - void setAutoLand(bool auto_land) - { - m_auto_land = auto_land; - } + 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) { + 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; + std::cout << "Cannot set vehicle velocity: offboard mode not started" + << std::endl; } return false; } @@ -944,11 +1003,12 @@ std::cout << "---- DEBUG timeout: " << timeout_sec << std::endl; return true; } - bool setForwardSpeed(double body_frd_vx) - { - if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) { + 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; + std::cout << "Cannot set vehicle velocity: offboard mode not started" + << std::endl; } return false; } @@ -963,11 +1023,12 @@ std::cout << "---- DEBUG timeout: " << timeout_sec << std::endl; return true; } - bool setLateralSpeed(double body_frd_vy) - { - if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) { + 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; + std::cout << "Cannot set vehicle velocity: offboard mode not started" + << std::endl; } return false; } @@ -981,11 +1042,12 @@ std::cout << "---- DEBUG timeout: " << timeout_sec << std::endl; return true; } - bool setVerticalSpeed(double body_frd_vz) - { - if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) { + 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; + std::cout << "Cannot set vehicle velocity: offboard mode not started" + << std::endl; } return false; } @@ -1001,14 +1063,11 @@ std::cout << "---- DEBUG timeout: " << timeout_sec << std::endl; bool getFlyingCapability() { return m_has_flying_capability; } - void setVerbose(bool verbose) - { - m_verbose = verbose; - } + void setVerbose(bool verbose) { m_verbose = verbose; } // void waitSystemReady() // { - // if (! m_system_ready) + // if (! m_system_ready) // { // while (!m_telemetry.get()->health_all_ok()) { // std::cout << "Waiting for system to be ready\n"; @@ -1020,14 +1079,16 @@ std::cout << "---- DEBUG timeout: " << timeout_sec << std::endl; private: //*** Attributes ***// - std::string m_address{}; ///< Ip address of the robot to discover on the network + std::string + m_address{}; ///< Ip address of the robot to discover on the network mavsdk::Mavsdk m_mavsdk{}; std::shared_ptr m_system; std::shared_ptr m_action; std::shared_ptr m_telemetry; std::shared_ptr m_offboard; - double m_takeoffAlt{1.0}; ///< The altitude to aim for when calling the function takeoff + double m_takeoffAlt{ + 1.0}; ///< The altitude to aim for when calling the function takeoff MAV_TYPE m_mav_type{}; // Vehicle type bool m_has_flying_capability{false}; @@ -1043,119 +1104,142 @@ std::cout << "---- DEBUG timeout: " << timeout_sec << std::endl; /*! * Constructor. * - * Initializes vehicle controller, by discovering vehicles connected either with an Ethernet TCP or UDP link, or with a - * serial link the computer is currently connected to. + * Initializes vehicle controller, by discovering vehicles connected either with + * an Ethernet TCP or UDP link, or with a serial link the computer is currently + * connected to. + * + * \warning This constructor should be called after the vehicle is turned on, + * and after the computer is connected to the vehicle Ethernet network or with a + * serial link. * - * \warning This constructor should be called after the vehicle is turned on, and after the computer is connected to the - * vehicle Ethernet network or with a serial link. + * \warning If the connection to the vehicle failed, the program will throw an + * exception. * - * \warning If the connection to the vehicle failed, the program will throw an exception. + * After having called this constructor, it is recommended to check if the + * vehicle is running with isRunning() before sending commands to the vehicle. * - * After having called this constructor, it is recommended to check if the vehicle is running with isRunning() before - * sending commands to the vehicle. + * Set default positioning incertitude to 0.05 meter in translation, and 5 + * degrees along yaw orientation. These default values are used to determine + * when a position is reached and could be changed using + * setPositioningIncertitude(). When the vehicle has flying capabilities, call + * by default land() in the destructor. This behavior could be changed using + * setAutoLand(). * - * Set default positioning incertitude to 0.05 meter in translation, and 5 degrees along yaw orientation. - * These default values are used to determine when a position is reached and could be changed using setPositioningIncertitude(). - * When the vehicle has flying capabilities, call by default land() in the destructor. This behavior could be changed - * using setAutoLand(). - * - * To control the vehicle using this class, you need to call takeControl() to start the off-board mode with PX4 or the - * guided mode with Ardupilot. After this call you can call setPosition() to move the vehicle to a desired position - * and yaw orientation or call setVelocity() to move the vehicle in velocity. + * To control the vehicle using this class, you need to call takeControl() to + * start the off-board mode with PX4 or the guided mode with Ardupilot. After + * this call you can call setPosition() to move the vehicle to a desired + * position and yaw orientation or call setVelocity() to move the vehicle in + * velocity. * - * \param[in] connection_info : Specify connection information. This parameter must be written following these - * conventions: + * \param[in] connection_info : Specify connection information. This parameter + * must be written following these conventions: * - for TCP link: tcp://[server_host][:server_port] * - for UDP link: udp://[bind_host][:bind_port] * - for Serial link: serial:///path/to/serial/dev[:baudrate]
* Examples: udp://192.168.30.111:14550 or serial:///dev/ttyACMO * - * For more information see [here](https://mavsdk.mavlink.io/main/en/cpp/guide/connections.html). + * For more information see + * [here](https://mavsdk.mavlink.io/main/en/cpp/guide/connections.html). + * + * \exception vpException::fatalError : If the program failed to connect to the + * vehicle. * - * \exception vpException::fatalError : If the program failed to connect to the vehicle. - * - * \sa setPositioningIncertitude(), setAutoLand(), takeControl(), releaseControl() + * \sa setPositioningIncertitude(), setAutoLand(), takeControl(), + * releaseControl() */ -vpRobotMavsdk::vpRobotMavsdk(const std::string &connection_info) : m_impl(new vpRobotMavsdkImpl(connection_info)) -{ +vpRobotMavsdk::vpRobotMavsdk(const std::string &connection_info) + : m_impl(new vpRobotMavsdkImpl(connection_info)) { m_impl->setPositioningIncertitude(0.05, vpMath::rad(5)); } /*! - * Default constructor without parameters. You need to use the connect() function afterwards. + * Default constructor without parameters. You need to use the connect() + * function afterwards. * - * Set default positioning incertitude to 0.05 meter in translation, and 5 degrees along yaw orientation. - * These default values are used to determine when a position is reached and could be changed using setPositioningIncertitude(). - * When the vehicle has flying capabilities, call by default land() in the destructor. This behavior could be changed - * using setAutoLand(). - * - * To control the vehicle using this class, you need to call takeControl() to start the off-board mode with PX4 or the - * guided mode with Ardupilot. After this call you can call setPosition() to move the vehicle to a desired position - * and yaw orientation or call setVelocity() to move the vehicle in velocity. + * Set default positioning incertitude to 0.05 meter in translation, and 5 + * degrees along yaw orientation. These default values are used to determine + * when a position is reached and could be changed using + * setPositioningIncertitude(). When the vehicle has flying capabilities, call + * by default land() in the destructor. This behavior could be changed using + * setAutoLand(). + * + * To control the vehicle using this class, you need to call takeControl() to + * start the off-board mode with PX4 or the guided mode with Ardupilot. After + * this call you can call setPosition() to move the vehicle to a desired + * position and yaw orientation or call setVelocity() to move the vehicle in + * velocity. * * \sa connect(), setPositioningIncertitude() */ -vpRobotMavsdk::vpRobotMavsdk() : m_impl(new vpRobotMavsdkImpl()) -{ +vpRobotMavsdk::vpRobotMavsdk() : m_impl(new vpRobotMavsdkImpl()) { m_impl->setPositioningIncertitude(0.05, vpMath::rad(5)); } /*! * Destructor. - * When the vehicle has flying capabilities and when auto land mode is enabled, lands the vehicle if not landed - * and safely disconnects everything. - * + * When the vehicle has flying capabilities and when auto land mode is enabled, + * lands the vehicle if not landed and safely disconnects everything. + * * \sa setAutoLand() */ vpRobotMavsdk::~vpRobotMavsdk() { delete m_impl; } /*! * Connects to the vehicle and setups the different controllers. - * \param[in] connection_info : The connection information given to connect to the vehicle. You may use: + * \param[in] connection_info : The connection information given to connect to + * the vehicle. You may use: * - for TCP link: tcp://[server_host][:server_port] * - for UDP link: udp://[bind_host][:bind_port] * - for Serial link: serial:///path/to/serial/dev[:baudrate]
* Examples: udp://192.168.30.111:14550 or serial:///dev/ttyACMO * - * For more information see [here](https://mavsdk.mavlink.io/main/en/cpp/guide/connections.html). + * For more information see + * [here](https://mavsdk.mavlink.io/main/en/cpp/guide/connections.html). * * \sa getAddress() */ -void vpRobotMavsdk::connect(const std::string &connection_info) { m_impl->connect(connection_info); } +void vpRobotMavsdk::connect(const std::string &connection_info) { + m_impl->connect(connection_info); +} /*! - * Checks if the vehicle is running, ie if the vehicle is connected and ready to receive commands. + * Checks if the vehicle is running, ie if the vehicle is connected and ready to + * receive commands. */ bool vpRobotMavsdk::isRunning() const { return m_impl->isRunning(); } /*! * Sends MoCap position data to the vehicle. * - * We consider here that the MoCap global reference frame is ENU (East-North-Up). - * The vehicle body frame if FLU (Front-Left-Up) where X axis is aligned - * with the vehicle front axis and Z axis going upward. - * - * Internally, this pose called `enu_M_flu` is transformed to match the requirements of the Pixhawk - * into `ned_M_frd` corresponding to the FRD (Front-Right-Down) body frame position in the NED (North-East-Down) - * local reference frame. - * - * \return true if the MoCap data was successfully sent to the vehicle, false otherwise. - * \param[in] enu_M_flu : Homogeneous matrix containing the pose of the vehicle given by the MoCap system. - * To be more precise, this matrix gives the pose of the vehicle FLU body frame returned by the MoCap where - * MoCap global reference frame is defined as ENU. - * \param[in] display_fps : Display `ned_M_frd` pose internally sent through mavlink at the given framerate. A value of 0 can - * be used to disable this display. - * - * Internally we transform this FRD pose in a NED global reference frame as expected by Pixhawk convention. + * We consider here that the MoCap global reference frame is ENU + * (East-North-Up). The vehicle body frame if FLU (Front-Left-Up) where X axis + * is aligned with the vehicle front axis and Z axis going upward. + * + * Internally, this pose called `enu_M_flu` is transformed to match the + * requirements of the Pixhawk into `ned_M_frd` corresponding to the FRD + * (Front-Right-Down) body frame position in the NED (North-East-Down) local + * reference frame. + * + * \return true if the MoCap data was successfully sent to the vehicle, false + * otherwise. \param[in] enu_M_flu : Homogeneous matrix containing the pose of + * the vehicle given by the MoCap system. To be more precise, this matrix gives + * the pose of the vehicle FLU body frame returned by the MoCap where MoCap + * global reference frame is defined as ENU. \param[in] display_fps : Display + * `ned_M_frd` pose internally sent through mavlink at the given framerate. A + * value of 0 can be used to disable this display. + * + * Internally we transform this FRD pose in a NED global reference frame as + * expected by Pixhawk convention. */ -bool vpRobotMavsdk::sendMocapData(const vpHomogeneousMatrix &enu_M_flu, int display_fps) -{ +bool vpRobotMavsdk::sendMocapData(const vpHomogeneousMatrix &enu_M_flu, + int display_fps) { return m_impl->sendMocapData(enu_M_flu, display_fps); } /*! * Gives the address given to connect to the vehicle. - * \return : A string corresponding to the Ethernet or serial address used for the connection to the vehicle. + * \return : A string corresponding to the Ethernet or serial address used for + * the connection to the vehicle. * * \sa connect() */ @@ -1163,16 +1247,21 @@ std::string vpRobotMavsdk::getAddress() const { return m_impl->getAddress(); } /*! * Gets current battery level in volts. - * \warning When the vehicle battery gets below a certain threshold (around 14.8 for a 4S battery), you should recharge - * it. + * \warning When the vehicle battery gets below a certain threshold (around 14.8 + * for a 4S battery), you should recharge it. */ -float vpRobotMavsdk::getBatteryLevel() const { return m_impl->getBatteryLevel(); } +float vpRobotMavsdk::getBatteryLevel() const { + return m_impl->getBatteryLevel(); +} /*! * Gets the current vehicle FRD position in its local NED frame. - * \param[in] ned_M_frd : Homogeneous matrix describing the position and attitude of the vehicle returned by telemetry. + * \param[in] ned_M_frd : Homogeneous matrix describing the position and + * attitude of the vehicle returned by telemetry. */ -void vpRobotMavsdk::getPosition(vpHomogeneousMatrix &ned_M_frd) const { m_impl->getPosition(ned_M_frd); } +void vpRobotMavsdk::getPosition(vpHomogeneousMatrix &ned_M_frd) const { + m_impl->getPosition(ned_M_frd); +} /*! * Gets the current vehicle FRD position in its local NED frame. @@ -1181,9 +1270,9 @@ void vpRobotMavsdk::getPosition(vpHomogeneousMatrix &ned_M_frd) const { m_impl-> * \param[in] ned_down : Position of the vehicle along NED down axis in [m]. * \param[in] ned_yaw : Yaw angle in [rad] of the vehicle along NED down axis. */ -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); +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); } /*! @@ -1192,10 +1281,13 @@ void vpRobotMavsdk::getPosition(float &ned_north, float &ned_east, float &ned_do * \warning Only available if the GPS is initialized, for example * in simulation. */ -std::tuple vpRobotMavsdk::getHome() const { return m_impl->getHome(); } +std::tuple vpRobotMavsdk::getHome() const { + return m_impl->getHome(); +} /*! - * Sends a flat trim command to the vehicle, to calibrate accelerometer and gyro. + * Sends a flat trim command to the vehicle, to calibrate accelerometer and + * gyro. * * \warning Should be executed only when the vehicle is on a flat surface. */ @@ -1203,12 +1295,14 @@ void vpRobotMavsdk::doFlatTrim() {} /*! * Sets the take off altitude. - * \param[in] altitude : Desired altitude for take off in meters, equal to 1.0 m by default. - * \warning The altitude must be positive. + * \param[in] altitude : Desired altitude for take off in meters, equal to 1.0 m + * by default. \warning The altitude must be positive. * * \sa takeOff() */ -void vpRobotMavsdk::setTakeOffAlt(double altitude) { m_impl->setTakeOffAlt(altitude); } +void vpRobotMavsdk::setTakeOffAlt(double altitude) { + m_impl->setTakeOffAlt(altitude); +} /*! * Arms the vehicle. @@ -1224,41 +1318,44 @@ bool vpRobotMavsdk::disarm() { return m_impl->disarm(); } /*! * Sends take off command when the vehicle has flying capabilities. - * \param[in] interactive : If true asks the user if the offboard mode is to be forced through the terminal. If false - * offboard mode is automatically set. + * \param[in] interactive : If true asks the user if the offboard mode is to be + * forced through the terminal. If false offboard mode is automatically set. * \param[in] timeout_sec : Time out in seconds to acchieve takeoff. * \return - * - If the vehicle has flying capabilities, returns true if the take off is successful, false otherwise, - * typically when a timeout occurs. If the vehicle has flying capabilities and is already flying, return true. + * - If the vehicle has flying capabilities, returns true if the take off is + * successful, false otherwise, typically when a timeout occurs. If the vehicle + * has flying capabilities and is already flying, return true. * - If the vehicle doesn't have flying capabilities, returns true. * \warning This function is blocking. * \sa setTakeOffAlt(), land(), hasFlyingCapability() */ -bool vpRobotMavsdk::takeOff(bool interactive, int timeout_sec) { return m_impl->takeOff(interactive, timeout_sec); } +bool vpRobotMavsdk::takeOff(bool interactive, int timeout_sec) { + return m_impl->takeOff(interactive, timeout_sec); +} /*! * Sends take off command when the vehicle has flying capabilities. - * \param[in] interactive : If true asks the user if the offboard mode is to be forced through the terminal. If false - * offboard mode is automatically set. - * \param[in] takeoff_altitude : Take off altitude in [m]. Should be a positive value. - * \param[in] timeout_sec : Time out in seconds to acchieve takeoff. + * \param[in] interactive : If true asks the user if the offboard mode is to be + * forced through the terminal. If false offboard mode is automatically set. + * \param[in] takeoff_altitude : Take off altitude in [m]. Should be a positive + * value. \param[in] timeout_sec : Time out in seconds to acchieve takeoff. * \return - * - If the vehicle has flying capabilities, returns true if the take off is successful, false otherwise, - * typically when a timeout occurs. + * - If the vehicle has flying capabilities, returns true if the take off is + * successful, false otherwise, typically when a timeout occurs. * - If the vehicle doesn't have flying capabilities, returns true. * \warning This function is blocking. * \sa setTakeOffAlt(), land(), hasFlyingCapability() */ -bool vpRobotMavsdk::takeOff(bool interactive, double takeoff_altitude, int timeout_sec) -{ +bool vpRobotMavsdk::takeOff(bool interactive, double takeoff_altitude, + int timeout_sec) { m_impl->setTakeOffAlt(takeoff_altitude); - return m_impl->takeOff(interactive, timeout_sec); + return m_impl->takeOff(interactive, timeout_sec); } /*! * Makes the vehicle hold its position. - * \warning When the vehicle is equipped with a GPS, switches to hold mode. It means that takeControl() - * needs to be called after. + * \warning When the vehicle is equipped with a GPS, switches to hold mode. It + * means that takeControl() needs to be called after. * * \return true when success, false otherwise. */ @@ -1266,110 +1363,130 @@ bool vpRobotMavsdk::holdPosition() { return m_impl->holdPosition(); }; /*! * Stops any vehicle movement. - * \warning Depending on the speed of the vehicle when the function is called, it may still move a bit until it - * stabilizes. + * \warning Depending on the speed of the vehicle when the function is called, + * it may still move a bit until it stabilizes. */ -bool vpRobotMavsdk::stopMoving() { return m_impl->stopMoving(); }; +bool vpRobotMavsdk::stopMoving() { return m_impl->stopMoving(); }; /*! * Sends landing command if the vehicle has flying capabilities. * \return - * - If the vehicle has flying capabilities, returns true if the landing is successful, false otherwise. + * - If the vehicle has flying capabilities, returns true if the landing is + * successful, false otherwise. * - If the vehicle doesn't have flying capabilities, returns true. * \sa takeOff(), hasFlyingCapability() */ bool vpRobotMavsdk::land() { return m_impl->land(); } /*! - * Moves the vehicle Front-Right-Down (FRD) body frame with respect to the global reference NED frame. + * Moves the vehicle Front-Right-Down (FRD) body frame with respect to the + * global reference NED frame. * * \param[in] ned_north : Absolute position to reach along north axis (meters). * \param[in] ned_east : Absolute position to reach along east axis (meters). * \param[in] ned_down : Absolute position to reach along down axis (meters). * \param[in] ned_yaw : Absolute position to reach of the heading (radians). - * \param[in] blocking : When true this function is blocking until the position is reached. - * \param[in] timeout_sec : Timeout value in seconds applied when `blocking` is set to true. - * \return true when positioning succeed, false otherwise, typically when timeout occurs before reaching the position. + * \param[in] blocking : When true this function is blocking until the position + * is reached. \param[in] timeout_sec : Timeout value in seconds applied when + * `blocking` is set to true. \return true when positioning succeed, false + * otherwise, typically when timeout occurs before reaching the position. * * \sa setPosition(const vpHomogeneousMatrix &, bool, float) * \sa setPositionRelative(float, float, float, float, bool, float) */ -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(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); } /*! - * Moves the vehicle Front-Right-Down (FRD) body frame with respect to the global reference NED frame. + * Moves the vehicle Front-Right-Down (FRD) body frame with respect to the + * global reference NED frame. * - * \param[in] ned_M_frd : Homogeneous matrix that express the FRD absolute position to reach by the vehicle expressed - * in the NED global reference frame. - * \param[in] blocking : When true this function is blocking until the position is reached. - * \param[in] timeout_sec : Timeout value in seconds applied when `blocking` is set to true. - * \return true when positioning succeed, false otherwise, typically when timeout occurs before reaching the position. + * \param[in] ned_M_frd : Homogeneous matrix that express the FRD absolute + * position to reach by the vehicle expressed in the NED global reference frame. + * \param[in] blocking : When true this function is blocking until the position + * is reached. \param[in] timeout_sec : Timeout value in seconds applied when + * `blocking` is set to true. \return true when positioning succeed, false + * otherwise, typically when timeout occurs before reaching the position. * - * \warning The rotation around the FRD X and Y axes should be equal to 0, as the vehicle (drone or rover) - * cannot rotate around these axes. - * \warning This function is blocking. + * \warning The rotation around the FRD X and Y axes should be equal to 0, as + * the vehicle (drone or rover) cannot rotate around these axes. \warning This + * function is blocking. * * \sa setPosition(float, float, float, float, bool, float) * \sa setPositionRelative(const vpHomogeneousMatrix &, bool, float) */ -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::setPosition(const vpHomogeneousMatrix &ned_M_frd, + bool blocking, int timeout_sec) { + return m_impl->setPosition(ned_M_frd, blocking, timeout_sec); } /*! - * Moves the vehicle Front-Right-Down (FRD) body frame with respect to the global reference NED frame. + * Moves the vehicle Front-Right-Down (FRD) body frame with respect to the + * global reference NED frame. * * \param[in] ned_delta_north : Relative displacement along north (meters). * \param[in] ned_delta_east : Relative displacement along east (meters). * \param[in] ned_delta_down : Relative displacement along down axis (meters). * \param[in] ned_delta_yaw : Relative rotation of the heading (radians). - * \param[in] blocking : When true this function is blocking until the position is reached. - * \param[in] timeout_sec : Timeout value in seconds applied when `blocking` is set to true. - * \return true when positioning succeed, false otherwise, typically when timeout occurs before reaching the position. + * \param[in] blocking : When true this function is blocking until the position + * is reached. \param[in] timeout_sec : Timeout value in seconds applied when + * `blocking` is set to true. \return true when positioning succeed, false + * otherwise, typically when timeout occurs before reaching the position. * * \sa setPositionRelative(const vpHomogeneousMatrix &, bool, float) * \sa setPosition(float, float, float, float, bool, float) */ -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(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); } /*! - * Moves the vehicle Front-Right-Down (FRD) body frame with respect to the global reference NED frame. + * Moves the vehicle Front-Right-Down (FRD) body frame with respect to the + * global reference NED frame. * - * \param[in] delta_frd_M_frd : Homogeneous matrix that express the FRD absolute position to reach by the vehicle expressed - * in the NED global reference frame. - * \param[in] blocking : When true this function is blocking until the position is reached. - * \param[in] timeout_sec : Timeout value in seconds applied when `blocking` is set to true. - * \return true when positioning succeed, false otherwise, typically when timeout occurs before reaching the position. + * \param[in] delta_frd_M_frd : Homogeneous matrix that express the FRD absolute + * position to reach by the vehicle expressed in the NED global reference frame. + * \param[in] blocking : When true this function is blocking until the position + * is reached. \param[in] timeout_sec : Timeout value in seconds applied when + * `blocking` is set to true. \return true when positioning succeed, false + * otherwise, typically when timeout occurs before reaching the position. * - * \warning The rotation around the FRD X and Y axes should be equal to 0, as the vehicle (drone or rover) - * cannot rotate around these axes. - * \warning This function is blocking. + * \warning The rotation around the FRD X and Y axes should be equal to 0, as + * the vehicle (drone or rover) cannot rotate around these axes. \warning This + * function is blocking. * * \sa setPositionRelative(float, float, float, float, bool, float) * \sa setPosition(const vpHomogeneousMatrix &, bool, float) */ -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::setPositionRelative( + const vpHomogeneousMatrix &delta_frd_M_frd, bool blocking, + int timeout_sec) { + return m_impl->setPositionRelative(delta_frd_M_frd, blocking, timeout_sec); } /*! * Sets the vehicle velocity in its own Front-Right-Down (FRD) body frame. * - * \param[in] frd_vel_cmd : 4-dim vehicle velocity commands, vx, vy, vz, wz. Translation velocities (vx, vy, vz) should - * be expressed in m/s and rotation velocity (wz) in rad/s. + * \param[in] frd_vel_cmd : 4-dim vehicle velocity commands, vx, vy, vz, wz. + * Translation velocities (vx, vy, vz) should be expressed in m/s and rotation + * velocity (wz) in rad/s. * - * \warning The dimension of the velocity vector should be equal to 4, as the vehicle cannot rotate around X and Y axes. - * \warning The vehicle applies this command until given another one. + * \warning The dimension of the velocity vector should be equal to 4, as the + * vehicle cannot rotate around X and Y axes. \warning The vehicle applies this + * command until given another one. */ -bool vpRobotMavsdk::setVelocity(const vpColVector &frd_vel_cmd) { return m_impl->setVelocity(frd_vel_cmd); } +bool vpRobotMavsdk::setVelocity(const vpColVector &frd_vel_cmd) { + return m_impl->setVelocity(frd_vel_cmd); +} /*! * Cuts the motors like a kill switch. Should only be used in emergency cases. @@ -1381,7 +1498,8 @@ bool vpRobotMavsdk::kill() { return m_impl->kill(); } /*! * Sets the yaw speed, expressed in rad/s, in the Front-Right-Down body frame. * - * \warning The vehicle will not stop moving in that direction until you send another motion command. + * \warning The vehicle will not stop moving in that direction until you send + * another motion command. * * \param[in] body_frd_wz : Desired FRD body frame yaw speed in rad/s. * - Positive values will make the vehicle turn to its right (clockwise) @@ -1389,12 +1507,15 @@ bool vpRobotMavsdk::kill() { return m_impl->kill(); } * * \return true when success, false otherwise. */ -bool vpRobotMavsdk::setYawSpeed(double body_frd_wz) { return m_impl->setYawSpeed(body_frd_wz); } +bool vpRobotMavsdk::setYawSpeed(double body_frd_wz) { + return m_impl->setYawSpeed(body_frd_wz); +} /*! * Sets the forward speed, expressed in m/s, in the Front-Right-Down body frame. * - * \warning The vehicle will not stop moving in that direction until you send another motion command. + * \warning The vehicle will not stop moving in that direction until you send + * another motion command. * * \param[in] body_frd_vx : Desired FRD body frame forward speed in m/s. * - Positive values will make the vehicle go forward @@ -1402,12 +1523,15 @@ bool vpRobotMavsdk::setYawSpeed(double body_frd_wz) { return m_impl->setYawSpeed * * \return true when success, false otherwise. */ -bool vpRobotMavsdk::setForwardSpeed(double body_frd_vx) { return m_impl->setForwardSpeed(body_frd_vx); } +bool vpRobotMavsdk::setForwardSpeed(double body_frd_vx) { + return m_impl->setForwardSpeed(body_frd_vx); +} /*! * Sets the lateral speed, expressed in m/s, in the Front-Right-Down body frame. * - * \warning The vehicle will not stop moving in that direction until you send another motion command. + * \warning The vehicle will not stop moving in that direction until you send + * another motion command. * * \param[in] body_frd_vy : Desired FRD body frame lateral speed in m/s. * - Positive values will make the vehicle go right @@ -1415,18 +1539,20 @@ bool vpRobotMavsdk::setForwardSpeed(double body_frd_vx) { return m_impl->setForw * * \return true when success, false otherwise. */ -bool vpRobotMavsdk::setLateralSpeed(double body_frd_vy) { return m_impl->setLateralSpeed(body_frd_vy); } +bool vpRobotMavsdk::setLateralSpeed(double body_frd_vy) { + return m_impl->setLateralSpeed(body_frd_vy); +} /*! - * Allows to set GPS global origin to initialize the Kalman filter when the vehicle is not - * equipped with a GPS. + * Allows to set GPS global origin to initialize the Kalman filter when the + * vehicle is not equipped with a GPS. * * \param latitude : Latitude in deg (WGS84). * \param longitude : Longitude in deg (WGS84). * \param altitude : Altitude in meter. Positive for up. */ -bool vpRobotMavsdk::setGPSGlobalOrigin(double latitude, double longitude, double altitude) -{ +bool vpRobotMavsdk::setGPSGlobalOrigin(double latitude, double longitude, + double altitude) { return m_impl->setGPSGlobalOrigin(latitude, longitude, altitude); } @@ -1435,59 +1561,58 @@ bool vpRobotMavsdk::setGPSGlobalOrigin(double latitude, double longitude, double * - When using the PX4 flight stack start the off-board mode, * - When using Ardupilot stack start the guided mode. * - * \return true when off-board or guided mode are successfully started, false otherwise. + * \return true when off-board or guided mode are successfully started, false + * otherwise. * * This method should be called before using setPosition(), setVelocity() - * + * * \sa releaseControl() */ -bool vpRobotMavsdk::takeControl() -{ - return m_impl->takeControl(); -} +bool vpRobotMavsdk::takeControl() { return m_impl->takeControl(); } /*! * Release control allowing running software outside of the autopilot: * - When using the PX4 flight stack stop the off-board mode, * - When using Ardupilot stack stop the guided mode. * - * \return true when off-board or guided mode are successfully stopped, false otherwise. + * \return true when off-board or guided mode are successfully stopped, false + * otherwise. * * \sa takeControl() */ -bool vpRobotMavsdk::releaseControl() -{ - return m_impl->releaseControl(); -} +bool vpRobotMavsdk::releaseControl() { return m_impl->releaseControl(); } /*! * Enable/disable auto land mode in the destructor. - * \param[in] auto_land : When true auto land mode is enabled and the destructor calls land() when - * the vehicle has flying capabilities. When false the destructor doesn't call land(). - * + * \param[in] auto_land : When true auto land mode is enabled and the destructor + * calls land() when the vehicle has flying capabilities. When false the + * destructor doesn't call land(). + * * \sa land() */ -void vpRobotMavsdk::setAutoLand(bool auto_land) -{ +void vpRobotMavsdk::setAutoLand(bool auto_land) { m_impl->setAutoLand(auto_land); } /*! - * Incertitude used to decided if a position is reached when using setPosition() and setPositionRelative(). - * \param[in] position_incertitude : Position incertitude in [m]. - * \param[in] yaw_incertitude : Yaw angle incertitude in [rad]. + * Incertitude used to decided if a position is reached when using setPosition() + * and setPositionRelative(). \param[in] position_incertitude : Position + * incertitude in [m]. \param[in] yaw_incertitude : Yaw angle incertitude in + * [rad]. * * \sa setPosition(), setPositionRelative() */ -void vpRobotMavsdk::setPositioningIncertitude(float position_incertitude, float yaw_incertitude) -{ +void vpRobotMavsdk::setPositioningIncertitude(float position_incertitude, + float yaw_incertitude) { m_impl->setPositioningIncertitude(position_incertitude, yaw_incertitude); } /*! - * Sets the vertical speed, expressed in m/s, in the Front-Right-Down body frame. + * Sets the vertical speed, expressed in m/s, in the Front-Right-Down body + * frame. * - * \warning The vehicle will not stop moving in that direction until you send another motion command. + * \warning The vehicle will not stop moving in that direction until you send + * another motion command. * * \param[in] body_frd_vz : Desired FRD body frame vertical speed in m/s. * - Positive values will make the vehicle go down. @@ -1495,24 +1620,26 @@ void vpRobotMavsdk::setPositioningIncertitude(float position_incertitude, float * * \return true when success, false otherwise. */ -bool vpRobotMavsdk::setVerticalSpeed(double body_frd_vz) { return m_impl->setVerticalSpeed(body_frd_vz); } +bool vpRobotMavsdk::setVerticalSpeed(double body_frd_vz) { + return m_impl->setVerticalSpeed(body_frd_vz); +} /*! * Enable/disable verbose mode. * * \param[in] verbose : When true enable verbose mode. */ -void vpRobotMavsdk::setVerbose(bool verbose) -{ - m_impl->setVerbose(verbose); -} +void vpRobotMavsdk::setVerbose(bool verbose) { m_impl->setVerbose(verbose); } /*! * Return true if the vehicle has flying capabilities. - * Ground rover, surface boat and submarine vehicles are considered with non flying capabilities, while - * all the other vehicles are considered with flying capabilities. + * Ground rover, surface boat and submarine vehicles are considered with non + * flying capabilities, while all the other vehicles are considered with flying + * capabilities. */ -bool vpRobotMavsdk::hasFlyingCapability() { return m_impl->getFlyingCapability(); } +bool vpRobotMavsdk::hasFlyingCapability() { + return m_impl->getFlyingCapability(); +} #elif !defined(VISP_BUILD_SHARED_LIBS) // Work around to avoid warning: libvisp_robot.a(vpRobotMavsdk.cpp.o) has no From dea8e57ac66860ed1e71a1bcf86bb1149d3aab6c Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Tue, 4 Apr 2023 18:34:15 +0200 Subject: [PATCH 2/6] Pass through clang-format --- .../src/real-robot/mavsdk/vpRobotMavsdk.cpp | 572 ++++++++---------- 1 file changed, 245 insertions(+), 327 deletions(-) diff --git a/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp b/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp index 438c769412..a396eabf06 100644 --- a/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp +++ b/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp @@ -57,14 +57,14 @@ using std::chrono::seconds; using std::this_thread::sleep_for; #ifndef DOXYGEN_SHOULD_SKIP_THIS -class vpRobotMavsdk::vpRobotMavsdkImpl { +class vpRobotMavsdk::vpRobotMavsdkImpl +{ public: vpRobotMavsdkImpl() : m_takeoffAlt(1.0) {} - vpRobotMavsdkImpl(const std::string &connection_info) : m_takeoffAlt(1.0) { - connect(connection_info); - } + vpRobotMavsdkImpl(const std::string &connection_info) : m_takeoffAlt(1.0) { connect(connection_info); } - virtual ~vpRobotMavsdkImpl() { + virtual ~vpRobotMavsdkImpl() + { if (m_has_flying_capability && m_auto_land) { land(); } @@ -76,33 +76,33 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { * it. \param[in] mavsdk : the Mavsdk object we will use to subscribe to the * system. \returns Returns a shared pointer to the system. */ - std::shared_ptr getSystem(mavsdk::Mavsdk &mavsdk) { + std::shared_ptr getSystem(mavsdk::Mavsdk &mavsdk) + { std::cout << "Waiting to discover system..." << std::endl; - auto prom = std::promise>{}; + auto prom = std::promise >{}; 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]() { + 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(); + auto system = mavsdk.systems().back(); - if (system->has_autopilot()) { - std::cout << "Discovered autopilot" << std::endl; + 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); + mavsdk.unsubscribe_on_new_system(handle); #else mavsdk.subscribe_on_new_system(nullptr); #endif - prom.set_value(system); - } - }); + prom.set_value(system); + } + }); // We usually receive heartbeats at 1Hz, therefore we should find a // system after around 3 seconds max, surely. @@ -115,39 +115,36 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return fut.get(); } - MAV_TYPE getVehicleType() { + MAV_TYPE getVehicleType() + { auto passthrough = mavsdk::MavlinkPassthrough{m_system}; auto prom = std::promise{}; 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) { + 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) { + 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; - } + // 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); + 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); + passthrough.unsubscribe_message(handle); #else - passthrough.subscribe_message_async(MAVLINK_MSG_ID_HEARTBEAT, - nullptr); + passthrough.subscribe_message_async(MAVLINK_MSG_ID_HEARTBEAT, nullptr); #endif - prom.set_value(static_cast(heartbeat.type)); - }); + prom.set_value(static_cast(heartbeat.type)); + }); // We usually receive heartbeats at 1Hz, therefore we should find a // system after around 3 seconds max, surely. @@ -160,24 +157,23 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return fut.get(); } - void calibrate_accelerometer(mavsdk::Calibration &calibration) { + void calibrate_accelerometer(mavsdk::Calibration &calibration) + { std::cout << "Calibrating accelerometer..." << std::endl; std::promise calibration_promise; auto calibration_future = calibration_promise.get_future(); - calibration.calibrate_accelerometer_async( - create_calibration_callback(calibration_promise)); + calibration.calibrate_accelerometer_async(create_calibration_callback(calibration_promise)); calibration_future.wait(); } - std::function - create_calibration_callback(std::promise &calibration_promise) { - return [&calibration_promise]( - const mavsdk::Calibration::Result result, - const mavsdk::Calibration::ProgressData progress_data) { + std::function + create_calibration_callback(std::promise &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; @@ -188,36 +184,34 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { std::cout << " Progress: " << progress_data.progress << std::endl; } if (progress_data.has_status_text) { - std::cout << " Instruction: " << progress_data.status_text - << std::endl; + std::cout << " Instruction: " << progress_data.status_text << std::endl; } break; default: - std::cout << "--- Calibration failed with message: " << result - << std::endl; + std::cout << "--- Calibration failed with message: " << result << std::endl; calibration_promise.set_value(); break; } }; } - void calibrate_gyro(mavsdk::Calibration &calibration) { + void calibrate_gyro(mavsdk::Calibration &calibration) + { std::cout << "Calibrating gyro..." << std::endl; std::promise calibration_promise; auto calibration_future = calibration_promise.get_future(); - calibration.calibrate_gyro_async( - create_calibration_callback(calibration_promise)); + calibration.calibrate_gyro_async(create_calibration_callback(calibration_promise)); calibration_future.wait(); } public: - void connect(const std::string &connectionInfo) { + void connect(const std::string &connectionInfo) + { m_address = connectionInfo; - mavsdk::ConnectionResult connection_result = - m_mavsdk.add_any_connection(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; @@ -227,16 +221,14 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { m_system = getSystem(m_mavsdk); if (!m_system) { - throw vpException(vpException::fatalError, "Unable to connect to: %s", - connectionInfo.c_str()); + 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::cout << (m_has_flying_capability ? "Connected to a flying vehicle" : "Connected to a non flying vehicle") << std::endl; m_action = std::make_shared(m_system); @@ -244,7 +236,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { m_offboard = std::make_shared(m_system); } - bool hasFlyingCapability(MAV_TYPE mav_type) { + bool hasFlyingCapability(MAV_TYPE mav_type) + { switch (mav_type) { case MAV_TYPE::MAV_TYPE_GROUND_ROVER: case MAV_TYPE::MAV_TYPE_SURFACE_BOAT: @@ -255,7 +248,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { } } - bool isRunning() const { + bool isRunning() const + { if (m_system == NULL) { return false; } else { @@ -263,7 +257,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { } } - std::string getAddress() const { + std::string getAddress() const + { std::string sequence; std::stringstream ss(m_address); std::string actual_address; @@ -284,22 +279,23 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { } } - float getBatteryLevel() const { + float getBatteryLevel() const + { mavsdk::Telemetry::Battery battery = m_telemetry.get()->battery(); return battery.voltage_v; } - void getPosition(vpHomogeneousMatrix &ned_M_frd) const { + 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}; + 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 { + 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; @@ -308,12 +304,14 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { ned_yaw = vpMath::rad(angles.yaw_deg); } - std::tuple getHome() const { + std::tuple 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) { + 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 @@ -342,8 +340,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { 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); + 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; @@ -353,29 +350,28 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { 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 << "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; + << " , Yaw [rad]: " << pose_estimate.angle_body.yaw_rad << " ." << std::endl; } } return true; } } - void setTakeOffAlt(double altitude) { + void setTakeOffAlt(double altitude) + { if (altitude > 0) { m_takeoffAlt = altitude; } else { - std::cerr << "ERROR : The take off altitude must be positive." - << std::endl; + std::cerr << "ERROR : The take off altitude must be positive." << std::endl; } } - void doFlatTrim() { + void doFlatTrim() + { // Instantiate plugin. auto calibration = mavsdk::Calibration(m_system); @@ -384,7 +380,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { calibrate_gyro(calibration); } - bool arm() { + bool arm() + { // Arm vehicle std::cout << "Arming...\n"; const mavsdk::Action::Result arm_result = m_action.get()->arm(); @@ -396,7 +393,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool disarm() { + bool disarm() + { // Arm vehicle std::cout << "Disarming...\n"; const mavsdk::Action::Result arm_result = m_action.get()->disarm(); @@ -408,7 +406,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool setGPSGlobalOrigin(double latitude, double longitude, double altitude) { + 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; @@ -416,8 +415,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { 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, + 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) { @@ -427,13 +425,13 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool takeControl() { + bool takeControl() + { if (m_verbose) { std::cout << "Starting offboard mode..." << std::endl; } - if (m_telemetry.get()->flight_mode() != - mavsdk::Telemetry::FlightMode::Offboard) { + if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) { const mavsdk::Offboard::VelocityBodyYawspeed stay{}; m_offboard.get()->set_velocity_body(stay); @@ -448,8 +446,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { // Wait to ensure offboard mode active in telemetry double t = vpTime::measureTimeMs(); - while (m_telemetry.get()->flight_mode() != - mavsdk::Telemetry::FlightMode::Offboard) { + 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; @@ -462,16 +459,16 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - void setPositioningIncertitude(float position_incertitude, - float yaw_incertitude) { + 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 takeOff(bool interactive, int timeout_sec) + { if (!m_has_flying_capability) { - std::cerr << "Warning: Cannot takeoff this non flying vehicle" - << std::endl; + std::cerr << "Warning: Cannot takeoff this non flying vehicle" << std::endl; return true; } @@ -480,13 +477,11 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { if (!interactive) { authorize_takeoff = true; } else { - if (m_telemetry.get()->flight_mode() == - mavsdk::Telemetry::FlightMode::Offboard) { + 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") { + 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; @@ -499,8 +494,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { } if (m_telemetry.get()->in_air()) { - std::cerr << "Cannot take off as the robot is already flying." - << std::endl; + std::cerr << "Cannot take off as the robot is already flying." << std::endl; return true; } else if (authorize_takeoff) { // Arm vehicle @@ -512,8 +506,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { if (interactive) { std::string answer; - while (answer != "Y" && answer != "y" && answer != "N" && - answer != "n") { + 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") { @@ -525,8 +518,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { } // Takeoff - if (m_telemetry.get()->gps_info().fix_type == - mavsdk::Telemetry::FixType::NoGps) { + if (m_telemetry.get()->gps_info().fix_type == mavsdk::Telemetry::FixType::NoGps) { // 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 @@ -560,29 +552,25 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { // 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) { + 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()->subscribe_landed_state(nullptr); + m_telemetry.get()->unsubscribe_landed_state(handle); in_air_promise.set_value(); } - std::cout << "state: " << state << std::endl; }); +#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) { + 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); @@ -597,10 +585,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { #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) { + [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(); @@ -608,18 +594,15 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { }); #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) { + [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) { + 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); @@ -630,8 +613,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { } } else { // GPS connected, we use Action::takeoff() - std::cout << "---- DEBUG: GPS detected: use action::takeoff()" - << std::endl; + 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; @@ -646,29 +628,25 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { auto in_air_promise = std::promise{}; 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) { + 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()->subscribe_landed_state(nullptr); + m_telemetry.get()->unsubscribe_landed_state(handle); in_air_promise.set_value(); } - std::cout << "state: " << state << std::endl; }); +#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) { + 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) @@ -684,10 +662,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { #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) { + [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(); @@ -695,18 +671,15 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { }); #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) { + [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) { + 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); @@ -722,14 +695,14 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool land() { + bool land() + { if (!m_has_flying_capability) { std::cerr << "Warning: Cannot land this non flying vehicle" << std::endl; return true; } - if (m_telemetry.get()->flight_mode() != - mavsdk::Telemetry::FlightMode::Land) { + 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) { @@ -752,8 +725,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool setPosition(float ned_north, float ned_east, float ned_down, - float ned_yaw, bool blocking, int timeout_sec) { + 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; @@ -761,16 +734,13 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { 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; + 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_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) { if (m_verbose) { - std::cout << "Cannot set vehicle position: offboard mode not started" - << std::endl; + std::cout << "Cannot set vehicle position: offboard mode not started" << std::endl; } return false; } @@ -782,19 +752,16 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { #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) { + [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)); + 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::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(); @@ -802,27 +769,23 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { }); #else m_telemetry.get()->subscribe_odometry( - [this, &position_reached_promise, - &position_target](mavsdk::Telemetry::Odometry odom) { + [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)); + 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::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) { + if (position_reached_future.wait_for(seconds(timeout_sec)) == std::future_status::timeout) { std::cerr << "Positioning failed: position not reached.\n"; return false; } @@ -832,9 +795,9 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { 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) { + 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{}; @@ -853,48 +816,42 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { 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); + 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) { + 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; + 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; + 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); + 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) { + 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; + 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; + 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); + return setPositionRelative(M.getTranslationVector()[0], M.getTranslationVector()[1], M.getTranslationVector()[2], + XYZvec[2], blocking, timeout_sec); } - bool setVelocity(const vpColVector &frd_vel_cmd) { + 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 " @@ -902,11 +859,9 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { frd_vel_cmd.size())); } - if (m_telemetry.get()->flight_mode() != - mavsdk::Telemetry::FlightMode::Offboard) { + 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; + std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl; } return false; } @@ -920,7 +875,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool kill() { + 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; @@ -929,10 +885,10 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool holdPosition() { + bool holdPosition() + { if (m_telemetry.get()->in_air()) { - if (m_telemetry.get()->gps_info().fix_type != - mavsdk::Telemetry::FixType::NoGps) { + 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) { @@ -940,12 +896,9 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return false; } } else { - if (m_telemetry.get()->flight_mode() != - mavsdk::Telemetry::FlightMode::Offboard) { + 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; + std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl; } return false; } @@ -956,12 +909,11 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool stopMoving() { - if (m_telemetry.get()->flight_mode() != - mavsdk::Telemetry::FlightMode::Offboard) { + 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; + std::cout << "Cannot stop moving: offboard mode not started" << std::endl; } return false; } @@ -972,7 +924,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool releaseControl() { + bool releaseControl() + { auto offboard_result = m_offboard.get()->stop(); if (offboard_result != mavsdk::Offboard::Result::Success) { std::cerr << "Offboard stop failed: " << offboard_result << '\n'; @@ -984,12 +937,11 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { 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) { + 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; + std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl; } return false; } @@ -1003,12 +955,11 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool setForwardSpeed(double body_frd_vx) { - if (m_telemetry.get()->flight_mode() != - mavsdk::Telemetry::FlightMode::Offboard) { + 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; + std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl; } return false; } @@ -1023,12 +974,11 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool setLateralSpeed(double body_frd_vy) { - if (m_telemetry.get()->flight_mode() != - mavsdk::Telemetry::FlightMode::Offboard) { + 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; + std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl; } return false; } @@ -1042,12 +992,11 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool setVerticalSpeed(double body_frd_vz) { - if (m_telemetry.get()->flight_mode() != - mavsdk::Telemetry::FlightMode::Offboard) { + 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; + std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl; } return false; } @@ -1079,16 +1028,14 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { private: //*** Attributes ***// - std::string - m_address{}; ///< Ip address of the robot to discover on the network + std::string m_address{}; ///< Ip address of the robot to discover on the network mavsdk::Mavsdk m_mavsdk{}; std::shared_ptr m_system; std::shared_ptr m_action; std::shared_ptr m_telemetry; std::shared_ptr m_offboard; - double m_takeoffAlt{ - 1.0}; ///< The altitude to aim for when calling the function takeoff + double m_takeoffAlt{1.0}; ///< The altitude to aim for when calling the function takeoff MAV_TYPE m_mav_type{}; // Vehicle type bool m_has_flying_capability{false}; @@ -1147,8 +1094,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { * \sa setPositioningIncertitude(), setAutoLand(), takeControl(), * releaseControl() */ -vpRobotMavsdk::vpRobotMavsdk(const std::string &connection_info) - : m_impl(new vpRobotMavsdkImpl(connection_info)) { +vpRobotMavsdk::vpRobotMavsdk(const std::string &connection_info) : m_impl(new vpRobotMavsdkImpl(connection_info)) +{ m_impl->setPositioningIncertitude(0.05, vpMath::rad(5)); } @@ -1171,7 +1118,8 @@ vpRobotMavsdk::vpRobotMavsdk(const std::string &connection_info) * * \sa connect(), setPositioningIncertitude() */ -vpRobotMavsdk::vpRobotMavsdk() : m_impl(new vpRobotMavsdkImpl()) { +vpRobotMavsdk::vpRobotMavsdk() : m_impl(new vpRobotMavsdkImpl()) +{ m_impl->setPositioningIncertitude(0.05, vpMath::rad(5)); } @@ -1198,9 +1146,7 @@ vpRobotMavsdk::~vpRobotMavsdk() { delete m_impl; } * * \sa getAddress() */ -void vpRobotMavsdk::connect(const std::string &connection_info) { - m_impl->connect(connection_info); -} +void vpRobotMavsdk::connect(const std::string &connection_info) { m_impl->connect(connection_info); } /*! * Checks if the vehicle is running, ie if the vehicle is connected and ready to @@ -1231,8 +1177,8 @@ bool vpRobotMavsdk::isRunning() const { return m_impl->isRunning(); } * Internally we transform this FRD pose in a NED global reference frame as * expected by Pixhawk convention. */ -bool vpRobotMavsdk::sendMocapData(const vpHomogeneousMatrix &enu_M_flu, - int display_fps) { +bool vpRobotMavsdk::sendMocapData(const vpHomogeneousMatrix &enu_M_flu, int display_fps) +{ return m_impl->sendMocapData(enu_M_flu, display_fps); } @@ -1250,18 +1196,14 @@ std::string vpRobotMavsdk::getAddress() const { return m_impl->getAddress(); } * \warning When the vehicle battery gets below a certain threshold (around 14.8 * for a 4S battery), you should recharge it. */ -float vpRobotMavsdk::getBatteryLevel() const { - return m_impl->getBatteryLevel(); -} +float vpRobotMavsdk::getBatteryLevel() const { return m_impl->getBatteryLevel(); } /*! * Gets the current vehicle FRD position in its local NED frame. * \param[in] ned_M_frd : Homogeneous matrix describing the position and * attitude of the vehicle returned by telemetry. */ -void vpRobotMavsdk::getPosition(vpHomogeneousMatrix &ned_M_frd) const { - m_impl->getPosition(ned_M_frd); -} +void vpRobotMavsdk::getPosition(vpHomogeneousMatrix &ned_M_frd) const { m_impl->getPosition(ned_M_frd); } /*! * Gets the current vehicle FRD position in its local NED frame. @@ -1270,8 +1212,8 @@ void vpRobotMavsdk::getPosition(vpHomogeneousMatrix &ned_M_frd) const { * \param[in] ned_down : Position of the vehicle along NED down axis in [m]. * \param[in] ned_yaw : Yaw angle in [rad] of the vehicle along NED down axis. */ -void vpRobotMavsdk::getPosition(float &ned_north, float &ned_east, - float &ned_down, float &ned_yaw) const { +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); } @@ -1281,9 +1223,7 @@ void vpRobotMavsdk::getPosition(float &ned_north, float &ned_east, * \warning Only available if the GPS is initialized, for example * in simulation. */ -std::tuple vpRobotMavsdk::getHome() const { - return m_impl->getHome(); -} +std::tuple vpRobotMavsdk::getHome() const { return m_impl->getHome(); } /*! * Sends a flat trim command to the vehicle, to calibrate accelerometer and @@ -1300,9 +1240,7 @@ void vpRobotMavsdk::doFlatTrim() {} * * \sa takeOff() */ -void vpRobotMavsdk::setTakeOffAlt(double altitude) { - m_impl->setTakeOffAlt(altitude); -} +void vpRobotMavsdk::setTakeOffAlt(double altitude) { m_impl->setTakeOffAlt(altitude); } /*! * Arms the vehicle. @@ -1329,9 +1267,7 @@ bool vpRobotMavsdk::disarm() { return m_impl->disarm(); } * \warning This function is blocking. * \sa setTakeOffAlt(), land(), hasFlyingCapability() */ -bool vpRobotMavsdk::takeOff(bool interactive, int timeout_sec) { - return m_impl->takeOff(interactive, timeout_sec); -} +bool vpRobotMavsdk::takeOff(bool interactive, int timeout_sec) { return m_impl->takeOff(interactive, timeout_sec); } /*! * Sends take off command when the vehicle has flying capabilities. @@ -1346,8 +1282,8 @@ bool vpRobotMavsdk::takeOff(bool interactive, int timeout_sec) { * \warning This function is blocking. * \sa setTakeOffAlt(), land(), hasFlyingCapability() */ -bool vpRobotMavsdk::takeOff(bool interactive, double takeoff_altitude, - int timeout_sec) { +bool vpRobotMavsdk::takeOff(bool interactive, double takeoff_altitude, int timeout_sec) +{ m_impl->setTakeOffAlt(takeoff_altitude); return m_impl->takeOff(interactive, timeout_sec); } @@ -1394,10 +1330,10 @@ bool vpRobotMavsdk::land() { return m_impl->land(); } * \sa setPosition(const vpHomogeneousMatrix &, bool, float) * \sa setPositionRelative(float, float, float, float, bool, float) */ -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(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); } /*! @@ -1418,8 +1354,8 @@ bool vpRobotMavsdk::setPosition(float ned_north, float ned_east, float ned_down, * \sa setPosition(float, float, float, float, bool, float) * \sa setPositionRelative(const vpHomogeneousMatrix &, bool, float) */ -bool vpRobotMavsdk::setPosition(const vpHomogeneousMatrix &ned_M_frd, - bool blocking, int 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); } @@ -1439,13 +1375,10 @@ bool vpRobotMavsdk::setPosition(const vpHomogeneousMatrix &ned_M_frd, * \sa setPositionRelative(const vpHomogeneousMatrix &, bool, float) * \sa setPosition(float, float, float, float, bool, float) */ -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, +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); } @@ -1467,9 +1400,8 @@ bool vpRobotMavsdk::setPositionRelative(float ned_delta_north, * \sa setPositionRelative(float, float, float, float, bool, float) * \sa setPosition(const vpHomogeneousMatrix &, bool, float) */ -bool vpRobotMavsdk::setPositionRelative( - const vpHomogeneousMatrix &delta_frd_M_frd, bool blocking, - int 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); } @@ -1484,9 +1416,7 @@ bool vpRobotMavsdk::setPositionRelative( * vehicle cannot rotate around X and Y axes. \warning The vehicle applies this * command until given another one. */ -bool vpRobotMavsdk::setVelocity(const vpColVector &frd_vel_cmd) { - return m_impl->setVelocity(frd_vel_cmd); -} +bool vpRobotMavsdk::setVelocity(const vpColVector &frd_vel_cmd) { return m_impl->setVelocity(frd_vel_cmd); } /*! * Cuts the motors like a kill switch. Should only be used in emergency cases. @@ -1507,9 +1437,7 @@ bool vpRobotMavsdk::kill() { return m_impl->kill(); } * * \return true when success, false otherwise. */ -bool vpRobotMavsdk::setYawSpeed(double body_frd_wz) { - return m_impl->setYawSpeed(body_frd_wz); -} +bool vpRobotMavsdk::setYawSpeed(double body_frd_wz) { return m_impl->setYawSpeed(body_frd_wz); } /*! * Sets the forward speed, expressed in m/s, in the Front-Right-Down body frame. @@ -1523,9 +1451,7 @@ bool vpRobotMavsdk::setYawSpeed(double body_frd_wz) { * * \return true when success, false otherwise. */ -bool vpRobotMavsdk::setForwardSpeed(double body_frd_vx) { - return m_impl->setForwardSpeed(body_frd_vx); -} +bool vpRobotMavsdk::setForwardSpeed(double body_frd_vx) { return m_impl->setForwardSpeed(body_frd_vx); } /*! * Sets the lateral speed, expressed in m/s, in the Front-Right-Down body frame. @@ -1539,9 +1465,7 @@ bool vpRobotMavsdk::setForwardSpeed(double body_frd_vx) { * * \return true when success, false otherwise. */ -bool vpRobotMavsdk::setLateralSpeed(double body_frd_vy) { - return m_impl->setLateralSpeed(body_frd_vy); -} +bool vpRobotMavsdk::setLateralSpeed(double body_frd_vy) { return m_impl->setLateralSpeed(body_frd_vy); } /*! * Allows to set GPS global origin to initialize the Kalman filter when the @@ -1551,8 +1475,8 @@ bool vpRobotMavsdk::setLateralSpeed(double body_frd_vy) { * \param longitude : Longitude in deg (WGS84). * \param altitude : Altitude in meter. Positive for up. */ -bool vpRobotMavsdk::setGPSGlobalOrigin(double latitude, double longitude, - double altitude) { +bool vpRobotMavsdk::setGPSGlobalOrigin(double latitude, double longitude, double altitude) +{ return m_impl->setGPSGlobalOrigin(latitude, longitude, altitude); } @@ -1590,9 +1514,7 @@ bool vpRobotMavsdk::releaseControl() { return m_impl->releaseControl(); } * * \sa land() */ -void vpRobotMavsdk::setAutoLand(bool auto_land) { - m_impl->setAutoLand(auto_land); -} +void vpRobotMavsdk::setAutoLand(bool auto_land) { m_impl->setAutoLand(auto_land); } /*! * Incertitude used to decided if a position is reached when using setPosition() @@ -1602,8 +1524,8 @@ void vpRobotMavsdk::setAutoLand(bool auto_land) { * * \sa setPosition(), setPositionRelative() */ -void vpRobotMavsdk::setPositioningIncertitude(float position_incertitude, - float yaw_incertitude) { +void vpRobotMavsdk::setPositioningIncertitude(float position_incertitude, float yaw_incertitude) +{ m_impl->setPositioningIncertitude(position_incertitude, yaw_incertitude); } @@ -1620,9 +1542,7 @@ void vpRobotMavsdk::setPositioningIncertitude(float position_incertitude, * * \return true when success, false otherwise. */ -bool vpRobotMavsdk::setVerticalSpeed(double body_frd_vz) { - return m_impl->setVerticalSpeed(body_frd_vz); -} +bool vpRobotMavsdk::setVerticalSpeed(double body_frd_vz) { return m_impl->setVerticalSpeed(body_frd_vz); } /*! * Enable/disable verbose mode. @@ -1637,9 +1557,7 @@ void vpRobotMavsdk::setVerbose(bool verbose) { m_impl->setVerbose(verbose); } * flying capabilities, while all the other vehicles are considered with flying * capabilities. */ -bool vpRobotMavsdk::hasFlyingCapability() { - return m_impl->getFlyingCapability(); -} +bool vpRobotMavsdk::hasFlyingCapability() { return m_impl->getFlyingCapability(); } #elif !defined(VISP_BUILD_SHARED_LIBS) // Work around to avoid warning: libvisp_robot.a(vpRobotMavsdk.cpp.o) has no From 8e9751fe489148f40c367def8224776ace4a7956 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Wed, 5 Apr 2023 07:23:07 +0200 Subject: [PATCH 3/6] Indent code as it was before changes introduced by Tony --- .../src/real-robot/mavsdk/vpRobotMavsdk.cpp | 606 ++++++++---------- 1 file changed, 280 insertions(+), 326 deletions(-) diff --git a/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp b/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp index a396eabf06..683c06ab81 100644 --- a/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp +++ b/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp @@ -72,9 +72,9 @@ class vpRobotMavsdk::vpRobotMavsdkImpl private: /*! - * Connects a first time to the robot in order to get the system running on - * it. \param[in] mavsdk : the Mavsdk object we will use to subscribe to the - * system. \returns Returns a shared pointer to the system. + * Connects a first time to the robot in order to get the system running on it. + * \param[in] mavsdk : the Mavsdk object we will use to subscribe to the system. + * \returns Returns a shared pointer to the system. */ std::shared_ptr getSystem(mavsdk::Mavsdk &mavsdk) { @@ -128,23 +128,23 @@ class vpRobotMavsdk::vpRobotMavsdkImpl 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; - } + // 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); + 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); + passthrough.unsubscribe_message(handle); #else - passthrough.subscribe_message_async(MAVLINK_MSG_ID_HEARTBEAT, nullptr); + passthrough.subscribe_message_async(MAVLINK_MSG_ID_HEARTBEAT, nullptr); #endif - prom.set_value(static_cast(heartbeat.type)); - }); + prom.set_value(static_cast(heartbeat.type)); + }); // We usually receive heartbeats at 1Hz, therefore we should find a // system after around 3 seconds max, surely. @@ -272,9 +272,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl } return actual_address; } else { - std::cout << "ERROR : The address parameter must start with \"serial:\" " - "or \"udp:\" or \"tcp:\"." - << std::endl; + std::cout << "ERROR : The address parameter must start with \"serial:\" or \"udp:\" or \"tcp:\"." << std::endl; return std::string(); } } @@ -314,9 +312,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { 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). + // 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; @@ -346,14 +343,13 @@ class vpRobotMavsdk::vpRobotMavsdkImpl return false; } else { if (display_fps > 0) { - double display_time_ms = 1000. / display_fps; + 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 + 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; } } @@ -440,7 +436,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl std::cerr << "Offboard mode failed: " << offboard_result << std::endl; return false; } - } else if (m_verbose) { + } + else if (m_verbose) { std::cout << "Already in offboard mode" << std::endl; } @@ -482,8 +479,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl } 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::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") { @@ -498,7 +494,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl return true; } else if (authorize_takeoff) { // Arm vehicle - if (!arm()) { + if (! arm()) { return false; } @@ -520,9 +516,9 @@ class vpRobotMavsdk::vpRobotMavsdkImpl // Takeoff if (m_telemetry.get()->gps_info().fix_type == mavsdk::Telemetry::FixType::NoGps) { // 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 + // 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(); @@ -548,19 +544,18 @@ class vpRobotMavsdk::vpRobotMavsdkImpl 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 + // Possibility is to use set_position_velocity_ned(); to speed up takeoff -#if (VISP_HAVE_MAVSDK_VERSION > 0x010412) + #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 + [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."; @@ -569,49 +564,49 @@ class vpRobotMavsdk::vpRobotMavsdkImpl } std::cout << "state: " << state << std::endl; }); -#endif + #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) + #if (VISP_HAVE_MAVSDK_VERSION > 0x010412) m_telemetry.get()->unsubscribe_landed_state(handle); -#else + #else m_telemetry.get()->subscribe_landed_state(nullptr); -#endif + #endif return false; } // Add check with Altitude auto takeoff_finished_promise = std::promise{}; auto takeoff_finished_future = takeoff_finished_promise.get_future(); -#if (VISP_HAVE_MAVSDK_VERSION > 0x010412) + #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 + [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) + #if (VISP_HAVE_MAVSDK_VERSION > 0x010412) m_telemetry.get()->unsubscribe_odometry(handle); -#else + #else m_telemetry.get()->subscribe_odometry(nullptr); -#endif + #endif return false; } - } else { + } + else { // GPS connected, we use Action::takeoff() std::cout << "---- DEBUG: GPS detected: use action::takeoff()" << std::endl; @@ -627,16 +622,16 @@ class vpRobotMavsdk::vpRobotMavsdkImpl auto in_air_promise = std::promise{}; auto in_air_future = in_air_promise.get_future(); -#if (VISP_HAVE_MAVSDK_VERSION > 0x010412) + #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 + [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."; @@ -645,40 +640,38 @@ class vpRobotMavsdk::vpRobotMavsdkImpl } std::cout << "state: " << state << std::endl; }); -#endif + #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) + #if (VISP_HAVE_MAVSDK_VERSION > 0x010412) m_telemetry.get()->unsubscribe_landed_state(handle); -#else + #else m_telemetry.get()->subscribe_landed_state(nullptr); -#endif - // return false; + #endif } // Add check with Altitude auto takeoff_finished_promise = std::promise{}; auto takeoff_finished_future = takeoff_finished_promise.get_future(); -#if (VISP_HAVE_MAVSDK_VERSION > 0x010412) + #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 + [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) @@ -718,8 +711,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl } std::cout << "Landed!" << std::endl; - // We are relying on auto-disarming but let's keep watching the telemetry - // for a bit longer. + // 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; @@ -734,8 +726,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl 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; + 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) { @@ -750,53 +741,51 @@ class vpRobotMavsdk::vpRobotMavsdkImpl auto position_reached_promise = std::promise{}; auto position_reached_future = position_reached_promise.get_future(); -#if (VISP_HAVE_MAVSDK_VERSION > 0x010412) + #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 + [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; +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) + 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; @@ -816,8 +805,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl 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); + 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) @@ -831,8 +819,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl 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); + 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) @@ -846,16 +833,14 @@ class vpRobotMavsdk::vpRobotMavsdkImpl 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); + 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.", + "ERROR : Can't set velocity, dimension of the velocity vector %d should be equal to 4.", frd_vel_cmd.size())); } @@ -935,7 +920,10 @@ class vpRobotMavsdk::vpRobotMavsdkImpl return true; } - void setAutoLand(bool auto_land) { m_auto_land = auto_land; } + void setAutoLand(bool auto_land) + { + m_auto_land = auto_land; + } bool setYawSpeed(double body_frd_wz) { @@ -1012,7 +1000,10 @@ class vpRobotMavsdk::vpRobotMavsdkImpl bool getFlyingCapability() { return m_has_flying_capability; } - void setVerbose(bool verbose) { m_verbose = verbose; } + void setVerbose(bool verbose) + { + m_verbose = verbose; + } // void waitSystemReady() // { @@ -1051,48 +1042,38 @@ class vpRobotMavsdk::vpRobotMavsdkImpl /*! * Constructor. * - * Initializes vehicle controller, by discovering vehicles connected either with - * an Ethernet TCP or UDP link, or with a serial link the computer is currently - * connected to. + * Initializes vehicle controller, by discovering vehicles connected either with an Ethernet TCP or UDP link, or with a + * serial link the computer is currently connected to. * - * \warning This constructor should be called after the vehicle is turned on, - * and after the computer is connected to the vehicle Ethernet network or with a - * serial link. + * \warning This constructor should be called after the vehicle is turned on, and after the computer is connected to the + * vehicle Ethernet network or with a serial link. * - * \warning If the connection to the vehicle failed, the program will throw an - * exception. + * \warning If the connection to the vehicle failed, the program will throw an exception. * - * After having called this constructor, it is recommended to check if the - * vehicle is running with isRunning() before sending commands to the vehicle. + * After having called this constructor, it is recommended to check if the vehicle is running with isRunning() before + * sending commands to the vehicle. * - * Set default positioning incertitude to 0.05 meter in translation, and 5 - * degrees along yaw orientation. These default values are used to determine - * when a position is reached and could be changed using - * setPositioningIncertitude(). When the vehicle has flying capabilities, call - * by default land() in the destructor. This behavior could be changed using - * setAutoLand(). + * Set default positioning incertitude to 0.05 meter in translation, and 5 degrees along yaw orientation. + * These default values are used to determine when a position is reached and could be changed using setPositioningIncertitude(). + * When the vehicle has flying capabilities, call by default land() in the destructor. This behavior could be changed + * using setAutoLand(). * - * To control the vehicle using this class, you need to call takeControl() to - * start the off-board mode with PX4 or the guided mode with Ardupilot. After - * this call you can call setPosition() to move the vehicle to a desired - * position and yaw orientation or call setVelocity() to move the vehicle in - * velocity. + * To control the vehicle using this class, you need to call takeControl() to start the off-board mode with PX4 or the + * guided mode with Ardupilot. After this call you can call setPosition() to move the vehicle to a desired position + * and yaw orientation or call setVelocity() to move the vehicle in velocity. * - * \param[in] connection_info : Specify connection information. This parameter - * must be written following these conventions: + * \param[in] connection_info : Specify connection information. This parameter must be written following these + * conventions: * - for TCP link: tcp://[server_host][:server_port] * - for UDP link: udp://[bind_host][:bind_port] * - for Serial link: serial:///path/to/serial/dev[:baudrate]
* Examples: udp://192.168.30.111:14550 or serial:///dev/ttyACMO * - * For more information see - * [here](https://mavsdk.mavlink.io/main/en/cpp/guide/connections.html). + * For more information see [here](https://mavsdk.mavlink.io/main/en/cpp/guide/connections.html). * - * \exception vpException::fatalError : If the program failed to connect to the - * vehicle. + * \exception vpException::fatalError : If the program failed to connect to the vehicle. * - * \sa setPositioningIncertitude(), setAutoLand(), takeControl(), - * releaseControl() + * \sa setPositioningIncertitude(), setAutoLand(), takeControl(), releaseControl() */ vpRobotMavsdk::vpRobotMavsdk(const std::string &connection_info) : m_impl(new vpRobotMavsdkImpl(connection_info)) { @@ -1100,21 +1081,16 @@ vpRobotMavsdk::vpRobotMavsdk(const std::string &connection_info) : m_impl(new vp } /*! - * Default constructor without parameters. You need to use the connect() - * function afterwards. - * - * Set default positioning incertitude to 0.05 meter in translation, and 5 - * degrees along yaw orientation. These default values are used to determine - * when a position is reached and could be changed using - * setPositioningIncertitude(). When the vehicle has flying capabilities, call - * by default land() in the destructor. This behavior could be changed using - * setAutoLand(). - * - * To control the vehicle using this class, you need to call takeControl() to - * start the off-board mode with PX4 or the guided mode with Ardupilot. After - * this call you can call setPosition() to move the vehicle to a desired - * position and yaw orientation or call setVelocity() to move the vehicle in - * velocity. + * Default constructor without parameters. You need to use the connect() function afterwards. + * + * Set default positioning incertitude to 0.05 meter in translation, and 5 degrees along yaw orientation. + * These default values are used to determine when a position is reached and could be changed using setPositioningIncertitude(). + * When the vehicle has flying capabilities, call by default land() in the destructor. This behavior could be changed + * using setAutoLand(). + * + * To control the vehicle using this class, you need to call takeControl() to start the off-board mode with PX4 or the + * guided mode with Ardupilot. After this call you can call setPosition() to move the vehicle to a desired position + * and yaw orientation or call setVelocity() to move the vehicle in velocity. * * \sa connect(), setPositioningIncertitude() */ @@ -1125,8 +1101,8 @@ vpRobotMavsdk::vpRobotMavsdk() : m_impl(new vpRobotMavsdkImpl()) /*! * Destructor. - * When the vehicle has flying capabilities and when auto land mode is enabled, - * lands the vehicle if not landed and safely disconnects everything. + * When the vehicle has flying capabilities and when auto land mode is enabled, lands the vehicle if not landed + * and safely disconnects everything. * * \sa setAutoLand() */ @@ -1134,48 +1110,42 @@ vpRobotMavsdk::~vpRobotMavsdk() { delete m_impl; } /*! * Connects to the vehicle and setups the different controllers. - * \param[in] connection_info : The connection information given to connect to - * the vehicle. You may use: + * \param[in] connection_info : The connection information given to connect to the vehicle. You may use: * - for TCP link: tcp://[server_host][:server_port] * - for UDP link: udp://[bind_host][:bind_port] * - for Serial link: serial:///path/to/serial/dev[:baudrate]
* Examples: udp://192.168.30.111:14550 or serial:///dev/ttyACMO * - * For more information see - * [here](https://mavsdk.mavlink.io/main/en/cpp/guide/connections.html). + * For more information see [here](https://mavsdk.mavlink.io/main/en/cpp/guide/connections.html). * * \sa getAddress() */ void vpRobotMavsdk::connect(const std::string &connection_info) { m_impl->connect(connection_info); } /*! - * Checks if the vehicle is running, ie if the vehicle is connected and ready to - * receive commands. + * Checks if the vehicle is running, ie if the vehicle is connected and ready to receive commands. */ bool vpRobotMavsdk::isRunning() const { return m_impl->isRunning(); } /*! * Sends MoCap position data to the vehicle. * - * We consider here that the MoCap global reference frame is ENU - * (East-North-Up). The vehicle body frame if FLU (Front-Left-Up) where X axis - * is aligned with the vehicle front axis and Z axis going upward. - * - * Internally, this pose called `enu_M_flu` is transformed to match the - * requirements of the Pixhawk into `ned_M_frd` corresponding to the FRD - * (Front-Right-Down) body frame position in the NED (North-East-Down) local - * reference frame. - * - * \return true if the MoCap data was successfully sent to the vehicle, false - * otherwise. \param[in] enu_M_flu : Homogeneous matrix containing the pose of - * the vehicle given by the MoCap system. To be more precise, this matrix gives - * the pose of the vehicle FLU body frame returned by the MoCap where MoCap - * global reference frame is defined as ENU. \param[in] display_fps : Display - * `ned_M_frd` pose internally sent through mavlink at the given framerate. A - * value of 0 can be used to disable this display. - * - * Internally we transform this FRD pose in a NED global reference frame as - * expected by Pixhawk convention. + * We consider here that the MoCap global reference frame is ENU (East-North-Up). + * The vehicle body frame if FLU (Front-Left-Up) where X axis is aligned + * with the vehicle front axis and Z axis going upward. + * + * Internally, this pose called `enu_M_flu` is transformed to match the requirements of the Pixhawk + * into `ned_M_frd` corresponding to the FRD (Front-Right-Down) body frame position in the NED (North-East-Down) + * local reference frame. + * + * \return true if the MoCap data was successfully sent to the vehicle, false otherwise. + * \param[in] enu_M_flu : Homogeneous matrix containing the pose of the vehicle given by the MoCap system. + * To be more precise, this matrix gives the pose of the vehicle FLU body frame returned by the MoCap where + * MoCap global reference frame is defined as ENU. + * \param[in] display_fps : Display `ned_M_frd` pose internally sent through mavlink at the given framerate. A value of 0 can + * be used to disable this display. + * + * Internally we transform this FRD pose in a NED global reference frame as expected by Pixhawk convention. */ bool vpRobotMavsdk::sendMocapData(const vpHomogeneousMatrix &enu_M_flu, int display_fps) { @@ -1184,8 +1154,7 @@ bool vpRobotMavsdk::sendMocapData(const vpHomogeneousMatrix &enu_M_flu, int disp /*! * Gives the address given to connect to the vehicle. - * \return : A string corresponding to the Ethernet or serial address used for - * the connection to the vehicle. + * \return : A string corresponding to the Ethernet or serial address used for the connection to the vehicle. * * \sa connect() */ @@ -1193,15 +1162,14 @@ std::string vpRobotMavsdk::getAddress() const { return m_impl->getAddress(); } /*! * Gets current battery level in volts. - * \warning When the vehicle battery gets below a certain threshold (around 14.8 - * for a 4S battery), you should recharge it. + * \warning When the vehicle battery gets below a certain threshold (around 14.8 for a 4S battery), you should recharge + * it. */ float vpRobotMavsdk::getBatteryLevel() const { return m_impl->getBatteryLevel(); } /*! * Gets the current vehicle FRD position in its local NED frame. - * \param[in] ned_M_frd : Homogeneous matrix describing the position and - * attitude of the vehicle returned by telemetry. + * \param[in] ned_M_frd : Homogeneous matrix describing the position and attitude of the vehicle returned by telemetry. */ void vpRobotMavsdk::getPosition(vpHomogeneousMatrix &ned_M_frd) const { m_impl->getPosition(ned_M_frd); } @@ -1226,8 +1194,7 @@ void vpRobotMavsdk::getPosition(float &ned_north, float &ned_east, float &ned_do std::tuple vpRobotMavsdk::getHome() const { return m_impl->getHome(); } /*! - * Sends a flat trim command to the vehicle, to calibrate accelerometer and - * gyro. + * Sends a flat trim command to the vehicle, to calibrate accelerometer and gyro. * * \warning Should be executed only when the vehicle is on a flat surface. */ @@ -1235,8 +1202,8 @@ void vpRobotMavsdk::doFlatTrim() {} /*! * Sets the take off altitude. - * \param[in] altitude : Desired altitude for take off in meters, equal to 1.0 m - * by default. \warning The altitude must be positive. + * \param[in] altitude : Desired altitude for take off in meters, equal to 1.0 m by default. + * \warning The altitude must be positive. * * \sa takeOff() */ @@ -1256,13 +1223,12 @@ bool vpRobotMavsdk::disarm() { return m_impl->disarm(); } /*! * Sends take off command when the vehicle has flying capabilities. - * \param[in] interactive : If true asks the user if the offboard mode is to be - * forced through the terminal. If false offboard mode is automatically set. - * \param[in] timeout_sec : Time out in seconds to acchieve takeoff. + * \param[in] interactive : If true asks the user if the offboard mode is to be forced through the terminal. If false + * offboard mode is automatically set. + * \param[in] timeout_sec : Time out in seconds to achieve takeoff. * \return - * - If the vehicle has flying capabilities, returns true if the take off is - * successful, false otherwise, typically when a timeout occurs. If the vehicle - * has flying capabilities and is already flying, return true. + * - If the vehicle has flying capabilities, returns true if the take off is successful, false otherwise, + * typically when a timeout occurs. If the vehicle has flying capabilities and is already flying, return true. * - If the vehicle doesn't have flying capabilities, returns true. * \warning This function is blocking. * \sa setTakeOffAlt(), land(), hasFlyingCapability() @@ -1271,13 +1237,13 @@ bool vpRobotMavsdk::takeOff(bool interactive, int timeout_sec) { return m_impl-> /*! * Sends take off command when the vehicle has flying capabilities. - * \param[in] interactive : If true asks the user if the offboard mode is to be - * forced through the terminal. If false offboard mode is automatically set. - * \param[in] takeoff_altitude : Take off altitude in [m]. Should be a positive - * value. \param[in] timeout_sec : Time out in seconds to acchieve takeoff. + * \param[in] interactive : If true asks the user if the offboard mode is to be forced through the terminal. If false + * offboard mode is automatically set. + * \param[in] takeoff_altitude : Take off altitude in [m]. Should be a positive value. + * \param[in] timeout_sec : Time out in seconds to achieve takeoff. * \return - * - If the vehicle has flying capabilities, returns true if the take off is - * successful, false otherwise, typically when a timeout occurs. + * - If the vehicle has flying capabilities, returns true if the take off is successful, false otherwise, + * typically when a timeout occurs. * - If the vehicle doesn't have flying capabilities, returns true. * \warning This function is blocking. * \sa setTakeOffAlt(), land(), hasFlyingCapability() @@ -1290,8 +1256,8 @@ bool vpRobotMavsdk::takeOff(bool interactive, double takeoff_altitude, int timeo /*! * Makes the vehicle hold its position. - * \warning When the vehicle is equipped with a GPS, switches to hold mode. It - * means that takeControl() needs to be called after. + * \warning When the vehicle is equipped with a GPS, switches to hold mode. It means that takeControl() + * needs to be called after. * * \return true when success, false otherwise. */ @@ -1299,57 +1265,51 @@ bool vpRobotMavsdk::holdPosition() { return m_impl->holdPosition(); }; /*! * Stops any vehicle movement. - * \warning Depending on the speed of the vehicle when the function is called, - * it may still move a bit until it stabilizes. + * \warning Depending on the speed of the vehicle when the function is called, it may still move a bit until it + * stabilizes. */ bool vpRobotMavsdk::stopMoving() { return m_impl->stopMoving(); }; /*! * Sends landing command if the vehicle has flying capabilities. * \return - * - If the vehicle has flying capabilities, returns true if the landing is - * successful, false otherwise. + * - If the vehicle has flying capabilities, returns true if the landing is successful, false otherwise. * - If the vehicle doesn't have flying capabilities, returns true. * \sa takeOff(), hasFlyingCapability() */ bool vpRobotMavsdk::land() { return m_impl->land(); } /*! - * Moves the vehicle Front-Right-Down (FRD) body frame with respect to the - * global reference NED frame. + * Moves the vehicle Front-Right-Down (FRD) body frame with respect to the global reference NED frame. * * \param[in] ned_north : Absolute position to reach along north axis (meters). * \param[in] ned_east : Absolute position to reach along east axis (meters). * \param[in] ned_down : Absolute position to reach along down axis (meters). * \param[in] ned_yaw : Absolute position to reach of the heading (radians). - * \param[in] blocking : When true this function is blocking until the position - * is reached. \param[in] timeout_sec : Timeout value in seconds applied when - * `blocking` is set to true. \return true when positioning succeed, false - * otherwise, typically when timeout occurs before reaching the position. + * \param[in] blocking : When true this function is blocking until the position is reached. + * \param[in] timeout_sec : Timeout value in seconds applied when `blocking` is set to true. + * \return true when positioning succeed, false otherwise, typically when timeout occurs before reaching the position. * * \sa setPosition(const vpHomogeneousMatrix &, bool, float) * \sa setPositionRelative(float, float, float, float, bool, float) */ -bool vpRobotMavsdk::setPosition(float ned_north, float ned_east, float ned_down, float ned_yaw, bool blocking, - int timeout_sec) +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); } /*! - * Moves the vehicle Front-Right-Down (FRD) body frame with respect to the - * global reference NED frame. + * Moves the vehicle Front-Right-Down (FRD) body frame with respect to the global reference NED frame. * - * \param[in] ned_M_frd : Homogeneous matrix that express the FRD absolute - * position to reach by the vehicle expressed in the NED global reference frame. - * \param[in] blocking : When true this function is blocking until the position - * is reached. \param[in] timeout_sec : Timeout value in seconds applied when - * `blocking` is set to true. \return true when positioning succeed, false - * otherwise, typically when timeout occurs before reaching the position. + * \param[in] ned_M_frd : Homogeneous matrix that express the FRD absolute position to reach by the vehicle expressed + * in the NED global reference frame. + * \param[in] blocking : When true this function is blocking until the position is reached. + * \param[in] timeout_sec : Timeout value in seconds applied when `blocking` is set to true. + * \return true when positioning succeed, false otherwise, typically when timeout occurs before reaching the position. * - * \warning The rotation around the FRD X and Y axes should be equal to 0, as - * the vehicle (drone or rover) cannot rotate around these axes. \warning This - * function is blocking. + * \warning The rotation around the FRD X and Y axes should be equal to 0, as the vehicle (drone or rover) + * cannot rotate around these axes. + * \warning This function is blocking. * * \sa setPosition(float, float, float, float, bool, float) * \sa setPositionRelative(const vpHomogeneousMatrix &, bool, float) @@ -1360,42 +1320,36 @@ bool vpRobotMavsdk::setPosition(const vpHomogeneousMatrix &ned_M_frd, bool block } /*! - * Moves the vehicle Front-Right-Down (FRD) body frame with respect to the - * global reference NED frame. + * Moves the vehicle Front-Right-Down (FRD) body frame with respect to the global reference NED frame. * * \param[in] ned_delta_north : Relative displacement along north (meters). * \param[in] ned_delta_east : Relative displacement along east (meters). * \param[in] ned_delta_down : Relative displacement along down axis (meters). * \param[in] ned_delta_yaw : Relative rotation of the heading (radians). - * \param[in] blocking : When true this function is blocking until the position - * is reached. \param[in] timeout_sec : Timeout value in seconds applied when - * `blocking` is set to true. \return true when positioning succeed, false - * otherwise, typically when timeout occurs before reaching the position. + * \param[in] blocking : When true this function is blocking until the position is reached. + * \param[in] timeout_sec : Timeout value in seconds applied when `blocking` is set to true. + * \return true when positioning succeed, false otherwise, typically when timeout occurs before reaching the position. * * \sa setPositionRelative(const vpHomogeneousMatrix &, bool, float) * \sa setPosition(float, float, float, float, bool, float) */ -bool vpRobotMavsdk::setPositionRelative(float ned_delta_north, float ned_delta_east, float ned_delta_down, - float ned_delta_yaw, bool blocking, int 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); + return m_impl->setPositionRelative(ned_delta_north, ned_delta_east, ned_delta_down, ned_delta_yaw, blocking, timeout_sec); } /*! - * Moves the vehicle Front-Right-Down (FRD) body frame with respect to the - * global reference NED frame. + * Moves the vehicle Front-Right-Down (FRD) body frame with respect to the global reference NED frame. * - * \param[in] delta_frd_M_frd : Homogeneous matrix that express the FRD absolute - * position to reach by the vehicle expressed in the NED global reference frame. - * \param[in] blocking : When true this function is blocking until the position - * is reached. \param[in] timeout_sec : Timeout value in seconds applied when - * `blocking` is set to true. \return true when positioning succeed, false - * otherwise, typically when timeout occurs before reaching the position. + * \param[in] delta_frd_M_frd : Homogeneous matrix that express the FRD absolute position to reach by the vehicle expressed + * in the NED global reference frame. + * \param[in] blocking : When true this function is blocking until the position is reached. + * \param[in] timeout_sec : Timeout value in seconds applied when `blocking` is set to true. + * \return true when positioning succeed, false otherwise, typically when timeout occurs before reaching the position. * - * \warning The rotation around the FRD X and Y axes should be equal to 0, as - * the vehicle (drone or rover) cannot rotate around these axes. \warning This - * function is blocking. + * \warning The rotation around the FRD X and Y axes should be equal to 0, as the vehicle (drone or rover) + * cannot rotate around these axes. + * \warning This function is blocking. * * \sa setPositionRelative(float, float, float, float, bool, float) * \sa setPosition(const vpHomogeneousMatrix &, bool, float) @@ -1408,13 +1362,11 @@ bool vpRobotMavsdk::setPositionRelative(const vpHomogeneousMatrix &delta_frd_M_f /*! * Sets the vehicle velocity in its own Front-Right-Down (FRD) body frame. * - * \param[in] frd_vel_cmd : 4-dim vehicle velocity commands, vx, vy, vz, wz. - * Translation velocities (vx, vy, vz) should be expressed in m/s and rotation - * velocity (wz) in rad/s. + * \param[in] frd_vel_cmd : 4-dim vehicle velocity commands, vx, vy, vz, wz. Translation velocities (vx, vy, vz) should + * be expressed in m/s and rotation velocity (wz) in rad/s. * - * \warning The dimension of the velocity vector should be equal to 4, as the - * vehicle cannot rotate around X and Y axes. \warning The vehicle applies this - * command until given another one. + * \warning The dimension of the velocity vector should be equal to 4, as the vehicle cannot rotate around X and Y axes. + * \warning The vehicle applies this command until given another one. */ bool vpRobotMavsdk::setVelocity(const vpColVector &frd_vel_cmd) { return m_impl->setVelocity(frd_vel_cmd); } @@ -1428,8 +1380,7 @@ bool vpRobotMavsdk::kill() { return m_impl->kill(); } /*! * Sets the yaw speed, expressed in rad/s, in the Front-Right-Down body frame. * - * \warning The vehicle will not stop moving in that direction until you send - * another motion command. + * \warning The vehicle will not stop moving in that direction until you send another motion command. * * \param[in] body_frd_wz : Desired FRD body frame yaw speed in rad/s. * - Positive values will make the vehicle turn to its right (clockwise) @@ -1442,8 +1393,7 @@ bool vpRobotMavsdk::setYawSpeed(double body_frd_wz) { return m_impl->setYawSpeed /*! * Sets the forward speed, expressed in m/s, in the Front-Right-Down body frame. * - * \warning The vehicle will not stop moving in that direction until you send - * another motion command. + * \warning The vehicle will not stop moving in that direction until you send another motion command. * * \param[in] body_frd_vx : Desired FRD body frame forward speed in m/s. * - Positive values will make the vehicle go forward @@ -1456,8 +1406,7 @@ bool vpRobotMavsdk::setForwardSpeed(double body_frd_vx) { return m_impl->setForw /*! * Sets the lateral speed, expressed in m/s, in the Front-Right-Down body frame. * - * \warning The vehicle will not stop moving in that direction until you send - * another motion command. + * \warning The vehicle will not stop moving in that direction until you send another motion command. * * \param[in] body_frd_vy : Desired FRD body frame lateral speed in m/s. * - Positive values will make the vehicle go right @@ -1468,8 +1417,8 @@ bool vpRobotMavsdk::setForwardSpeed(double body_frd_vx) { return m_impl->setForw bool vpRobotMavsdk::setLateralSpeed(double body_frd_vy) { return m_impl->setLateralSpeed(body_frd_vy); } /*! - * Allows to set GPS global origin to initialize the Kalman filter when the - * vehicle is not equipped with a GPS. + * Allows to set GPS global origin to initialize the Kalman filter when the vehicle is not + * equipped with a GPS. * * \param latitude : Latitude in deg (WGS84). * \param longitude : Longitude in deg (WGS84). @@ -1485,42 +1434,47 @@ bool vpRobotMavsdk::setGPSGlobalOrigin(double latitude, double longitude, double * - When using the PX4 flight stack start the off-board mode, * - When using Ardupilot stack start the guided mode. * - * \return true when off-board or guided mode are successfully started, false - * otherwise. + * \return true when off-board or guided mode are successfully started, false otherwise. * * This method should be called before using setPosition(), setVelocity() * * \sa releaseControl() */ -bool vpRobotMavsdk::takeControl() { return m_impl->takeControl(); } +bool vpRobotMavsdk::takeControl() +{ + return m_impl->takeControl(); +} /*! * Release control allowing running software outside of the autopilot: * - When using the PX4 flight stack stop the off-board mode, * - When using Ardupilot stack stop the guided mode. * - * \return true when off-board or guided mode are successfully stopped, false - * otherwise. + * \return true when off-board or guided mode are successfully stopped, false otherwise. * * \sa takeControl() */ -bool vpRobotMavsdk::releaseControl() { return m_impl->releaseControl(); } +bool vpRobotMavsdk::releaseControl() +{ + return m_impl->releaseControl(); +} /*! * Enable/disable auto land mode in the destructor. - * \param[in] auto_land : When true auto land mode is enabled and the destructor - * calls land() when the vehicle has flying capabilities. When false the - * destructor doesn't call land(). + * \param[in] auto_land : When true auto land mode is enabled and the destructor calls land() when + * the vehicle has flying capabilities. When false the destructor doesn't call land(). * * \sa land() */ -void vpRobotMavsdk::setAutoLand(bool auto_land) { m_impl->setAutoLand(auto_land); } +void vpRobotMavsdk::setAutoLand(bool auto_land) +{ + m_impl->setAutoLand(auto_land); +} /*! - * Incertitude used to decided if a position is reached when using setPosition() - * and setPositionRelative(). \param[in] position_incertitude : Position - * incertitude in [m]. \param[in] yaw_incertitude : Yaw angle incertitude in - * [rad]. + * Incertitude used to decided if a position is reached when using setPosition() and setPositionRelative(). + * \param[in] position_incertitude : Position incertitude in [m]. + * \param[in] yaw_incertitude : Yaw angle incertitude in [rad]. * * \sa setPosition(), setPositionRelative() */ @@ -1530,11 +1484,9 @@ void vpRobotMavsdk::setPositioningIncertitude(float position_incertitude, float } /*! - * Sets the vertical speed, expressed in m/s, in the Front-Right-Down body - * frame. + * Sets the vertical speed, expressed in m/s, in the Front-Right-Down body frame. * - * \warning The vehicle will not stop moving in that direction until you send - * another motion command. + * \warning The vehicle will not stop moving in that direction until you send another motion command. * * \param[in] body_frd_vz : Desired FRD body frame vertical speed in m/s. * - Positive values will make the vehicle go down. @@ -1549,13 +1501,15 @@ bool vpRobotMavsdk::setVerticalSpeed(double body_frd_vz) { return m_impl->setVer * * \param[in] verbose : When true enable verbose mode. */ -void vpRobotMavsdk::setVerbose(bool verbose) { m_impl->setVerbose(verbose); } +void vpRobotMavsdk::setVerbose(bool verbose) +{ + m_impl->setVerbose(verbose); +} /*! * Return true if the vehicle has flying capabilities. - * Ground rover, surface boat and submarine vehicles are considered with non - * flying capabilities, while all the other vehicles are considered with flying - * capabilities. + * Ground rover, surface boat and submarine vehicles are considered with non flying capabilities, while + * all the other vehicles are considered with flying capabilities. */ bool vpRobotMavsdk::hasFlyingCapability() { return m_impl->getFlyingCapability(); } From ac85b90adf0afe7468e105aa1b92fc8f7b52f98b Mon Sep 17 00:00:00 2001 From: Antonio Date: Mon, 14 Aug 2023 22:33:05 +0200 Subject: [PATCH 4/6] improved land --- .drone.yml | 210 ++++++ .../robot/include/visp3/robot/vpRobotMavsdk.h | 12 +- .../src/real-robot/mavsdk/vpRobotMavsdk.cpp | 699 +++++++++--------- 3 files changed, 575 insertions(+), 346 deletions(-) create mode 100644 .drone.yml diff --git a/.drone.yml b/.drone.yml new file mode 100644 index 0000000000..550bbef477 --- /dev/null +++ b/.drone.yml @@ -0,0 +1,210 @@ +--- +kind: pipeline +name: arm64_clang_cmake_opencv + +platform: + os: linux + arch: arm64 + +steps: +- name: Build and Test + image: ubuntu:18.04 + environment: + CC: clang + CMAKE_FLAGS: '-DBUILD_DEMOS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_JAVA=OFF -DBUILD_MODULE_visp_java_bindings_generator=OFF -DBUILD_TUTORIALS=OFF' + commands: + - lscpu + - cat /proc/cpuinfo + - uname -m + - uname -r + - echo "CMAKE_FLAGS:= $CMAKE_FLAGS" + - export DEBIAN_FRONTEND=noninteractive + - apt-get update -y + # To overcome link error "cannot find -lgfortran", gfortran package needs to be installed + - apt-get install -y make $CC cmake git-core libopencv-dev libx11-dev libv4l-dev liblapack-dev libeigen3-dev gfortran + - $CC --version + - gfortran --version + - cd .. + - git clone --depth 1 https://github.com/lagadic/visp-images + - export VISP_INPUT_IMAGE_PATH=$(pwd)/visp-images + # build OpenBLAS from source, version in Bionic is 0.2.20+ds-4 (24 Jul 2017) + - git clone --depth 1 https://github.com/xianyi/OpenBLAS.git + - cd OpenBLAS + - mkdir install + - make -j$(nproc) + - make -j$(nproc) install PREFIX=$(pwd)/install + - export OpenBLAS_HOME=$(pwd)/install + - cd ../src + - mkdir build && cd build + - export CC=/usr/bin/clang + - export CXX=/usr/bin/clang++ + - cmake $CMAKE_FLAGS .. + - make -j$(nproc) + - ctest -j$(nproc) --output-on-failure + +--- +kind: pipeline +name: arm64_gcc_cmake_opencv + +platform: + os: linux + arch: arm64 + +steps: +- name: Build and Test + image: ubuntu:18.04 + environment: + CC: gcc + CMAKE_FLAGS: '-DBUILD_DEMOS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_JAVA=OFF -DBUILD_MODULE_visp_java_bindings_generator=OFF -DBUILD_TUTORIALS=OFF -DDART_TESTING_TIMEOUT=2500' + commands: + - lscpu + - cat /proc/cpuinfo + - uname -m + - uname -r + - echo "CMAKE_FLAGS:= $CMAKE_FLAGS" + - export DEBIAN_FRONTEND=noninteractive + - apt-get update -y + # To overcome link error "cannot find -lgfortran", gfortran package needs to be installed + - apt-get install -y make $CC g++ cmake git-core libopencv-dev libx11-dev libv4l-dev liblapack-dev libeigen3-dev gfortran + - $CC --version + - gfortran --version + - cd .. + - git clone --depth 1 https://github.com/lagadic/visp-images + - export VISP_INPUT_IMAGE_PATH=$(pwd)/visp-images + # build OpenBLAS from source, version in Bionic is 0.2.20+ds-4 (24 Jul 2017) + - git clone --depth 1 https://github.com/xianyi/OpenBLAS.git + - cd OpenBLAS + - mkdir install + - make -j$(nproc) + - make -j$(nproc) install PREFIX=$(pwd)/install + - export OpenBLAS_HOME=$(pwd)/install + - cd ../src + - mkdir build && cd build + - cmake $CMAKE_FLAGS .. + - make -j$(nproc) + - ctest -j$(nproc) --output-on-failure + +--- +kind: pipeline +name: arm64_gcc_cmake + +platform: + os: linux + arch: arm64 + +steps: +- name: Build and Test without OpenCV + image: ubuntu:18.04 + environment: + CC: gcc + CMAKE_FLAGS: '-DBUILD_DEMOS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_JAVA=OFF -DBUILD_MODULE_visp_java_bindings_generator=OFF -DBUILD_TUTORIALS=OFF -DDART_TESTING_TIMEOUT=2500' + commands: + - lscpu + - cat /proc/cpuinfo + - uname -m + - uname -r + - echo "CMAKE_FLAGS:= $CMAKE_FLAGS" + - export DEBIAN_FRONTEND=noninteractive + - apt-get update -y + # To overcome link error "cannot find -lgfortran", gfortran package needs to be installed + - apt-get install -y make $CC g++ cmake git-core libx11-dev libv4l-dev liblapack-dev libeigen3-dev gfortran + - $CC --version + - gfortran --version + - cd .. + - git clone --depth 1 https://github.com/lagadic/visp-images + - export VISP_INPUT_IMAGE_PATH=$(pwd)/visp-images + # build OpenBLAS from source, version in Bionic is 0.2.20+ds-4 (24 Jul 2017) + - git clone --depth 1 https://github.com/xianyi/OpenBLAS.git + - cd OpenBLAS + - mkdir install + - make -j$(nproc) + - make -j$(nproc) install PREFIX=$(pwd)/install + - export OpenBLAS_HOME=$(pwd)/install + - cd ../src + - mkdir build && cd build + - cmake $CMAKE_FLAGS .. + - make -j$(nproc) + - ctest -j$(nproc) --output-on-failure + +# --- +# kind: pipeline +# name: arm32_gcc_cmake_opencv + +# platform: +# os: linux +# arch: arm + +# steps: +# - name: Build and Test (32 bits) +# image: ubuntu:18.04 +# environment: +# CC: gcc +# CMAKE_FLAGS: '-DBUILD_DEMOS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_JAVA=OFF -DBUILD_MODULE_visp_java_bindings_generator=OFF -DBUILD_TUTORIALS=OFF' +# commands: +# - lscpu +# - cat /proc/cpuinfo +# - uname -m +# - uname -r +# - echo "CMAKE_FLAGS:= $CMAKE_FLAGS" +# - export DEBIAN_FRONTEND=noninteractive +# - apt-get update -y +# # To overcome link error "cannot find -lgfortran", gfortran package needs to be installed +# - apt-get install -y make $CC g++ cmake git-core libopencv-dev libx11-dev libv4l-dev liblapack-dev libeigen3-dev gfortran +# - $CC --version +# - gfortran --version +# - cd .. +# - git clone --depth 1 https://github.com/lagadic/visp-images +# - export VISP_INPUT_IMAGE_PATH=$(pwd)/visp-images +# # build OpenBLAS from source, version in Bionic is 0.2.20+ds-4 (24 Jul 2017) +# - git clone --depth 1 https://github.com/xianyi/OpenBLAS.git +# - cd OpenBLAS +# - mkdir install +# - make -j$(nproc) +# - make -j$(nproc) install PREFIX=$(pwd)/install +# - export OpenBLAS_HOME=$(pwd)/install +# - cd ../src +# - mkdir build && cd build +# - cmake $CMAKE_FLAGS .. +# - make -j$(nproc) +# - ctest -j$(nproc) --output-on-failure + +# --- +# kind: pipeline +# name: arm32_gcc_cmake + +# platform: +# os: linux +# arch: arm + +# steps: +# - name: Build and Test without OpenCV (32 bits) +# image: ubuntu:18.04 +# environment: +# CC: gcc +# CMAKE_FLAGS: '-DBUILD_DEMOS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_JAVA=OFF -DBUILD_MODULE_visp_java_bindings_generator=OFF -DBUILD_TUTORIALS=OFF' +# commands: +# - lscpu +# - cat /proc/cpuinfo +# - uname -m +# - uname -r +# - echo "CMAKE_FLAGS:= $CMAKE_FLAGS" +# - export DEBIAN_FRONTEND=noninteractive +# - apt-get update -y +# # Here we don't have the link error "cannot find -lgfortran" +# - apt-get install -y make $CC g++ cmake git-core libx11-dev libv4l-dev liblapack-dev libeigen3-dev +# - $CC --version +# - cd .. +# - git clone --depth 1 https://github.com/lagadic/visp-images +# - export VISP_INPUT_IMAGE_PATH=$(pwd)/visp-images +# # build OpenBLAS from source, version in Bionic is 0.2.20+ds-4 (24 Jul 2017) +# - git clone --depth 1 https://github.com/xianyi/OpenBLAS.git +# - cd OpenBLAS +# - mkdir install +# - make -j$(nproc) +# - make -j$(nproc) install PREFIX=$(pwd)/install +# - export OpenBLAS_HOME=$(pwd)/install +# - cd ../src +# - mkdir build && cd build +# - cmake $CMAKE_FLAGS .. +# - make -j$(nproc) +# - ctest -j$(nproc) --output-on-failure \ No newline at end of file diff --git a/modules/robot/include/visp3/robot/vpRobotMavsdk.h b/modules/robot/include/visp3/robot/vpRobotMavsdk.h index de2ac95cf0..baf36f1c53 100644 --- a/modules/robot/include/visp3/robot/vpRobotMavsdk.h +++ b/modules/robot/include/visp3/robot/vpRobotMavsdk.h @@ -105,7 +105,7 @@ class VISP_EXPORT vpRobotMavsdk void getPosition(vpHomogeneousMatrix &ned_M_frd) const; std::tuple getHome() const; std::string getAddress() const; - bool isRunning() const; + bool isRunning() const; //@} //! \name Robot commands @@ -124,9 +124,11 @@ class VISP_EXPORT vpRobotMavsdk bool setLateralSpeed(double body_frd_vy); bool setGPSGlobalOrigin(double latitude, double longitude, double altitude); void setPositioningIncertitude(float position_incertitude, float yaw_incertitude); - bool setPosition(float ned_north, float ned_east, float ned_down, float ned_yaw, bool blocking = true, int timeout_sec = 10); + bool setPosition(float ned_north, float ned_east, float ned_down, float ned_yaw, bool blocking = true, + int timeout_sec = 10); bool setPosition(const vpHomogeneousMatrix &ned_M_frd, bool blocking = true, int timeout_sec = 10); - bool setPositionRelative(float ned_delta_north, float ned_delta_east, float ned_delta_down, float ned_delta_yaw, bool blocking = true, int timeout_sec = 10); + bool setPositionRelative(float ned_delta_north, float ned_delta_east, float ned_delta_down, float ned_delta_yaw, + bool blocking = true, int timeout_sec = 10); bool setPositionRelative(const vpHomogeneousMatrix &delta_frd_M_frd, bool blocking = true, int timeout_sec = 10); bool setVelocity(const vpColVector &frd_vel_cmd); bool setVerticalSpeed(double body_frd_vz); @@ -135,8 +137,8 @@ class VISP_EXPORT vpRobotMavsdk void setVerbose(bool verbose); bool stopMoving(); bool takeControl(); - bool takeOff(bool interactive = true, int timeout_sec = 10); - bool takeOff(bool interactive, double takeoff_altitude, int timeout_sec = 10); + bool takeOff(bool interactive = true, int timeout_sec = 10, bool use_gps = false); + bool takeOff(bool interactive, double takeoff_altitude, int timeout_sec = 10, bool use_gps = false); //@} private: diff --git a/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp b/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp index 438c769412..42d43793a1 100644 --- a/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp +++ b/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp @@ -57,14 +57,14 @@ using std::chrono::seconds; using std::this_thread::sleep_for; #ifndef DOXYGEN_SHOULD_SKIP_THIS -class vpRobotMavsdk::vpRobotMavsdkImpl { +class vpRobotMavsdk::vpRobotMavsdkImpl +{ public: vpRobotMavsdkImpl() : m_takeoffAlt(1.0) {} - vpRobotMavsdkImpl(const std::string &connection_info) : m_takeoffAlt(1.0) { - connect(connection_info); - } + vpRobotMavsdkImpl(const std::string &connection_info) : m_takeoffAlt(1.0) { connect(connection_info); } - virtual ~vpRobotMavsdkImpl() { + virtual ~vpRobotMavsdkImpl() + { if (m_has_flying_capability && m_auto_land) { land(); } @@ -76,33 +76,33 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { * it. \param[in] mavsdk : the Mavsdk object we will use to subscribe to the * system. \returns Returns a shared pointer to the system. */ - std::shared_ptr getSystem(mavsdk::Mavsdk &mavsdk) { + std::shared_ptr getSystem(mavsdk::Mavsdk &mavsdk) + { std::cout << "Waiting to discover system..." << std::endl; - auto prom = std::promise>{}; + auto prom = std::promise >{}; 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]() { + 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(); + auto system = mavsdk.systems().back(); - if (system->has_autopilot()) { - std::cout << "Discovered autopilot" << std::endl; + 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); + mavsdk.unsubscribe_on_new_system(handle); #else mavsdk.subscribe_on_new_system(nullptr); #endif - prom.set_value(system); - } - }); + prom.set_value(system); + } + }); // We usually receive heartbeats at 1Hz, therefore we should find a // system after around 3 seconds max, surely. @@ -115,39 +115,36 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return fut.get(); } - MAV_TYPE getVehicleType() { + MAV_TYPE getVehicleType() + { auto passthrough = mavsdk::MavlinkPassthrough{m_system}; auto prom = std::promise{}; 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) { + 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) { + 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; - } + // 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); + 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); + passthrough.unsubscribe_message(handle); #else - passthrough.subscribe_message_async(MAVLINK_MSG_ID_HEARTBEAT, - nullptr); + passthrough.subscribe_message_async(MAVLINK_MSG_ID_HEARTBEAT, nullptr); #endif - prom.set_value(static_cast(heartbeat.type)); - }); + prom.set_value(static_cast(heartbeat.type)); + }); // We usually receive heartbeats at 1Hz, therefore we should find a // system after around 3 seconds max, surely. @@ -160,24 +157,23 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return fut.get(); } - void calibrate_accelerometer(mavsdk::Calibration &calibration) { + void calibrate_accelerometer(mavsdk::Calibration &calibration) + { std::cout << "Calibrating accelerometer..." << std::endl; std::promise calibration_promise; auto calibration_future = calibration_promise.get_future(); - calibration.calibrate_accelerometer_async( - create_calibration_callback(calibration_promise)); + calibration.calibrate_accelerometer_async(create_calibration_callback(calibration_promise)); calibration_future.wait(); } - std::function - create_calibration_callback(std::promise &calibration_promise) { - return [&calibration_promise]( - const mavsdk::Calibration::Result result, - const mavsdk::Calibration::ProgressData progress_data) { + std::function + create_calibration_callback(std::promise &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; @@ -188,36 +184,34 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { std::cout << " Progress: " << progress_data.progress << std::endl; } if (progress_data.has_status_text) { - std::cout << " Instruction: " << progress_data.status_text - << std::endl; + std::cout << " Instruction: " << progress_data.status_text << std::endl; } break; default: - std::cout << "--- Calibration failed with message: " << result - << std::endl; + std::cout << "--- Calibration failed with message: " << result << std::endl; calibration_promise.set_value(); break; } }; } - void calibrate_gyro(mavsdk::Calibration &calibration) { + void calibrate_gyro(mavsdk::Calibration &calibration) + { std::cout << "Calibrating gyro..." << std::endl; std::promise calibration_promise; auto calibration_future = calibration_promise.get_future(); - calibration.calibrate_gyro_async( - create_calibration_callback(calibration_promise)); + calibration.calibrate_gyro_async(create_calibration_callback(calibration_promise)); calibration_future.wait(); } public: - void connect(const std::string &connectionInfo) { + void connect(const std::string &connectionInfo) + { m_address = connectionInfo; - mavsdk::ConnectionResult connection_result = - m_mavsdk.add_any_connection(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; @@ -227,16 +221,14 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { m_system = getSystem(m_mavsdk); if (!m_system) { - throw vpException(vpException::fatalError, "Unable to connect to: %s", - connectionInfo.c_str()); + 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::cout << (m_has_flying_capability ? "Connected to a flying vehicle" : "Connected to a non flying vehicle") << std::endl; m_action = std::make_shared(m_system); @@ -244,7 +236,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { m_offboard = std::make_shared(m_system); } - bool hasFlyingCapability(MAV_TYPE mav_type) { + bool hasFlyingCapability(MAV_TYPE mav_type) + { switch (mav_type) { case MAV_TYPE::MAV_TYPE_GROUND_ROVER: case MAV_TYPE::MAV_TYPE_SURFACE_BOAT: @@ -255,7 +248,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { } } - bool isRunning() const { + bool isRunning() const + { if (m_system == NULL) { return false; } else { @@ -263,7 +257,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { } } - std::string getAddress() const { + std::string getAddress() const + { std::string sequence; std::stringstream ss(m_address); std::string actual_address; @@ -284,22 +279,23 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { } } - float getBatteryLevel() const { + float getBatteryLevel() const + { mavsdk::Telemetry::Battery battery = m_telemetry.get()->battery(); return battery.voltage_v; } - void getPosition(vpHomogeneousMatrix &ned_M_frd) const { + 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}; + 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 { + 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; @@ -308,12 +304,14 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { ned_yaw = vpMath::rad(angles.yaw_deg); } - std::tuple getHome() const { + std::tuple 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) { + 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 @@ -342,8 +340,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { 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); + 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; @@ -353,29 +350,28 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { 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 << "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; + << " , Yaw [rad]: " << pose_estimate.angle_body.yaw_rad << " ." << std::endl; } } return true; } } - void setTakeOffAlt(double altitude) { + void setTakeOffAlt(double altitude) + { if (altitude > 0) { m_takeoffAlt = altitude; } else { - std::cerr << "ERROR : The take off altitude must be positive." - << std::endl; + std::cerr << "ERROR : The take off altitude must be positive." << std::endl; } } - void doFlatTrim() { + void doFlatTrim() + { // Instantiate plugin. auto calibration = mavsdk::Calibration(m_system); @@ -384,7 +380,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { calibrate_gyro(calibration); } - bool arm() { + bool arm() + { // Arm vehicle std::cout << "Arming...\n"; const mavsdk::Action::Result arm_result = m_action.get()->arm(); @@ -396,7 +393,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool disarm() { + bool disarm() + { // Arm vehicle std::cout << "Disarming...\n"; const mavsdk::Action::Result arm_result = m_action.get()->disarm(); @@ -408,7 +406,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool setGPSGlobalOrigin(double latitude, double longitude, double altitude) { + 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; @@ -416,8 +415,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { 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, + 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) { @@ -427,13 +425,13 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool takeControl() { + bool takeControl() + { if (m_verbose) { std::cout << "Starting offboard mode..." << std::endl; } - if (m_telemetry.get()->flight_mode() != - mavsdk::Telemetry::FlightMode::Offboard) { + if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) { const mavsdk::Offboard::VelocityBodyYawspeed stay{}; m_offboard.get()->set_velocity_body(stay); @@ -448,8 +446,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { // Wait to ensure offboard mode active in telemetry double t = vpTime::measureTimeMs(); - while (m_telemetry.get()->flight_mode() != - mavsdk::Telemetry::FlightMode::Offboard) { + 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; @@ -462,16 +459,16 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - void setPositioningIncertitude(float position_incertitude, - float yaw_incertitude) { + 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 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; + std::cerr << "Warning: Cannot takeoff this non flying vehicle" << std::endl; return true; } @@ -480,13 +477,11 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { if (!interactive) { authorize_takeoff = true; } else { - if (m_telemetry.get()->flight_mode() == - mavsdk::Telemetry::FlightMode::Offboard) { + 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") { + 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; @@ -499,8 +494,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { } if (m_telemetry.get()->in_air()) { - std::cerr << "Cannot take off as the robot is already flying." - << std::endl; + std::cerr << "Cannot take off as the robot is already flying." << std::endl; return true; } else if (authorize_takeoff) { // Arm vehicle @@ -512,8 +506,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { if (interactive) { std::string answer; - while (answer != "Y" && answer != "y" && answer != "N" && - answer != "n") { + 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") { @@ -525,8 +518,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { } // Takeoff - if (m_telemetry.get()->gps_info().fix_type == - mavsdk::Telemetry::FixType::NoGps) { + 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 @@ -560,29 +552,25 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { // 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) { + 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()->subscribe_landed_state(nullptr); + m_telemetry.get()->unsubscribe_landed_state(handle); in_air_promise.set_value(); } - std::cout << "state: " << state << std::endl; }); +#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) { + 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); @@ -597,10 +585,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { #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) { + [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(); @@ -608,18 +594,15 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { }); #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) { + [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) { + 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); @@ -630,8 +613,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { } } else { // GPS connected, we use Action::takeoff() - std::cout << "---- DEBUG: GPS detected: use action::takeoff()" - << std::endl; + 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; @@ -646,29 +628,25 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { auto in_air_promise = std::promise{}; 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) { + 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()->subscribe_landed_state(nullptr); + m_telemetry.get()->unsubscribe_landed_state(handle); in_air_promise.set_value(); } - std::cout << "state: " << state << std::endl; }); +#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) { + 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) @@ -684,10 +662,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { #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) { + [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(); @@ -695,18 +671,15 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { }); #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) { + [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) { + 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); @@ -722,38 +695,134 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool land() { + 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 Z_init = odom.position_body.z_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 + // takeoff + + auto in_air_promise = std::promise{}; + 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 || state == mavsdk::Telemetry::LandedState::OnGround) { + std::cout << "Drone is landing \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 || state == mavsdk::Telemetry::LandedState::OnGround) { + std::cout << "Drone is landing \n."; + m_telemetry.get()->subscribe_landed_state(nullptr); + in_air_promise.set_value(); + } + std::cout << "state: " << state << std::endl; + }); +#endif + + // Add check with Altitude + auto landing_finished_promise = std::promise{}; + 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, &handle](mavsdk::Telemetry::Odometry odom) { + if (odom.position_body.z_m < 0.20) { + std::cout << "Landed \n."; + + m_telemetry.get()->unsubscribe_odometry(handle_odom); + landing_finished_promise.set_value(); + } + }); +#else + m_telemetry.get()->subscribe_odometry( + [this, &landing_finished_promise, &Z_init](mavsdk::Telemetry::Odometry odom) { + if (odom.position_body.z_m < 0.20) { + std::cout << "Landing altitude reached\n."; - 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; + 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 << "Landed 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; } - // Check if vehicle is still in air - while (m_telemetry.get()->in_air()) { - std::cout << "Vehicle is landing..." << std::endl; - sleep_for(seconds(1)); + sleep_for(seconds(10)); + this->holdPosition(); + this->kill(); + return true; + } else { + 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; } - 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; + return false; } - bool setPosition(float ned_north, float ned_east, float ned_down, - float ned_yaw, bool blocking, int timeout_sec) { + 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; @@ -761,16 +830,13 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { 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; + 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_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) { if (m_verbose) { - std::cout << "Cannot set vehicle position: offboard mode not started" - << std::endl; + std::cout << "Cannot set vehicle position: offboard mode not started" << std::endl; } return false; } @@ -782,19 +848,16 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { #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) { + [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)); + 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::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(); @@ -802,27 +865,23 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { }); #else m_telemetry.get()->subscribe_odometry( - [this, &position_reached_promise, - &position_target](mavsdk::Telemetry::Odometry odom) { + [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)); + 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::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) { + if (position_reached_future.wait_for(seconds(timeout_sec)) == std::future_status::timeout) { std::cerr << "Positioning failed: position not reached.\n"; return false; } @@ -832,9 +891,9 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { 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) { + 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{}; @@ -853,48 +912,42 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { 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); + 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) { + 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; + 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; + 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); + 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) { + 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; + 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; + 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); + return setPositionRelative(M.getTranslationVector()[0], M.getTranslationVector()[1], M.getTranslationVector()[2], + XYZvec[2], blocking, timeout_sec); } - bool setVelocity(const vpColVector &frd_vel_cmd) { + 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 " @@ -902,11 +955,9 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { frd_vel_cmd.size())); } - if (m_telemetry.get()->flight_mode() != - mavsdk::Telemetry::FlightMode::Offboard) { + 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; + std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl; } return false; } @@ -920,7 +971,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool kill() { + 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; @@ -929,10 +981,10 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool holdPosition() { + bool holdPosition() + { if (m_telemetry.get()->in_air()) { - if (m_telemetry.get()->gps_info().fix_type != - mavsdk::Telemetry::FixType::NoGps) { + 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) { @@ -940,12 +992,9 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return false; } } else { - if (m_telemetry.get()->flight_mode() != - mavsdk::Telemetry::FlightMode::Offboard) { + 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; + std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl; } return false; } @@ -956,12 +1005,11 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool stopMoving() { - if (m_telemetry.get()->flight_mode() != - mavsdk::Telemetry::FlightMode::Offboard) { + 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; + std::cout << "Cannot stop moving: offboard mode not started" << std::endl; } return false; } @@ -972,7 +1020,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool releaseControl() { + bool releaseControl() + { auto offboard_result = m_offboard.get()->stop(); if (offboard_result != mavsdk::Offboard::Result::Success) { std::cerr << "Offboard stop failed: " << offboard_result << '\n'; @@ -984,12 +1033,11 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { 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) { + 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; + std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl; } return false; } @@ -1003,12 +1051,11 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool setForwardSpeed(double body_frd_vx) { - if (m_telemetry.get()->flight_mode() != - mavsdk::Telemetry::FlightMode::Offboard) { + 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; + std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl; } return false; } @@ -1023,12 +1070,11 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool setLateralSpeed(double body_frd_vy) { - if (m_telemetry.get()->flight_mode() != - mavsdk::Telemetry::FlightMode::Offboard) { + 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; + std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl; } return false; } @@ -1042,12 +1088,11 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { return true; } - bool setVerticalSpeed(double body_frd_vz) { - if (m_telemetry.get()->flight_mode() != - mavsdk::Telemetry::FlightMode::Offboard) { + 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; + std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl; } return false; } @@ -1079,16 +1124,14 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { private: //*** Attributes ***// - std::string - m_address{}; ///< Ip address of the robot to discover on the network + std::string m_address{}; ///< Ip address of the robot to discover on the network mavsdk::Mavsdk m_mavsdk{}; std::shared_ptr m_system; std::shared_ptr m_action; std::shared_ptr m_telemetry; std::shared_ptr m_offboard; - double m_takeoffAlt{ - 1.0}; ///< The altitude to aim for when calling the function takeoff + double m_takeoffAlt{1.0}; ///< The altitude to aim for when calling the function takeoff MAV_TYPE m_mav_type{}; // Vehicle type bool m_has_flying_capability{false}; @@ -1147,8 +1190,8 @@ class vpRobotMavsdk::vpRobotMavsdkImpl { * \sa setPositioningIncertitude(), setAutoLand(), takeControl(), * releaseControl() */ -vpRobotMavsdk::vpRobotMavsdk(const std::string &connection_info) - : m_impl(new vpRobotMavsdkImpl(connection_info)) { +vpRobotMavsdk::vpRobotMavsdk(const std::string &connection_info) : m_impl(new vpRobotMavsdkImpl(connection_info)) +{ m_impl->setPositioningIncertitude(0.05, vpMath::rad(5)); } @@ -1171,7 +1214,8 @@ vpRobotMavsdk::vpRobotMavsdk(const std::string &connection_info) * * \sa connect(), setPositioningIncertitude() */ -vpRobotMavsdk::vpRobotMavsdk() : m_impl(new vpRobotMavsdkImpl()) { +vpRobotMavsdk::vpRobotMavsdk() : m_impl(new vpRobotMavsdkImpl()) +{ m_impl->setPositioningIncertitude(0.05, vpMath::rad(5)); } @@ -1198,9 +1242,7 @@ vpRobotMavsdk::~vpRobotMavsdk() { delete m_impl; } * * \sa getAddress() */ -void vpRobotMavsdk::connect(const std::string &connection_info) { - m_impl->connect(connection_info); -} +void vpRobotMavsdk::connect(const std::string &connection_info) { m_impl->connect(connection_info); } /*! * Checks if the vehicle is running, ie if the vehicle is connected and ready to @@ -1231,8 +1273,8 @@ bool vpRobotMavsdk::isRunning() const { return m_impl->isRunning(); } * Internally we transform this FRD pose in a NED global reference frame as * expected by Pixhawk convention. */ -bool vpRobotMavsdk::sendMocapData(const vpHomogeneousMatrix &enu_M_flu, - int display_fps) { +bool vpRobotMavsdk::sendMocapData(const vpHomogeneousMatrix &enu_M_flu, int display_fps) +{ return m_impl->sendMocapData(enu_M_flu, display_fps); } @@ -1250,18 +1292,14 @@ std::string vpRobotMavsdk::getAddress() const { return m_impl->getAddress(); } * \warning When the vehicle battery gets below a certain threshold (around 14.8 * for a 4S battery), you should recharge it. */ -float vpRobotMavsdk::getBatteryLevel() const { - return m_impl->getBatteryLevel(); -} +float vpRobotMavsdk::getBatteryLevel() const { return m_impl->getBatteryLevel(); } /*! * Gets the current vehicle FRD position in its local NED frame. * \param[in] ned_M_frd : Homogeneous matrix describing the position and * attitude of the vehicle returned by telemetry. */ -void vpRobotMavsdk::getPosition(vpHomogeneousMatrix &ned_M_frd) const { - m_impl->getPosition(ned_M_frd); -} +void vpRobotMavsdk::getPosition(vpHomogeneousMatrix &ned_M_frd) const { m_impl->getPosition(ned_M_frd); } /*! * Gets the current vehicle FRD position in its local NED frame. @@ -1270,8 +1308,8 @@ void vpRobotMavsdk::getPosition(vpHomogeneousMatrix &ned_M_frd) const { * \param[in] ned_down : Position of the vehicle along NED down axis in [m]. * \param[in] ned_yaw : Yaw angle in [rad] of the vehicle along NED down axis. */ -void vpRobotMavsdk::getPosition(float &ned_north, float &ned_east, - float &ned_down, float &ned_yaw) const { +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); } @@ -1281,9 +1319,7 @@ void vpRobotMavsdk::getPosition(float &ned_north, float &ned_east, * \warning Only available if the GPS is initialized, for example * in simulation. */ -std::tuple vpRobotMavsdk::getHome() const { - return m_impl->getHome(); -} +std::tuple vpRobotMavsdk::getHome() const { return m_impl->getHome(); } /*! * Sends a flat trim command to the vehicle, to calibrate accelerometer and @@ -1300,9 +1336,7 @@ void vpRobotMavsdk::doFlatTrim() {} * * \sa takeOff() */ -void vpRobotMavsdk::setTakeOffAlt(double altitude) { - m_impl->setTakeOffAlt(altitude); -} +void vpRobotMavsdk::setTakeOffAlt(double altitude) { m_impl->setTakeOffAlt(altitude); } /*! * Arms the vehicle. @@ -1329,8 +1363,9 @@ bool vpRobotMavsdk::disarm() { return m_impl->disarm(); } * \warning This function is blocking. * \sa setTakeOffAlt(), land(), hasFlyingCapability() */ -bool vpRobotMavsdk::takeOff(bool interactive, int timeout_sec) { - return m_impl->takeOff(interactive, timeout_sec); +bool vpRobotMavsdk::takeOff(bool interactive, int timeout_sec, bool use_gps) +{ + return m_impl->takeOff(interactive, timeout_sec, use_gps); } /*! @@ -1346,10 +1381,10 @@ bool vpRobotMavsdk::takeOff(bool interactive, int timeout_sec) { * \warning This function is blocking. * \sa setTakeOffAlt(), land(), hasFlyingCapability() */ -bool vpRobotMavsdk::takeOff(bool interactive, double takeoff_altitude, - int timeout_sec) { +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); + return m_impl->takeOff(interactive, timeout_sec, use_gps); } /*! @@ -1394,10 +1429,10 @@ bool vpRobotMavsdk::land() { return m_impl->land(); } * \sa setPosition(const vpHomogeneousMatrix &, bool, float) * \sa setPositionRelative(float, float, float, float, bool, float) */ -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(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); } /*! @@ -1418,8 +1453,8 @@ bool vpRobotMavsdk::setPosition(float ned_north, float ned_east, float ned_down, * \sa setPosition(float, float, float, float, bool, float) * \sa setPositionRelative(const vpHomogeneousMatrix &, bool, float) */ -bool vpRobotMavsdk::setPosition(const vpHomogeneousMatrix &ned_M_frd, - bool blocking, int 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); } @@ -1439,13 +1474,10 @@ bool vpRobotMavsdk::setPosition(const vpHomogeneousMatrix &ned_M_frd, * \sa setPositionRelative(const vpHomogeneousMatrix &, bool, float) * \sa setPosition(float, float, float, float, bool, float) */ -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, +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); } @@ -1467,9 +1499,8 @@ bool vpRobotMavsdk::setPositionRelative(float ned_delta_north, * \sa setPositionRelative(float, float, float, float, bool, float) * \sa setPosition(const vpHomogeneousMatrix &, bool, float) */ -bool vpRobotMavsdk::setPositionRelative( - const vpHomogeneousMatrix &delta_frd_M_frd, bool blocking, - int 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); } @@ -1484,9 +1515,7 @@ bool vpRobotMavsdk::setPositionRelative( * vehicle cannot rotate around X and Y axes. \warning The vehicle applies this * command until given another one. */ -bool vpRobotMavsdk::setVelocity(const vpColVector &frd_vel_cmd) { - return m_impl->setVelocity(frd_vel_cmd); -} +bool vpRobotMavsdk::setVelocity(const vpColVector &frd_vel_cmd) { return m_impl->setVelocity(frd_vel_cmd); } /*! * Cuts the motors like a kill switch. Should only be used in emergency cases. @@ -1507,9 +1536,7 @@ bool vpRobotMavsdk::kill() { return m_impl->kill(); } * * \return true when success, false otherwise. */ -bool vpRobotMavsdk::setYawSpeed(double body_frd_wz) { - return m_impl->setYawSpeed(body_frd_wz); -} +bool vpRobotMavsdk::setYawSpeed(double body_frd_wz) { return m_impl->setYawSpeed(body_frd_wz); } /*! * Sets the forward speed, expressed in m/s, in the Front-Right-Down body frame. @@ -1523,9 +1550,7 @@ bool vpRobotMavsdk::setYawSpeed(double body_frd_wz) { * * \return true when success, false otherwise. */ -bool vpRobotMavsdk::setForwardSpeed(double body_frd_vx) { - return m_impl->setForwardSpeed(body_frd_vx); -} +bool vpRobotMavsdk::setForwardSpeed(double body_frd_vx) { return m_impl->setForwardSpeed(body_frd_vx); } /*! * Sets the lateral speed, expressed in m/s, in the Front-Right-Down body frame. @@ -1539,9 +1564,7 @@ bool vpRobotMavsdk::setForwardSpeed(double body_frd_vx) { * * \return true when success, false otherwise. */ -bool vpRobotMavsdk::setLateralSpeed(double body_frd_vy) { - return m_impl->setLateralSpeed(body_frd_vy); -} +bool vpRobotMavsdk::setLateralSpeed(double body_frd_vy) { return m_impl->setLateralSpeed(body_frd_vy); } /*! * Allows to set GPS global origin to initialize the Kalman filter when the @@ -1551,8 +1574,8 @@ bool vpRobotMavsdk::setLateralSpeed(double body_frd_vy) { * \param longitude : Longitude in deg (WGS84). * \param altitude : Altitude in meter. Positive for up. */ -bool vpRobotMavsdk::setGPSGlobalOrigin(double latitude, double longitude, - double altitude) { +bool vpRobotMavsdk::setGPSGlobalOrigin(double latitude, double longitude, double altitude) +{ return m_impl->setGPSGlobalOrigin(latitude, longitude, altitude); } @@ -1590,9 +1613,7 @@ bool vpRobotMavsdk::releaseControl() { return m_impl->releaseControl(); } * * \sa land() */ -void vpRobotMavsdk::setAutoLand(bool auto_land) { - m_impl->setAutoLand(auto_land); -} +void vpRobotMavsdk::setAutoLand(bool auto_land) { m_impl->setAutoLand(auto_land); } /*! * Incertitude used to decided if a position is reached when using setPosition() @@ -1602,8 +1623,8 @@ void vpRobotMavsdk::setAutoLand(bool auto_land) { * * \sa setPosition(), setPositionRelative() */ -void vpRobotMavsdk::setPositioningIncertitude(float position_incertitude, - float yaw_incertitude) { +void vpRobotMavsdk::setPositioningIncertitude(float position_incertitude, float yaw_incertitude) +{ m_impl->setPositioningIncertitude(position_incertitude, yaw_incertitude); } @@ -1620,9 +1641,7 @@ void vpRobotMavsdk::setPositioningIncertitude(float position_incertitude, * * \return true when success, false otherwise. */ -bool vpRobotMavsdk::setVerticalSpeed(double body_frd_vz) { - return m_impl->setVerticalSpeed(body_frd_vz); -} +bool vpRobotMavsdk::setVerticalSpeed(double body_frd_vz) { return m_impl->setVerticalSpeed(body_frd_vz); } /*! * Enable/disable verbose mode. @@ -1637,9 +1656,7 @@ void vpRobotMavsdk::setVerbose(bool verbose) { m_impl->setVerbose(verbose); } * flying capabilities, while all the other vehicles are considered with flying * capabilities. */ -bool vpRobotMavsdk::hasFlyingCapability() { - return m_impl->getFlyingCapability(); -} +bool vpRobotMavsdk::hasFlyingCapability() { return m_impl->getFlyingCapability(); } #elif !defined(VISP_BUILD_SHARED_LIBS) // Work around to avoid warning: libvisp_robot.a(vpRobotMavsdk.cpp.o) has no From 543b805aaf5d9a30c09ca6070a8f3a5d84c19836 Mon Sep 17 00:00:00 2001 From: Antonio Date: Tue, 22 Aug 2023 15:23:05 +0200 Subject: [PATCH 5/6] fixed bug in landing --- modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp b/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp index 2fd72869fe..924a2b5f99 100644 --- a/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp +++ b/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp @@ -757,7 +757,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl #if (VISP_HAVE_MAVSDK_VERSION > 0x010412) auto handle_odom = m_telemetry.get()->subscribe_odometry( [this, &landing_finished_promise, &handle](mavsdk::Telemetry::Odometry odom) { - if (odom.position_body.z_m < 0.20) { + if (odom.position_body.z_m < 0.30) { std::cout << "Landed \n."; m_telemetry.get()->unsubscribe_odometry(handle_odom); @@ -767,7 +767,7 @@ class vpRobotMavsdk::vpRobotMavsdkImpl #else m_telemetry.get()->subscribe_odometry( [this, &landing_finished_promise, &Z_init](mavsdk::Telemetry::Odometry odom) { - if (odom.position_body.z_m < 0.20) { + if (odom.position_body.z_m < 0.30) { std::cout << "Landing altitude reached\n."; m_telemetry.get()->subscribe_odometry(nullptr); @@ -787,8 +787,10 @@ class vpRobotMavsdk::vpRobotMavsdkImpl sleep_for(seconds(10)); this->holdPosition(); + sleep_for(seconds(1)); this->kill(); return true; + } else { if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Land) { std::cout << "Landing...\n"; From 968259e1b28d5fdf3e003b0547be24b229785dba Mon Sep 17 00:00:00 2001 From: Antonio Date: Sun, 27 Aug 2023 12:07:48 +0200 Subject: [PATCH 6/6] fixed landing --- .../src/real-robot/mavsdk/vpRobotMavsdk.cpp | 43 ++++++++----------- 1 file changed, 18 insertions(+), 25 deletions(-) diff --git a/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp b/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp index 924a2b5f99..dc8e5b943a 100644 --- a/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp +++ b/modules/robot/src/real-robot/mavsdk/vpRobotMavsdk.cpp @@ -786,36 +786,29 @@ class vpRobotMavsdk::vpRobotMavsdkImpl } sleep_for(seconds(10)); - this->holdPosition(); - sleep_for(seconds(1)); - this->kill(); - return true; - - } else { - 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)); - } + 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; } - 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; + // Check if vehicle is still in air + while (m_telemetry.get()->in_air()) { + std::cout << "Vehicle is landing..." << std::endl; + sleep_for(seconds(1)); + } } - return false; + 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)