Visual Servoing Platform  version 3.5.1 under development (2023-05-30)
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  // Suppress warnings
1154  // Get the value
1155  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE,
1156  arg);
1157 
1158  if (arg != NULL) {
1159  // Enums are stored as I32
1160  flyingState = static_cast<eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE>(arg->value.I32);
1161  }
1162  }
1163  }
1164  return flyingState;
1165  } else {
1166  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error when checking flying state : drone isn't connected.");
1167  return ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_MAX;
1168  }
1169 }
1170 
1175 eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED vpRobotBebop2::getStreamingState()
1176 {
1177  if (m_deviceController != NULL) {
1178  eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED streamingState =
1179  ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_MAX;
1180  eARCONTROLLER_ERROR error;
1181 
1182  ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary = ARCONTROLLER_ARDrone3_GetCommandElements(
1183  m_deviceController->aRDrone3, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED,
1184  &error);
1185 
1186  if (error == ARCONTROLLER_OK && elementDictionary != NULL) {
1187  ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1188  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1189 
1190  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1191 
1192  if (element != NULL) {
1193  // Get the value
1194  HASH_FIND_STR(element->arguments,
1195  ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED, arg);
1196 
1197  if (arg != NULL) {
1198  // Enums are stored as I32
1199  streamingState =
1200  static_cast<eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED>(arg->value.I32);
1201  }
1202  }
1203  }
1204  return streamingState;
1205  } else {
1206  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error when checking streaming state : drone isn't connected.");
1207  return ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_MAX;
1208  }
1209 }
1210 
1215 ARDISCOVERY_Device_t *vpRobotBebop2::discoverDrone()
1216 {
1217  eARDISCOVERY_ERROR errorDiscovery = ARDISCOVERY_OK;
1218 
1219  ARDISCOVERY_Device_t *device = ARDISCOVERY_Device_New(&errorDiscovery);
1220 
1221  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Starting drone Wifi discovery ...");
1222  const char *charIpAddress = m_ipAddress.c_str();
1223  errorDiscovery =
1224  ARDISCOVERY_Device_InitWifi(device, ARDISCOVERY_PRODUCT_BEBOP_2, "bebop2", charIpAddress, m_discoveryPort);
1225 
1226  if (errorDiscovery != ARDISCOVERY_OK) {
1227  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Discovery error :%s", ARDISCOVERY_Error_ToString(errorDiscovery));
1228  }
1229  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Drone controller created.");
1230  return device;
1231 }
1232 
1239 void vpRobotBebop2::createDroneController(ARDISCOVERY_Device_t *discoveredDrone)
1240 {
1241  m_deviceController = ARCONTROLLER_Device_New(discoveredDrone, &m_errorController);
1242  if (m_errorController != ARCONTROLLER_OK) {
1243  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Creation of deviceController failed.");
1244  }
1245  ARDISCOVERY_Device_Delete(&discoveredDrone);
1246  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Device created.");
1247 }
1248 
1253 void vpRobotBebop2::setupCallbacks()
1254 {
1255  // Adding stateChanged callback, called when the state of the controller has changed
1256  m_errorController = ARCONTROLLER_Device_AddStateChangedCallback(m_deviceController, stateChangedCallback, this);
1257  if (m_errorController != ARCONTROLLER_OK) {
1258  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "add State callback failed.");
1259  }
1260 
1261  // Adding commendReceived callback, called when the a command has been received from the device
1262  m_errorController = ARCONTROLLER_Device_AddCommandReceivedCallback(m_deviceController, commandReceivedCallback, this);
1263 
1264  if (m_errorController != ARCONTROLLER_OK) {
1265  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "add Command callback failed.");
1266  }
1267 
1268 #ifdef VISP_HAVE_FFMPEG
1269  // Adding frame received callback, called when a streaming frame has been received from the device
1270  m_errorController = ARCONTROLLER_Device_SetVideoStreamCallbacks(m_deviceController, decoderConfigCallback,
1271  didReceiveFrameCallback, NULL, this);
1272 
1273  if (m_errorController != ARCONTROLLER_OK) {
1274  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error: %s", ARCONTROLLER_Error_ToString(m_errorController));
1275  }
1276 #endif
1277  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Callbacks set up.");
1278 }
1279 
1284 void vpRobotBebop2::startController()
1285 {
1286  // Starts the controller
1287  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Connecting ...");
1288  m_errorController = ARCONTROLLER_Device_Start(m_deviceController);
1289 
1290  if (m_errorController != ARCONTROLLER_OK) {
1291  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1292  }
1293 
1294  // Waits for the stateChangedCallback to unclock the semaphore
1295  ARSAL_Sem_Wait(&(m_stateSem));
1296 
1297  // Checks the device state
1298  m_deviceState = ARCONTROLLER_Device_GetState(m_deviceController, &m_errorController);
1299 
1300  if ((m_errorController != ARCONTROLLER_OK) || (m_deviceState != ARCONTROLLER_DEVICE_STATE_RUNNING)) {
1301  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- deviceState :%d", m_deviceState);
1302  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1303  }
1304  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Controller started.");
1305 }
1306 
1307 #ifdef VISP_HAVE_FFMPEG
1313 void vpRobotBebop2::initCodec()
1314 {
1315  av_register_all();
1316  avcodec_register_all();
1317  avformat_network_init();
1318 
1319  // Finds the correct codec
1320  AVCodec *codec = avcodec_find_decoder(AV_CODEC_ID_H264);
1321  if (!codec) {
1322  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Codec not found.");
1323  return;
1324  }
1325 
1326  // Allocates memory for codec
1327  m_codecContext = avcodec_alloc_context3(codec);
1328 
1329  if (!m_codecContext) {
1330  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Failed to allocate codec context.");
1331  return;
1332  }
1333 
1334  // Sets codec parameters (TODO : should be done automaticaly by drone callback decoderConfigCallback)
1335  m_codecContext->pix_fmt = AV_PIX_FMT_YUV420P;
1336  m_codecContext->skip_frame = AVDISCARD_DEFAULT;
1337  m_codecContext->error_concealment = FF_EC_GUESS_MVS | FF_EC_DEBLOCK;
1338  m_codecContext->skip_loop_filter = AVDISCARD_DEFAULT;
1339  m_codecContext->workaround_bugs = AVMEDIA_TYPE_VIDEO;
1340  m_codecContext->codec_id = AV_CODEC_ID_H264;
1341  m_codecContext->skip_idct = AVDISCARD_DEFAULT;
1342 
1343  m_codecContext->width = m_videoWidth;
1344  m_codecContext->height = m_videoHeight;
1345 
1346  if (codec->capabilities & AV_CODEC_CAP_TRUNCATED) {
1347  m_codecContext->flags |= AV_CODEC_FLAG_TRUNCATED;
1348  }
1349  m_codecContext->flags2 |= AV_CODEC_FLAG2_CHUNKS;
1350 
1351  // Opens the codec
1352  if (avcodec_open2(m_codecContext, codec, NULL) < 0) {
1353  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Failed to open codec.");
1354  return;
1355  }
1356 
1357  AVPixelFormat pFormat = AV_PIX_FMT_BGR24;
1358  int numBytes = av_image_get_buffer_size(pFormat, m_codecContext->width, m_codecContext->height, 1);
1359  m_buffer = static_cast<uint8_t *>(av_malloc(static_cast<unsigned long>(numBytes) * sizeof(uint8_t)));
1360 
1361  av_init_packet(&m_packet); // Packed used to send data to the decoder
1362  m_picture = av_frame_alloc(); // Frame used to receive data from the decoder
1363 
1364  m_bgr_picture_mutex.lock();
1365  m_bgr_picture = av_frame_alloc(); // Frame used to store rescaled frame received from the decoder
1366  m_bgr_picture_mutex.unlock();
1367 
1368  m_img_convert_ctx = sws_getContext(m_codecContext->width, m_codecContext->height, m_codecContext->pix_fmt,
1369  m_codecContext->width, m_codecContext->height, pFormat, SWS_BICUBIC, NULL, NULL,
1370  NULL); // Used to rescale frame received from the decoder
1371 }
1372 
1378 void vpRobotBebop2::cleanUpCodec()
1379 {
1380  m_videoDecodingStarted = false;
1381  av_packet_unref(&m_packet);
1382 
1383  if (m_codecContext) {
1384  avcodec_flush_buffers(m_codecContext);
1385  avcodec_free_context(&m_codecContext);
1386  }
1387 
1388  if (m_picture) {
1389  av_frame_free(&m_picture);
1390  }
1391 
1392  if (m_bgr_picture) {
1393  m_bgr_picture_mutex.lock();
1394  av_frame_free(&m_bgr_picture);
1395  m_bgr_picture_mutex.unlock();
1396  }
1397 
1398  if (m_img_convert_ctx) {
1399  sws_freeContext(m_img_convert_ctx);
1400  }
1401  if (m_buffer) {
1402  av_free(m_buffer);
1403  }
1404 }
1405 
1411 void vpRobotBebop2::startVideoDecoding()
1412 {
1413  if (!m_videoDecodingStarted) {
1414  initCodec();
1415  m_videoDecodingStarted = true;
1416  } else {
1417  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Video decoding is already started.");
1418  }
1419 }
1420 
1426 void vpRobotBebop2::stopVideoDecoding()
1427 {
1428  if (m_videoDecodingStarted) {
1429  cleanUpCodec();
1430  } else {
1431  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Video decoding is already stopped.");
1432  }
1433 }
1434 
1442 void vpRobotBebop2::computeFrame(ARCONTROLLER_Frame_t *frame)
1443 {
1444 
1445  // Updating codec parameters from SPS and PPS buffers received from the drone in decoderConfigCallback
1446  if (m_update_codec_params && m_codec_params_data.size()) {
1447  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Updating H264 codec parameters (Buffer Size: %lu) ...",
1448  m_codec_params_data.size());
1449 
1450  m_packet.data = &m_codec_params_data[0];
1451  m_packet.size = static_cast<int>(m_codec_params_data.size());
1452 
1453  int ret = avcodec_send_packet(m_codecContext, &m_packet);
1454 
1455  if (ret == 0) {
1456 
1457  ret = avcodec_receive_frame(m_codecContext, m_picture);
1458 
1459  if (ret == 0 || ret == AVERROR(EAGAIN)) {
1460  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "H264 codec parameters updated.");
1461  } else {
1462  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Unexpected error while updating H264 parameters.");
1463  }
1464  } else {
1465  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Unexpected error while sending H264 parameters.");
1466  }
1467  m_update_codec_params = false;
1468  av_packet_unref(&m_packet);
1469  av_frame_unref(m_picture);
1470  }
1471 
1472  // Decoding frame comming from the drone
1473  m_packet.data = frame->data;
1474  m_packet.size = static_cast<int>(frame->used);
1475 
1476  int ret = avcodec_send_packet(m_codecContext, &m_packet);
1477  if (ret < 0) {
1478 
1479  char *errbuff = new char[AV_ERROR_MAX_STRING_SIZE];
1480  av_strerror(ret, errbuff, AV_ERROR_MAX_STRING_SIZE);
1481  std::string err(errbuff);
1482  delete[] errbuff;
1483  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error sending a packet for decoding : %d, %s", ret, err.c_str());
1484 
1485  } else {
1486 
1487  ret = avcodec_receive_frame(m_codecContext, m_picture);
1488 
1489  if (ret < 0) {
1490 
1491  if (ret == AVERROR(EAGAIN)) {
1492  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "AVERROR(EAGAIN)"); // Not an error, need to send data again
1493  } else if (ret == AVERROR_EOF) {
1494  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "AVERROR_EOF"); // End of file reached, should not happen with drone footage
1495  } else {
1496 
1497  char *errbuff = new char[AV_ERROR_MAX_STRING_SIZE];
1498  av_strerror(ret, errbuff, AV_ERROR_MAX_STRING_SIZE);
1499  std::string err(errbuff);
1500  delete[] errbuff;
1501  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error receiving a decoded frame : %d, %s", ret, err.c_str());
1502  }
1503  } else {
1504  m_bgr_picture_mutex.lock();
1505  av_frame_unref(m_bgr_picture);
1506  av_image_fill_arrays(m_bgr_picture->data, m_bgr_picture->linesize, m_buffer, AV_PIX_FMT_BGR24,
1507  m_codecContext->width, m_codecContext->height, 1);
1508 
1509  sws_scale(m_img_convert_ctx, (m_picture)->data, (m_picture)->linesize, 0, m_codecContext->height,
1510  (m_bgr_picture)->data, (m_bgr_picture)->linesize);
1511 
1512  m_bgr_picture_mutex.unlock();
1513  }
1514  }
1515 
1516  av_packet_unref(&m_packet);
1517 
1518  av_frame_unref(m_picture);
1519 }
1520 #endif // #ifdef VISP_HAVE_FFMPEG
1526 void vpRobotBebop2::cleanUp()
1527 {
1528  if (m_deviceController != NULL) {
1529  // Lands the drone if not landed
1530  land();
1531 
1532 #ifdef VISP_HAVE_FFMPEG
1533  // Stops the streaming if not stopped
1534  stopStreaming();
1535 #endif
1536 
1537  // Deletes the controller
1538  m_deviceState = ARCONTROLLER_Device_GetState(m_deviceController, &m_errorController);
1539  if ((m_errorController == ARCONTROLLER_OK) && (m_deviceState != ARCONTROLLER_DEVICE_STATE_STOPPED)) {
1540 
1541  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Disconnecting ...");
1542  m_errorController = ARCONTROLLER_Device_Stop(m_deviceController);
1543 
1544  if (m_errorController == ARCONTROLLER_OK) {
1545  // Wait for the semaphore to increment, it will when the controller changes its state to 'stopped'
1546  ARSAL_Sem_Wait(&(m_stateSem));
1547  }
1548  }
1549  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Deleting device controller ...");
1550  ARCONTROLLER_Device_Delete(&m_deviceController);
1551 
1552  // Destroys the semaphore
1553  ARSAL_Sem_Destroy(&(m_stateSem));
1554 
1555  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Cleanup done.");
1556  } else {
1557 
1558  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error while cleaning up memory.");
1559  }
1560  m_running = false;
1561 }
1562 
1563 //*** ***//
1564 //*** Callbacks ***//
1565 //*** ***//
1566 
1575 void vpRobotBebop2::stateChangedCallback(eARCONTROLLER_DEVICE_STATE newState, eARCONTROLLER_ERROR error,
1576  void *customData)
1577 {
1578  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Controller state changed, new state: %d.", newState);
1579  (void)error;
1580 
1581  vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1582  switch (newState) {
1583  case ARCONTROLLER_DEVICE_STATE_STOPPED:
1584  // Stopping the programm
1585  drone->m_running = false;
1586  // Incrementing semaphore
1587  ARSAL_Sem_Post(&(drone->m_stateSem));
1588  break;
1589 
1590  case ARCONTROLLER_DEVICE_STATE_RUNNING:
1591  // Incrementing semaphore
1592  ARSAL_Sem_Post(&(drone->m_stateSem));
1593  break;
1594 
1595  default:
1596  break;
1597  }
1598 }
1599 
1600 #ifdef VISP_HAVE_FFMPEG
1610 eARCONTROLLER_ERROR vpRobotBebop2::decoderConfigCallback(ARCONTROLLER_Stream_Codec_t codec, void *customData)
1611 {
1612  vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1613 
1614  uint8_t *sps_buffer_ptr = codec.parameters.h264parameters.spsBuffer;
1615  uint32_t sps_buffer_size = static_cast<uint32_t>(codec.parameters.h264parameters.spsSize);
1616  uint8_t *pps_buffer_ptr = codec.parameters.h264parameters.ppsBuffer;
1617  uint32_t pps_buffer_size = static_cast<uint32_t>(codec.parameters.h264parameters.ppsSize);
1618 
1619  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "H264 configuration packet received: #SPS: %d #PPS: %d", sps_buffer_size,
1620  pps_buffer_size);
1621 
1622  drone->m_update_codec_params = (sps_buffer_ptr && pps_buffer_ptr && sps_buffer_size && pps_buffer_size &&
1623  (pps_buffer_size < 32) && (sps_buffer_size < 32));
1624 
1625  if (drone->m_update_codec_params) {
1626  // If codec parameters where received from the drone, we store them to pass them to the decoder in the next call of
1627  // computeFrame
1628  drone->m_codec_params_data.resize(sps_buffer_size + pps_buffer_size);
1629  std::copy(sps_buffer_ptr, sps_buffer_ptr + sps_buffer_size, drone->m_codec_params_data.begin());
1630  std::copy(pps_buffer_ptr, pps_buffer_ptr + pps_buffer_size, drone->m_codec_params_data.begin() + sps_buffer_size);
1631  } else {
1632  // If data is invalid, we clear the vector
1633  drone->m_codec_params_data.clear();
1634  }
1635  return ARCONTROLLER_OK;
1636 }
1637 
1646 eARCONTROLLER_ERROR vpRobotBebop2::didReceiveFrameCallback(ARCONTROLLER_Frame_t *frame, void *customData)
1647 {
1648  vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1649 
1650  if (frame != NULL) {
1651 
1652  if (drone->m_videoDecodingStarted) {
1653  drone->computeFrame(frame);
1654  }
1655 
1656  } else {
1657  ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, "frame is NULL.");
1658  }
1659 
1660  return ARCONTROLLER_OK;
1661 }
1662 #endif // #ifdef VISP_HAVE_FFMPEG
1663 
1673 void vpRobotBebop2::cmdBatteryStateChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1674  vpRobotBebop2 *drone)
1675 {
1676  ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1677  ARCONTROLLER_DICTIONARY_ELEMENT_t *singleElement = NULL;
1678 
1679  if (elementDictionary == NULL) {
1680  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "elements is NULL");
1681  return;
1682  }
1683 
1684  // Get the command received in the device controller
1685  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, singleElement);
1686 
1687  if (singleElement == NULL) {
1688  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "singleElement is NULL");
1689  return;
1690  }
1691 
1692  // Get the value
1693  HASH_FIND_STR(singleElement->arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_BATTERYSTATECHANGED_PERCENT,
1694  arg);
1695 
1696  if (arg == NULL) {
1697  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "arg is NULL");
1698  return;
1699  }
1700  drone->m_batteryLevel = arg->value.U8;
1701  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Battery level changed : %u percent remaining.", drone->m_batteryLevel);
1702 
1703  if (drone->m_batteryLevel <= 5) {
1704  ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, " - WARNING, very low battery level, drone will stop soon !");
1705  } else if (drone->m_batteryLevel <= 10) {
1706  ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, " - Warning, low battery level !");
1707  }
1708 }
1709 
1720 void vpRobotBebop2::cmdCameraOrientationChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1721  vpRobotBebop2 *drone)
1722 {
1723  ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1724  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1725  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1726  if (element != NULL) {
1727  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2_TILT, arg);
1728 
1729  if (arg != NULL) {
1730  drone->m_currentCameraTilt = static_cast<double>(arg->value.Float);
1731  }
1732 
1733  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2_PAN, arg);
1734  if (arg != NULL) {
1735  drone->m_currentCameraPan = static_cast<double>(arg->value.Float);
1736  }
1737  }
1738 }
1739 
1751 void vpRobotBebop2::cmdCameraSettingsRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone)
1752 {
1753  ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1754  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1755  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1756  if (element != NULL) {
1757  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_FOV,
1758  arg);
1759  if (arg != NULL) {
1760  drone->m_cameraHorizontalFOV = static_cast<double>(arg->value.Float);
1761  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Camera horizontal FOV : %f degrees.",
1762  static_cast<double>(drone->m_cameraHorizontalFOV));
1763  }
1764  HASH_FIND_STR(element->arguments,
1765  ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_PANMAX, arg);
1766  if (arg != NULL) {
1767  drone->m_maxCameraPan = static_cast<double>(arg->value.Float);
1768  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Max camera pan : %f degrees.",
1769  static_cast<double>(drone->m_maxCameraPan));
1770  }
1771  HASH_FIND_STR(element->arguments,
1772  ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_PANMIN, arg);
1773  if (arg != NULL) {
1774  drone->m_minCameraPan = static_cast<double>(arg->value.Float);
1775  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Min camera pan : %f degrees.",
1776  static_cast<double>(drone->m_minCameraPan));
1777  }
1778  HASH_FIND_STR(element->arguments,
1779  ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_TILTMAX, arg);
1780  if (arg != NULL) {
1781  drone->m_maxCameraTilt = static_cast<double>(arg->value.Float);
1782  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Max camera tilt : %f degrees.",
1783  static_cast<double>(drone->m_maxCameraTilt));
1784  }
1785  HASH_FIND_STR(element->arguments,
1786  ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_TILTMIN, arg);
1787  if (arg != NULL) {
1788  drone->m_minCameraTilt = static_cast<double>(arg->value.Float);
1789  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Min camera tilt : %f degrees.",
1790  static_cast<double>(drone->m_minCameraTilt));
1791  }
1792  }
1793 }
1794 
1805 void vpRobotBebop2::cmdMaxPitchRollChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1806  vpRobotBebop2 *drone)
1807 {
1808  ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1809  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1810 
1811  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1812  if (element != NULL) {
1813  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXTILTCHANGED_CURRENT,
1814  arg);
1815  if (arg != NULL) {
1816  drone->m_maxTilt = static_cast<double>(arg->value.Float);
1817  }
1818  }
1819 }
1820 
1831 void vpRobotBebop2::cmdRelativeMoveEndedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone)
1832 {
1833  ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1834  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1835 
1836  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1837 
1838  if (element != NULL) {
1839  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR, arg);
1840 
1841  if (arg != NULL) {
1842  eARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR error =
1843  static_cast<eARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR>(arg->value.I32);
1844  if ((error != ARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR_OK) &&
1845  (error != ARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR_INTERRUPTED)) {
1846  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Relative move ended with error %d", error);
1847  }
1848  drone->m_relativeMoveEnded = true;
1849  }
1850  }
1851 }
1852 
1863 void vpRobotBebop2::cmdExposureSetRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone)
1864 {
1865  ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1866  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1867 
1868  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1869 
1870  if (element != NULL) {
1871 
1872  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED_VALUE,
1873  arg);
1874 
1875  if (arg != NULL) {
1876  drone->m_exposureSet = true;
1877  }
1878  }
1879 }
1880 
1890 void vpRobotBebop2::commandReceivedCallback(eARCONTROLLER_DICTIONARY_KEY commandKey,
1891  ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, void *customData)
1892 {
1893  vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1894 
1895  if (drone == NULL)
1896  return;
1897 
1898  switch (commandKey) {
1899  case ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_BATTERYSTATECHANGED:
1900  // If the command received is a battery state changed
1901  cmdBatteryStateChangedRcv(elementDictionary, drone);
1902  break;
1903 
1904  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXTILTCHANGED:
1905  // If the command receivend is a max pitch/roll changed
1906  cmdMaxPitchRollChangedRcv(elementDictionary, drone);
1907  break;
1908 
1909  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGEVENT_MOVEBYEND:
1910  // If the command received is a relative move ended
1911  cmdRelativeMoveEndedRcv(elementDictionary, drone);
1912  break;
1913 
1914  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLATTRIMCHANGED:
1915  // If the command received is a flat trim finished
1916  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Flat trim finished ...");
1917  drone->m_flatTrimFinished = true;
1918  break;
1919 
1920  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED:
1921  // If the command received is a exposition changed
1922  cmdExposureSetRcv(elementDictionary, drone);
1923  break;
1924 
1925  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_VIDEORESOLUTIONSCHANGED:
1926  // If the command received is a resolution changed
1927  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Video resolution set ...");
1928  drone->m_videoResolutionSet = true;
1929  break;
1930 
1931  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED:
1932  // If the command received is a streaming started
1933  drone->m_streamingStarted = true;
1934  break;
1935 
1936  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOSTREAMMODECHANGED:
1937  // If the command received is a streaming mode changed
1938  drone->m_streamingModeSet = true;
1939  break;
1940 
1941  case ARCONTROLLER_DICTIONARY_KEY_COMMON_SETTINGSSTATE_RESETCHANGED:
1942  // If the command received is a settings reset
1943  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Settings reset ...");
1944  drone->m_settingsReset = true;
1945  break;
1946 
1947  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2:
1948  // If the command received is a camera orientation changed
1949  cmdCameraOrientationChangedRcv(elementDictionary, drone);
1950  break;
1951 
1952  case ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED:
1953  // If the command received is a camera information sent
1954  cmdCameraSettingsRcv(elementDictionary, drone);
1955  break;
1956 
1957  default:
1958  break;
1959  }
1960 }
1961 
1962 #undef TAG
1963 
1964 #elif !defined(VISP_BUILD_SHARED_LIBS)
1965 // Work around to avoid warning: libvisp_robot.a(vpRobotBebop2.cpp.o) has
1966 // no symbols
1967 void dummy_vpRobotBebop2(){};
1968 #endif // VISP_HAVE_ARSDK
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:294
Implementation of column vector and the associated operations.
Definition: vpColVector.h:172
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ fatalError
Fatal error.
Definition: vpException.h:96
static vpHomogeneousMatrix direct(const vpColVector &v)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
vpTranslationVector getTranslationVector() const
static void BGRToGrey(unsigned char *bgr, unsigned char *grey, unsigned int width, unsigned int height, bool flip=false, unsigned int nThreads=0)
static void BGRToRGBa(unsigned char *bgr, unsigned char *rgba, unsigned int width, unsigned int height, bool flip=false)
unsigned int getWidth() const
Definition: vpImage.h:247
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:800
Type * bitmap
points toward the bitmap
Definition: vpImage.h:144
unsigned int getHeight() const
Definition: vpImage.h:189
void setPitch(int value)
std::string getIpAddress()
double getMinCameraPan() const
void resetAllSettings()
void setMaxTilt(double maxTilt)
void setPosition(float dX, float dY, float dZ, float dPsi, bool blocking)
void setVelocity(const vpColVector &vel, double delta_t)
void setExposure(float expo)
double getCurrentCameraPan() const
void setVideoStabilisationMode(int mode)
void setRoll(int value)
double getMinCameraTilt() const
double getMaxCameraTilt() const
void setVerbose(bool verbose)
void setVerticalSpeed(int value)
void getGrayscaleImage(vpImage< unsigned char > &I)
void setStreamingMode(int mode)
static void land()
double getMaxTilt()
unsigned int getBatteryLevel()
void getRGBaImage(vpImage< vpRGBa > &I)
void setYawSpeed(int value)
void setCameraPan(double pan, bool blocking=false)
void takeOff(bool blocking=true)
virtual ~vpRobotBebop2()
double getCurrentCameraTilt() const
double getCameraHorizontalFOV() const
void setVideoResolution(int mode)
void setCameraTilt(double tilt, bool blocking=false)
vpRobotBebop2(bool verbose=false, bool setDefaultSettings=true, std::string ipAddress="192.168.42.1", int discoveryPort=44444)
void setCameraOrientation(double tilt, double pan, bool blocking=false)
double getMaxCameraPan() const
vpThetaUVector getThetaUVector()
Class that consider the case of a translation vector.
VISP_EXPORT void sleepMs(double t)