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