Visual Servoing Platform version 3.5.0
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
49extern "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
71class VISP_EXPORT vpRobotBebop2
72{
73public:
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
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
154private:
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;
179
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 column vector and the associated operations.
Definition: vpColVector.h:131
Implementation of an homogeneous matrix and operations on such kind of matrices.