36 #include <visp3/core/vpConfig.h>
39 #if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) \
40 && defined(VISP_HAVE_THREADS)
46 #include <mavsdk/mavsdk.h>
47 #include <mavsdk/plugins/action/action.h>
48 #include <mavsdk/plugins/calibration/calibration.h>
49 #include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
50 #include <mavsdk/plugins/mocap/mocap.h>
51 #include <mavsdk/plugins/offboard/offboard.h>
52 #include <mavsdk/plugins/telemetry/telemetry.h>
54 #include <visp3/core/vpExponentialMap.h>
55 #include <visp3/robot/vpRobotMavsdk.h>
57 using std::chrono::milliseconds;
58 using std::chrono::seconds;
59 using std::this_thread::sleep_for;
60 using namespace std::chrono_literals;
62 #ifndef DOXYGEN_SHOULD_SKIP_THIS
63 class vpRobotMavsdk::vpRobotMavsdkImpl
66 vpRobotMavsdkImpl() : m_takeoffAlt(1.0) { }
67 vpRobotMavsdkImpl(
const std::string &connection_info) : m_takeoffAlt(1.0) { connect(connection_info); }
69 virtual ~vpRobotMavsdkImpl()
71 if (m_has_flying_capability && m_auto_land) {
82 std::shared_ptr<mavsdk::System> getSystem(mavsdk::Mavsdk &mavsdk)
84 std::cout <<
"Waiting to discover system..." << std::endl;
85 auto prom = std::promise<std::shared_ptr<mavsdk::System> > {};
86 auto fut = prom.get_future();
90 #if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
91 mavsdk::Mavsdk::NewSystemHandle handle = mavsdk.subscribe_on_new_system([&mavsdk, &prom, &handle]() {
93 mavsdk.subscribe_on_new_system([&mavsdk, &prom]() {
95 auto system = mavsdk.systems().back();
97 if (system->has_autopilot()) {
98 std::cout <<
"Discovered autopilot" << std::endl;
101 #if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
102 mavsdk.unsubscribe_on_new_system(handle);
104 mavsdk.subscribe_on_new_system(nullptr);
106 prom.set_value(system);
112 if (fut.wait_for(seconds(3)) == std::future_status::timeout) {
113 std::cerr <<
"No autopilot found." << std::endl;
121 MAV_TYPE getVehicleType()
123 auto passthrough = mavsdk::MavlinkPassthrough { m_system };
125 auto prom = std::promise<MAV_TYPE> {};
126 auto fut = prom.get_future();
127 #if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
128 mavsdk::MavlinkPassthrough::MessageHandle handle = passthrough.subscribe_message(
129 MAVLINK_MSG_ID_HEARTBEAT, [&passthrough, &prom, &handle](
const mavlink_message_t &message) {
131 passthrough.subscribe_message_async(MAVLINK_MSG_ID_HEARTBEAT,
132 [&passthrough, &prom](
const mavlink_message_t &message) {
135 if (message.compid != MAV_COMP_ID_AUTOPILOT1) {
139 mavlink_heartbeat_t heartbeat;
140 mavlink_msg_heartbeat_decode(&message, &heartbeat);
143 #if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
144 passthrough.unsubscribe_message(handle);
146 passthrough.subscribe_message_async(MAVLINK_MSG_ID_HEARTBEAT,
nullptr);
149 prom.set_value(
static_cast<MAV_TYPE
>(heartbeat.type));
154 if (fut.wait_for(seconds(3)) == std::future_status::timeout) {
155 std::cerr <<
"No heartbeat received to get vehicle type." << std::endl;
163 void calibrate_accelerometer(mavsdk::Calibration &calibration)
165 std::cout <<
"Calibrating accelerometer..." << std::endl;
167 std::promise<void> calibration_promise;
168 auto calibration_future = calibration_promise.get_future();
170 calibration.calibrate_accelerometer_async(create_calibration_callback(calibration_promise));
172 calibration_future.wait();
175 std::function<void(mavsdk::Calibration::Result, mavsdk::Calibration::ProgressData)>
176 create_calibration_callback(std::promise<void> &calibration_promise)
178 return [&calibration_promise](
const mavsdk::Calibration::Result result,
179 const mavsdk::Calibration::ProgressData progress_data) {
181 case mavsdk::Calibration::Result::Success:
182 std::cout <<
"--- Calibration succeeded!" << std::endl;
183 calibration_promise.set_value();
185 case mavsdk::Calibration::Result::Next:
186 if (progress_data.has_progress) {
187 std::cout <<
" Progress: " << progress_data.progress << std::endl;
189 if (progress_data.has_status_text) {
190 std::cout <<
" Instruction: " << progress_data.status_text << std::endl;
194 std::cout <<
"--- Calibration failed with message: " << result << std::endl;
195 calibration_promise.set_value();
201 void calibrate_gyro(mavsdk::Calibration &calibration)
203 std::cout <<
"Calibrating gyro..." << std::endl;
205 std::promise<void> calibration_promise;
206 auto calibration_future = calibration_promise.get_future();
208 calibration.calibrate_gyro_async(create_calibration_callback(calibration_promise));
210 calibration_future.wait();
214 void connect(
const std::string &connectionInfo)
216 m_address = connectionInfo;
217 mavsdk::ConnectionResult connection_result = m_mavsdk.add_any_connection(connectionInfo);
219 if (connection_result != mavsdk::ConnectionResult::Success) {
220 std::cerr <<
"Connection failed: " << connection_result << std::endl;
224 m_system = getSystem(m_mavsdk);
230 m_mav_type = getVehicleType();
232 m_has_flying_capability = hasFlyingCapability(m_mav_type);
234 std::cout << (m_has_flying_capability ?
"Connected to a flying vehicle" :
"Connected to a non flying vehicle")
237 m_action = std::make_shared<mavsdk::Action>(m_system);
238 m_telemetry = std::make_shared<mavsdk::Telemetry>(m_system);
239 m_offboard = std::make_shared<mavsdk::Offboard>(m_system);
242 bool hasFlyingCapability(MAV_TYPE mav_type)
245 case MAV_TYPE::MAV_TYPE_GROUND_ROVER:
246 case MAV_TYPE::MAV_TYPE_SURFACE_BOAT:
247 case MAV_TYPE::MAV_TYPE_SUBMARINE:
254 bool isRunning()
const
256 if (m_system ==
nullptr) {
264 std::string getAddress()
const
266 std::string sequence;
267 std::stringstream ss(m_address);
268 std::string actual_address;
269 std::getline(ss, sequence,
':');
270 if (sequence ==
"serial" || sequence ==
"udp" || sequence ==
"tcp") {
271 getline(ss, sequence,
':');
272 for (
const char &c : sequence) {
274 actual_address.append(1, c);
277 return actual_address;
280 std::cout <<
"ERROR : The address parameter must start with \"serial:\" or \"udp:\" or \"tcp:\"." << std::endl;
281 return std::string();
285 float getBatteryLevel()
const
287 mavsdk::Telemetry::Battery battery = m_telemetry.get()->battery();
288 return battery.voltage_v;
293 auto quat = m_telemetry.get()->attitude_quaternion();
294 auto posvel = m_telemetry.get()->position_velocity_ned();
296 vpTranslationVector t { posvel.position.north_m, posvel.position.east_m, posvel.position.down_m };
300 void getPosition(
float &ned_north,
float &ned_east,
float &ned_down,
float &ned_yaw)
const
302 auto odom = m_telemetry.get()->odometry();
303 auto angles = m_telemetry.get()->attitude_euler();
304 ned_north = odom.position_body.x_m;
305 ned_east = odom.position_body.y_m;
306 ned_down = odom.position_body.z_m;
310 std::tuple<float, float> getHome()
const
312 auto position = m_telemetry.get()->home();
313 return { float(position.latitude_deg), float(position.longitude_deg) };
324 flu_M_frd[1][1] = -1;
325 flu_M_frd[2][2] = -1;
328 auto mocap = mavsdk::Mocap { m_system };
329 mavsdk::Mocap::VisionPositionEstimate pose_estimate;
333 pose_estimate.angle_body.roll_rad = ned_rxyz_frd[0];
334 pose_estimate.angle_body.pitch_rad = ned_rxyz_frd[1];
335 pose_estimate.angle_body.yaw_rad = ned_rxyz_frd[2];
338 pose_estimate.position_body.x_m = ned_t_frd[0];
339 pose_estimate.position_body.y_m = ned_t_frd[1];
340 pose_estimate.position_body.z_m = ned_t_frd[2];
342 pose_estimate.pose_covariance.covariance_matrix.push_back(NAN);
343 pose_estimate.time_usec = 0;
345 const mavsdk::Mocap::Result set_position_result = mocap.set_vision_position_estimate(pose_estimate);
346 if (set_position_result != mavsdk::Mocap::Result::Success) {
347 std::cerr <<
"Set position failed: " << set_position_result <<
'\n';
351 if (display_fps > 0) {
352 double display_time_ms = 1000. / display_fps;
355 std::cout <<
"Send ned_M_frd MoCap data: " << std::endl;
356 std::cout <<
"Translation [m]: " << pose_estimate.position_body.x_m <<
" , "
357 << pose_estimate.position_body.y_m <<
" , " << pose_estimate.position_body.z_m << std::endl;
358 std::cout <<
"Roll [rad]: " << pose_estimate.angle_body.roll_rad
359 <<
" , Pitch [rad]: " << pose_estimate.angle_body.pitch_rad
360 <<
" , Yaw [rad]: " << pose_estimate.angle_body.yaw_rad <<
" ." << std::endl;
367 void setTakeOffAlt(
double altitude)
370 m_takeoffAlt = altitude;
373 std::cerr <<
"ERROR : The take off altitude must be positive." << std::endl;
380 auto calibration = mavsdk::Calibration(m_system);
383 calibrate_accelerometer(calibration);
384 calibrate_gyro(calibration);
390 std::cout <<
"Arming...\n";
391 const mavsdk::Action::Result arm_result = m_action.get()->arm();
393 if (arm_result != mavsdk::Action::Result::Success) {
394 std::cerr <<
"Arming failed: " << arm_result << std::endl;
403 std::cout <<
"Disarming...\n";
404 const mavsdk::Action::Result arm_result = m_action.get()->disarm();
406 if (arm_result != mavsdk::Action::Result::Success) {
407 std::cerr <<
"Disarming failed: " << arm_result << std::endl;
413 bool setGPSGlobalOrigin(
double latitude,
double longitude,
double altitude)
415 auto passthrough = mavsdk::MavlinkPassthrough { m_system };
416 mavlink_set_gps_global_origin_t gps_global_origin;
417 gps_global_origin.latitude = latitude * 10000000;
418 gps_global_origin.longitude = longitude * 10000000;
419 gps_global_origin.altitude = altitude * 1000;
420 gps_global_origin.target_system = m_system->get_system_id();
421 mavlink_message_t msg;
422 mavlink_msg_set_gps_global_origin_encode(passthrough.get_our_sysid(), passthrough.get_our_compid(), &msg,
424 auto resp = passthrough.send_message(msg);
425 if (resp != mavsdk::MavlinkPassthrough::Result::Success) {
426 std::cerr <<
"Set GPS global position failed: " << resp << std::endl;
435 std::cout <<
"Starting offboard mode..." << std::endl;
438 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
439 const mavsdk::Offboard::VelocityBodyYawspeed stay {};
440 m_offboard.get()->set_velocity_body(stay);
442 mavsdk::Offboard::Result offboard_result = m_offboard.get()->start();
443 if (offboard_result != mavsdk::Offboard::Result::Success) {
444 std::cerr <<
"Offboard mode failed: " << offboard_result << std::endl;
448 else if (m_verbose) {
449 std::cout <<
"Already in offboard mode" << std::endl;
454 while (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
456 std::cout <<
"Time out received in takeControl()" << std::endl;
462 std::cout <<
"Offboard mode started" << std::endl;
467 void setPositioningIncertitude(
float position_incertitude,
float yaw_incertitude)
469 m_position_incertitude = position_incertitude;
470 m_yaw_incertitude = yaw_incertitude;
473 bool takeOff(
bool interactive,
int timeout_sec,
bool use_gps)
475 if (!m_has_flying_capability) {
476 std::cerr <<
"Warning: Cannot takeoff this non flying vehicle" << std::endl;
480 bool authorize_takeoff =
false;
483 authorize_takeoff =
true;
486 if (m_telemetry.get()->flight_mode() == mavsdk::Telemetry::FlightMode::Offboard) {
487 authorize_takeoff =
true;
491 while (answer !=
"Y" && answer !=
"y" && answer !=
"N" && answer !=
"n") {
492 std::cout <<
"Current flight mode is not the offboard mode. Do you "
493 "want to force offboard mode ? (y/n)"
496 if (answer ==
"Y" || answer ==
"y") {
497 authorize_takeoff =
true;
503 if (m_telemetry.get()->in_air()) {
504 std::cerr <<
"Cannot take off as the robot is already flying." << std::endl;
507 else if (authorize_takeoff) {
517 while (answer !=
"Y" && answer !=
"y" && answer !=
"N" && answer !=
"n") {
518 std::cout <<
"If vehicle armed ? (y/n)" << std::endl;
520 if (answer ==
"N" || answer ==
"n") {
529 if (m_telemetry.get()->gps_info().fix_type == mavsdk::Telemetry::FixType::NoGps || !use_gps) {
538 auto in_air_promise = std::promise<void> {};
539 auto in_air_future = in_air_promise.get_future();
541 mavsdk::Telemetry::Odometry odom = m_telemetry.get()->odometry();
546 double X_init = odom.position_body.x_m;
547 double Y_init = odom.position_body.y_m;
548 double Z_init = odom.position_body.z_m;
551 std::cout <<
"Takeoff using position NED." << std::endl;
553 mavsdk::Offboard::PositionNedYaw takeoff {};
554 takeoff.north_m = X_init;
555 takeoff.east_m = Y_init;
556 takeoff.down_m = Z_init - m_takeoffAlt;
557 takeoff.yaw_deg = yaw_init;
558 m_offboard.get()->set_position_ned(takeoff);
561 #if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
562 Telemetry::LandedStateHandle handle = m_telemetry.get()->subscribe_landed_state(
563 [
this, &in_air_promise, &handle](mavsdk::Telemetry::LandedState state) {
564 if (state == mavsdk::Telemetry::LandedState::InAir) {
565 std::cout <<
"Drone is taking off\n.";
566 m_telemetry.get()->unsubscribe_landed_state(handle);
567 in_air_promise.set_value();
571 m_telemetry.get()->subscribe_landed_state([
this, &in_air_promise](mavsdk::Telemetry::LandedState state) {
572 if (state == mavsdk::Telemetry::LandedState::InAir) {
573 std::cout <<
"Drone is taking off\n.";
574 m_telemetry.get()->subscribe_landed_state(
nullptr);
575 in_air_promise.set_value();
577 std::cout <<
"state: " << state << std::endl;
580 if (in_air_future.wait_for(seconds(timeout_sec)) == std::future_status::timeout) {
581 std::cerr <<
"Takeoff failed: drone not in air.\n";
582 #if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
583 m_telemetry.get()->unsubscribe_landed_state(handle);
585 m_telemetry.get()->subscribe_landed_state(
nullptr);
590 auto takeoff_finished_promise = std::promise<void> {};
591 auto takeoff_finished_future = takeoff_finished_promise.get_future();
593 #if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
594 auto handle_odom = m_telemetry.get()->subscribe_odometry(
595 [
this, &takeoff_finished_promise, &handle, &Z_init](mavsdk::Telemetry::Odometry odom) {
596 if (odom.position_body.z_m < 0.90 * (Z_init - m_takeoffAlt) + m_position_incertitude) {
597 std::cout <<
"Takeoff altitude reached\n.";
598 m_telemetry.get()->unsubscribe_odometry(handle_odom);
599 takeoff_finished_promise.set_value();
603 m_telemetry.get()->subscribe_odometry(
604 [
this, &takeoff_finished_promise, &Z_init](mavsdk::Telemetry::Odometry odom) {
605 if (odom.position_body.z_m < 0.90 * (Z_init - m_takeoffAlt) + m_position_incertitude) {
606 std::cout <<
"Takeoff altitude reached\n.";
607 m_telemetry.get()->subscribe_odometry(nullptr);
608 takeoff_finished_promise.set_value();
612 if (takeoff_finished_future.wait_for(seconds(timeout_sec)) == std::future_status::timeout) {
613 std::cerr <<
"Takeoff failed: altitude not reached.\n";
614 #if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
615 m_telemetry.get()->unsubscribe_odometry(handle);
617 m_telemetry.get()->subscribe_odometry(
nullptr);
624 std::cout <<
"---- DEBUG: GPS detected: use action::takeoff()" << std::endl;
626 mavsdk::Telemetry::Odometry odom = m_telemetry.get()->odometry();
627 double Z_init = odom.position_body.z_m;
629 m_action.get()->set_takeoff_altitude(m_takeoffAlt);
630 const auto takeoff_result = m_action.get()->takeoff();
631 if (takeoff_result != mavsdk::Action::Result::Success) {
632 std::cerr <<
"Takeoff failed: " << takeoff_result <<
'\n';
636 auto in_air_promise = std::promise<void> {};
637 auto in_air_future = in_air_promise.get_future();
638 #if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
639 Telemetry::LandedStateHandle handle = m_telemetry.get()->subscribe_landed_state(
640 [
this, &in_air_promise, &handle](mavsdk::Telemetry::LandedState state) {
641 if (state == mavsdk::Telemetry::LandedState::InAir) {
642 std::cout <<
"Taking off has finished\n.";
643 m_telemetry.get()->unsubscribe_landed_state(handle);
644 in_air_promise.set_value();
648 m_telemetry.get()->subscribe_landed_state([
this, &in_air_promise](mavsdk::Telemetry::LandedState state) {
649 if (state == mavsdk::Telemetry::LandedState::InAir) {
650 std::cout <<
"Taking off has finished\n.";
651 m_telemetry.get()->subscribe_landed_state(
nullptr);
652 in_air_promise.set_value();
654 std::cout <<
"state: " << state << std::endl;
657 if (in_air_future.wait_for(seconds(timeout_sec)) == std::future_status::timeout) {
659 std::cerr <<
"Takeoff timed out.\n";
660 #if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
661 m_telemetry.get()->unsubscribe_landed_state(handle);
663 m_telemetry.get()->subscribe_landed_state(
nullptr);
667 auto takeoff_finished_promise = std::promise<void> {};
668 auto takeoff_finished_future = takeoff_finished_promise.get_future();
670 #if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
671 auto handle_odom = m_telemetry.get()->subscribe_odometry(
672 [
this, &takeoff_finished_promise, &handle, &Z_init](mavsdk::Telemetry::Odometry odom) {
673 if (odom.position_body.z_m < 0.90 * (Z_init - m_takeoffAlt) + m_position_incertitude) {
674 std::cout <<
"Takeoff altitude reached\n.";
675 m_telemetry.get()->unsubscribe_odometry(handle_odom);
676 takeoff_finished_promise.set_value();
680 m_telemetry.get()->subscribe_odometry(
681 [
this, &takeoff_finished_promise, &Z_init](mavsdk::Telemetry::Odometry odom) {
682 if (odom.position_body.z_m < 0.90 * (Z_init - m_takeoffAlt) + m_position_incertitude) {
683 std::cout <<
"Takeoff altitude reached\n.";
684 m_telemetry.get()->subscribe_odometry(nullptr);
685 takeoff_finished_promise.set_value();
689 if (takeoff_finished_future.wait_for(seconds(timeout_sec)) == std::future_status::timeout) {
690 std::cerr <<
"Takeoff failed: altitude not reached.\n";
691 #if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
692 m_telemetry.get()->unsubscribe_odometry(handle);
694 m_telemetry.get()->subscribe_odometry(
nullptr);
705 bool land(
bool use_buildin =
false)
707 if (!m_has_flying_capability) {
708 std::cerr <<
"Warning: Cannot land this non flying vehicle" << std::endl;
721 mavsdk::Telemetry::Odometry odom = m_telemetry.get()->odometry();
726 double X_init = odom.position_body.x_m;
727 double Y_init = odom.position_body.y_m;
730 std::cout <<
"Landing using position NED." << std::endl;
732 mavsdk::Offboard::PositionNedYaw landing {};
733 landing.north_m = X_init;
734 landing.east_m = Y_init;
736 landing.yaw_deg = yaw_init;
737 m_offboard.get()->set_position_ned(landing);
739 bool success =
false;
742 auto landing_finished_promise = std::promise<void> {};
743 auto landing_finished_future = landing_finished_promise.get_future();
745 #if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
746 auto handle_odom = m_telemetry.get()->subscribe_odometry(
747 [
this, &landing_finished_promise, &success, &handle](mavsdk::Telemetry::Odometry odom) {
748 if (odom.position_body.z_m > -0.15) {
749 std::cout <<
"Landing altitude reached \n.";
752 m_telemetry.get()->unsubscribe_odometry(handle_odom);
753 landing_finished_promise.set_value();
757 m_telemetry.get()->subscribe_odometry(
758 [
this, &landing_finished_promise, &success](mavsdk::Telemetry::Odometry odom) {
759 if (odom.position_body.z_m > -0.15) {
760 std::cout <<
"Landing altitude reached\n.";
763 m_telemetry.get()->subscribe_odometry(nullptr);
764 landing_finished_promise.set_value();
768 if (landing_finished_future.wait_for(seconds(10)) == std::future_status::timeout) {
769 std::cerr <<
"failed: altitude not reached.\n";
774 std::cout <<
"Descending\n.";
779 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Land) {
780 std::cout <<
"Landing...\n";
781 const mavsdk::Action::Result land_result = m_action.get()->land();
782 if (land_result != mavsdk::Action::Result::Success) {
783 std::cerr <<
"Land failed: " << land_result << std::endl;
788 while (m_telemetry.get()->in_air()) {
789 std::cout <<
"Vehicle is landing..." << std::endl;
790 sleep_for(seconds(1));
794 std::cout <<
"Landed!" << std::endl;
797 sleep_for(seconds(5));
798 std::cout <<
"Finished..." << std::endl;
802 bool setPosition(
float ned_north,
float ned_east,
float ned_down,
float ned_yaw,
bool blocking,
int timeout_sec)
804 mavsdk::Offboard::PositionNedYaw position_target {};
806 position_target.north_m = ned_north;
807 position_target.east_m = ned_east;
808 position_target.down_m = ned_down;
811 std::cout <<
"NED Pos to reach: " << position_target.north_m <<
" " << position_target.east_m <<
" "
812 << position_target.down_m <<
" " << position_target.yaw_deg << std::endl;
813 m_offboard.get()->set_position_ned(position_target);
815 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
817 std::cout <<
"Cannot set vehicle position: offboard mode not started" << std::endl;
824 auto position_reached_promise = std::promise<void> {};
825 auto position_reached_future = position_reached_promise.get_future();
827 #if (VISP_HAVE_MAVSDK_VERSION > 0x010412)
828 auto handle_odom = m_telemetry.get()->subscribe_odometry(
829 [
this, &position_reached_promise, &handle, &position_target](mavsdk::Telemetry::Odometry odom) {
834 double distance_to_target = std::sqrt(
vpMath::sqr(odom.position_body.x_m - position_target.north_m) +
835 vpMath::sqr(odom.position_body.y_m - position_target.east_m) +
836 vpMath::sqr(odom.position_body.z_m - position_target.down_m));
837 if (distance_to_target < m_position_incertitude &&
838 std::fabs(odom_yaw - position_target.yaw_deg) < m_yaw_incertitude) {
839 std::cout <<
"Position reached\n.";
840 m_telemetry.get()->unsubscribe_odometry(handle_odom);
841 position_reached_promise.set_value();
845 m_telemetry.get()->subscribe_odometry(
846 [
this, &position_reached_promise, &position_target](mavsdk::Telemetry::Odometry odom) {
851 double distance_to_target = std::sqrt(
vpMath::sqr(odom.position_body.x_m - position_target.north_m) +
852 vpMath::sqr(odom.position_body.y_m - position_target.east_m) +
853 vpMath::sqr(odom.position_body.z_m - position_target.down_m));
854 if (distance_to_target < m_position_incertitude &&
855 std::fabs(odom_yaw - position_target.yaw_deg) < m_yaw_incertitude) {
856 std::cout <<
"Position reached\n.";
857 m_telemetry.get()->subscribe_odometry(
nullptr);
858 position_reached_promise.set_value();
862 if (position_reached_future.wait_for(seconds(timeout_sec)) == std::future_status::timeout) {
863 std::cerr <<
"Positioning failed: position not reached.\n";
868 std::cout <<
"---- DEBUG timeout: " << timeout_sec << std::endl;
872 bool setPositionRelative(
float ned_delta_north,
float ned_delta_east,
float ned_delta_down,
float ned_delta_yaw,
873 bool blocking,
int timeout_sec)
875 mavsdk::Telemetry::Odometry odom;
876 mavsdk::Telemetry::EulerAngle angles;
877 mavsdk::Offboard::PositionNedYaw position_target {};
879 position_target.north_m = ned_delta_north;
880 position_target.east_m = ned_delta_east;
881 position_target.down_m = ned_delta_down;
882 position_target.yaw_deg =
vpMath::deg(ned_delta_yaw);
885 odom = m_telemetry.get()->odometry();
886 angles = m_telemetry.get()->attitude_euler();
888 position_target.north_m += odom.position_body.x_m;
889 position_target.east_m += odom.position_body.y_m;
890 position_target.down_m += odom.position_body.z_m;
891 position_target.yaw_deg += angles.yaw_deg;
893 return setPosition(position_target.north_m, position_target.east_m, position_target.down_m,
894 vpMath::rad(position_target.yaw_deg), blocking, timeout_sec);
900 if (XYZvec[0] != 0.0) {
901 std::cerr <<
"ERROR : Can't move, rotation around X axis should be 0." << std::endl;
904 if (XYZvec[1] != 0.0) {
905 std::cerr <<
"ERROR : Can't move, rotation around Y axis should be 0." << std::endl;
909 absolute, timeout_sec);
915 if (XYZvec[0] != 0.0) {
916 std::cerr <<
"ERROR : Can't move, rotation around X axis should be 0." << std::endl;
919 if (XYZvec[1] != 0.0) {
920 std::cerr <<
"ERROR : Can't move, rotation around Y axis should be 0." << std::endl;
924 XYZvec[2], blocking, timeout_sec);
929 if (frd_vel_cmd.
size() != 4) {
931 "ERROR : Can't set velocity, dimension of the velocity vector %d should be equal to 4.",
932 frd_vel_cmd.
size()));
935 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
937 std::cout <<
"Cannot set vehicle velocity: offboard mode not started" << std::endl;
941 mavsdk::Offboard::VelocityBodyYawspeed velocity_comm {};
942 velocity_comm.forward_m_s = frd_vel_cmd[0];
943 velocity_comm.right_m_s = frd_vel_cmd[1];
944 velocity_comm.down_m_s = frd_vel_cmd[2];
945 velocity_comm.yawspeed_deg_s =
vpMath::deg(frd_vel_cmd[3]);
946 m_offboard.get()->set_velocity_body(velocity_comm);
953 const mavsdk::Action::Result kill_result = m_action.get()->kill();
954 if (kill_result != mavsdk::Action::Result::Success) {
955 std::cerr <<
"Kill failed: " << kill_result << std::endl;
963 if (m_telemetry.get()->in_air()) {
964 if (m_telemetry.get()->gps_info().fix_type != mavsdk::Telemetry::FixType::NoGps) {
966 const mavsdk::Action::Result hold_result = m_action.get()->hold();
967 if (hold_result != mavsdk::Action::Result::Success) {
968 std::cerr <<
"Hold failed: " << hold_result << std::endl;
973 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
975 std::cout <<
"Cannot set vehicle velocity: offboard mode not started" << std::endl;
980 setPositionRelative(0., 0., 0., 0.,
false, 10.);
988 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
990 std::cout <<
"Cannot stop moving: offboard mode not started" << std::endl;
995 const mavsdk::Offboard::VelocityBodyYawspeed stay {};
996 m_offboard.get()->set_velocity_body(stay);
1001 bool releaseControl()
1003 auto offboard_result = m_offboard.get()->stop();
1004 if (offboard_result != mavsdk::Offboard::Result::Success) {
1005 std::cerr <<
"Offboard stop failed: " << offboard_result <<
'\n';
1008 std::cout <<
"Offboard stopped\n";
1012 void setAutoLand(
bool auto_land) { m_auto_land = auto_land; }
1014 bool setYawSpeed(
double body_frd_wz)
1016 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
1018 std::cout <<
"Cannot set vehicle velocity: offboard mode not started" << std::endl;
1022 mavsdk::Offboard::VelocityBodyYawspeed velocity_comm {};
1023 velocity_comm.forward_m_s = 0.0;
1024 velocity_comm.right_m_s = 0.0;
1025 velocity_comm.down_m_s = 0.0;
1026 velocity_comm.yawspeed_deg_s =
vpMath::deg(body_frd_wz);
1027 m_offboard.get()->set_velocity_body(velocity_comm);
1032 bool setForwardSpeed(
double body_frd_vx)
1034 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
1036 std::cout <<
"Cannot set vehicle velocity: offboard mode not started" << std::endl;
1041 mavsdk::Offboard::VelocityBodyYawspeed velocity_comm {};
1042 velocity_comm.forward_m_s = body_frd_vx;
1043 velocity_comm.right_m_s = 0.0;
1044 velocity_comm.down_m_s = 0.0;
1045 velocity_comm.yawspeed_deg_s = 0.0;
1046 m_offboard.get()->set_velocity_body(velocity_comm);
1051 bool setLateralSpeed(
double body_frd_vy)
1053 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
1055 std::cout <<
"Cannot set vehicle velocity: offboard mode not started" << std::endl;
1059 mavsdk::Offboard::VelocityBodyYawspeed velocity_comm {};
1060 velocity_comm.forward_m_s = 0.0;
1061 velocity_comm.right_m_s = body_frd_vy;
1062 velocity_comm.down_m_s = 0.0;
1063 velocity_comm.yawspeed_deg_s = 0.0;
1064 m_offboard.get()->set_velocity_body(velocity_comm);
1069 bool setVerticalSpeed(
double body_frd_vz)
1071 if (m_telemetry.get()->flight_mode() != mavsdk::Telemetry::FlightMode::Offboard) {
1073 std::cout <<
"Cannot set vehicle velocity: offboard mode not started" << std::endl;
1077 mavsdk::Offboard::VelocityBodyYawspeed velocity_comm {};
1078 velocity_comm.forward_m_s = 0.0;
1079 velocity_comm.right_m_s = 0.0;
1080 velocity_comm.down_m_s = body_frd_vz;
1081 velocity_comm.yawspeed_deg_s = 0.0;
1082 m_offboard.get()->set_velocity_body(velocity_comm);
1087 bool getFlyingCapability() {
return m_has_flying_capability; }
1089 void setVerbose(
bool verbose) { m_verbose = verbose; }
1093 std::string m_address {};
1094 mavsdk::Mavsdk m_mavsdk {};
1095 std::shared_ptr<mavsdk::System> m_system;
1096 std::shared_ptr<mavsdk::Action> m_action;
1097 std::shared_ptr<mavsdk::Telemetry> m_telemetry;
1098 std::shared_ptr<mavsdk::Offboard> m_offboard;
1100 double m_takeoffAlt { 1.0 };
1102 MAV_TYPE m_mav_type {};
1103 bool m_has_flying_capability {
false };
1105 float m_position_incertitude { 0.05 };
1106 float m_yaw_incertitude { 0.09 };
1107 bool m_verbose {
false };
1108 bool m_auto_land {
true };
1148 vpRobotMavsdk::vpRobotMavsdk(
const std::string &connection_info) : m_impl(new vpRobotMavsdkImpl(connection_info))
1150 m_impl->setPositioningIncertitude(0.05,
vpMath::rad(5));
1167 vpRobotMavsdk::vpRobotMavsdk() : m_impl(new vpRobotMavsdkImpl())
1169 m_impl->setPositioningIncertitude(0.05,
vpMath::rad(5));
1179 vpRobotMavsdk::~vpRobotMavsdk() {
delete m_impl; }
1193 void vpRobotMavsdk::connect(
const std::string &connection_info) { m_impl->connect(connection_info); }
1198 bool vpRobotMavsdk::isRunning()
const {
return m_impl->isRunning(); }
1222 return m_impl->sendMocapData(enu_M_flu, display_fps);
1231 std::string vpRobotMavsdk::getAddress()
const {
return m_impl->getAddress(); }
1238 float vpRobotMavsdk::getBatteryLevel()
const {
return m_impl->getBatteryLevel(); }
1244 void vpRobotMavsdk::getPosition(
vpHomogeneousMatrix &ned_M_frd)
const { m_impl->getPosition(ned_M_frd); }
1253 void vpRobotMavsdk::getPosition(
float &ned_north,
float &ned_east,
float &ned_down,
float &ned_yaw)
const
1255 m_impl->getPosition(ned_north, ned_east, ned_down, ned_yaw);
1264 std::tuple<float, float> vpRobotMavsdk::getHome()
const {
return m_impl->getHome(); }
1271 void vpRobotMavsdk::doFlatTrim() { }
1280 void vpRobotMavsdk::setTakeOffAlt(
double altitude) { m_impl->setTakeOffAlt(altitude); }
1286 bool vpRobotMavsdk::arm() {
return m_impl->arm(); }
1292 bool vpRobotMavsdk::disarm() {
return m_impl->disarm(); }
1308 bool vpRobotMavsdk::takeOff(
bool interactive,
int timeout_sec,
bool use_gps)
1310 return m_impl->takeOff(interactive, timeout_sec, use_gps);
1327 bool vpRobotMavsdk::takeOff(
bool interactive,
double takeoff_altitude,
int timeout_sec,
bool use_gps)
1329 m_impl->setTakeOffAlt(takeoff_altitude);
1330 return m_impl->takeOff(interactive, timeout_sec, use_gps);
1340 bool vpRobotMavsdk::holdPosition() {
return m_impl->holdPosition(); };
1347 bool vpRobotMavsdk::stopMoving() {
return m_impl->stopMoving(); };
1356 bool vpRobotMavsdk::land() {
return m_impl->land(); }
1372 bool vpRobotMavsdk::setPosition(
float ned_north,
float ned_east,
float ned_down,
float ned_yaw,
bool blocking,
1375 return m_impl->setPosition(ned_north, ned_east, ned_down, ned_yaw, blocking, timeout_sec);
1394 bool vpRobotMavsdk::setPosition(
const vpHomogeneousMatrix &ned_M_frd,
bool blocking,
int timeout_sec)
1396 return m_impl->setPosition(ned_M_frd, blocking, timeout_sec);
1413 bool vpRobotMavsdk::setPositionRelative(
float ned_delta_north,
float ned_delta_east,
float ned_delta_down,
1414 float ned_delta_yaw,
bool blocking,
int timeout_sec)
1416 return m_impl->setPositionRelative(ned_delta_north, ned_delta_east, ned_delta_down, ned_delta_yaw, blocking,
1435 bool vpRobotMavsdk::setPositionRelative(
const vpHomogeneousMatrix &delta_frd_M_frd,
bool blocking,
int timeout_sec)
1437 return m_impl->setPositionRelative(delta_frd_M_frd, blocking, timeout_sec);
1449 bool vpRobotMavsdk::setVelocity(
const vpColVector &frd_vel_cmd) {
return m_impl->setVelocity(frd_vel_cmd); }
1456 bool vpRobotMavsdk::kill() {
return m_impl->kill(); }
1469 bool vpRobotMavsdk::setYawSpeed(
double body_frd_wz) {
return m_impl->setYawSpeed(body_frd_wz); }
1482 bool vpRobotMavsdk::setForwardSpeed(
double body_frd_vx) {
return m_impl->setForwardSpeed(body_frd_vx); }
1495 bool vpRobotMavsdk::setLateralSpeed(
double body_frd_vy) {
return m_impl->setLateralSpeed(body_frd_vy); }
1505 bool vpRobotMavsdk::setGPSGlobalOrigin(
double latitude,
double longitude,
double altitude)
1507 return m_impl->setGPSGlobalOrigin(latitude, longitude, altitude);
1521 bool vpRobotMavsdk::takeControl() {
return m_impl->takeControl(); }
1532 bool vpRobotMavsdk::releaseControl() {
return m_impl->releaseControl(); }
1541 void vpRobotMavsdk::setAutoLand(
bool auto_land) { m_impl->setAutoLand(auto_land); }
1550 void vpRobotMavsdk::setPositioningIncertitude(
float position_incertitude,
float yaw_incertitude)
1552 m_impl->setPositioningIncertitude(position_incertitude, yaw_incertitude);
1566 bool vpRobotMavsdk::setVerticalSpeed(
double body_frd_vz) {
return m_impl->setVerticalSpeed(body_frd_vz); }
1573 void vpRobotMavsdk::setVerbose(
bool verbose) { m_impl->setVerbose(verbose); }
1580 bool vpRobotMavsdk::hasFlyingCapability() {
return m_impl->getFlyingCapability(); }
1582 #elif !defined(VISP_BUILD_SHARED_LIBS)
1584 void dummy_vpRobotMavsdk() { };
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
@ dimensionError
Bad dimension.
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)
static double sqr(double x)
static vpHomogeneousMatrix enu2ned(const vpHomogeneousMatrix &enu_M)
static double deg(double rad)
Implementation of a rotation vector as quaternion angle minimal representation.
const double & x() const
Returns the x-component of the quaternion.
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()