Visual Servoing Platform  version 3.6.1 under development (2024-12-04)
testPixhawkDroneTakeoff.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Simple example to demonstrate how takeoff and land using mavsdk on
32  * a drone equipped with a Pixhawk connected to a Jetson TX2.
33  */
34 
44 #include <iostream>
45 
46 #include <visp3/core/vpConfig.h>
47 
48 // Check if std:c++17 or higher
49 #if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
50 
51 #include <visp3/robot/vpRobotMavsdk.h>
52 
53 void usage(const std::string &bin_name)
54 {
55  std::cerr << "Usage : " << bin_name << " <connection information>\n"
56  << "Connection information format should be :\n"
57  << " - For TCP: tcp://[server_host][:server_port]\n"
58  << " - For UDP: udp://[bind_host][:bind_port]\n"
59  << " - For Serial: serial:///path/to/serial/dev[:baudrate]\n"
60  << "For example, to connect to the simulator use URL: udp://:14540\n";
61 }
62 
63 int main(int argc, char **argv)
64 {
65 #ifdef ENABLE_VISP_NAMESPACE
66  using namespace VISP_NAMESPACE_NAME;
67 #endif
68  if (argc != 2) {
69  usage(argv[0]);
70  return EXIT_SUCCESS;
71  }
72 
73  auto drone = vpRobotMavsdk(argv[1]);
74 
75  drone.setTakeOffAlt(1.);
76  drone.setVerbose(true);
77  drone.takeControl(); // Start off-board or guided mode
78 
79  // Drone takeoff
80  if (!drone.takeOff()) {
81  std::cout << "Takeoff failed" << std::endl;
82  return EXIT_FAILURE;
83  }
84 
85  // Get position in NED local frame after takeoff
86  float ned_north, ned_east, ned_down, ned_yaw;
87  drone.getPosition(ned_north, ned_east, ned_down, ned_yaw);
88  std::cout << "Vehicle position in NED frame: " << ned_north << " " << ned_east << " " << ned_down << " [m] and " << vpMath::deg(ned_yaw) << " [deg]" << std::endl;
89 
90  vpHomogeneousMatrix ned_M_frd;
91  drone.getPosition(ned_M_frd);
92  vpRxyzVector rxyz(ned_M_frd.getRotationMatrix());
93  std::cout << "Vehicle position in NED frame: "
94  << ned_M_frd.getTranslationVector().t() << " [m] and "
95  << vpMath::deg(rxyz).t() << " [deg]"<< std::endl;
96 
97  std::cout << "Hold position for 4 sec" << std::endl;
98  drone.holdPosition();
99  vpTime::wait(4000);
100 
101  // Land drone
102  drone.land();
103 
104  return EXIT_SUCCESS;
105 }
106 
107 #else
108 
109 int 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 !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
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 deg(double rad)
Definition: vpMath.h:119
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
vpRowVector t() const
VISP_EXPORT int wait(double t0, double t)