Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpRobotMavsdk.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Interface to mavlink compatible controller using mavsdk 3rd party
33 *
34 *****************************************************************************/
35
36#include <visp3/core/vpConfig.h>
37
38#if defined(VISP_HAVE_MAVSDK) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
39
40#include <iostream>
41#include <math.h>
42#include <thread>
43
44#include <mavsdk/mavsdk.h>
45#include <mavsdk/plugins/action/action.h>
46#include <mavsdk/plugins/calibration/calibration.h>
47#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
48#include <mavsdk/plugins/mocap/mocap.h>
49#include <mavsdk/plugins/offboard/offboard.h>
50#include <mavsdk/plugins/telemetry/telemetry.h>
51
52#include <visp3/core/vpExponentialMap.h> // For velocity computation
53#include <visp3/robot/vpRobotMavsdk.h>
54
55using std::chrono::milliseconds;
56using std::chrono::seconds;
57using std::this_thread::sleep_for;
58using namespace std::chrono_literals;
59
60#ifndef DOXYGEN_SHOULD_SKIP_THIS
61class vpRobotMavsdk::vpRobotMavsdkImpl
62{
63public:
64 vpRobotMavsdkImpl() : m_takeoffAlt(1.0) {}
65 vpRobotMavsdkImpl(const std::string &connection_info) : m_takeoffAlt(1.0) { connect(connection_info); }
66
67 virtual ~vpRobotMavsdkImpl()
68 {
69 if (m_has_flying_capability && m_auto_land) {
70 land();
71 }
72 }
73
74private:
80 std::shared_ptr<mavsdk::System> getSystem(mavsdk::Mavsdk &mavsdk)
81 {
82 std::cout << "Waiting to discover system..." << std::endl;
83 auto prom = std::promise<std::shared_ptr<mavsdk::System> >{};
84 auto fut = prom.get_future();
85
86 // We wait for new systems to be discovered, once we find one that has an
87 // autopilot, we decide to use it.
88#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
89 mavsdk::Mavsdk::NewSystemHandle handle = mavsdk.subscribe_on_new_system([&mavsdk, &prom, &handle]() {
90#else
91 mavsdk.subscribe_on_new_system([&mavsdk, &prom]() {
92#endif
93 auto system = mavsdk.systems().back();
94
95 if (system->has_autopilot()) {
96 std::cout << "Discovered autopilot" << std::endl;
97
98 // Unsubscribe again as we only want to find one system.
99#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
100 mavsdk.unsubscribe_on_new_system(handle);
101#else
102 mavsdk.subscribe_on_new_system(nullptr);
103#endif
104 prom.set_value(system);
105 }
106 });
107
108 // We usually receive heartbeats at 1Hz, therefore we should find a
109 // system after around 3 seconds max, surely.
110 if (fut.wait_for(seconds(3)) == std::future_status::timeout) {
111 std::cerr << "No autopilot found." << std::endl;
112 return {};
113 }
114
115 // Get discovered system now.
116 return fut.get();
117 }
118
119 MAV_TYPE getVehicleType()
120 {
121 auto passthrough = mavsdk::MavlinkPassthrough{m_system};
122
123 auto prom = std::promise<MAV_TYPE>{};
124 auto fut = prom.get_future();
125#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
126 mavsdk::MavlinkPassthrough::MessageHandle handle = passthrough.subscribe_message(
127 MAVLINK_MSG_ID_HEARTBEAT, [&passthrough, &prom, &handle](const mavlink_message_t &message) {
128#else
129 passthrough.subscribe_message_async(MAVLINK_MSG_ID_HEARTBEAT,
130 [&passthrough, &prom](const mavlink_message_t &message) {
131#endif
132 // Process only Heartbeat coming from the autopilot
133 if (message.compid != MAV_COMP_ID_AUTOPILOT1) {
134 return;
135 }
136
137 mavlink_heartbeat_t heartbeat;
138 mavlink_msg_heartbeat_decode(&message, &heartbeat);
139
140 // Unsubscribe again as we only want to find one system.
141#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
142 passthrough.unsubscribe_message(handle);
143#else
144 passthrough.subscribe_message_async(MAVLINK_MSG_ID_HEARTBEAT, nullptr);
145#endif
146
147 prom.set_value(static_cast<MAV_TYPE>(heartbeat.type));
148 });
149
150 // We usually receive heartbeats at 1Hz, therefore we should find a
151 // system after around 3 seconds max, surely.
152 if (fut.wait_for(seconds(3)) == std::future_status::timeout) {
153 std::cerr << "No heartbeat received to get vehicle type." << std::endl;
154 return {};
155 }
156
157 // Get discovered system now.
158 return fut.get();
159 }
160
161 void calibrate_accelerometer(mavsdk::Calibration &calibration)
162 {
163 std::cout << "Calibrating accelerometer..." << std::endl;
164
165 std::promise<void> calibration_promise;
166 auto calibration_future = calibration_promise.get_future();
167
168 calibration.calibrate_accelerometer_async(create_calibration_callback(calibration_promise));
169
170 calibration_future.wait();
171 }
172
173 std::function<void(mavsdk::Calibration::Result, mavsdk::Calibration::ProgressData)>
174 create_calibration_callback(std::promise<void> &calibration_promise)
175 {
176 return [&calibration_promise](const mavsdk::Calibration::Result result,
177 const mavsdk::Calibration::ProgressData progress_data) {
178 switch (result) {
179 case mavsdk::Calibration::Result::Success:
180 std::cout << "--- Calibration succeeded!" << std::endl;
181 calibration_promise.set_value();
182 break;
183 case mavsdk::Calibration::Result::Next:
184 if (progress_data.has_progress) {
185 std::cout << " Progress: " << progress_data.progress << std::endl;
186 }
187 if (progress_data.has_status_text) {
188 std::cout << " Instruction: " << progress_data.status_text << std::endl;
189 }
190 break;
191 default:
192 std::cout << "--- Calibration failed with message: " << result << std::endl;
193 calibration_promise.set_value();
194 break;
195 }
196 };
197 }
198
199 void calibrate_gyro(mavsdk::Calibration &calibration)
200 {
201 std::cout << "Calibrating gyro..." << std::endl;
202
203 std::promise<void> calibration_promise;
204 auto calibration_future = calibration_promise.get_future();
205
206 calibration.calibrate_gyro_async(create_calibration_callback(calibration_promise));
207
208 calibration_future.wait();
209 }
210
211public:
212 void connect(const std::string &connectionInfo)
213 {
214 m_address = connectionInfo;
215 mavsdk::ConnectionResult connection_result = m_mavsdk.add_any_connection(connectionInfo);
216
217 if (connection_result != mavsdk::ConnectionResult::Success) {
218 std::cerr << "Connection failed: " << connection_result << std::endl;
219 return;
220 }
221
222 m_system = getSystem(m_mavsdk);
223
224 if (!m_system) {
225 throw vpException(vpException::fatalError, "Unable to connect to: %s", connectionInfo.c_str());
226 }
227
228 m_mav_type = getVehicleType();
229
230 m_has_flying_capability = hasFlyingCapability(m_mav_type);
231
232 std::cout << (m_has_flying_capability ? "Connected to a flying vehicle" : "Connected to a non flying vehicle")
233 << std::endl;
234
235 m_action = std::make_shared<mavsdk::Action>(m_system);
236 m_telemetry = std::make_shared<mavsdk::Telemetry>(m_system);
237 m_offboard = std::make_shared<mavsdk::Offboard>(m_system);
238 }
239
240 bool hasFlyingCapability(MAV_TYPE mav_type)
241 {
242 switch (mav_type) {
243 case MAV_TYPE::MAV_TYPE_GROUND_ROVER:
244 case MAV_TYPE::MAV_TYPE_SURFACE_BOAT:
245 case MAV_TYPE::MAV_TYPE_SUBMARINE:
246 return false;
247 default:
248 return true;
249 }
250 }
251
252 bool isRunning() const
253 {
254 if (m_system == NULL) {
255 return false;
256 } else {
257 return true;
258 }
259 }
260
261 std::string getAddress() const
262 {
263 std::string sequence;
264 std::stringstream ss(m_address);
265 std::string actual_address;
266 std::getline(ss, sequence, ':');
267 if (sequence == "serial" || sequence == "udp" || sequence == "tcp") {
268 getline(ss, sequence, ':');
269 for (const char &c : sequence) {
270 if (c != '/') {
271 actual_address.append(1, c);
272 }
273 }
274 return actual_address;
275 } else {
276 std::cout << "ERROR : The address parameter must start with \"serial:\" or \"udp:\" or \"tcp:\"." << std::endl;
277 return std::string();
278 }
279 }
280
281 float getBatteryLevel() const
282 {
283 mavsdk::Telemetry::Battery battery = m_telemetry.get()->battery();
284 return battery.voltage_v;
285 }
286
287 void getPosition(vpHomogeneousMatrix &ned_M_frd) const
288 {
289 auto quat = m_telemetry.get()->attitude_quaternion();
290 auto posvel = m_telemetry.get()->position_velocity_ned();
291 vpQuaternionVector q{quat.x, quat.y, quat.z, quat.w};
292 vpTranslationVector t{posvel.position.north_m, posvel.position.east_m, posvel.position.down_m};
293 ned_M_frd.buildFrom(t, q);
294 }
295
296 void getPosition(float &ned_north, float &ned_east, float &ned_down, float &ned_yaw) const
297 {
298 auto odom = m_telemetry.get()->odometry();
299 auto angles = m_telemetry.get()->attitude_euler();
300 ned_north = odom.position_body.x_m;
301 ned_east = odom.position_body.y_m;
302 ned_down = odom.position_body.z_m;
303 ned_yaw = vpMath::rad(angles.yaw_deg);
304 }
305
306 std::tuple<float, float> getHome() const
307 {
308 auto position = m_telemetry.get()->home();
309 return {float(position.latitude_deg), float(position.longitude_deg)};
310 }
311
312 bool sendMocapData(const vpHomogeneousMatrix &enu_M_flu, int display_fps)
313 {
314 static double time_prev = vpTime::measureTimeMs();
315
316 // We suppose here that the body frame which pose is given by the MoCap is FLU (Front Left Up).
317 // Thus we need to transform this frame to FRD (Front Right Down).
318 vpHomogeneousMatrix flu_M_frd;
319 flu_M_frd.eye();
320 flu_M_frd[1][1] = -1;
321 flu_M_frd[2][2] = -1;
322
323 vpHomogeneousMatrix enu_M_frd = enu_M_flu * flu_M_frd;
324 auto mocap = mavsdk::Mocap{m_system};
325 mavsdk::Mocap::VisionPositionEstimate pose_estimate;
326
327 vpHomogeneousMatrix ned_M_frd = vpMath::enu2ned(enu_M_frd);
328 vpRxyzVector ned_rxyz_frd = vpRxyzVector(ned_M_frd.getRotationMatrix());
329 pose_estimate.angle_body.roll_rad = ned_rxyz_frd[0];
330 pose_estimate.angle_body.pitch_rad = ned_rxyz_frd[1];
331 pose_estimate.angle_body.yaw_rad = ned_rxyz_frd[2];
332
333 vpTranslationVector ned_t_frd = ned_M_frd.getTranslationVector();
334 pose_estimate.position_body.x_m = ned_t_frd[0];
335 pose_estimate.position_body.y_m = ned_t_frd[1];
336 pose_estimate.position_body.z_m = ned_t_frd[2];
337
338 pose_estimate.pose_covariance.covariance_matrix.push_back(NAN);
339 pose_estimate.time_usec = 0; // We are using the back end timestamp
340
341 const mavsdk::Mocap::Result set_position_result = mocap.set_vision_position_estimate(pose_estimate);
342 if (set_position_result != mavsdk::Mocap::Result::Success) {
343 std::cerr << "Set position failed: " << set_position_result << '\n';
344 return false;
345 } else {
346 if (display_fps > 0) {
347 double display_time_ms = 1000. / display_fps;
348 if (vpTime::measureTimeMs() - time_prev > display_time_ms) {
349 time_prev = vpTime::measureTimeMs();
350 std::cout << "Send ned_M_frd MoCap data: " << std::endl;
351 std::cout << "Translation [m]: " << pose_estimate.position_body.x_m << " , "
352 << pose_estimate.position_body.y_m << " , " << pose_estimate.position_body.z_m << std::endl;
353 std::cout << "Roll [rad]: " << pose_estimate.angle_body.roll_rad
354 << " , Pitch [rad]: " << pose_estimate.angle_body.pitch_rad
355 << " , Yaw [rad]: " << pose_estimate.angle_body.yaw_rad << " ." << std::endl;
356 }
357 }
358 return true;
359 }
360 }
361
362 void setTakeOffAlt(double altitude)
363 {
364 if (altitude > 0) {
365 m_takeoffAlt = altitude;
366 } else {
367 std::cerr << "ERROR : The take off altitude must be positive." << std::endl;
368 }
369 }
370
371 void doFlatTrim()
372 {
373 // Instantiate plugin.
374 auto calibration = mavsdk::Calibration(m_system);
375
376 // Run calibrations
377 calibrate_accelerometer(calibration);
378 calibrate_gyro(calibration);
379 }
380
381 bool arm()
382 {
383 // Arm vehicle
384 std::cout << "Arming...\n";
385 const mavsdk::Action::Result arm_result = m_action.get()->arm();
386
387 if (arm_result != mavsdk::Action::Result::Success) {
388 std::cerr << "Arming failed: " << arm_result << std::endl;
389 return false;
390 }
391 return true;
392 }
393
394 bool disarm()
395 {
396 // Arm vehicle
397 std::cout << "Disarming...\n";
398 const mavsdk::Action::Result arm_result = m_action.get()->disarm();
399
400 if (arm_result != mavsdk::Action::Result::Success) {
401 std::cerr << "Disarming failed: " << arm_result << std::endl;
402 return false;
403 }
404 return true;
405 }
406
407 bool setGPSGlobalOrigin(double latitude, double longitude, double altitude)
408 {
409 auto passthrough = mavsdk::MavlinkPassthrough{m_system};
410 mavlink_set_gps_global_origin_t gps_global_origin;
411 gps_global_origin.latitude = latitude * 10000000;
412 gps_global_origin.longitude = longitude * 10000000;
413 gps_global_origin.altitude = altitude * 1000; // in mm
414 gps_global_origin.target_system = m_system->get_system_id();
415 mavlink_message_t msg;
416 mavlink_msg_set_gps_global_origin_encode(passthrough.get_our_sysid(), passthrough.get_our_compid(), &msg,
417 &gps_global_origin);
418 auto resp = passthrough.send_message(msg);
419 if (resp != mavsdk::MavlinkPassthrough::Result::Success) {
420 std::cerr << "Set GPS global position failed: " << resp << std::endl;
421 return false;
422 }
423 return true;
424 }
425
426 bool takeControl()
427 {
428 if (m_verbose) {
429 std::cout << "Starting offboard mode..." << std::endl;
430 }
431
432 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
433 const mavsdk::Offboard::VelocityBodyYawspeed stay{};
434 m_offboard.get()->set_velocity_body(stay);
435
436 mavsdk::Offboard::Result offboard_result = m_offboard.get()->start();
437 if (offboard_result != mavsdk::Offboard::Result::Success) {
438 std::cerr << "Offboard mode failed: " << offboard_result << std::endl;
439 return false;
440 }
441 } else if (m_verbose) {
442 std::cout << "Already in offboard mode" << std::endl;
443 }
444
445 // Wait to ensure offboard mode active in telemetry
446 double t = vpTime::measureTimeMs();
447 while (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
448 if (vpTime::measureTimeMs() - t > 3. * 1000.) {
449 std::cout << "Time out received in takeControl()" << std::endl;
450 break;
451 }
452 };
453
454 if (m_verbose) {
455 std::cout << "Offboard mode started" << std::endl;
456 }
457 return true;
458 }
459
460 void setPositioningIncertitude(float position_incertitude, float yaw_incertitude)
461 {
462 m_position_incertitude = position_incertitude;
463 m_yaw_incertitude = yaw_incertitude;
464 }
465
466 bool takeOff(bool interactive, int timeout_sec, bool use_gps)
467 {
468 if (!m_has_flying_capability) {
469 std::cerr << "Warning: Cannot takeoff this non flying vehicle" << std::endl;
470 return true;
471 }
472
473 bool authorize_takeoff = false;
474
475 if (!interactive) {
476 authorize_takeoff = true;
477 } else {
478 if (m_telemetry.get()->flight_mode() == mavsdk::Telemetry::FlightMode::Offboard) {
479 authorize_takeoff = true;
480 } else {
481 std::string answer;
482 while (answer != "Y" && answer != "y" && answer != "N" && answer != "n") {
483 std::cout << "Current flight mode is not the offboard mode. Do you "
484 "want to force offboard mode ? (y/n)"
485 << std::endl;
486 std::cin >> answer;
487 if (answer == "Y" || answer == "y") {
488 authorize_takeoff = true;
489 }
490 }
491 }
492 }
493
494 if (m_telemetry.get()->in_air()) {
495 std::cerr << "Cannot take off as the robot is already flying." << std::endl;
496 return true;
497 } else if (authorize_takeoff) {
498 // Arm vehicle
499 if (!arm()) {
500 return false;
501 }
502
503 vpTime::wait(2000);
504
505 if (interactive) {
506 std::string answer;
507 while (answer != "Y" && answer != "y" && answer != "N" && answer != "n") {
508 std::cout << "If vehicle armed ? (y/n)" << std::endl;
509 std::cin >> answer;
510 if (answer == "N" || answer == "n") {
511 disarm();
512 kill();
513 return false;
514 }
515 }
516 }
517
518 // Takeoff
519 if (m_telemetry.get()->gps_info().fix_type == mavsdk::Telemetry::FixType::NoGps || !use_gps) {
520 // No GPS connected.
521 // When using odometry from MoCap, Action::takeoff() behavior is to takeoff at 0,0,0,alt
522 // that is weird when the drone is not placed at 0,0,0.
523 // That's why here use set_position_ned() to takeoff
524
525 // Start off-board or guided mode
526 takeControl();
527
528 auto in_air_promise = std::promise<void>{};
529 auto in_air_future = in_air_promise.get_future();
530
531 mavsdk::Telemetry::Odometry odom = m_telemetry.get()->odometry();
532 vpQuaternionVector q{odom.q.x, odom.q.y, odom.q.z, odom.q.w};
533 vpRotationMatrix R(q);
534 vpRxyzVector rxyz(R);
535
536 double X_init = odom.position_body.x_m;
537 double Y_init = odom.position_body.y_m;
538 double Z_init = odom.position_body.z_m;
539 double yaw_init = vpMath::deg(rxyz[2]);
540
541 std::cout << "Takeoff using position NED." << std::endl;
542
543 mavsdk::Offboard::PositionNedYaw takeoff{};
544 takeoff.north_m = X_init;
545 takeoff.east_m = Y_init;
546 takeoff.down_m = Z_init - m_takeoffAlt;
547 takeoff.yaw_deg = yaw_init;
548 m_offboard.get()->set_position_ned(takeoff);
549 // Possibility is to use set_position_velocity_ned(); to speed up takeoff
550
551#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
552 Telemetry::LandedStateHandle handle = m_telemetry.get()->subscribe_landed_state(
553 [this, &in_air_promise, &handle](mavsdk::Telemetry::LandedState state) {
554 if (state == mavsdk::Telemetry::LandedState::InAir) {
555 std::cout << "Drone is taking off\n.";
556 m_telemetry.get()->unsubscribe_landed_state(handle);
557 in_air_promise.set_value();
558 }
559 });
560#else
561 m_telemetry.get()->subscribe_landed_state([this, &in_air_promise](mavsdk::Telemetry::LandedState state) {
562 if (state == mavsdk::Telemetry::LandedState::InAir) {
563 std::cout << "Drone is taking off\n.";
564 m_telemetry.get()->subscribe_landed_state(nullptr);
565 in_air_promise.set_value();
566 }
567 std::cout << "state: " << state << std::endl;
568 });
569#endif
570 if (in_air_future.wait_for(seconds(timeout_sec)) == std::future_status::timeout) {
571 std::cerr << "Takeoff failed: drone not in air.\n";
572#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
573 m_telemetry.get()->unsubscribe_landed_state(handle);
574#else
575 m_telemetry.get()->subscribe_landed_state(nullptr);
576#endif
577 return false;
578 }
579 // Add check with Altitude
580 auto takeoff_finished_promise = std::promise<void>{};
581 auto takeoff_finished_future = takeoff_finished_promise.get_future();
582
583#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
584 auto handle_odom = m_telemetry.get()->subscribe_odometry(
585 [this, &takeoff_finished_promise, &handle, &Z_init](mavsdk::Telemetry::Odometry odom) {
586 if (odom.position_body.z_m < 0.90 * (Z_init - m_takeoffAlt) + m_position_incertitude) {
587 std::cout << "Takeoff altitude reached\n.";
588 m_telemetry.get()->unsubscribe_odometry(handle_odom);
589 takeoff_finished_promise.set_value();
590 }
591 });
592#else
593 m_telemetry.get()->subscribe_odometry(
594 [this, &takeoff_finished_promise, &Z_init](mavsdk::Telemetry::Odometry odom) {
595 if (odom.position_body.z_m < 0.90 * (Z_init - m_takeoffAlt) + m_position_incertitude) {
596 std::cout << "Takeoff altitude reached\n.";
597 m_telemetry.get()->subscribe_odometry(nullptr);
598 takeoff_finished_promise.set_value();
599 }
600 });
601#endif
602 if (takeoff_finished_future.wait_for(seconds(timeout_sec)) == std::future_status::timeout) {
603 std::cerr << "Takeoff failed: altitude not reached.\n";
604#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
605 m_telemetry.get()->unsubscribe_odometry(handle);
606#else
607 m_telemetry.get()->subscribe_odometry(nullptr);
608#endif
609 return false;
610 }
611 } else {
612 // GPS connected, we use Action::takeoff()
613 std::cout << "---- DEBUG: GPS detected: use action::takeoff()" << std::endl;
614
615 mavsdk::Telemetry::Odometry odom = m_telemetry.get()->odometry();
616 double Z_init = odom.position_body.z_m;
617
618 m_action.get()->set_takeoff_altitude(m_takeoffAlt);
619 const auto takeoff_result = m_action.get()->takeoff();
620 if (takeoff_result != mavsdk::Action::Result::Success) {
621 std::cerr << "Takeoff failed: " << takeoff_result << '\n';
622 return false;
623 }
624
625 auto in_air_promise = std::promise<void>{};
626 auto in_air_future = in_air_promise.get_future();
627#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
628 Telemetry::LandedStateHandle handle = m_telemetry.get()->subscribe_landed_state(
629 [this, &in_air_promise, &handle](mavsdk::Telemetry::LandedState state) {
630 if (state == mavsdk::Telemetry::LandedState::InAir) {
631 std::cout << "Taking off has finished\n.";
632 m_telemetry.get()->unsubscribe_landed_state(handle);
633 in_air_promise.set_value();
634 }
635 });
636#else
637 m_telemetry.get()->subscribe_landed_state([this, &in_air_promise](mavsdk::Telemetry::LandedState state) {
638 if (state == mavsdk::Telemetry::LandedState::InAir) {
639 std::cout << "Taking off has finished\n.";
640 m_telemetry.get()->subscribe_landed_state(nullptr);
641 in_air_promise.set_value();
642 }
643 std::cout << "state: " << state << std::endl;
644 });
645#endif
646 if (in_air_future.wait_for(seconds(timeout_sec)) == std::future_status::timeout) {
647 // Add check with Altitude
648 std::cerr << "Takeoff timed out.\n";
649#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
650 m_telemetry.get()->unsubscribe_landed_state(handle);
651#else
652 m_telemetry.get()->subscribe_landed_state(nullptr);
653#endif
654 }
655 // Add check with Altitude
656 auto takeoff_finished_promise = std::promise<void>{};
657 auto takeoff_finished_future = takeoff_finished_promise.get_future();
658
659#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
660 auto handle_odom = m_telemetry.get()->subscribe_odometry(
661 [this, &takeoff_finished_promise, &handle, &Z_init](mavsdk::Telemetry::Odometry odom) {
662 if (odom.position_body.z_m < 0.90 * (Z_init - m_takeoffAlt) + m_position_incertitude) {
663 std::cout << "Takeoff altitude reached\n.";
664 m_telemetry.get()->unsubscribe_odometry(handle_odom);
665 takeoff_finished_promise.set_value();
666 }
667 });
668#else
669 m_telemetry.get()->subscribe_odometry(
670 [this, &takeoff_finished_promise, &Z_init](mavsdk::Telemetry::Odometry odom) {
671 if (odom.position_body.z_m < 0.90 * (Z_init - m_takeoffAlt) + m_position_incertitude) {
672 std::cout << "Takeoff altitude reached\n.";
673 m_telemetry.get()->subscribe_odometry(nullptr);
674 takeoff_finished_promise.set_value();
675 }
676 });
677#endif
678 if (takeoff_finished_future.wait_for(seconds(timeout_sec)) == std::future_status::timeout) {
679 std::cerr << "Takeoff failed: altitude not reached.\n";
680#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
681 m_telemetry.get()->unsubscribe_odometry(handle);
682#else
683 m_telemetry.get()->subscribe_odometry(nullptr);
684#endif
685 return false;
686 }
687 // Start off-board or guided mode
688 takeControl();
689 }
690 }
691 return true;
692 }
693
694 bool land(bool use_buildin = false)
695 {
696 if (!m_has_flying_capability) {
697 std::cerr << "Warning: Cannot land this non flying vehicle" << std::endl;
698 return true;
699 }
700 // Takeoff
701 if (!use_buildin) {
702 // No GPS connected.
703 // When using odometry from MoCap, Action::takeoff() behavior is to
704 // takeoff at 0,0,0,alt that is weird when the drone is not placed at
705 // 0,0,0. That's why here use set_position_ned() to takeoff
706
707 // Start off-board or guided mode
708 takeControl();
709
710 mavsdk::Telemetry::Odometry odom = m_telemetry.get()->odometry();
711 vpQuaternionVector q{odom.q.x, odom.q.y, odom.q.z, odom.q.w};
712 vpRotationMatrix R(q);
713 vpRxyzVector rxyz(R);
714
715 double X_init = odom.position_body.x_m;
716 double Y_init = odom.position_body.y_m;
717 double Z_init = odom.position_body.z_m;
718 double yaw_init = vpMath::deg(rxyz[2]);
719
720 std::cout << "Landing using position NED." << std::endl;
721
722 mavsdk::Offboard::PositionNedYaw landing{};
723 landing.north_m = X_init;
724 landing.east_m = Y_init;
725 landing.down_m = 0.;
726 landing.yaw_deg = yaw_init;
727 m_offboard.get()->set_position_ned(landing);
728 // Possibility is to use set_position_velocity_ned(); to speed up
729 bool success = false;
730
731 // Add check with Altitude
732 auto landing_finished_promise = std::promise<void>{};
733 auto landing_finished_future = landing_finished_promise.get_future();
734
735#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
736 auto handle_odom = m_telemetry.get()->subscribe_odometry(
737 [this, &landing_finished_promise, &success, &handle](mavsdk::Telemetry::Odometry odom) {
738 if (odom.position_body.z_m > -0.15) {
739 std::cout << "Landing altitude reached \n.";
740
741 success = true;
742 m_telemetry.get()->unsubscribe_odometry(handle_odom);
743 landing_finished_promise.set_value();
744 }
745 });
746#else
747 m_telemetry.get()->subscribe_odometry(
748 [this, &landing_finished_promise, &success](mavsdk::Telemetry::Odometry odom) {
749 if (odom.position_body.z_m > -0.15) {
750 std::cout << "Landing altitude reached\n.";
751
752 success = true;
753 m_telemetry.get()->subscribe_odometry(nullptr);
754 landing_finished_promise.set_value();
755 }
756 });
757#endif
758 if (landing_finished_future.wait_for(seconds(10)) == std::future_status::timeout) {
759 std::cerr << "failed: altitude not reached.\n";
760 success = true; // go to automatic landing
761 }
762
763 while (!success) {
764 std::cout << "Descending\n.";
765 sleep_for(100ms);
766 }
767 }
768
769 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Land) {
770 std::cout << "Landing...\n";
771 const mavsdk::Action::Result land_result = m_action.get()->land();
772 if (land_result != mavsdk::Action::Result::Success) {
773 std::cerr << "Land failed: " << land_result << std::endl;
774 return false;
775 }
776
777 // Check if vehicle is still in air
778 while (m_telemetry.get()->in_air()) {
779 std::cout << "Vehicle is landing..." << std::endl;
780 sleep_for(seconds(1));
781 }
782 }
783
784 std::cout << "Landed!" << std::endl;
785 // We are relying on auto-disarming but let's keep watching the telemetry
786 // for a bit longer.
787 sleep_for(seconds(5));
788 std::cout << "Finished..." << std::endl;
789 return true;
790 }
791
792 bool setPosition(float ned_north, float ned_east, float ned_down, float ned_yaw, bool blocking, int timeout_sec)
793 {
794 mavsdk::Offboard::PositionNedYaw position_target{};
795
796 position_target.north_m = ned_north;
797 position_target.east_m = ned_east;
798 position_target.down_m = ned_down;
799 position_target.yaw_deg = vpMath::deg(ned_yaw);
800
801 std::cout << "NED Pos to reach: " << position_target.north_m << " " << position_target.east_m << " "
802 << position_target.down_m << " " << position_target.yaw_deg << std::endl;
803 m_offboard.get()->set_position_ned(position_target);
804
805 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
806 if (m_verbose) {
807 std::cout << "Cannot set vehicle position: offboard mode not started" << std::endl;
808 }
809 return false;
810 }
811
812 if (blocking) {
813 // Add check with Altitude
814 auto position_reached_promise = std::promise<void>{};
815 auto position_reached_future = position_reached_promise.get_future();
816
817#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
818 auto handle_odom = m_telemetry.get()->subscribe_odometry(
819 [this, &position_reached_promise, &handle, &position_target](mavsdk::Telemetry::Odometry odom) {
820 vpQuaternionVector q{odom.q.x, odom.q.y, odom.q.z, odom.q.w};
821 vpRotationMatrix R(q);
822 vpRxyzVector rxyz(R);
823 double odom_yaw = vpMath::deg(rxyz[2]);
824 double distance_to_target = std::sqrt(vpMath::sqr(odom.position_body.x_m - position_target.north_m) +
825 vpMath::sqr(odom.position_body.y_m - position_target.east_m) +
826 vpMath::sqr(odom.position_body.z_m - position_target.down_m));
827 if (distance_to_target < m_position_incertitude &&
828 std::fabs(odom_yaw - position_target.yaw_deg) < m_yaw_incertitude) {
829 std::cout << "Position reached\n.";
830 m_telemetry.get()->unsubscribe_odometry(handle_odom);
831 position_reached_promise.set_value();
832 }
833 });
834#else
835 m_telemetry.get()->subscribe_odometry(
836 [this, &position_reached_promise, &position_target](mavsdk::Telemetry::Odometry odom) {
837 vpQuaternionVector q{odom.q.x, odom.q.y, odom.q.z, odom.q.w};
838 vpRotationMatrix R(q);
839 vpRxyzVector rxyz(R);
840 double odom_yaw = vpMath::deg(rxyz[2]);
841 double distance_to_target = std::sqrt(vpMath::sqr(odom.position_body.x_m - position_target.north_m) +
842 vpMath::sqr(odom.position_body.y_m - position_target.east_m) +
843 vpMath::sqr(odom.position_body.z_m - position_target.down_m));
844 if (distance_to_target < m_position_incertitude &&
845 std::fabs(odom_yaw - position_target.yaw_deg) < m_yaw_incertitude) {
846 std::cout << "Position reached\n.";
847 m_telemetry.get()->subscribe_odometry(nullptr);
848 position_reached_promise.set_value();
849 }
850 });
851#endif
852 if (position_reached_future.wait_for(seconds(timeout_sec)) == std::future_status::timeout) {
853 std::cerr << "Positioning failed: position not reached.\n";
854 return false;
855 }
856 }
857
858 std::cout << "---- DEBUG timeout: " << timeout_sec << std::endl;
859 return true;
860 }
861
862 bool setPositionRelative(float ned_delta_north, float ned_delta_east, float ned_delta_down, float ned_delta_yaw,
863 bool blocking, int timeout_sec)
864 {
865 mavsdk::Telemetry::Odometry odom;
866 mavsdk::Telemetry::EulerAngle angles;
867 mavsdk::Offboard::PositionNedYaw position_target{};
868
869 position_target.north_m = ned_delta_north;
870 position_target.east_m = ned_delta_east;
871 position_target.down_m = ned_delta_down;
872 position_target.yaw_deg = vpMath::deg(ned_delta_yaw);
873
874 // Set a relative position
875 odom = m_telemetry.get()->odometry();
876 angles = m_telemetry.get()->attitude_euler();
877
878 position_target.north_m += odom.position_body.x_m;
879 position_target.east_m += odom.position_body.y_m;
880 position_target.down_m += odom.position_body.z_m;
881 position_target.yaw_deg += angles.yaw_deg;
882
883 return setPosition(position_target.north_m, position_target.east_m, position_target.down_m,
884 vpMath::rad(position_target.yaw_deg), blocking, timeout_sec);
885 }
886
887 bool setPosition(const vpHomogeneousMatrix &M, bool absolute, int timeout_sec)
888 {
889 auto XYZvec = vpRxyzVector(M.getRotationMatrix());
890 if (XYZvec[0] != 0.0) {
891 std::cerr << "ERROR : Can't move, rotation around X axis should be 0." << std::endl;
892 return false;
893 }
894 if (XYZvec[1] != 0.0) {
895 std::cerr << "ERROR : Can't move, rotation around Y axis should be 0." << std::endl;
896 return false;
897 }
898 return setPosition(M.getTranslationVector()[0], M.getTranslationVector()[1], M.getTranslationVector()[2], XYZvec[2],
899 absolute, timeout_sec);
900 }
901
902 bool setPositionRelative(const vpHomogeneousMatrix &M, bool blocking, int timeout_sec)
903 {
904 auto XYZvec = vpRxyzVector(M.getRotationMatrix());
905 if (XYZvec[0] != 0.0) {
906 std::cerr << "ERROR : Can't move, rotation around X axis should be 0." << std::endl;
907 return false;
908 }
909 if (XYZvec[1] != 0.0) {
910 std::cerr << "ERROR : Can't move, rotation around Y axis should be 0." << std::endl;
911 return false;
912 }
913 return setPositionRelative(M.getTranslationVector()[0], M.getTranslationVector()[1], M.getTranslationVector()[2],
914 XYZvec[2], blocking, timeout_sec);
915 }
916
917 bool setVelocity(const vpColVector &frd_vel_cmd)
918 {
919 if (frd_vel_cmd.size() != 4) {
921 "ERROR : Can't set velocity, dimension of the velocity vector %d should be equal to 4.",
922 frd_vel_cmd.size()));
923 }
924
925 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
926 if (m_verbose) {
927 std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl;
928 }
929 return false;
930 }
931 mavsdk::Offboard::VelocityBodyYawspeed velocity_comm{};
932 velocity_comm.forward_m_s = frd_vel_cmd[0];
933 velocity_comm.right_m_s = frd_vel_cmd[1];
934 velocity_comm.down_m_s = frd_vel_cmd[2];
935 velocity_comm.yawspeed_deg_s = vpMath::deg(frd_vel_cmd[3]);
936 m_offboard.get()->set_velocity_body(velocity_comm);
937
938 return true;
939 }
940
941 bool kill()
942 {
943 const mavsdk::Action::Result kill_result = m_action.get()->kill();
944 if (kill_result != mavsdk::Action::Result::Success) {
945 std::cerr << "Kill failed: " << kill_result << std::endl;
946 return false;
947 }
948 return true;
949 }
950
951 bool holdPosition()
952 {
953 if (m_telemetry.get()->in_air()) {
954 if (m_telemetry.get()->gps_info().fix_type != mavsdk::Telemetry::FixType::NoGps) {
955 // Action::hold() doesn't work with PX4 when in offboard mode
956 const mavsdk::Action::Result hold_result = m_action.get()->hold();
957 if (hold_result != mavsdk::Action::Result::Success) {
958 std::cerr << "Hold failed: " << hold_result << std::endl;
959 return false;
960 }
961 } else {
962 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
963 if (m_verbose) {
964 std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl;
965 }
966 return false;
967 }
968
969 setPositionRelative(0., 0., 0., 0., false, 10.); // timeout not used
970 }
971 }
972 return true;
973 }
974
975 bool stopMoving()
976 {
977 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
978 if (m_verbose) {
979 std::cout << "Cannot stop moving: offboard mode not started" << std::endl;
980 }
981 return false;
982 }
983
984 const mavsdk::Offboard::VelocityBodyYawspeed stay{};
985 m_offboard.get()->set_velocity_body(stay);
986
987 return true;
988 }
989
990 bool releaseControl()
991 {
992 auto offboard_result = m_offboard.get()->stop();
993 if (offboard_result != mavsdk::Offboard::Result::Success) {
994 std::cerr << "Offboard stop failed: " << offboard_result << '\n';
995 return false;
996 }
997 std::cout << "Offboard stopped\n";
998 return true;
999 }
1000
1001 void setAutoLand(bool auto_land) { m_auto_land = auto_land; }
1002
1003 bool setYawSpeed(double body_frd_wz)
1004 {
1005 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
1006 if (m_verbose) {
1007 std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl;
1008 }
1009 return false;
1010 }
1011 mavsdk::Offboard::VelocityBodyYawspeed velocity_comm{};
1012 velocity_comm.forward_m_s = 0.0;
1013 velocity_comm.right_m_s = 0.0;
1014 velocity_comm.down_m_s = 0.0;
1015 velocity_comm.yawspeed_deg_s = vpMath::deg(body_frd_wz);
1016 m_offboard.get()->set_velocity_body(velocity_comm);
1017
1018 return true;
1019 }
1020
1021 bool setForwardSpeed(double body_frd_vx)
1022 {
1023 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
1024 if (m_verbose) {
1025 std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl;
1026 }
1027 return false;
1028 }
1029
1030 mavsdk::Offboard::VelocityBodyYawspeed velocity_comm{};
1031 velocity_comm.forward_m_s = body_frd_vx;
1032 velocity_comm.right_m_s = 0.0;
1033 velocity_comm.down_m_s = 0.0;
1034 velocity_comm.yawspeed_deg_s = 0.0;
1035 m_offboard.get()->set_velocity_body(velocity_comm);
1036
1037 return true;
1038 }
1039
1040 bool setLateralSpeed(double body_frd_vy)
1041 {
1042 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
1043 if (m_verbose) {
1044 std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl;
1045 }
1046 return false;
1047 }
1048 mavsdk::Offboard::VelocityBodyYawspeed velocity_comm{};
1049 velocity_comm.forward_m_s = 0.0;
1050 velocity_comm.right_m_s = body_frd_vy;
1051 velocity_comm.down_m_s = 0.0;
1052 velocity_comm.yawspeed_deg_s = 0.0;
1053 m_offboard.get()->set_velocity_body(velocity_comm);
1054
1055 return true;
1056 }
1057
1058 bool setVerticalSpeed(double body_frd_vz)
1059 {
1060 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
1061 if (m_verbose) {
1062 std::cout << "Cannot set vehicle velocity: offboard mode not started" << std::endl;
1063 }
1064 return false;
1065 }
1066 mavsdk::Offboard::VelocityBodyYawspeed velocity_comm{};
1067 velocity_comm.forward_m_s = 0.0;
1068 velocity_comm.right_m_s = 0.0;
1069 velocity_comm.down_m_s = body_frd_vz;
1070 velocity_comm.yawspeed_deg_s = 0.0;
1071 m_offboard.get()->set_velocity_body(velocity_comm);
1072
1073 return true;
1074 }
1075
1076 bool getFlyingCapability() { return m_has_flying_capability; }
1077
1078 void setVerbose(bool verbose) { m_verbose = verbose; }
1079
1080 // void waitSystemReady()
1081 // {
1082 // if (! m_system_ready)
1083 // {
1084 // while (!m_telemetry.get()->health_all_ok()) {
1085 // std::cout << "Waiting for system to be ready\n";
1086 // sleep_for(seconds(1));
1087 // }
1088 // std::cout << "System is ready\n";
1089 // }
1090 // }
1091
1092private:
1093 //*** Attributes ***//
1094 std::string m_address{};
1095 mavsdk::Mavsdk m_mavsdk{};
1096 std::shared_ptr<mavsdk::System> m_system;
1097 std::shared_ptr<mavsdk::Action> m_action;
1098 std::shared_ptr<mavsdk::Telemetry> m_telemetry;
1099 std::shared_ptr<mavsdk::Offboard> m_offboard;
1100
1101 double m_takeoffAlt{1.0};
1102
1103 MAV_TYPE m_mav_type{}; // Vehicle type
1104 bool m_has_flying_capability{false};
1105
1106 bool m_system_ready{false};
1107 float m_position_incertitude{0.05};
1108 float m_yaw_incertitude{0.09}; // 5 deg
1109 bool m_verbose{false};
1110 bool m_auto_land{true};
1111};
1112#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
1113
1150vpRobotMavsdk::vpRobotMavsdk(const std::string &connection_info) : m_impl(new vpRobotMavsdkImpl(connection_info))
1151{
1152 m_impl->setPositioningIncertitude(0.05, vpMath::rad(5));
1153}
1154
1169vpRobotMavsdk::vpRobotMavsdk() : m_impl(new vpRobotMavsdkImpl())
1170{
1171 m_impl->setPositioningIncertitude(0.05, vpMath::rad(5));
1172}
1173
1182
1195void vpRobotMavsdk::connect(const std::string &connection_info) { m_impl->connect(connection_info); }
1196
1200bool vpRobotMavsdk::isRunning() const { return m_impl->isRunning(); }
1201
1222bool vpRobotMavsdk::sendMocapData(const vpHomogeneousMatrix &enu_M_flu, int display_fps)
1223{
1224 return m_impl->sendMocapData(enu_M_flu, display_fps);
1225}
1226
1233std::string vpRobotMavsdk::getAddress() const { return m_impl->getAddress(); }
1234
1240float vpRobotMavsdk::getBatteryLevel() const { return m_impl->getBatteryLevel(); }
1241
1246void vpRobotMavsdk::getPosition(vpHomogeneousMatrix &ned_M_frd) const { m_impl->getPosition(ned_M_frd); }
1247
1255void vpRobotMavsdk::getPosition(float &ned_north, float &ned_east, float &ned_down, float &ned_yaw) const
1256{
1257 m_impl->getPosition(ned_north, ned_east, ned_down, ned_yaw);
1258}
1259
1266std::tuple<float, float> vpRobotMavsdk::getHome() const { return m_impl->getHome(); }
1267
1274
1282void vpRobotMavsdk::setTakeOffAlt(double altitude) { m_impl->setTakeOffAlt(altitude); }
1283
1288bool vpRobotMavsdk::arm() { return m_impl->arm(); }
1289
1294bool vpRobotMavsdk::disarm() { return m_impl->disarm(); }
1295
1308bool vpRobotMavsdk::takeOff(bool interactive, int timeout_sec, bool use_gps)
1309{
1310 return m_impl->takeOff(interactive, timeout_sec, use_gps);
1311}
1312
1326bool vpRobotMavsdk::takeOff(bool interactive, double takeoff_altitude, int timeout_sec, bool use_gps)
1327{
1328 m_impl->setTakeOffAlt(takeoff_altitude);
1329 return m_impl->takeOff(interactive, timeout_sec, use_gps);
1330}
1331
1339bool vpRobotMavsdk::holdPosition() { return m_impl->holdPosition(); };
1340
1346bool vpRobotMavsdk::stopMoving() { return m_impl->stopMoving(); };
1347
1355bool vpRobotMavsdk::land() { return m_impl->land(); }
1356
1371bool vpRobotMavsdk::setPosition(float ned_north, float ned_east, float ned_down, float ned_yaw, bool blocking,
1372 int timeout_sec)
1373{
1374 return m_impl->setPosition(ned_north, ned_east, ned_down, ned_yaw, blocking, timeout_sec);
1375}
1376
1393bool vpRobotMavsdk::setPosition(const vpHomogeneousMatrix &ned_M_frd, bool blocking, int timeout_sec)
1394{
1395 return m_impl->setPosition(ned_M_frd, blocking, timeout_sec);
1396}
1397
1412bool vpRobotMavsdk::setPositionRelative(float ned_delta_north, float ned_delta_east, float ned_delta_down,
1413 float ned_delta_yaw, bool blocking, int timeout_sec)
1414{
1415 return m_impl->setPositionRelative(ned_delta_north, ned_delta_east, ned_delta_down, ned_delta_yaw, blocking,
1416 timeout_sec);
1417}
1418
1434bool vpRobotMavsdk::setPositionRelative(const vpHomogeneousMatrix &delta_frd_M_frd, bool blocking, int timeout_sec)
1435{
1436 return m_impl->setPositionRelative(delta_frd_M_frd, blocking, timeout_sec);
1437}
1438
1448bool vpRobotMavsdk::setVelocity(const vpColVector &frd_vel_cmd) { return m_impl->setVelocity(frd_vel_cmd); }
1449
1455bool vpRobotMavsdk::kill() { return m_impl->kill(); }
1456
1468bool vpRobotMavsdk::setYawSpeed(double body_frd_wz) { return m_impl->setYawSpeed(body_frd_wz); }
1469
1481bool vpRobotMavsdk::setForwardSpeed(double body_frd_vx) { return m_impl->setForwardSpeed(body_frd_vx); }
1482
1494bool vpRobotMavsdk::setLateralSpeed(double body_frd_vy) { return m_impl->setLateralSpeed(body_frd_vy); }
1495
1504bool vpRobotMavsdk::setGPSGlobalOrigin(double latitude, double longitude, double altitude)
1505{
1506 return m_impl->setGPSGlobalOrigin(latitude, longitude, altitude);
1507}
1508
1520bool vpRobotMavsdk::takeControl() { return m_impl->takeControl(); }
1521
1531bool vpRobotMavsdk::releaseControl() { return m_impl->releaseControl(); }
1532
1540void vpRobotMavsdk::setAutoLand(bool auto_land) { m_impl->setAutoLand(auto_land); }
1541
1549void vpRobotMavsdk::setPositioningIncertitude(float position_incertitude, float yaw_incertitude)
1550{
1551 m_impl->setPositioningIncertitude(position_incertitude, yaw_incertitude);
1552}
1553
1565bool vpRobotMavsdk::setVerticalSpeed(double body_frd_vz) { return m_impl->setVerticalSpeed(body_frd_vz); }
1566
1572void vpRobotMavsdk::setVerbose(bool verbose) { m_impl->setVerbose(verbose); }
1573
1579bool vpRobotMavsdk::hasFlyingCapability() { return m_impl->getFlyingCapability(); }
1580
1581#elif !defined(VISP_BUILD_SHARED_LIBS)
1582// Work around to avoid warning: libvisp_robot.a(vpRobotMavsdk.cpp.o) has no
1583// symbols
1584void dummy_vpRobotMavsdk(){};
1585#endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ dimensionError
Bad dimension.
Definition vpException.h:83
@ fatalError
Fatal error.
Definition vpException.h:84
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
vpTranslationVector getTranslationVector() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double rad(double deg)
Definition vpMath.h:116
static double sqr(double x)
Definition vpMath.h:124
static vpHomogeneousMatrix enu2ned(const vpHomogeneousMatrix &enu_M)
Definition vpMath.cpp:736
static double deg(double rad)
Definition vpMath.h:106
Implementation of a rotation vector as quaternion angle minimal representation.
const double & x() const
Returns the x-component of the quaternion.
float getBatteryLevel() const
std::string getAddress() const
void setPositioningIncertitude(float position_incertitude, float yaw_incertitude)
std::tuple< float, float > getHome() const
bool sendMocapData(const vpHomogeneousMatrix &enu_M_flu, int display_fps=1)
bool takeOff(bool interactive=true, int timeout_sec=10, bool use_gps=false)
void getPosition(float &ned_north, float &ned_east, float &ned_down, float &ned_yaw) const
bool setLateralSpeed(double body_frd_vy)
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 setVerticalSpeed(double body_frd_vz)
virtual ~vpRobotMavsdk()
void setAutoLand(bool auto_land)
bool setForwardSpeed(double body_frd_vx)
void setTakeOffAlt(double altitude)
void setVerbose(bool verbose)
bool setVelocity(const vpColVector &frd_vel_cmd)
void connect(const std::string &connection_info)
bool setGPSGlobalOrigin(double latitude, double longitude, double altitude)
bool setPosition(float ned_north, float ned_east, float ned_down, float ned_yaw, bool blocking=true, int timeout_sec=10)
bool setYawSpeed(double body_frd_wz)
bool isRunning() const
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Class that consider the case of a translation vector.
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()