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