Visual Servoing Platform version 3.5.0
keyboardControlBebop2.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 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 http://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 * Example that shows how to setup keyboard control of Parrot Bebop 2 drone in ViSP.
33 *
34 * Authors:
35 * Fabien Spindler
36 * Gatien Gaumerais
37 *
38 *****************************************************************************/
39
40#include <visp3/core/vpConfig.h>
41
42#include <visp3/core/vpTime.h>
43#include <visp3/gui/vpDisplayX.h>
44#include <visp3/io/vpKeyboard.h>
45
46#include <visp3/robot/vpRobotBebop2.h>
47
48#include <iostream>
49
50#ifdef VISP_HAVE_ARSDK
51
60bool handleKeyboardInput(vpRobotBebop2 &drone, int key)
61{
62 bool running = true;
63 if (drone.isRunning()) {
64 switch (key) {
65 case 'q':
66 // Quit
67 drone.land();
68 running = false;
69 break;
70
71 case 'e':
72 // Emergency
73 drone.cutMotors();
74 running = false;
75 break;
76
77 case 't':
78 // Takeoff
79 drone.takeOff(false);
80 break;
81
82 case ' ':
83 // Landing
84 drone.land();
85 break;
86
87 case 'i':
88 // Up
89 drone.setVerticalSpeed(50);
90 break;
91
92 case 'k':
93 // Down
94 drone.setVerticalSpeed(-50);
95 break;
96
97 case 'l':
98 // Right
99 drone.setYawSpeed(50);
100 break;
101
102 case 'j':
103 // Left
104 drone.setYawSpeed(-50);
105 break;
106
107 case 'r':
108 // Forward
109 drone.setPitch(50);
110 break;
111
112 case 'f':
113 // Backward
114 drone.setPitch(-50);
115 break;
116
117 case 'd':
118 // Roll left
119 drone.setRoll(-50);
120 break;
121
122 case 'g':
123 // Roll right
124 drone.setRoll(50);
125 break;
126
127 default:
128 // No inputs -> drone stops moving
129 drone.stopMoving();
130 break;
131 }
132 vpTime::wait(25); // We wait 25ms to give the drone the time to process the command
133 } else {
134 running = false;
135 }
136 return running;
137}
138
149int main(int argc, char **argv)
150{
151 try {
152
153 std::string ip_address = "192.168.42.1";
154
155 int stream_res = 0; // Default 480p resolution
156
157 bool verbose = false;
158
159 for (int i = 1; i < argc; i++) {
160 if (std::string(argv[i]) == "--ip" && i + 1 < argc) {
161 ip_address = std::string(argv[i + 1]);
162 i++;
163 } else if (std::string(argv[i]) == "--hd_stream") {
164 stream_res = 1;
165 } else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") {
166 verbose = true;
167 } else if (argc >= 2 && (std::string(argv[1]) == "--help" || std::string(argv[1]) == "-h")) {
168 std::cout << "\nUsage:\n"
169 << " " << argv[0] << "[--ip <drone ip>] [--hd_stream] [--verbose] [-v] [--help] [-h]\n"
170 << std::endl
171 << "Description:\n"
172 << " --ip <drone ip>\n"
173 << " Ip address of the drone to which you want to connect (default : 192.168.42.1).\n\n"
174 << " --hd_stream\n"
175 << " Enables HD 720p streaming instead of default 480p.\n"
176 << " --verbose, -v\n"
177 << " Enables verbose (drone information messages and velocity commands\n"
178 << " are then displayed).\n\n"
179 << " --help, -h\n"
180 << " Print help message.\n"
181 << std::endl;
182 return 0;
183 } else {
184 std::cout << "Error : unknown parameter " << argv[i] << std::endl
185 << "See " << argv[0] << " --help" << std::endl;
186 return 0;
187 }
188 }
189
190 std::cout << "\nWARNING: this program does no sensing or avoiding of obstacles, "
191 "the drone WILL collide with any objects in the way! Make sure the "
192 "drone has approximately 3 meters of free space on all sides.\n"
193 << std::endl;
194
195 vpRobotBebop2 drone(verbose, true,
196 ip_address); // Create the drone with low verbose level, settings reset and corresponding IP
197
198 if (drone.isRunning()) {
199
200 int k = 0;
201 bool running = true;
202
203 std::cout << "\nConfiguring drone settings ...\n" << std::endl;
204
205 drone.setMaxTilt(10); // Setting the max roll and pitch values, the drone speed will depend on it
206
207 drone.doFlatTrim(); // Flat trim calibration
208
209#ifdef VISP_HAVE_FFMPEG
210 drone.setVideoResolution(stream_res); // Setting desired stream video resolution
211 drone.setStreamingMode(0); // Set streaming mode 0 : lowest latency
212 std::cout << "\nWaiting for streaming to start ...\n" << std::endl;
213 drone.startStreaming();
214
215 // Prepare image for display
216 vpImage<vpRGBa> I(1, 1, 0);
217 drone.getRGBaImage(I);
218 vpDisplayX display(I, 100, 100, "DRONE VIEW");
221#endif
222
223 vpKeyboard keyboard;
224 std::cout << "\n| Control the drone with the keyboard :\n"
225 "| 't' to takeoff / spacebar to land / 'e' for emergency stop\n"
226 "| ('r','f','d','g') and ('i','k','j','l') to move\n"
227 "| 'q' to quit.\n"
228 << std::endl;
229
230 while (running && drone.isRunning() && drone.isStreaming()) {
231
232 k = '0'; // If no key is hit, we send a non-assigned key
233 if (keyboard.kbhit()) {
234 k = keyboard.getchar();
235 }
236 running = handleKeyboardInput(drone, k);
237
238#ifdef VISP_HAVE_FFMPEG
239 drone.getRGBaImage(I);
241 vpDisplay::displayText(I, 10, 10, "Press q to quit", vpColor::red);
243#endif
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 std::cout << "\nThis example requires Parrot ARSDK3 library. You should install it.\n" << std::endl;
262 return EXIT_SUCCESS;
263}
264
265#endif // #if defined(VISP_HAVE_ARSDK)
static const vpColor red
Definition: vpColor.h:217
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:135
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
Definition: vpException.h:72
Keybord management under unix (Linux or OSX). This class is not available under windows.
Definition: vpKeyboard.h:86
int getchar()
Definition: vpKeyboard.cpp:63
int kbhit()
Definition: vpKeyboard.cpp:75
void setPitch(int value)
void setMaxTilt(double maxTilt)
void setRoll(int value)
void setVerticalSpeed(int value)
void setStreamingMode(int mode)
static void land()
void getRGBaImage(vpImage< vpRGBa > &I)
void setYawSpeed(int value)
void takeOff(bool blocking=true)
void setVideoResolution(int mode)
VISP_EXPORT int wait(double t0, double t)