Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
testPixhawkDroneTakeoff.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 takeoff and land using mavsdk on
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 // Check if std:c++17 or higher
51 #if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
52 
53 #include <visp3/robot/vpRobotMavsdk.h>
54 
55 void usage(const std::string &bin_name)
56 {
57  std::cerr << "Usage : " << bin_name << " <connection information>\n"
58  << "Connection information format should be :\n"
59  << " - For TCP: tcp://[server_host][:server_port]\n"
60  << " - For UDP: udp://[bind_host][:bind_port]\n"
61  << " - For Serial: serial:///path/to/serial/dev[:baudrate]\n"
62  << "For example, to connect to the simulator use URL: udp://:14540\n";
63 }
64 
65 int main(int argc, char **argv)
66 {
67  if (argc != 2) {
68  usage(argv[0]);
69  return EXIT_SUCCESS;
70  }
71 
72  auto drone = vpRobotMavsdk(argv[1]);
73 
74  drone.setTakeOffAlt(1.);
75  drone.setVerbose(true);
76  drone.takeControl(); // Start off-board or guided mode
77 
78  // Drone takeoff
79  if (!drone.takeOff()) {
80  std::cout << "Takeoff failed" << std::endl;
81  return EXIT_FAILURE;
82  }
83 
84  // Get position in NED local frame after takeoff
85  float ned_north, ned_east, ned_down, ned_yaw;
86  drone.getPosition(ned_north, ned_east, ned_down, ned_yaw);
87  std::cout << "Vehicle position in NED frame: " << ned_north << " " << ned_east << " " << ned_down << " [m] and " << 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: "
93  << ned_M_frd.getTranslationVector().t() << " [m] and "
94  << vpMath::deg(rxyz).t() << " [deg]"<< std::endl;
95 
96  std::cout << "Hold position for 4 sec" << std::endl;
97  drone.holdPosition();
98  vpTime::wait(4000);
99 
100  // Land drone
101  drone.land();
102 
103  return EXIT_SUCCESS;
104 }
105 
106 #else
107 
108 int main()
109 {
110 #ifndef VISP_HAVE_MAVSDK
111  std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n"
112  << std::endl;
113 #endif
114 #if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
115  std::cout
116  << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and "
117  "rebuild ViSP.\n"
118  << std::endl;
119 #endif
120  return EXIT_SUCCESS;
121 }
122 
123 #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:117
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:176
vpRowVector t() const
VISP_EXPORT int wait(double t0, double t)