vpRobotBebop2.h

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Interface for the Irisa's Afma6 robot.
 *
 * Authors:
 * Gatien Gaumerais
 * Fabien Spindler
 *
 *****************************************************************************/

#ifndef _vpRobotBebop2_h_
#define _vpRobotBebop2_h_

#include <visp3/core/vpConfig.h>

#ifdef VISP_HAVE_ARSDK

#include <visp3/core/vpImage.h>

extern "C" {
#include <libARController/ARController.h> // For drone control
#include <libARSAL/ARSAL.h> // For semaphore

#ifdef VISP_HAVE_FFMPEG
#include <libavcodec/avcodec.h> // For H264 video decoding
#include <libswscale/swscale.h> // For rescaling decoded frames
#endif
}

#include <mutex>
#include <signal.h>
#include <string>

class VISP_EXPORT vpRobotBebop2
{
public:
  vpRobotBebop2(bool verbose = false, bool setDefaultSettings = true, std::string ipAddress = "192.168.42.1",
                int discoveryPort = 44444);
  virtual ~vpRobotBebop2();

  std::string getIpAddress();
  int getDiscoveryPort();

  void doFlatTrim();
  unsigned int getBatteryLevel();
  void setVerbose(bool verbose);
  void resetAllSettings();

  bool isFlying();
  bool isHovering();
  bool isLanded();
  bool isRunning();
  bool isStreaming();

  //*** Motion commands ***//
  void cutMotors();
  double getMaxTilt();
  void setMaxTilt(double maxTilt);
  void setPitch(int value);
  void setPosition(float dX, float dY, float dZ, float dPsi, bool blocking);
  void setPosition(const vpHomogeneousMatrix &M, bool blocking);
  void setRoll(int value);
  void setVelocity(const vpColVector &vel, double delta_t);
  void setVerticalSpeed(int value);
  void setYawSpeed(int value);
  void stopMoving();
  void takeOff(bool blocking = true);
  static void land();
  //*** ***//

  //*** Streaming commands ***//
#ifdef VISP_HAVE_FFMPEG

  void getGrayscaleImage(vpImage<unsigned char> &I);
  void getRGBaImage(vpImage<vpRGBa> &I);
  int getVideoHeight();
  int getVideoWidth();
  void setExposure(float expo);
  void setStreamingMode(int mode);
  void setVideoResolution(int mode);
  void setVideoStabilisationMode(int mode);
  void startStreaming();
  void stopStreaming();
#endif
  //*** ***//

  //*** Camera control commands ***//
  double getCameraHorizontalFOV() const;
  double getCurrentCameraPan() const;
  double getMaxCameraPan() const;
  double getMinCameraPan() const;
  double getCurrentCameraTilt() const;
  double getMaxCameraTilt() const;
  double getMinCameraTilt() const;
  void setCameraOrientation(double tilt, double pan, bool blocking = false);
  void setCameraPan(double pan, bool blocking = false);
  void setCameraTilt(double tilt, bool blocking = false);
  //*** ***//

private:
  //*** Attributes ***//
  std::string m_ipAddress; 
  int m_discoveryPort; 

  ARSAL_Sem_t m_stateSem;    
  struct sigaction m_sigAct; 

#ifdef VISP_HAVE_FFMPEG
  AVCodecContext *m_codecContext; 
  AVPacket m_packet;              
  AVFrame *m_picture;             
  std::mutex m_bgr_picture_mutex; 
  AVFrame *m_bgr_picture;         
  SwsContext *m_img_convert_ctx;  
  uint8_t *m_buffer;              

  bool m_videoDecodingStarted; 

  int m_videoWidth;  
  int m_videoHeight; 
#endif

  static bool m_running; 

  bool m_exposureSet;       
  bool m_flatTrimFinished;  
  bool m_relativeMoveEnded; 
  bool m_videoResolutionSet; 
  bool m_streamingStarted;   
  bool m_streamingModeSet;   
  bool m_settingsReset;      

  bool m_update_codec_params;               
  std::vector<uint8_t> m_codec_params_data; 

  unsigned int m_batteryLevel; 
  double m_maxTilt;            

  double m_cameraHorizontalFOV; 

  double m_currentCameraTilt; 
  double m_minCameraTilt;     
  double m_maxCameraTilt;     

  double m_currentCameraPan; 
  double m_minCameraPan;     
  double m_maxCameraPan;     

  static ARCONTROLLER_Device_t *m_deviceController; 

  eARCONTROLLER_ERROR m_errorController;    
  eARCONTROLLER_DEVICE_STATE m_deviceState; 
  //*** ***//

  [[noreturn]] static void sighandler(int signo);

  eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE getFlyingState();
  eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED getStreamingState();

  //*** Setup functions ***//
  void cleanUp();
  ARDISCOVERY_Device_t *discoverDrone();
  void createDroneController(ARDISCOVERY_Device_t *discoveredDrone);
  void setupCallbacks();
  void startController();

#ifdef VISP_HAVE_FFMPEG
  //*** Video streaming functions ***//
  void initCodec();
  void cleanUpCodec();

  void startVideoDecoding();
  void stopVideoDecoding();
  void computeFrame(ARCONTROLLER_Frame_t *frame);
  //*** ***//
#endif

  //*** Callbacks ***//
  static void stateChangedCallback(eARCONTROLLER_DEVICE_STATE newState, eARCONTROLLER_ERROR error, void *customData);
#ifdef VISP_HAVE_FFMPEG
  static eARCONTROLLER_ERROR decoderConfigCallback(ARCONTROLLER_Stream_Codec_t codec, void *customData);
  static eARCONTROLLER_ERROR didReceiveFrameCallback(ARCONTROLLER_Frame_t *frame, void *customData);
#endif

  static void cmdBatteryStateChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone);
  static void cmdCameraOrientationChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
                                             vpRobotBebop2 *drone);
  static void cmdCameraSettingsRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone);
  static void cmdExposureSetRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone);
  static void cmdMaxPitchRollChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone);
  static void cmdRelativeMoveEndedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone);
  static void commandReceivedCallback(eARCONTROLLER_DICTIONARY_KEY commandKey,
                                      ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, void *customData);
  //*** ***//
};

#endif //#ifdef VISP_HAVE_ARSDK
#endif //#ifndef _vpRobotBebop2_h_