Visual Servoing Platform  version 3.4.0 under development (2021-04-11)
vpRobotBebop2.h
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  * Interface for the Irisa's Afma6 robot.
33  *
34  * Authors:
35  * Gatien Gaumerais
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
40 #ifndef _vpRobotBebop2_h_
41 #define _vpRobotBebop2_h_
42 
43 #include <visp3/core/vpConfig.h>
44 
45 #ifdef VISP_HAVE_ARSDK
46 
47 #include <visp3/core/vpImage.h>
48 
49 extern "C" {
50 #include <libARController/ARController.h> // For drone control
51 #include <libARSAL/ARSAL.h> // For semaphore
52 
53 #ifdef VISP_HAVE_FFMPEG
54 #include <libavcodec/avcodec.h> // For H264 video decoding
55 #include <libswscale/swscale.h> // For rescaling decoded frames
56 #endif
57 }
58 
59 #include <mutex>
60 #include <signal.h>
61 #include <string>
62 
71 class VISP_EXPORT vpRobotBebop2
72 {
73 public:
74  vpRobotBebop2(bool verbose = false, bool setDefaultSettings = true, std::string ipAddress = "192.168.42.1",
75  int discoveryPort = 44444);
76  virtual ~vpRobotBebop2();
77 
80  std::string getIpAddress();
81  int getDiscoveryPort();
83 
86  void doFlatTrim();
87  unsigned int getBatteryLevel();
88  void setVerbose(bool verbose);
89  void resetAllSettings();
91 
94  bool isFlying();
95  bool isHovering();
96  bool isLanded();
97  bool isRunning();
98  bool isStreaming();
100 
101  //*** Motion commands ***//
104  void cutMotors();
105  double getMaxTilt();
106  void setMaxTilt(double maxTilt);
107  void setPitch(int value);
108  void setPosition(float dX, float dY, float dZ, float dPsi, bool blocking);
109  void setPosition(const vpHomogeneousMatrix &M, bool blocking);
110  void setRoll(int value);
111  void setVelocity(const vpColVector &vel, double delta_t);
112  void setVerticalSpeed(int value);
113  void setYawSpeed(int value);
114  void stopMoving();
115  void takeOff(bool blocking = true);
117  static void land();
118  //*** ***//
119 
120  //*** Streaming commands ***//
121 #ifdef VISP_HAVE_FFMPEG
122 
124  void getGrayscaleImage(vpImage<unsigned char> &I);
125  void getRGBaImage(vpImage<vpRGBa> &I);
126  int getVideoHeight();
127  int getVideoWidth();
128  void setExposure(float expo);
129  void setStreamingMode(int mode);
130  void setVideoResolution(int mode);
131  void setVideoStabilisationMode(int mode);
132  void startStreaming();
133  void stopStreaming();
135 #endif
136  //*** ***//
137 
138  //*** Camera control commands ***//
141  double getCameraHorizontalFOV() const;
142  double getCurrentCameraPan() const;
143  double getMaxCameraPan() const;
144  double getMinCameraPan() const;
145  double getCurrentCameraTilt() const;
146  double getMaxCameraTilt() const;
147  double getMinCameraTilt() const;
148  void setCameraOrientation(double tilt, double pan, bool blocking = false);
149  void setCameraPan(double pan, bool blocking = false);
150  void setCameraTilt(double tilt, bool blocking = false);
152  //*** ***//
153 
154 private:
155  //*** Attributes ***//
156  std::string m_ipAddress;
157  int m_discoveryPort;
158 
159  ARSAL_Sem_t m_stateSem;
160  struct sigaction m_sigAct;
161 
162 #ifdef VISP_HAVE_FFMPEG
163  AVCodecContext *m_codecContext;
164  AVPacket m_packet;
165  AVFrame *m_picture;
166  std::mutex m_bgr_picture_mutex;
167  AVFrame *m_bgr_picture;
168  SwsContext *m_img_convert_ctx;
169  uint8_t *m_buffer;
170 
171  bool m_videoDecodingStarted;
172 
173  int m_videoWidth;
174  int m_videoHeight;
175 #endif
176 
177  static bool m_running;
178 
180  bool m_exposureSet;
181  bool m_flatTrimFinished;
182  bool m_relativeMoveEnded;
183  bool m_videoResolutionSet;
184  bool m_streamingStarted;
185  bool m_streamingModeSet;
186  bool m_settingsReset;
187 
188  bool m_update_codec_params;
189  std::vector<uint8_t> m_codec_params_data;
190 
191  unsigned int m_batteryLevel;
192  double m_maxTilt;
193 
194  double m_cameraHorizontalFOV;
195 
196  double m_currentCameraTilt;
197  double m_minCameraTilt;
198  double m_maxCameraTilt;
199 
200  double m_currentCameraPan;
201  double m_minCameraPan;
202  double m_maxCameraPan;
203 
204  static ARCONTROLLER_Device_t *m_deviceController;
205 
206  eARCONTROLLER_ERROR m_errorController;
207  eARCONTROLLER_DEVICE_STATE m_deviceState;
208  //*** ***//
209 
210  [[noreturn]] static void sighandler(int signo);
211 
212  eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE getFlyingState();
213  eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED getStreamingState();
214 
215  //*** Setup functions ***//
216  void cleanUp();
217  ARDISCOVERY_Device_t *discoverDrone();
218  void createDroneController(ARDISCOVERY_Device_t *discoveredDrone);
219  void setupCallbacks();
220  void startController();
221 
222 #ifdef VISP_HAVE_FFMPEG
223  //*** Video streaming functions ***//
224  void initCodec();
225  void cleanUpCodec();
226 
227  void startVideoDecoding();
228  void stopVideoDecoding();
229  void computeFrame(ARCONTROLLER_Frame_t *frame);
230  //*** ***//
231 #endif
232 
233  //*** Callbacks ***//
234  static void stateChangedCallback(eARCONTROLLER_DEVICE_STATE newState, eARCONTROLLER_ERROR error, void *customData);
235 #ifdef VISP_HAVE_FFMPEG
236  static eARCONTROLLER_ERROR decoderConfigCallback(ARCONTROLLER_Stream_Codec_t codec, void *customData);
237  static eARCONTROLLER_ERROR didReceiveFrameCallback(ARCONTROLLER_Frame_t *frame, void *customData);
238 #endif
239 
240  static void cmdBatteryStateChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone);
241  static void cmdCameraOrientationChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
242  vpRobotBebop2 *drone);
243  static void cmdCameraSettingsRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone);
244  static void cmdExposureSetRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone);
245  static void cmdMaxPitchRollChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone);
246  static void cmdRelativeMoveEndedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone);
247  static void commandReceivedCallback(eARCONTROLLER_DICTIONARY_KEY commandKey,
248  ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, void *customData);
249  //*** ***//
250 };
251 
252 #endif //#ifdef VISP_HAVE_ARSDK
253 #endif //#ifndef _vpRobotBebop2_h_
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130