Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
testPixhawkDroneKeyboard.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  * Test to control from keyboard a drone equipped with a Pixhawk thanks to mavsdk.
33  *
34 *****************************************************************************/
35 
45 #include <iostream>
46 
47 #include <visp3/core/vpConfig.h>
48 
49 // Check if std:c++17 or higher
50 #if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
51 
52 #include <visp3/core/vpTime.h>
53 #include <visp3/gui/vpDisplayX.h>
54 #include <visp3/io/vpKeyboard.h>
55 #include <visp3/robot/vpRobotMavsdk.h>
56 
57 bool handleKeyboardInput(vpRobotMavsdk &drone, int key, bool &flying, double &lastCommandTime)
58 {
59  bool running = true;
60  double currentTime = vpTime::measureTimeMs();
61  if (drone.isRunning()) {
62  switch (key) {
63  case 'q':
64  // Quit
65  std::cout << "sending command" << std::endl;
66  drone.land();
67  flying = false;
68  running = false;
69  lastCommandTime = vpTime::measureTimeMs();
70  break;
71 
72  case 'a':
73  // Land
74  if (flying == true) {
75  std::cout << "sending command" << std::endl;
76  drone.land();
77  flying = false;
78  lastCommandTime = vpTime::measureTimeMs();
79  }
80  break;
81 
82  case 'e':
83  // Emergency
84  std::cout << "sending command" << std::endl;
85  drone.kill();
86  flying = false;
87  running = false;
88  lastCommandTime = vpTime::measureTimeMs();
89  break;
90 
91  case 't':
92  // Takeoff
93  std::cout << "sending command" << std::endl;
94  drone.takeOff();
95  flying = true;
96  lastCommandTime = vpTime::measureTimeMs();
97  vpTime::wait(100);
98  drone.takeControl();
99  break;
100 
101  case ' ':
102  // Down
103  if (flying == true) {
104  drone.setVerticalSpeed(0.2);
105  lastCommandTime = vpTime::measureTimeMs();
106  }
107  break;
108 
109  case 'u':
110  // Up
111  if (flying == true) {
112  drone.setVerticalSpeed(-0.2);
113  lastCommandTime = vpTime::measureTimeMs();
114  }
115  break;
116 
117  case 'd':
118  // turn Right
119  if (flying == true) {
120  drone.setYawSpeed(0.4);
121  lastCommandTime = vpTime::measureTimeMs();
122  }
123  break;
124 
125  case 'g':
126  // turn Left
127  if (flying == true) {
128  drone.setYawSpeed(-0.4);
129  lastCommandTime = vpTime::measureTimeMs();
130  }
131  break;
132 
133  case 'i':
134  // go Forward
135  if (flying == true) {
136  drone.setForwardSpeed(0.2);
137  lastCommandTime = vpTime::measureTimeMs();
138  }
139  break;
140 
141  case 'k':
142  // go Backwards
143  if (flying == true) {
144  drone.setForwardSpeed(-0.2);
145  lastCommandTime = vpTime::measureTimeMs();
146  }
147  break;
148 
149  case 'j':
150  // go Left
151  if (flying == true) {
152  drone.setLateralSpeed(-0.2);
153  lastCommandTime = vpTime::measureTimeMs();
154  }
155  break;
156 
157  case 'l':
158  // go Right
159  if (flying == true) {
160  drone.setLateralSpeed(0.2);
161  lastCommandTime = vpTime::measureTimeMs();
162  }
163  break;
164 
165  default:
166  // No inputs -> drone stops moving
167  if ((flying == true) && (currentTime - lastCommandTime > 1500.)) { // We stop moving after 1.5s without commands.
168  std::cout << "1.5 s without order, sending command : stop moving." << std::endl;
169  drone.stopMoving();
170  lastCommandTime = vpTime::measureTimeMs();
171  }
172  break;
173  }
174  vpTime::wait(40); // We wait 40ms to give the drone the time to process the command
175  }
176  else {
177  running = false;
178  }
179  return running;
180 }
181 
182 int main(int argc, char **argv)
183 {
184  try {
185  std::string opt_connecting_info = "udp://192.168.30.111:14552";
186 
187  for (int i = 1; i < argc; i++) {
188  if (std::string(argv[i]) == "--co" && i + 1 < argc) {
189  opt_connecting_info = std::string(argv[i + 1]);
190  i++;
191  }
192  else if (argc >= 2 && (std::string(argv[1]) == "--help" || std::string(argv[1]) == "-h")) {
193  std::cout << "\nUsage:\n"
194  << " " << argv[0] << "[--co <connection information>] [--help] [-h]\n"
195  << std::endl
196  << "Description:\n"
197  << " --co <connection information>\n"
198  << " - UDP: udp://[host][:port]\n"
199  << " - TCP: tcp://[host][:port]\n"
200  << " - serial: serial://[path][:baudrate]\n"
201  << " - Default: udp://192.168.30.111:14552).\n\n"
202  << " For example, to connect to the simulator use URL: udp://:14552\n"
203  << " --help, -h\n"
204  << " Print help message.\n"
205  << std::endl;
206  return EXIT_SUCCESS;
207  }
208  else {
209  std::cout << "Error : unknown parameter " << argv[i] << std::endl
210  << "See " << argv[0] << " --help" << std::endl;
211  return EXIT_SUCCESS;
212  }
213  }
214 
215  std::cout << std::endl
216  << "WARNING: this program does no sensing or avoiding of obstacles, "
217  << "the drone WILL collide with any objects in the way! Make sure the "
218  << "drone has approximately 3 meters of free space on all sides." << std::endl
219  << std::endl;
220 
221 // Connect to the drone
222  vpRobotMavsdk drone(opt_connecting_info);
223 
224  if (drone.isRunning()) {
225  int k = 0;
226  bool running = true;
227  bool flying = false;
228  double lastCommandTime = vpTime::measureTimeMs();
229 
230  std::cout << "\nConfiguring drone settings ...\n" << std::endl;
231 
232  drone.setTakeOffAlt(1.0);
233 
234  vpKeyboard keyboard;
235  std::cout << "\n| Control the drone with the keyboard :\n"
236  "| 't' to takeoff / 'l' to land / 'e' for emergency stop\n"
237  "| ('space','u','d','g') and ('i','k','j','l') to move\n"
238  "| 'q' to quit.\n"
239  << std::endl;
240 
241  while (running && drone.isRunning()) {
242 
243  k = '0'; // If no key is hit, we send a non-assigned key
244  if (keyboard.kbhit()) {
245  k = keyboard.getchar();
246  }
247  running = handleKeyboardInput(drone, k, flying, lastCommandTime);
248  }
249  std::cout << "\nQuitting ...\n" << std::endl;
250 
251  }
252  else {
253  std::cout << "ERROR : failed to setup drone control." << std::endl;
254  return EXIT_FAILURE;
255  }
256  }
257  catch (const vpException &e) {
258  std::cout << "\nCaught an exception: " << e << std::endl;
259  return EXIT_FAILURE;
260  }
261 }
262 
263 #else
264 
265 int main()
266 {
267 #ifndef VISP_HAVE_MAVSDK
268  std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n"
269  << std::endl;
270 #endif
271 #if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
272  std::cout
273  << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and "
274  "rebuild ViSP.\n"
275  << std::endl;
276 #endif
277  return EXIT_SUCCESS;
278 }
279 
280 #endif // #if defined(VISP_HAVE_MAVSDK)
error that can be emitted by ViSP classes.
Definition: vpException.h:59
Keyboard management under unix (Linux or OSX). This class is not available under windows.
Definition: vpKeyboard.h:82
int getchar()
Definition: vpKeyboard.cpp:78
int kbhit()
Definition: vpKeyboard.cpp:96
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()