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