Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
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 // 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 URL 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  drone.setAutoLand(true);
74  drone.setTakeOffAlt(1.0);
75  drone.setVerbose(true);
76 
77  if (!drone.takeOff()) {
78  std::cout << "Takeoff failed" << std::endl;
79  return EXIT_FAILURE;
80  }
81 
82  drone.takeControl(); // Start PX4 offboard
83 
84  // Get position
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 "
88  << 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: " << ned_M_frd.getTranslationVector().t() << " [m] and "
94  << vpMath::deg(rxyz).t() << " [deg]" << std::endl;
95 
96 // Set position in NED frame
97  drone.setPositioningIncertitude(0.10, vpMath::rad(5.));
98 
99  drone.setPositionRelative(0.0, 1.0, 0.0, 0.0); // Right
100  drone.setPositionRelative(1.0, 0.0, 0.0, 0.0); // Front
101  drone.setPositionRelative(0.0, -1.0, 0.0, 0.0); // Left
102  drone.setPositionRelative(-1.0, 0.0, 0.0, 0.0); // Rear
103 
104  // Land drone during destruction
105  return EXIT_SUCCESS;
106 }
107 
108 #else
109 
110 int main()
111 {
112 #ifndef VISP_HAVE_MAVSDK
113  std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n"
114  << std::endl;
115 #endif
116 #if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
117  std::cout
118  << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and "
119  "rebuild ViSP.\n"
120  << std::endl;
121 #endif
122  return EXIT_SUCCESS;
123 }
124 
125 #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:127
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