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