Visual Servoing Platform  version 3.3.1 under development (2020-09-18)
vpRobotBebop2.cpp
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 Parrot Bebop 2 drone.
33  *
34  * Authors:
35  * Gatien Gaumerais
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
40 #include <visp3/core/vpConfig.h>
41 
42 #ifdef VISP_HAVE_ARSDK
43 
44 #include <visp3/robot/vpRobotBebop2.h>
45 
46 #include <visp3/core/vpExponentialMap.h> // For velocity computation
47 
48 #ifdef VISP_HAVE_FFMPEG
49 extern "C" {
50 #include <libavcodec/avcodec.h>
51 #include <libavformat/avformat.h>
52 #include <libavutil/imgutils.h>
53 }
54 #include <visp3/core/vpImageConvert.h>
55 #endif
56 
57 #include <iostream>
58 #include <math.h>
59 
60 #define TAG "vpRobotBebop2" // For error messages of ARSDK
61 
76 bool vpRobotBebop2::m_running = false;
77 ARCONTROLLER_Device_t *vpRobotBebop2::m_deviceController = NULL;
78 
114 vpRobotBebop2::vpRobotBebop2(bool verbose, bool setDefaultSettings, std::string ipAddress, int discoveryPort)
115  : m_ipAddress(ipAddress), m_discoveryPort(discoveryPort)
116 {
117  // Setting up signal handling
118  memset(&m_sigAct, 0, sizeof(m_sigAct));
119  m_sigAct.sa_handler = vpRobotBebop2::sighandler;
120  sigaction(SIGINT, &m_sigAct, 0);
121  sigaction(SIGBUS, &m_sigAct, 0);
122  sigaction(SIGSEGV, &m_sigAct, 0);
123  sigaction(SIGKILL, &m_sigAct, 0);
124  sigaction(SIGQUIT, &m_sigAct, 0);
125 
126 #ifdef VISP_HAVE_FFMPEG
127  m_codecContext = NULL;
128  m_packet = AVPacket();
129  m_picture = NULL;
130  m_bgr_picture = NULL;
131  m_img_convert_ctx = NULL;
132  m_buffer = NULL;
133  m_videoDecodingStarted = false;
134 #endif
135 
136  m_batteryLevel = 100;
137 
138  m_exposureSet = true;
139  m_flatTrimFinished = true;
140  m_relativeMoveEnded = true;
141  m_videoResolutionSet = true;
142  m_streamingStarted = false;
143  m_streamingModeSet = false;
144  m_settingsReset = false;
145 
146  m_update_codec_params = false;
147  m_codec_params_data = std::vector<uint8_t>();
148 
149  m_maxTilt = -1;
150 
151  m_cameraHorizontalFOV = -1;
152  m_currentCameraTilt = -1;
153  m_minCameraTilt = -1;
154  m_maxCameraTilt = -1;
155  m_currentCameraPan = -1;
156  m_minCameraPan = -1;
157  m_maxCameraPan = -1;
158 
159  setVerbose(verbose);
160 
161  m_errorController = ARCONTROLLER_OK;
162  m_deviceState = ARCONTROLLER_DEVICE_STATE_MAX;
163 
164  // Initialises a semaphore
165  ARSAL_Sem_Init(&(m_stateSem), 0, 0);
166 
167  // Creates a discovery device to find the drone on the wifi network
168  ARDISCOVERY_Device_t *discoverDevice = discoverDrone();
169 
170  // Creates a drone controller with the discovery device
171  createDroneController(discoverDevice);
172 
173  // Sets up callbacks
174  setupCallbacks();
175 
176  // Start the drone controller, connects to the drone. If an error occurs, it will set m_errorController to the error.
177  startController();
178 
179  // We check if the drone was actually found and connected to the controller
180  if ((m_errorController != ARCONTROLLER_OK) || (m_deviceState != ARCONTROLLER_DEVICE_STATE_RUNNING)) {
181  cleanUp();
182  m_running = false;
183 
185  "Failed to connect to bebop2 with ip %s and port %d. Make sure that the ip address is correct "
186  "and that your computer is connected to the drone Wifi spot before starting",
187  ipAddress.c_str(), discoveryPort));
188  } else {
189  m_running = true;
190 
191 #ifdef VISP_HAVE_FFMPEG
193 #endif
194  if (setDefaultSettings) {
196 
197  setMaxTilt(10);
198 
199 #ifdef VISP_HAVE_FFMPEG
201  setExposure(0);
202  setStreamingMode(0);
203 #endif
204  setCameraOrientation(0, 0, true);
205  }
206  }
207 }
208 
215 
223 {
224  if (isRunning() && m_deviceController != NULL && isLanded()) {
225 
226  m_flatTrimFinished = false;
227 
228  m_deviceController->aRDrone3->sendPilotingFlatTrim(m_deviceController->aRDrone3);
229 
230  // m_flatTrimFinished is set back to true when the drone has finished the calibration, via a callback
231  while (!m_flatTrimFinished) {
232  vpTime::sleepMs(1);
233  }
234  } else {
235  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't do a flat trim : drone isn't landed.");
236  }
237 }
238 
243 std::string vpRobotBebop2::getIpAddress() { return m_ipAddress; }
244 
249 int vpRobotBebop2::getDiscoveryPort() { return m_discoveryPort; }
250 
255 double vpRobotBebop2::getMaxTilt() { return m_maxTilt; }
256 
263 unsigned int vpRobotBebop2::getBatteryLevel() { return m_batteryLevel; }
264 
269 double vpRobotBebop2::getCameraHorizontalFOV() const { return m_cameraHorizontalFOV; }
270 
275 double vpRobotBebop2::getCurrentCameraTilt() const { return m_currentCameraTilt; }
276 
281 double vpRobotBebop2::getMinCameraTilt() const { return m_minCameraTilt; }
282 
287 double vpRobotBebop2::getMaxCameraTilt() const { return m_maxCameraTilt; }
288 
293 double vpRobotBebop2::getCurrentCameraPan() const { return m_currentCameraPan; }
294 
299 double vpRobotBebop2::getMinCameraPan() const { return m_minCameraPan; }
300 
305 double vpRobotBebop2::getMaxCameraPan() const { return m_maxCameraPan; }
306 
319 void vpRobotBebop2::setCameraOrientation(double tilt, double pan, bool blocking)
320 {
321  if (isRunning() && m_deviceController != NULL) {
322 
323  m_deviceController->aRDrone3->sendCameraOrientationV2(m_deviceController->aRDrone3, static_cast<float>(tilt),
324  static_cast<float>(pan));
325 
326  if (blocking) {
327  while (std::abs(tilt - m_currentCameraTilt) > 0.01 || std::abs(pan - m_currentCameraPan) > 0.01) {
328  vpTime::sleepMs(1);
329  }
330  }
331 
332  } else {
333  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set camera orientation : drone isn't running.");
334  }
335 }
336 
348 void vpRobotBebop2::setCameraTilt(double tilt, bool blocking)
349 {
350  if (isRunning() && m_deviceController != NULL) {
351 
352  m_deviceController->aRDrone3->sendCameraOrientationV2(m_deviceController->aRDrone3, static_cast<float>(tilt),
353  static_cast<float>(getCurrentCameraPan()));
354 
355  if (blocking) {
356  while (std::abs(tilt - m_currentCameraTilt) > 0.01) {
357  vpTime::sleepMs(1);
358  }
359  }
360 
361  } else {
362  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set camera tilt value : drone isn't running.");
363  }
364 }
365 
377 void vpRobotBebop2::setCameraPan(double pan, bool blocking)
378 {
379  if (isRunning() && m_deviceController != NULL) {
380 
381  m_deviceController->aRDrone3->sendCameraOrientationV2(
382  m_deviceController->aRDrone3, static_cast<float>(getCurrentCameraTilt()), static_cast<float>(pan));
383 
384  if (blocking) {
385  while (std::abs(pan - m_currentCameraPan) > 0.01) {
386  vpTime::sleepMs(1);
387  }
388  }
389 
390  } else {
391  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set camera pan value : drone isn't running.");
392  }
393 }
394 
400 {
401  if (m_deviceController == NULL) {
402  return false;
403  } else {
404  return m_running;
405  }
406 }
407 
413 {
414 #ifdef VISP_HAVE_FFMPEG
415  return m_videoDecodingStarted;
416 #else
417  return false;
418 #endif
419 }
420 
426 {
427  return getFlyingState() == ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_HOVERING;
428 }
429 
435 {
436  return getFlyingState() == ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_FLYING;
437 }
438 
444 {
445  return getFlyingState() == ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_LANDED;
446 }
447 
455 void vpRobotBebop2::takeOff(bool blocking)
456 {
457  if (isRunning() && isLanded() && m_deviceController != NULL) {
458 
459  m_deviceController->aRDrone3->sendPilotingTakeOff(m_deviceController->aRDrone3);
460 
461  if (blocking) {
462  while (!isHovering()) {
463  vpTime::sleepMs(1);
464  }
465  }
466 
467  } else {
468  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't take off : drone isn't landed.");
469  }
470 }
471 
479 {
480  if (m_deviceController != NULL) {
481  m_deviceController->aRDrone3->sendPilotingLanding(m_deviceController->aRDrone3);
482  }
483 }
484 
496 {
497  if (isRunning() && m_deviceController != NULL && (isFlying() || isHovering())) {
498  m_errorController =
499  m_deviceController->aRDrone3->setPilotingPCMDGaz(m_deviceController->aRDrone3, static_cast<char>(value));
500 
501  if (m_errorController != ARCONTROLLER_OK) {
502  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error when sending move command : %s",
503  ARCONTROLLER_Error_ToString(m_errorController));
504  }
505 
506  } else {
507  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set vertical speed : drone isn't flying or hovering.");
508  }
509 }
510 
522 {
523  if (isRunning() && m_deviceController != NULL && (isFlying() || isHovering())) {
524 
525  m_errorController =
526  m_deviceController->aRDrone3->setPilotingPCMDYaw(m_deviceController->aRDrone3, static_cast<char>(value));
527 
528  if (m_errorController != ARCONTROLLER_OK) {
529  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error when sending move command : %s",
530  ARCONTROLLER_Error_ToString(m_errorController));
531  }
532 
533  } else {
534  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set yaw speed : drone isn't flying or hovering.");
535  }
536 }
537 
548 void vpRobotBebop2::setPitch(int value)
549 {
550  if (isRunning() && m_deviceController != NULL && (isFlying() || isHovering())) {
551 
552  m_errorController =
553  m_deviceController->aRDrone3->setPilotingPCMDPitch(m_deviceController->aRDrone3, static_cast<char>(value));
554  m_errorController = m_deviceController->aRDrone3->setPilotingPCMDFlag(m_deviceController->aRDrone3, 1);
555 
556  if (m_errorController != ARCONTROLLER_OK) {
557  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error when sending move command : %s",
558  ARCONTROLLER_Error_ToString(m_errorController));
559  }
560 
561  } else {
562  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set pitch value : drone isn't flying or hovering.");
563  }
564 }
565 
576 void vpRobotBebop2::setRoll(int value)
577 {
578  if (isRunning() && m_deviceController != NULL && (isFlying() || isHovering())) {
579 
580  m_errorController =
581  m_deviceController->aRDrone3->setPilotingPCMDRoll(m_deviceController->aRDrone3, static_cast<char>(value));
582  m_errorController = m_deviceController->aRDrone3->setPilotingPCMDFlag(m_deviceController->aRDrone3, 1);
583 
584  if (m_errorController != ARCONTROLLER_OK) {
585  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error when sending move command : %s",
586  ARCONTROLLER_Error_ToString(m_errorController));
587  }
588 
589  } else {
590  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set roll value : drone isn't flying or hovering.");
591  }
592 }
593 
601 {
602  if (m_deviceController != NULL) {
603  m_errorController = m_deviceController->aRDrone3->sendPilotingEmergency(m_deviceController->aRDrone3);
604  }
605 }
606 
622 void vpRobotBebop2::setPosition(float dX, float dY, float dZ, float dPsi, bool blocking)
623 {
624  if (isRunning() && m_deviceController != NULL && (isFlying() || isHovering())) {
625 
626  m_relativeMoveEnded = false;
627  m_deviceController->aRDrone3->sendPilotingMoveBy(m_deviceController->aRDrone3, dX, dY, dZ, dPsi);
628 
629  if (blocking) {
630 
631  // m_relativeMoveEnded is set back to true when the drone has finished his move, via a callback
632  while (!m_relativeMoveEnded) {
633  vpTime::sleepMs(1);
634  }
635  }
636  } else {
637  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't move : drone isn't flying or hovering.");
638  }
639 }
640 
654 void vpRobotBebop2::setPosition(const vpHomogeneousMatrix &M, bool blocking)
655 {
656  double epsilon = (std::numeric_limits<double>::epsilon());
657  if (std::abs(M.getRotationMatrix().getThetaUVector()[0]) >= epsilon) {
658  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't move : rotation around X axis should be 0.");
659  return;
660  }
661  if (std::abs(M.getRotationMatrix().getThetaUVector()[1]) >= epsilon) {
662  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't move : rotation around Y axis should be 0.");
663  return;
664  }
665  float dThetaZ = static_cast<float>(M.getRotationMatrix().getThetaUVector()[2]);
667  setPosition(static_cast<float>(t[0]), static_cast<float>(t[1]), static_cast<float>(t[2]), dThetaZ, blocking);
668 }
669 
681 void vpRobotBebop2::setVelocity(const vpColVector &vel_cmd, double delta_t)
682 {
683 
684  if (vel_cmd.size() != 4) {
685  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR",
686  "Can't set velocity : dimension of the velocity vector should be equal to 4.");
687  stopMoving();
688  return;
689  }
690 
691  vpColVector ve(6);
692 
693  ve[0] = vel_cmd[0];
694  ve[1] = vel_cmd[1];
695  ve[2] = vel_cmd[2];
696  ve[5] = vel_cmd[3];
697 
699  setPosition(M, false);
700 }
701 
710 void vpRobotBebop2::setVerbose(bool verbose)
711 {
712  if (verbose) {
713  ARSAL_Print_SetMinimumLevel(ARSAL_PRINT_INFO);
714  } else {
715  ARSAL_Print_SetMinimumLevel(ARSAL_PRINT_WARNING);
716  }
717 }
718 
724 {
725  if (isRunning() && m_deviceController != NULL) {
726 
727  m_settingsReset = false;
728  m_deviceController->common->sendSettingsReset(m_deviceController->common);
729 
730  while (!m_settingsReset) {
731  vpTime::sleepMs(1);
732  }
733 
734  } else {
735  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't reset drone settings : drone isn't running.");
736  }
737 }
738 
750 void vpRobotBebop2::setMaxTilt(double maxTilt)
751 {
752  if (isRunning() && m_deviceController != NULL) {
753  m_deviceController->aRDrone3->sendPilotingSettingsMaxTilt(m_deviceController->aRDrone3,
754  static_cast<float>(maxTilt));
755  } else {
756  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set tilt value : drone isn't running.");
757  }
758 }
759 
768 {
769  if (isRunning() && !isLanded() && m_deviceController != NULL) {
770  m_errorController = m_deviceController->aRDrone3->setPilotingPCMD(m_deviceController->aRDrone3, 0, 0, 0, 0, 0, 0);
771  }
772 }
773 
774 //*** ***//
775 //*** Streaming functions ***//
776 //*** ***//
777 
778 #ifdef VISP_HAVE_FFMPEG // Functions related to video streaming and decoding requiers FFmpeg
779 
789 {
790  if (m_videoDecodingStarted) {
791 
792  if (m_bgr_picture->data[0] != NULL) {
793  I.resize(static_cast<unsigned int>(m_videoHeight), static_cast<unsigned int>(m_videoWidth));
794 
795  m_bgr_picture_mutex.lock();
796  vpImageConvert::BGRToGrey(m_bgr_picture->data[0], reinterpret_cast<unsigned char *>(I.bitmap), I.getWidth(),
797  I.getHeight());
798  m_bgr_picture_mutex.unlock();
799  } else {
800  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Error while getting current grayscale image : image data is NULL");
801  }
802 
803  } else {
804  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't get current image : video streaming isn't started.");
805  }
806 }
807 
817 {
818  if (m_videoDecodingStarted) {
819 
820  if (m_bgr_picture->data[0] != NULL) {
821  I.resize(static_cast<unsigned int>(m_videoHeight), static_cast<unsigned int>(m_videoWidth));
822 
823  m_bgr_picture_mutex.lock();
824  vpImageConvert::BGRToRGBa(m_bgr_picture->data[0], reinterpret_cast<unsigned char *>(I.bitmap), I.getWidth(),
825  I.getHeight());
826  m_bgr_picture_mutex.unlock();
827  } else {
828  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Error while getting current RGBa image : image data is NULL");
829  }
830 
831  } else {
832  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't get current image : video streaming isn't started.");
833  }
834 }
835 
841 int vpRobotBebop2::getVideoHeight() { return m_videoHeight; }
842 
848 int vpRobotBebop2::getVideoWidth() { return m_videoWidth; }
849 
858 {
859  if (isRunning() && m_deviceController != NULL) {
860  expo = std::min(1.5f, std::max(-1.5f, expo));
861 
862  m_exposureSet = false;
863  m_deviceController->aRDrone3->sendPictureSettingsExpositionSelection(m_deviceController->aRDrone3, expo);
864 
865  // m_exposureSet is set back to true when the drone has finished his move, via a callback
866  while (!m_exposureSet) {
867  vpTime::sleepMs(1);
868  }
869  } else {
870  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set exposure : drone isn't running.");
871  }
872 }
873 
889 {
890  if (isRunning() && m_deviceController != NULL) {
891 
892  if (!isStreaming() && isLanded()) {
893  eARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE cmd_mode =
894  ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_LOW_LATENCY;
895  switch (mode) {
896  case 0:
897  cmd_mode = ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_LOW_LATENCY;
898  break;
899  case 1:
900  cmd_mode = ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_HIGH_RELIABILITY;
901  break;
902  case 2:
903  cmd_mode = ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_HIGH_RELIABILITY_LOW_FRAMERATE;
904  break;
905  default:
906  break;
907  }
908  m_streamingModeSet = false;
909  m_deviceController->aRDrone3->sendMediaStreamingVideoStreamMode(m_deviceController->aRDrone3, cmd_mode);
910 
911  // m_streamingModeSet is set back to true when the drone has finished setting the stream mode, via a callback
912  while (!m_streamingModeSet) {
913  vpTime::sleepMs(1);
914  }
915 
916  } else {
917  ARSAL_PRINT(
918  ARSAL_PRINT_ERROR, "ERROR",
919  "Can't set streaming mode : drone has to be landed and not streaming in order to set streaming mode.");
920  }
921  } else {
922  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set streaming mode : drone isn't running.");
923  }
924 }
925 
938 {
939  if (isRunning() && m_deviceController != NULL) {
940 
941  if (!isStreaming() && isLanded()) {
942 
943  eARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORESOLUTIONS_TYPE cmd_mode;
944 
945  switch (mode) {
946 
947  case 0:
948  default:
949  cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORESOLUTIONS_TYPE_REC1080_STREAM480;
950  m_videoWidth = 856;
951  m_videoHeight = 480;
952  break;
953 
954  case 1:
955  cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORESOLUTIONS_TYPE_REC720_STREAM720;
956  m_videoWidth = 1280;
957  m_videoHeight = 720;
958  break;
959  }
960 
961  m_videoResolutionSet = false;
962  m_deviceController->aRDrone3->sendPictureSettingsVideoResolutions(m_deviceController->aRDrone3, cmd_mode);
963 
964  // m_videoResolutionSet is set back to true when the drone has finished setting the resolution, via a callback
965  while (!m_videoResolutionSet) {
966  vpTime::sleepMs(1);
967  }
968 
969  } else {
970  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR",
971  "Can't set video resolution : drone has to be landed and not streaming in order to set streaming "
972  "parameters.");
973  }
974  } else {
975  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set video resolution : drone isn't running.");
976  }
977 }
978 
991 {
992  if (isRunning() && m_deviceController != NULL) {
993 
994  eARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE cmd_mode =
995  ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_NONE;
996  switch (mode) {
997 
998  case 0:
999  cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_NONE;
1000  break;
1001  case 1:
1002  cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_ROLL;
1003  break;
1004  case 2:
1005  cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_PITCH;
1006  break;
1007  case 3:
1008  cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_ROLL_PITCH;
1009  break;
1010 
1011  default:
1012  break;
1013  }
1014  m_deviceController->aRDrone3->sendPictureSettingsVideoStabilizationMode(m_deviceController->aRDrone3, cmd_mode);
1015 
1016  } else {
1017  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set video stabilisation mode : drone isn't running.");
1018  }
1019 }
1020 
1030 {
1031  if (isRunning() && m_deviceController != NULL) {
1032  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Starting video streaming ... ");
1033 
1034  // Sending command to the drone to start the video stream
1035  m_errorController = m_deviceController->aRDrone3->sendMediaStreamingVideoEnable(m_deviceController->aRDrone3, 1);
1036 
1037  if (m_errorController == ARCONTROLLER_OK) {
1038  m_streamingStarted = false;
1039  // Blocking until streaming is started
1040  while (!m_streamingStarted) {
1041  vpTime::sleepMs(1);
1042  }
1043  startVideoDecoding();
1044 
1045  /* We wait for the streaming to actually start (it has a delay before actually
1046  sending frames, even if it is indicated as started by the drone), or else the
1047  decoder is doesn't have time to synchronize with the stream */
1048  vpTime::sleepMs(4000);
1049 
1050  } else {
1051  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1052  }
1053 
1054  } else {
1055  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't start streaming : drone isn't running.");
1056  }
1057 }
1058 
1065 {
1066  if (m_videoDecodingStarted && m_deviceController != NULL) {
1067  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Stopping video streaming ... ");
1068 
1069  // Sending command to the drone to stop the video stream
1070  m_errorController = m_deviceController->aRDrone3->sendMediaStreamingVideoEnable(m_deviceController->aRDrone3, 0);
1071 
1072  if (m_errorController == ARCONTROLLER_OK) {
1073 
1074  // Blocking until streaming is stopped
1075  while (getStreamingState() != ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_DISABLED) {
1076  vpTime::sleepMs(1);
1077  }
1078  vpTime::sleepMs(500); // We wait for the streaming to actually stops or else it causes segmentation fault.
1079  stopVideoDecoding();
1080 
1081  } else {
1082  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1083  }
1084 
1085  } else {
1086  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't stop streaming : streaming already stopped.");
1087  }
1088 }
1089 
1090 #endif // #ifdef VISP_HAVE_FFMPEG
1091 
1092 //*** ***//
1093 //*** Private Functions ***//
1094 //*** ***//
1095 
1100 void vpRobotBebop2::sighandler(int signo)
1101 {
1102  std::cout << "Stopping Bebop2 because of detected signal (" << signo << "): " << static_cast<char>(7);
1103  switch (signo) {
1104  case SIGINT:
1105  std::cout << "SIGINT (stopped by ^C) " << std::endl;
1106  break;
1107  case SIGBUS:
1108  std::cout << "SIGBUS (stopped due to a bus error) " << std::endl;
1109  break;
1110  case SIGSEGV:
1111  std::cout << "SIGSEGV (stopped due to a segmentation fault) " << std::endl;
1112  break;
1113  case SIGKILL:
1114  std::cout << "SIGKILL (stopped by CTRL \\) " << std::endl;
1115  break;
1116  case SIGQUIT:
1117  std::cout << "SIGQUIT " << std::endl;
1118  break;
1119  default:
1120  std::cout << signo << std::endl;
1121  }
1122 
1123  vpRobotBebop2::m_running = false;
1124 
1125  // Landing the drone
1126  if (m_deviceController != NULL) {
1127  m_deviceController->aRDrone3->sendPilotingLanding(m_deviceController->aRDrone3);
1128  }
1129  std::exit(EXIT_FAILURE);
1130 }
1131 
1136 eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE vpRobotBebop2::getFlyingState()
1137 {
1138  if (m_deviceController != NULL) {
1139  eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE flyingState =
1140  ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_MAX;
1141  eARCONTROLLER_ERROR error;
1142 
1143  ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary = ARCONTROLLER_ARDrone3_GetCommandElements(
1144  m_deviceController->aRDrone3, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED, &error);
1145 
1146  if (error == ARCONTROLLER_OK && elementDictionary != NULL) {
1147  ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1148  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1149 
1150  HASH_FIND_STR (elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1151 
1152  if (element != NULL)
1153  {
1154  //Suppress warnings
1155  // Get the value
1156  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE, arg);
1157 
1158  if (arg != NULL)
1159  {
1160  // Enums are stored as I32
1161  flyingState = static_cast<eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE>(arg->value.I32);
1162  }
1163  }
1164  }
1165  return flyingState;
1166  } else {
1167  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error when checking flying state : drone isn't connected.");
1168  return ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_MAX;
1169  }
1170 }
1171 
1176 eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED vpRobotBebop2::getStreamingState()
1177 {
1178  if (m_deviceController != NULL) {
1179  eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED streamingState =
1180  ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_MAX;
1181  eARCONTROLLER_ERROR error;
1182 
1183  ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary = ARCONTROLLER_ARDrone3_GetCommandElements(
1184  m_deviceController->aRDrone3, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED,
1185  &error);
1186 
1187  if (error == ARCONTROLLER_OK && elementDictionary != NULL) {
1188  ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1189  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1190 
1191  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1192 
1193  if (element != NULL) {
1194  // Get the value
1195  HASH_FIND_STR(element->arguments,
1196  ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED, arg);
1197 
1198  if (arg != NULL) {
1199  // Enums are stored as I32
1200  streamingState =
1201  static_cast<eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED>(arg->value.I32);
1202  }
1203  }
1204  }
1205  return streamingState;
1206  } else {
1207  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error when checking streaming state : drone isn't connected.");
1208  return ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_MAX;
1209  }
1210 }
1211 
1216 ARDISCOVERY_Device_t * vpRobotBebop2::discoverDrone()
1217 {
1218  eARDISCOVERY_ERROR errorDiscovery = ARDISCOVERY_OK;
1219 
1220  ARDISCOVERY_Device_t * device = ARDISCOVERY_Device_New(&errorDiscovery);
1221 
1222  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Starting drone Wifi discovery ...");
1223  const char * charIpAddress = m_ipAddress.c_str();
1224  errorDiscovery = ARDISCOVERY_Device_InitWifi(device, ARDISCOVERY_PRODUCT_BEBOP_2, "bebop2", charIpAddress, m_discoveryPort);
1225 
1226  if (errorDiscovery != ARDISCOVERY_OK)
1227  {
1228  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Discovery error :%s", ARDISCOVERY_Error_ToString(errorDiscovery));
1229  }
1230  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Drone controller created.");
1231  return device;
1232 }
1233 
1240 void vpRobotBebop2::createDroneController(ARDISCOVERY_Device_t * discoveredDrone)
1241 {
1242  m_deviceController = ARCONTROLLER_Device_New (discoveredDrone, &m_errorController);
1243  if (m_errorController != ARCONTROLLER_OK)
1244  {
1245  ARSAL_PRINT (ARSAL_PRINT_ERROR, TAG, "Creation of deviceController failed.");
1246  }
1247  ARDISCOVERY_Device_Delete (&discoveredDrone);
1248  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Device created.");
1249 }
1250 
1255 void vpRobotBebop2::setupCallbacks()
1256 {
1257  //Adding stateChanged callback, called when the state of the controller has changed
1258  m_errorController = ARCONTROLLER_Device_AddStateChangedCallback(m_deviceController, stateChangedCallback, this);
1259  if(m_errorController != ARCONTROLLER_OK)
1260  {
1261  ARSAL_PRINT (ARSAL_PRINT_ERROR, TAG, "add State callback failed.");
1262  }
1263 
1264  //Adding commendReceived callback, called when the a command has been received from the device
1265  m_errorController = ARCONTROLLER_Device_AddCommandReceivedCallback(m_deviceController, commandReceivedCallback, this);
1266 
1267  if(m_errorController != ARCONTROLLER_OK)
1268  {
1269  ARSAL_PRINT (ARSAL_PRINT_ERROR, TAG, "add Command callback failed.");
1270  }
1271 
1272 #ifdef VISP_HAVE_FFMPEG
1273  //Adding frame received callback, called when a streaming frame has been received from the device
1274  m_errorController = ARCONTROLLER_Device_SetVideoStreamCallbacks (m_deviceController, decoderConfigCallback, didReceiveFrameCallback, NULL , this);
1275 
1276  if(m_errorController != ARCONTROLLER_OK)
1277  {
1278  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error: %s", ARCONTROLLER_Error_ToString(m_errorController));
1279  }
1280 #endif
1281  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Callbacks set up.");
1282 }
1283 
1288 void vpRobotBebop2::startController()
1289 {
1290  //Starts the controller
1291  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Connecting ...");
1292  m_errorController = ARCONTROLLER_Device_Start (m_deviceController);
1293 
1294  if(m_errorController != ARCONTROLLER_OK)
1295  {
1296  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1297  }
1298 
1299  // Waits for the stateChangedCallback to unclock the semaphore
1300  ARSAL_Sem_Wait (&(m_stateSem));
1301 
1302  //Checks the device state
1303  m_deviceState = ARCONTROLLER_Device_GetState (m_deviceController, &m_errorController);
1304 
1305  if((m_errorController != ARCONTROLLER_OK) || (m_deviceState != ARCONTROLLER_DEVICE_STATE_RUNNING))
1306  {
1307  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- deviceState :%d", m_deviceState);
1308  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1309  }
1310  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Controller started.");
1311 }
1312 
1313 #ifdef VISP_HAVE_FFMPEG
1314 
1319 void vpRobotBebop2::initCodec()
1320 {
1321  av_register_all();
1322  avcodec_register_all();
1323  avformat_network_init();
1324 
1325  // Finds the correct codec
1326  AVCodec *codec = avcodec_find_decoder(AV_CODEC_ID_H264);
1327  if (!codec) {
1328  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Codec not found.");
1329  return;
1330  }
1331 
1332  // Allocates memory for codec
1333  m_codecContext = avcodec_alloc_context3(codec);
1334 
1335  if (!m_codecContext) {
1336  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Failed to allocate codec context.");
1337  return;
1338  }
1339 
1340  // Sets codec parameters (TODO : should be done automaticaly by drone callback decoderConfigCallback)
1341  m_codecContext->pix_fmt = AV_PIX_FMT_YUV420P;
1342  m_codecContext->skip_frame = AVDISCARD_DEFAULT;
1343  m_codecContext->error_concealment = FF_EC_GUESS_MVS | FF_EC_DEBLOCK;
1344  m_codecContext->skip_loop_filter = AVDISCARD_DEFAULT;
1345  m_codecContext->workaround_bugs = AVMEDIA_TYPE_VIDEO;
1346  m_codecContext->codec_id = AV_CODEC_ID_H264;
1347  m_codecContext->skip_idct = AVDISCARD_DEFAULT;
1348 
1349  m_codecContext->width = m_videoWidth;
1350  m_codecContext->height = m_videoHeight;
1351 
1352  if (codec->capabilities & AV_CODEC_CAP_TRUNCATED) {
1353  m_codecContext->flags |= AV_CODEC_FLAG_TRUNCATED;
1354  }
1355  m_codecContext->flags2 |= AV_CODEC_FLAG2_CHUNKS;
1356 
1357  // Opens the codec
1358  if (avcodec_open2(m_codecContext, codec, NULL) < 0) {
1359  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Failed to open codec.");
1360  return;
1361  }
1362 
1363  AVPixelFormat pFormat = AV_PIX_FMT_BGR24;
1364  int numBytes = av_image_get_buffer_size(pFormat, m_codecContext->width, m_codecContext->height, 1);
1365  m_buffer = static_cast<uint8_t *>(av_malloc(static_cast<unsigned long>(numBytes) * sizeof(uint8_t)));
1366 
1367  av_init_packet(&m_packet); // Packed used to send data to the decoder
1368  m_picture = av_frame_alloc(); // Frame used to receive data from the decoder
1369 
1370  m_bgr_picture_mutex.lock();
1371  m_bgr_picture = av_frame_alloc(); // Frame used to store rescaled frame received from the decoder
1372  m_bgr_picture_mutex.unlock();
1373 
1374  m_img_convert_ctx = sws_getContext(m_codecContext->width, m_codecContext->height, m_codecContext->pix_fmt,
1375  m_codecContext->width, m_codecContext->height, pFormat, SWS_BICUBIC, NULL, NULL,
1376  NULL); // Used to rescale frame received from the decoder
1377 }
1378 
1384 void vpRobotBebop2::cleanUpCodec()
1385 {
1386  m_videoDecodingStarted = false;
1387  av_packet_unref(&m_packet);
1388 
1389  if (m_codecContext) {
1390  avcodec_flush_buffers(m_codecContext);
1391  avcodec_free_context(&m_codecContext);
1392  }
1393 
1394  if (m_picture) {
1395  av_frame_free(&m_picture);
1396  }
1397 
1398  if (m_bgr_picture) {
1399  m_bgr_picture_mutex.lock();
1400  av_frame_free(&m_bgr_picture);
1401  m_bgr_picture_mutex.unlock();
1402  }
1403 
1404  if (m_img_convert_ctx) {
1405  sws_freeContext(m_img_convert_ctx);
1406  }
1407  if (m_buffer) {
1408  av_free(m_buffer);
1409  }
1410 }
1411 
1417 void vpRobotBebop2::startVideoDecoding()
1418 {
1419  if (!m_videoDecodingStarted) {
1420  initCodec();
1421  m_videoDecodingStarted = true;
1422  } else {
1423  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Video decoding is already started.");
1424  }
1425 }
1426 
1432 void vpRobotBebop2::stopVideoDecoding()
1433 {
1434  if (m_videoDecodingStarted) {
1435  cleanUpCodec();
1436  } else {
1437  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Video decoding is already stopped.");
1438  }
1439 }
1440 
1448 void vpRobotBebop2::computeFrame(ARCONTROLLER_Frame_t *frame)
1449 {
1450 
1451  // Updating codec parameters from SPS and PPS buffers received from the drone in decoderConfigCallback
1452  if (m_update_codec_params && m_codec_params_data.size()) {
1453  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Updating H264 codec parameters (Buffer Size: %lu) ...",
1454  m_codec_params_data.size());
1455 
1456  m_packet.data = &m_codec_params_data[0];
1457  m_packet.size = static_cast<int>(m_codec_params_data.size());
1458 
1459  int ret = avcodec_send_packet(m_codecContext, &m_packet);
1460 
1461  if (ret == 0) {
1462 
1463  ret = avcodec_receive_frame(m_codecContext, m_picture);
1464 
1465  if (ret == 0 || ret == AVERROR(EAGAIN)) {
1466  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "H264 codec parameters updated.");
1467  } else {
1468  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Unexpected error while updating H264 parameters.");
1469  }
1470  } else {
1471  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Unexpected error while sending H264 parameters.");
1472  }
1473  m_update_codec_params = false;
1474  av_packet_unref(&m_packet);
1475  av_frame_unref(m_picture);
1476  }
1477 
1478  // Decoding frame comming from the drone
1479  m_packet.data = frame->data;
1480  m_packet.size = static_cast<int>(frame->used);
1481 
1482  int ret = avcodec_send_packet(m_codecContext, &m_packet);
1483  if (ret < 0) {
1484 
1485  char *errbuff = new char[AV_ERROR_MAX_STRING_SIZE];
1486  av_strerror(ret, errbuff, AV_ERROR_MAX_STRING_SIZE);
1487  std::string err(errbuff);
1488  delete[] errbuff;
1489  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error sending a packet for decoding : %d, %s", ret, err.c_str());
1490 
1491  } else {
1492 
1493  ret = avcodec_receive_frame(m_codecContext, m_picture);
1494 
1495  if (ret < 0) {
1496 
1497  if (ret == AVERROR(EAGAIN)) {
1498  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "AVERROR(EAGAIN)"); // Not an error, need to send data again
1499  } else if (ret == AVERROR_EOF) {
1500  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "AVERROR_EOF"); // End of file reached, should not happen with drone footage
1501  } else {
1502 
1503  char *errbuff = new char[AV_ERROR_MAX_STRING_SIZE];
1504  av_strerror(ret, errbuff, AV_ERROR_MAX_STRING_SIZE);
1505  std::string err(errbuff);
1506  delete[] errbuff;
1507  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error receiving a decoded frame : %d, %s", ret, err.c_str());
1508  }
1509  } else {
1510  m_bgr_picture_mutex.lock();
1511  av_frame_unref(m_bgr_picture);
1512  av_image_fill_arrays(m_bgr_picture->data, m_bgr_picture->linesize, m_buffer, AV_PIX_FMT_BGR24,
1513  m_codecContext->width, m_codecContext->height, 1);
1514 
1515  sws_scale(m_img_convert_ctx, (m_picture)->data, (m_picture)->linesize, 0, m_codecContext->height,
1516  (m_bgr_picture)->data, (m_bgr_picture)->linesize);
1517 
1518  m_bgr_picture_mutex.unlock();
1519  }
1520  }
1521 
1522  av_packet_unref(&m_packet);
1523 
1524  av_frame_unref(m_picture);
1525 }
1526 #endif // #ifdef VISP_HAVE_FFMPEG
1527 
1532 void vpRobotBebop2::cleanUp()
1533 {
1534  if (m_deviceController != NULL) {
1535  // Lands the drone if not landed
1536  land();
1537 
1538 #ifdef VISP_HAVE_FFMPEG
1539  // Stops the streaming if not stopped
1540  stopStreaming();
1541 #endif
1542 
1543  // Deletes the controller
1544  m_deviceState = ARCONTROLLER_Device_GetState(m_deviceController, &m_errorController);
1545  if ((m_errorController == ARCONTROLLER_OK) && (m_deviceState != ARCONTROLLER_DEVICE_STATE_STOPPED)) {
1546 
1547  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Disconnecting ...");
1548  m_errorController = ARCONTROLLER_Device_Stop(m_deviceController);
1549 
1550  if (m_errorController == ARCONTROLLER_OK) {
1551  // Wait for the semaphore to increment, it will when the controller changes its state to 'stopped'
1552  ARSAL_Sem_Wait(&(m_stateSem));
1553  }
1554  }
1555  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Deleting device controller ...");
1556  ARCONTROLLER_Device_Delete(&m_deviceController);
1557 
1558  // Destroys the semaphore
1559  ARSAL_Sem_Destroy(&(m_stateSem));
1560 
1561  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Cleanup done.");
1562  } else {
1563 
1564  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error while cleaning up memory.");
1565  }
1566  m_running = false;
1567 }
1568 
1569 //*** ***//
1570 //*** Callbacks ***//
1571 //*** ***//
1572 
1581 void vpRobotBebop2::stateChangedCallback(eARCONTROLLER_DEVICE_STATE newState, eARCONTROLLER_ERROR error,
1582  void *customData)
1583 {
1584  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Controller state changed, new state: %d.", newState);
1585  (void)error;
1586 
1587  vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1588  switch (newState)
1589  {
1590  case ARCONTROLLER_DEVICE_STATE_STOPPED:
1591  // Stopping the programm
1592  drone->m_running = false;
1593  // Incrementing semaphore
1594  ARSAL_Sem_Post(&(drone->m_stateSem));
1595  break;
1596 
1597  case ARCONTROLLER_DEVICE_STATE_RUNNING:
1598  // Incrementing semaphore
1599  ARSAL_Sem_Post(&(drone->m_stateSem));
1600  break;
1601 
1602  default:
1603  break;
1604  }
1605 }
1606 
1607 #ifdef VISP_HAVE_FFMPEG
1608 
1616 eARCONTROLLER_ERROR vpRobotBebop2::decoderConfigCallback(ARCONTROLLER_Stream_Codec_t codec, void * customData)
1617 {
1618  vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1619 
1620  uint8_t *sps_buffer_ptr = codec.parameters.h264parameters.spsBuffer;
1621  uint32_t sps_buffer_size = static_cast<uint32_t>(codec.parameters.h264parameters.spsSize);
1622  uint8_t *pps_buffer_ptr = codec.parameters.h264parameters.ppsBuffer;
1623  uint32_t pps_buffer_size = static_cast<uint32_t>(codec.parameters.h264parameters.ppsSize);
1624 
1625  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "H264 configuration packet received: #SPS: %d #PPS: %d", sps_buffer_size,
1626  pps_buffer_size);
1627 
1628  drone->m_update_codec_params = (sps_buffer_ptr && pps_buffer_ptr && sps_buffer_size && pps_buffer_size &&
1629  (pps_buffer_size < 32) && (sps_buffer_size < 32));
1630 
1631  if (drone->m_update_codec_params) {
1632  // If codec parameters where received from the drone, we store them to pass them to the decoder in the next call of
1633  // computeFrame
1634  drone->m_codec_params_data.resize(sps_buffer_size + pps_buffer_size);
1635  std::copy(sps_buffer_ptr, sps_buffer_ptr + sps_buffer_size, drone->m_codec_params_data.begin());
1636  std::copy(pps_buffer_ptr, pps_buffer_ptr + pps_buffer_size, drone->m_codec_params_data.begin() + sps_buffer_size);
1637  } else {
1638  // If data is invalid, we clear the vector
1639  drone->m_codec_params_data.clear();
1640  }
1641  return ARCONTROLLER_OK;
1642 }
1643 
1652 eARCONTROLLER_ERROR vpRobotBebop2::didReceiveFrameCallback(ARCONTROLLER_Frame_t *frame, void *customData)
1653 {
1654  vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1655 
1656  if (frame != NULL) {
1657 
1658  if (drone->m_videoDecodingStarted) {
1659  drone->computeFrame(frame);
1660  }
1661 
1662 
1663  } else {
1664  ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, "frame is NULL.");
1665  }
1666 
1667  return ARCONTROLLER_OK;
1668 }
1669 #endif // #ifdef VISP_HAVE_FFMPEG
1670 
1671 
1681 void vpRobotBebop2::cmdBatteryStateChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1682  vpRobotBebop2 *drone)
1683 {
1684  ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1685  ARCONTROLLER_DICTIONARY_ELEMENT_t *singleElement = NULL;
1686 
1687  if (elementDictionary == NULL) {
1688  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "elements is NULL");
1689  return;
1690  }
1691 
1692  // Get the command received in the device controller
1693  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, singleElement);
1694 
1695  if (singleElement == NULL) {
1696  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "singleElement is NULL");
1697  return;
1698  }
1699 
1700  // Get the value
1701  HASH_FIND_STR(singleElement->arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_BATTERYSTATECHANGED_PERCENT,
1702  arg);
1703 
1704  if (arg == NULL) {
1705  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "arg is NULL");
1706  return;
1707  }
1708  drone->m_batteryLevel = arg->value.U8;
1709  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Battery level changed : %u percent remaining.", drone->m_batteryLevel);
1710 
1711  if (drone->m_batteryLevel <= 5) {
1712  ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, " - WARNING, very low battery level, drone will stop soon !");
1713  } else if (drone->m_batteryLevel <= 10) {
1714  ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, " - Warning, low battery level !");
1715  }
1716 }
1717 
1728 void vpRobotBebop2::cmdCameraOrientationChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1729  vpRobotBebop2 *drone)
1730 {
1731  ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1732  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1733  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1734  if (element != NULL) {
1735  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2_TILT, arg);
1736 
1737  if (arg != NULL) {
1738  drone->m_currentCameraTilt = static_cast<double>(arg->value.Float);
1739  }
1740 
1741  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2_PAN, arg);
1742  if (arg != NULL) {
1743  drone->m_currentCameraPan = static_cast<double>(arg->value.Float);
1744  }
1745  }
1746 }
1747 
1759 void vpRobotBebop2::cmdCameraSettingsRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone)
1760 {
1761  ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1762  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1763  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1764  if (element != NULL) {
1765  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_FOV,
1766  arg);
1767  if (arg != NULL) {
1768  drone->m_cameraHorizontalFOV = static_cast<double>(arg->value.Float);
1769  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Camera horizontal FOV : %f degrees.",
1770  static_cast<double>(drone->m_cameraHorizontalFOV));
1771  }
1772  HASH_FIND_STR(element->arguments,
1773  ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_PANMAX, arg);
1774  if (arg != NULL) {
1775  drone->m_maxCameraPan = static_cast<double>(arg->value.Float);
1776  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Max camera pan : %f degrees.",
1777  static_cast<double>(drone->m_maxCameraPan));
1778  }
1779  HASH_FIND_STR(element->arguments,
1780  ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_PANMIN, arg);
1781  if (arg != NULL) {
1782  drone->m_minCameraPan = static_cast<double>(arg->value.Float);
1783  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Min camera pan : %f degrees.",
1784  static_cast<double>(drone->m_minCameraPan));
1785  }
1786  HASH_FIND_STR(element->arguments,
1787  ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_TILTMAX, arg);
1788  if (arg != NULL) {
1789  drone->m_maxCameraTilt = static_cast<double>(arg->value.Float);
1790  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Max camera tilt : %f degrees.",
1791  static_cast<double>(drone->m_maxCameraTilt));
1792  }
1793  HASH_FIND_STR(element->arguments,
1794  ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_TILTMIN, arg);
1795  if (arg != NULL) {
1796  drone->m_minCameraTilt = static_cast<double>(arg->value.Float);
1797  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Min camera tilt : %f degrees.",
1798  static_cast<double>(drone->m_minCameraTilt));
1799  }
1800  }
1801 }
1802 
1813 void vpRobotBebop2::cmdMaxPitchRollChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1814  vpRobotBebop2 *drone)
1815 {
1816  ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1817  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1818 
1819  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1820  if (element != NULL) {
1821  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXTILTCHANGED_CURRENT,
1822  arg);
1823  if (arg != NULL) {
1824  drone->m_maxTilt = static_cast<double>(arg->value.Float);
1825  }
1826  }
1827 }
1828 
1839 void vpRobotBebop2::cmdRelativeMoveEndedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone)
1840 {
1841  ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1842  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1843 
1844  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1845 
1846  if (element != NULL) {
1847  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR, arg);
1848 
1849  if (arg != NULL) {
1850  eARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR error =
1851  static_cast<eARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR>(arg->value.I32);
1852  if ((error != ARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR_OK) &&
1853  (error != ARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR_INTERRUPTED)) {
1854  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Relative move ended with error %d", error);
1855  }
1856  drone->m_relativeMoveEnded = true;
1857  }
1858  }
1859 }
1860 
1871 void vpRobotBebop2::cmdExposureSetRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone)
1872 {
1873  ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1874  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1875 
1876  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1877 
1878  if (element != NULL) {
1879 
1880  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED_VALUE,
1881  arg);
1882 
1883  if (arg != NULL) {
1884  drone->m_exposureSet = true;
1885  }
1886  }
1887 }
1888 
1898 void vpRobotBebop2::commandReceivedCallback(eARCONTROLLER_DICTIONARY_KEY commandKey,
1899  ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, void *customData)
1900 {
1901  vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1902 
1903  if (drone == NULL)
1904  return;
1905 
1906  switch (commandKey) {
1907  case ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_BATTERYSTATECHANGED:
1908  // If the command received is a battery state changed
1909  cmdBatteryStateChangedRcv(elementDictionary, drone);
1910  break;
1911 
1912  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXTILTCHANGED:
1913  // If the command receivend is a max pitch/roll changed
1914  cmdMaxPitchRollChangedRcv(elementDictionary, drone);
1915  break;
1916 
1917  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGEVENT_MOVEBYEND:
1918  // If the command received is a relative move ended
1919  cmdRelativeMoveEndedRcv(elementDictionary, drone);
1920  break;
1921 
1922  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLATTRIMCHANGED:
1923  // If the command received is a flat trim finished
1924  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Flat trim finished ...");
1925  drone->m_flatTrimFinished = true;
1926  break;
1927 
1928  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED:
1929  // If the command received is a exposition changed
1930  cmdExposureSetRcv(elementDictionary, drone);
1931  break;
1932 
1933  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_VIDEORESOLUTIONSCHANGED:
1934  // If the command received is a resolution changed
1935  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Video resolution set ...");
1936  drone->m_videoResolutionSet = true;
1937  break;
1938 
1939  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED:
1940  // If the command received is a streaming started
1941  drone->m_streamingStarted = true;
1942  break;
1943 
1944  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOSTREAMMODECHANGED:
1945  // If the command received is a streaming mode changed
1946  drone->m_streamingModeSet = true;
1947  break;
1948 
1949  case ARCONTROLLER_DICTIONARY_KEY_COMMON_SETTINGSSTATE_RESETCHANGED:
1950  // If the command received is a settings reset
1951  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Settings reset ...");
1952  drone->m_settingsReset = true;
1953  break;
1954 
1955  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2:
1956  // If the command received is a camera orientation changed
1957  cmdCameraOrientationChangedRcv(elementDictionary, drone);
1958  break;
1959 
1960  case ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED:
1961  // If the command received is a camera information sent
1962  cmdCameraSettingsRcv(elementDictionary, drone);
1963  break;
1964 
1965  default:
1966  break;
1967  }
1968 }
1969 
1970 #undef TAG
1971 
1972 #elif !defined(VISP_BUILD_SHARED_LIBS)
1973 // Work arround to avoid warning: libvisp_robot.a(vpRobotBebop2.cpp.o) has
1974 // no symbols
1975 void dummy_vpRobotBebop2(){};
1976 #endif // VISP_HAVE_ARSDK
static void BGRToRGBa(unsigned char *bgr, unsigned char *rgba, unsigned int width, unsigned int height, bool flip=false)
double getMaxCameraTilt() const
void getGrayscaleImage(vpImage< unsigned char > &I)
void getRGBaImage(vpImage< vpRGBa > &I)
void setYawSpeed(int value)
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:800
Implementation of an homogeneous matrix and operations on such kind of matrices.
Type * bitmap
points toward the bitmap
Definition: vpImage.h:143
void setCameraTilt(double tilt, bool blocking=false)
void setExposure(float expo)
error that can be emited by ViSP classes.
Definition: vpException.h:71
void setVideoStabilisationMode(int mode)
double getMaxTilt()
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
void setVerbose(bool verbose)
void setPosition(float dX, float dY, float dZ, float dPsi, bool blocking)
void setCameraPan(double pan, bool blocking=false)
static void land()
void setCameraOrientation(double tilt, double pan, bool blocking=false)
VISP_EXPORT void sleepMs(double t)
Definition: vpTime.cpp:271
virtual ~vpRobotBebop2()
void setVerticalSpeed(int value)
double getCameraHorizontalFOV() const
void setVideoResolution(int mode)
double getMinCameraPan() const
static void BGRToGrey(unsigned char *bgr, unsigned char *grey, unsigned int width, unsigned int height, bool flip=false, unsigned int nThreads=0)
void resetAllSettings()
vpTranslationVector getTranslationVector() const
vpRobotBebop2(bool verbose=false, bool setDefaultSettings=true, std::string ipAddress="192.168.42.1", int discoveryPort=44444)
void setMaxTilt(double maxTilt)
unsigned int getHeight() const
Definition: vpImage.h:188
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
double getCurrentCameraPan() const
static vpHomogeneousMatrix direct(const vpColVector &v)
void setPitch(int value)
std::string getIpAddress()
vpThetaUVector getThetaUVector()
double getMaxCameraPan() const
unsigned int getWidth() const
Definition: vpImage.h:246
void setVelocity(const vpColVector &vel, double delta_t)
double getCurrentCameraTilt() const
double getMinCameraTilt() const
void setStreamingMode(int mode)
Class that consider the case of a translation vector.
unsigned int getBatteryLevel()
vpRotationMatrix getRotationMatrix() const
void takeOff(bool blocking=true)
void setRoll(int value)