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 63 /*! 64 \class vpRobotBebop2 65 66 \ingroup group_robot_real_drone 67 68 Interface for Parrot ARSDK3, allowing to control the Bebop 2 drone and get images from the camera (if ViSP was built 69 with FFMpeg support). 70 */ 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 78 /** @name Drone networking information */ 79 //@{ 80 std::string getIpAddress(); 81 int getDiscoveryPort(); 82 //@} 83 84 /** @name General drone information */ 85 //@{ 86 void doFlatTrim(); 87 unsigned int getBatteryLevel(); 88 void setVerbose(bool verbose); 89 void resetAllSettings(); 90 //@} 91 92 /** @name Drone state checking */ 93 //@{ 94 bool isFlying(); 95 bool isHovering(); 96 bool isLanded(); 97 bool isRunning(); 98 bool isStreaming(); 99 //@} 100 101 //*** Motion commands ***// 102 /** @name Motion commands and parameters */ 103 //@{ 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); 116 //@} 117 static void land(); 118 //*** ***// 119 120 //*** Streaming commands ***// 121 #ifdef VISP_HAVE_FFMPEG 122 /** @name Streaming commands and parameters (only available if ViSP was built with FFMpeg support) */ 123 //@{ 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(); 134 //@} 135 #endif 136 //*** ***// 137 138 //*** Camera control commands ***// 139 /** @name Camera control commands and parameters */ 140 //@{ 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); 151 //@} 152 //*** ***// 153 154 private: 155 //*** Attributes ***// 156 std::string m_ipAddress; ///< Ip address of the drone to discover on the network 157 int m_discoveryPort; ///< Port of the drone to discover on the network 158 159 ARSAL_Sem_t m_stateSem; ///< Semaphore 160 struct sigaction m_sigAct; ///< Signal handler 161 162 #ifdef VISP_HAVE_FFMPEG 163 AVCodecContext *m_codecContext; ///< Codec context for video stream decoding 164 AVPacket m_packet; ///< Packed used to send data to the decoder 165 AVFrame *m_picture; ///< Frame used to receive data from the decoder 166 std::mutex m_bgr_picture_mutex; ///< Mutex to protect m_bgr_picture 167 AVFrame *m_bgr_picture; ///< Frame used to store rescaled frame received from the decoder 168 SwsContext *m_img_convert_ctx; ///< Used to rescale frame received from the decoder 169 uint8_t *m_buffer; ///< Buffer used to fill frame arrays 170 171 bool m_videoDecodingStarted; ///< Used to know if the drone is currently streaming and decoding its camera video feed 172 173 int m_videoWidth; ///< Width of the video streamed from the camera 174 int m_videoHeight; ///< Height of the video streamed from the camera 175 #endif 176 177 static bool m_running; ///< Used for checking if the drone is running ie if successfully connected and ready to 178 ///< receive commands 179 180 bool m_exposureSet; ///< Used to know if exposure compensation has been set 181 bool m_flatTrimFinished; ///< Used to know when the drone has finished a flat trim 182 bool m_relativeMoveEnded; ///< Used to know when the drone has ended a relative move 183 bool m_videoResolutionSet; ///< Used to know if video resolution has been set 184 bool m_streamingStarted; ///< Used to know if the streaming has been started 185 bool m_streamingModeSet; ///< Used to know if the streaming mode has been set 186 bool m_settingsReset; ///< Used to know when the drone a finished the settings reset 187 188 bool m_update_codec_params; ///< Used to know if codec parameters need to be updated 189 std::vector<uint8_t> m_codec_params_data; ///< Used to store codec parameters when they need to be updated 190 191 unsigned int m_batteryLevel; ///< Percentage of battery remaining 192 double m_maxTilt; ///< Max pitch and roll value of the drone 193 194 double m_cameraHorizontalFOV; ///< Camera horizontal FOV 195 196 double m_currentCameraTilt; ///< Current tilt of the camera 197 double m_minCameraTilt; ///< Minimum possible tilt of the camera 198 double m_maxCameraTilt; ///< Maximum possible tilt of the camera 199 200 double m_currentCameraPan; ///< Current pan of the camera 201 double m_minCameraPan; ///< Minimum possible tilt of the camera 202 double m_maxCameraPan; ///< Maximum possible tilt of the camera 203 204 static ARCONTROLLER_Device_t *m_deviceController; ///< Used for drone control 205 206 eARCONTROLLER_ERROR m_errorController; ///< Used for error handling 207 eARCONTROLLER_DEVICE_STATE m_deviceState; ///< Used to store device state 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_ 254