Visual Servoing Platform  version 3.6.1 under development (2024-11-15)
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 #if defined(VISP_HAVE_ARSDK) && defined(VISP_HAVE_THREADS)
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 
62 BEGIN_VISP_NAMESPACE
73 class VISP_EXPORT vpRobotBebop2
74 {
75 public:
76  vpRobotBebop2(bool verbose = false, bool setDefaultSettings = true, std::string ipAddress = "192.168.42.1",
77  int discoveryPort = 44444);
78  virtual ~vpRobotBebop2();
79 
81 
82  std::string getIpAddress();
83  int getDiscoveryPort();
85 
87 
88  void doFlatTrim();
89  unsigned int getBatteryLevel();
90  void setVerbose(bool verbose);
91  void resetAllSettings();
93 
95 
96  bool isFlying();
97  bool isHovering();
98  bool isLanded();
99  bool isRunning();
100  bool isStreaming();
102 
103  //*** Motion commands ***//
105 
106  void cutMotors();
107  double getMaxTilt();
108  void setMaxTilt(double maxTilt);
109  void setPitch(int value);
110  void setPosition(float dX, float dY, float dZ, float dPsi, bool blocking);
111  void setPosition(const vpHomogeneousMatrix &M, bool blocking);
112  void setRoll(int value);
113  void setVelocity(const vpColVector &vel, double delta_t);
114  void setVerticalSpeed(int value);
115  void setYawSpeed(int value);
116  void stopMoving();
117  void takeOff(bool blocking = true);
119  static void land();
120  //*** ***//
121 
122  //*** Streaming commands ***//
123 #ifdef VISP_HAVE_FFMPEG
126  void getGrayscaleImage(vpImage<unsigned char> &I);
127  void getRGBaImage(vpImage<vpRGBa> &I);
128  int getVideoHeight();
129  int getVideoWidth();
130  void setExposure(float expo);
131  void setStreamingMode(int mode);
132  void setVideoResolution(int mode);
133  void setVideoStabilisationMode(int mode);
134  void startStreaming();
135  void stopStreaming();
137 #endif
138  //*** ***//
139 
140  //*** Camera control commands ***//
143  double getCameraHorizontalFOV() const;
144  double getCurrentCameraPan() const;
145  double getMaxCameraPan() const;
146  double getMinCameraPan() const;
147  double getCurrentCameraTilt() const;
148  double getMaxCameraTilt() const;
149  double getMinCameraTilt() const;
150  void setCameraOrientation(double tilt, double pan, bool blocking = false);
151  void setCameraPan(double pan, bool blocking = false);
152  void setCameraTilt(double tilt, bool blocking = false);
154  //*** ***//
155 
156 private:
157  //*** Attributes ***//
158  std::string m_ipAddress;
159  int m_discoveryPort;
160 
161  ARSAL_Sem_t m_stateSem;
162  struct sigaction m_sigAct;
163 
164 #ifdef VISP_HAVE_FFMPEG
165  AVCodecContext *m_codecContext;
166  AVPacket m_packet;
167  AVFrame *m_picture;
168  std::mutex m_bgr_picture_mutex;
169  AVFrame *m_bgr_picture;
170  SwsContext *m_img_convert_ctx;
171  uint8_t *m_buffer;
172 
173  bool m_videoDecodingStarted;
174 
175  int m_videoWidth;
176  int m_videoHeight;
177 #endif
178 
179  static bool m_running;
181 
182  bool m_exposureSet;
183  bool m_flatTrimFinished;
184  bool m_relativeMoveEnded;
185  bool m_videoResolutionSet;
186  bool m_streamingStarted;
187  bool m_streamingModeSet;
188  bool m_settingsReset;
189 
190  bool m_update_codec_params;
191  std::vector<uint8_t> m_codec_params_data;
192 
193  unsigned int m_batteryLevel;
194  double m_maxTilt;
195 
196  double m_cameraHorizontalFOV;
197 
198  double m_currentCameraTilt;
199  double m_minCameraTilt;
200  double m_maxCameraTilt;
201 
202  double m_currentCameraPan;
203  double m_minCameraPan;
204  double m_maxCameraPan;
205 
206  static ARCONTROLLER_Device_t *m_deviceController;
207 
208  eARCONTROLLER_ERROR m_errorController;
209  eARCONTROLLER_DEVICE_STATE m_deviceState;
210  //*** ***//
211 
212  [[noreturn]] static void sighandler(int signo);
213 
214  eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE getFlyingState();
215  eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED getStreamingState();
216 
217  //*** Setup functions ***//
218  void cleanUp();
219  ARDISCOVERY_Device_t *discoverDrone();
220  void createDroneController(ARDISCOVERY_Device_t *discoveredDrone);
221  void setupCallbacks();
222  void startController();
223 
224 #ifdef VISP_HAVE_FFMPEG
225  //*** Video streaming functions ***//
226  void initCodec();
227  void cleanUpCodec();
228 
229  void startVideoDecoding();
230  void stopVideoDecoding();
231  void computeFrame(ARCONTROLLER_Frame_t *frame);
232  //*** ***//
233 #endif
234 
235  //*** Callbacks ***//
236  static void stateChangedCallback(eARCONTROLLER_DEVICE_STATE newState, eARCONTROLLER_ERROR error, void *customData);
237 #ifdef VISP_HAVE_FFMPEG
238  static eARCONTROLLER_ERROR decoderConfigCallback(ARCONTROLLER_Stream_Codec_t codec, void *customData);
239  static eARCONTROLLER_ERROR didReceiveFrameCallback(ARCONTROLLER_Frame_t *frame, void *customData);
240 #endif
241 
242  static void cmdBatteryStateChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone);
243  static void cmdCameraOrientationChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
244  vpRobotBebop2 *drone);
245  static void cmdCameraSettingsRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone);
246  static void cmdExposureSetRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone);
247  static void cmdMaxPitchRollChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone);
248  static void cmdRelativeMoveEndedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone);
249  static void commandReceivedCallback(eARCONTROLLER_DICTIONARY_KEY commandKey,
250  ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, void *customData);
251  //*** ***//
252 };
253 END_VISP_NAMESPACE
254 #endif //#ifdef VISP_HAVE_ARSDK
255 #endif //#ifndef _vpRobotBebop2_h_
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:221