Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testPixhawkDronePositionRelativeControl.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 * Simple example to demonstrate how to control in position using mavsdk
33 * a drone equipped with a Pixhawk connected to a Jetson TX2.
34 *
35*****************************************************************************/
36
46#include <iostream>
47
48#include <visp3/core/vpConfig.h>
49
50#if defined(VISP_HAVE_MAVSDK) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
51
52#include <visp3/robot/vpRobotMavsdk.h>
53
54void usage(const std::string &bin_name)
55{
56 std::cerr << "Usage : " << bin_name << " <connection information>\n"
57 << "Connection URL format should be :\n"
58 << " - For TCP : tcp://[server_host][:server_port]\n"
59 << " - For UDP : udp://[bind_host][:bind_port]\n"
60 << " - For Serial : serial:///path/to/serial/dev[:baudrate]\n"
61 << "For example, to connect to the simulator use URL: udp://:14540\n";
62}
63
64int main(int argc, char **argv)
65{
66 if (argc != 2) {
67 usage(argv[0]);
68 return EXIT_SUCCESS;
69 }
70
71 auto drone = vpRobotMavsdk(argv[1]);
72 drone.setAutoLand(true);
73 drone.setTakeOffAlt(1.0);
74 drone.setVerbose(true);
75
76 if (!drone.takeOff()) {
77 std::cout << "Takeoff failed" << std::endl;
78 return EXIT_FAILURE;
79 }
80
81 drone.takeControl(); // Start PX4 offboard
82
83 // Get position
84 float ned_north, ned_east, ned_down, ned_yaw;
85 drone.getPosition(ned_north, ned_east, ned_down, ned_yaw);
86 std::cout << "Vehicle position in NED frame: " << ned_north << " " << ned_east << " " << ned_down << " [m] and "
87 << vpMath::deg(ned_yaw) << " [deg]" << std::endl;
88
89 vpHomogeneousMatrix ned_M_frd;
90 drone.getPosition(ned_M_frd);
91 vpRxyzVector rxyz(ned_M_frd.getRotationMatrix());
92 std::cout << "Vehicle position in NED frame: " << ned_M_frd.getTranslationVector().t() << " [m] and "
93 << vpMath::deg(rxyz).t() << " [deg]" << std::endl;
94
95 // Set position in NED frame
96 drone.setPositioningIncertitude(0.10, vpMath::rad(5.));
97
98 drone.setPositionRelative(0.0, 1.0, 0.0, 0.0); // Right
99 drone.setPositionRelative(1.0, 0.0, 0.0, 0.0); // Front
100 drone.setPositionRelative(0.0, -1.0, 0.0, 0.0); // Left
101 drone.setPositionRelative(-1.0, 0.0, 0.0, 0.0); // Rear
102
103 // Land drone during destruction
104 return EXIT_SUCCESS;
105}
106
107#else
108
109int main()
110{
111#ifndef VISP_HAVE_MAVSDK
112 std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n"
113 << std::endl;
114#endif
115#if !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
116 std::cout
117 << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and "
118 "rebuild ViSP.\n"
119 << std::endl;
120#endif
121 return EXIT_SUCCESS;
122}
123
124#endif // #if defined(VISP_HAVE_MAVSDK)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
vpTranslationVector getTranslationVector() const
static double rad(double deg)
Definition vpMath.h:116
static double deg(double rad)
Definition vpMath.h:106
vpColVector t() const
Implementation of a rotation vector as Euler angle minimal representation.