Visual Servoing Platform  version 3.6.1 under development (2024-05-21)
vpRobotBebop2.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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  *
37 *****************************************************************************/
38 
39 #include <visp3/core/vpConfig.h>
40 
41 #ifdef VISP_HAVE_ARSDK
42 
43 #include <visp3/robot/vpRobotBebop2.h>
44 
45 #include <visp3/core/vpExponentialMap.h> // For velocity computation
46 
47 #ifdef VISP_HAVE_FFMPEG
48 extern "C" {
49 #include <libavcodec/avcodec.h>
50 #include <libavformat/avformat.h>
51 #include <libavutil/imgutils.h>
52 }
53 #include <visp3/core/vpImageConvert.h>
54 #endif
55 
56 #include <iostream>
57 #include <math.h>
58 
59 #define TAG "vpRobotBebop2" // For error messages of ARSDK
60 
75 bool vpRobotBebop2::m_running = false;
76 ARCONTROLLER_Device_t *vpRobotBebop2::m_deviceController = nullptr;
77 
113 vpRobotBebop2::vpRobotBebop2(bool verbose, bool setDefaultSettings, std::string ipAddress, int discoveryPort)
114  : m_ipAddress(ipAddress), m_discoveryPort(discoveryPort)
115 {
116  // Setting up signal handling
117  memset(&m_sigAct, 0, sizeof(m_sigAct));
118  m_sigAct.sa_handler = vpRobotBebop2::sighandler;
119  sigaction(SIGINT, &m_sigAct, 0);
120  sigaction(SIGBUS, &m_sigAct, 0);
121  sigaction(SIGSEGV, &m_sigAct, 0);
122  sigaction(SIGKILL, &m_sigAct, 0);
123  sigaction(SIGQUIT, &m_sigAct, 0);
124 
125 #ifdef VISP_HAVE_FFMPEG
126  m_codecContext = nullptr;
127  m_packet = AVPacket();
128  m_picture = nullptr;
129  m_bgr_picture = nullptr;
130  m_img_convert_ctx = nullptr;
131  m_buffer = nullptr;
132  m_videoDecodingStarted = false;
133 #endif
134 
135  m_batteryLevel = 100;
136 
137  m_exposureSet = true;
138  m_flatTrimFinished = true;
139  m_relativeMoveEnded = true;
140  m_videoResolutionSet = true;
141  m_streamingStarted = false;
142  m_streamingModeSet = false;
143  m_settingsReset = false;
144 
145  m_update_codec_params = false;
146  m_codec_params_data = std::vector<uint8_t>();
147 
148  m_maxTilt = -1;
149 
150  m_cameraHorizontalFOV = -1;
151  m_currentCameraTilt = -1;
152  m_minCameraTilt = -1;
153  m_maxCameraTilt = -1;
154  m_currentCameraPan = -1;
155  m_minCameraPan = -1;
156  m_maxCameraPan = -1;
157 
158  setVerbose(verbose);
159 
160  m_errorController = ARCONTROLLER_OK;
161  m_deviceState = ARCONTROLLER_DEVICE_STATE_MAX;
162 
163  // Initialises a semaphore
164  ARSAL_Sem_Init(&(m_stateSem), 0, 0);
165 
166  // Creates a discovery device to find the drone on the wifi network
167  ARDISCOVERY_Device_t *discoverDevice = discoverDrone();
168 
169  // Creates a drone controller with the discovery device
170  createDroneController(discoverDevice);
171 
172  // Sets up callbacks
173  setupCallbacks();
174 
175  // Start the drone controller, connects to the drone. If an error occurs, it will set m_errorController to the error.
176  startController();
177 
178  // We check if the drone was actually found and connected to the controller
179  if ((m_errorController != ARCONTROLLER_OK) || (m_deviceState != ARCONTROLLER_DEVICE_STATE_RUNNING)) {
180  cleanUp();
181  m_running = false;
182 
184  "Failed to connect to bebop2 with ip %s and port %d. Make sure that the ip address is correct "
185  "and that your computer is connected to the drone Wifi spot before starting",
186  ipAddress.c_str(), discoveryPort));
187  }
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 != nullptr && 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  }
235  else {
236  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't do a flat trim : drone isn't landed.");
237  }
238 }
239 
244 std::string vpRobotBebop2::getIpAddress() { return m_ipAddress; }
245 
250 int vpRobotBebop2::getDiscoveryPort() { return m_discoveryPort; }
251 
256 double vpRobotBebop2::getMaxTilt() { return m_maxTilt; }
257 
264 unsigned int vpRobotBebop2::getBatteryLevel() { return m_batteryLevel; }
265 
270 double vpRobotBebop2::getCameraHorizontalFOV() const { return m_cameraHorizontalFOV; }
271 
276 double vpRobotBebop2::getCurrentCameraTilt() const { return m_currentCameraTilt; }
277 
282 double vpRobotBebop2::getMinCameraTilt() const { return m_minCameraTilt; }
283 
288 double vpRobotBebop2::getMaxCameraTilt() const { return m_maxCameraTilt; }
289 
294 double vpRobotBebop2::getCurrentCameraPan() const { return m_currentCameraPan; }
295 
300 double vpRobotBebop2::getMinCameraPan() const { return m_minCameraPan; }
301 
306 double vpRobotBebop2::getMaxCameraPan() const { return m_maxCameraPan; }
307 
320 void vpRobotBebop2::setCameraOrientation(double tilt, double pan, bool blocking)
321 {
322  if (isRunning() && m_deviceController != nullptr) {
323 
324  m_deviceController->aRDrone3->sendCameraOrientationV2(m_deviceController->aRDrone3, static_cast<float>(tilt),
325  static_cast<float>(pan));
326 
327  if (blocking) {
328  while (std::abs(tilt - m_currentCameraTilt) > 0.01 || std::abs(pan - m_currentCameraPan) > 0.01) {
329  vpTime::sleepMs(1);
330  }
331  }
332 
333  }
334  else {
335  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set camera orientation : drone isn't running.");
336  }
337 }
338 
350 void vpRobotBebop2::setCameraTilt(double tilt, bool blocking)
351 {
352  if (isRunning() && m_deviceController != nullptr) {
353 
354  m_deviceController->aRDrone3->sendCameraOrientationV2(m_deviceController->aRDrone3, static_cast<float>(tilt),
355  static_cast<float>(getCurrentCameraPan()));
356 
357  if (blocking) {
358  while (std::abs(tilt - m_currentCameraTilt) > 0.01) {
359  vpTime::sleepMs(1);
360  }
361  }
362 
363  }
364  else {
365  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set camera tilt value : drone isn't running.");
366  }
367 }
368 
380 void vpRobotBebop2::setCameraPan(double pan, bool blocking)
381 {
382  if (isRunning() && m_deviceController != nullptr) {
383 
384  m_deviceController->aRDrone3->sendCameraOrientationV2(
385  m_deviceController->aRDrone3, static_cast<float>(getCurrentCameraTilt()), static_cast<float>(pan));
386 
387  if (blocking) {
388  while (std::abs(pan - m_currentCameraPan) > 0.01) {
389  vpTime::sleepMs(1);
390  }
391  }
392 
393  }
394  else {
395  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set camera pan value : drone isn't running.");
396  }
397 }
398 
404 {
405  if (m_deviceController == nullptr) {
406  return false;
407  }
408  else {
409  return m_running;
410  }
411 }
412 
418 {
419 #ifdef VISP_HAVE_FFMPEG
420  return m_videoDecodingStarted;
421 #else
422  return false;
423 #endif
424 }
425 
431 {
432  return getFlyingState() == ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_HOVERING;
433 }
434 
440 {
441  return getFlyingState() == ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_FLYING;
442 }
443 
449 {
450  return getFlyingState() == ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_LANDED;
451 }
452 
460 void vpRobotBebop2::takeOff(bool blocking)
461 {
462  if (isRunning() && isLanded() && m_deviceController != nullptr) {
463 
464  m_deviceController->aRDrone3->sendPilotingTakeOff(m_deviceController->aRDrone3);
465 
466  if (blocking) {
467  while (!isHovering()) {
468  vpTime::sleepMs(1);
469  }
470  }
471 
472  }
473  else {
474  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't take off : drone isn't landed.");
475  }
476 }
477 
485 {
486  if (m_deviceController != nullptr) {
487  m_deviceController->aRDrone3->sendPilotingLanding(m_deviceController->aRDrone3);
488  }
489 }
490 
502 {
503  if (isRunning() && m_deviceController != nullptr && (isFlying() || isHovering())) {
504  m_errorController =
505  m_deviceController->aRDrone3->setPilotingPCMDGaz(m_deviceController->aRDrone3, static_cast<char>(value));
506 
507  if (m_errorController != ARCONTROLLER_OK) {
508  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error when sending move command : %s",
509  ARCONTROLLER_Error_ToString(m_errorController));
510  }
511 
512  }
513  else {
514  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set vertical speed : drone isn't flying or hovering.");
515  }
516 }
517 
529 {
530  if (isRunning() && m_deviceController != nullptr && (isFlying() || isHovering())) {
531 
532  m_errorController =
533  m_deviceController->aRDrone3->setPilotingPCMDYaw(m_deviceController->aRDrone3, static_cast<char>(value));
534 
535  if (m_errorController != ARCONTROLLER_OK) {
536  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error when sending move command : %s",
537  ARCONTROLLER_Error_ToString(m_errorController));
538  }
539 
540  }
541  else {
542  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set yaw speed : drone isn't flying or hovering.");
543  }
544 }
545 
556 void vpRobotBebop2::setPitch(int value)
557 {
558  if (isRunning() && m_deviceController != nullptr && (isFlying() || isHovering())) {
559 
560  m_errorController =
561  m_deviceController->aRDrone3->setPilotingPCMDPitch(m_deviceController->aRDrone3, static_cast<char>(value));
562  m_errorController = m_deviceController->aRDrone3->setPilotingPCMDFlag(m_deviceController->aRDrone3, 1);
563 
564  if (m_errorController != ARCONTROLLER_OK) {
565  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error when sending move command : %s",
566  ARCONTROLLER_Error_ToString(m_errorController));
567  }
568 
569  }
570  else {
571  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set pitch value : drone isn't flying or hovering.");
572  }
573 }
574 
585 void vpRobotBebop2::setRoll(int value)
586 {
587  if (isRunning() && m_deviceController != nullptr && (isFlying() || isHovering())) {
588 
589  m_errorController =
590  m_deviceController->aRDrone3->setPilotingPCMDRoll(m_deviceController->aRDrone3, static_cast<char>(value));
591  m_errorController = m_deviceController->aRDrone3->setPilotingPCMDFlag(m_deviceController->aRDrone3, 1);
592 
593  if (m_errorController != ARCONTROLLER_OK) {
594  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error when sending move command : %s",
595  ARCONTROLLER_Error_ToString(m_errorController));
596  }
597 
598  }
599  else {
600  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set roll value : drone isn't flying or hovering.");
601  }
602 }
603 
611 {
612  if (m_deviceController != nullptr) {
613  m_errorController = m_deviceController->aRDrone3->sendPilotingEmergency(m_deviceController->aRDrone3);
614  }
615 }
616 
632 void vpRobotBebop2::setPosition(float dX, float dY, float dZ, float dPsi, bool blocking)
633 {
634  if (isRunning() && m_deviceController != nullptr && (isFlying() || isHovering())) {
635 
636  m_relativeMoveEnded = false;
637  m_deviceController->aRDrone3->sendPilotingMoveBy(m_deviceController->aRDrone3, dX, dY, dZ, dPsi);
638 
639  if (blocking) {
640 
641  // m_relativeMoveEnded is set back to true when the drone has finished his move, via a callback
642  while (!m_relativeMoveEnded) {
643  vpTime::sleepMs(1);
644  }
645  }
646  }
647  else {
648  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't move : drone isn't flying or hovering.");
649  }
650 }
651 
665 void vpRobotBebop2::setPosition(const vpHomogeneousMatrix &M, bool blocking)
666 {
667  double epsilon = (std::numeric_limits<double>::epsilon());
668  if (std::abs(M.getRotationMatrix().getThetaUVector()[0]) >= epsilon) {
669  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't move : rotation around X axis should be 0.");
670  return;
671  }
672  if (std::abs(M.getRotationMatrix().getThetaUVector()[1]) >= epsilon) {
673  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't move : rotation around Y axis should be 0.");
674  return;
675  }
676  float dThetaZ = static_cast<float>(M.getRotationMatrix().getThetaUVector()[2]);
678  setPosition(static_cast<float>(t[0]), static_cast<float>(t[1]), static_cast<float>(t[2]), dThetaZ, blocking);
679 }
680 
692 void vpRobotBebop2::setVelocity(const vpColVector &vel_cmd, double delta_t)
693 {
694 
695  if (vel_cmd.size() != 4) {
696  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR",
697  "Can't set velocity : dimension of the velocity vector should be equal to 4.");
698  stopMoving();
699  return;
700  }
701 
702  vpColVector ve(6);
703 
704  ve[0] = vel_cmd[0];
705  ve[1] = vel_cmd[1];
706  ve[2] = vel_cmd[2];
707  ve[5] = vel_cmd[3];
708 
710  setPosition(M, false);
711 }
712 
721 void vpRobotBebop2::setVerbose(bool verbose)
722 {
723  if (verbose) {
724  ARSAL_Print_SetMinimumLevel(ARSAL_PRINT_INFO);
725  }
726  else {
727  ARSAL_Print_SetMinimumLevel(ARSAL_PRINT_WARNING);
728  }
729 }
730 
736 {
737  if (isRunning() && m_deviceController != nullptr) {
738 
739  m_settingsReset = false;
740  m_deviceController->common->sendSettingsReset(m_deviceController->common);
741 
742  while (!m_settingsReset) {
743  vpTime::sleepMs(1);
744  }
745 
746  }
747  else {
748  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't reset drone settings : drone isn't running.");
749  }
750 }
751 
763 void vpRobotBebop2::setMaxTilt(double maxTilt)
764 {
765  if (isRunning() && m_deviceController != nullptr) {
766  m_deviceController->aRDrone3->sendPilotingSettingsMaxTilt(m_deviceController->aRDrone3,
767  static_cast<float>(maxTilt));
768  }
769  else {
770  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set tilt value : drone isn't running.");
771  }
772 }
773 
782 {
783  if (isRunning() && !isLanded() && m_deviceController != nullptr) {
784  m_errorController = m_deviceController->aRDrone3->setPilotingPCMD(m_deviceController->aRDrone3, 0, 0, 0, 0, 0, 0);
785  }
786 }
787 
788 //*** ***//
789 //*** Streaming functions ***//
790 //*** ***//
791 
792 #ifdef VISP_HAVE_FFMPEG // Functions related to video streaming and decoding requiers FFmpeg
793 
803 {
804  if (m_videoDecodingStarted) {
805 
806  if (m_bgr_picture->data[0] != nullptr) {
807  I.resize(static_cast<unsigned int>(m_videoHeight), static_cast<unsigned int>(m_videoWidth));
808 
809  m_bgr_picture_mutex.lock();
810  vpImageConvert::BGRToGrey(m_bgr_picture->data[0], reinterpret_cast<unsigned char *>(I.bitmap), I.getWidth(),
811  I.getHeight());
812  m_bgr_picture_mutex.unlock();
813  }
814  else {
815  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Error while getting current grayscale image : image data is nullptr");
816  }
817 
818  }
819  else {
820  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't get current image : video streaming isn't started.");
821  }
822 }
823 
833 {
834  if (m_videoDecodingStarted) {
835 
836  if (m_bgr_picture->data[0] != nullptr) {
837  I.resize(static_cast<unsigned int>(m_videoHeight), static_cast<unsigned int>(m_videoWidth));
838 
839  m_bgr_picture_mutex.lock();
840  vpImageConvert::BGRToRGBa(m_bgr_picture->data[0], reinterpret_cast<unsigned char *>(I.bitmap), I.getWidth(),
841  I.getHeight());
842  m_bgr_picture_mutex.unlock();
843  }
844  else {
845  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Error while getting current RGBa image : image data is nullptr");
846  }
847 
848  }
849  else {
850  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't get current image : video streaming isn't started.");
851  }
852 }
853 
859 int vpRobotBebop2::getVideoHeight() { return m_videoHeight; }
860 
866 int vpRobotBebop2::getVideoWidth() { return m_videoWidth; }
867 
876 {
877  if (isRunning() && m_deviceController != nullptr) {
878  expo = std::min<float>(1.5f, std::max<float>(-1.5f, expo));
879 
880  m_exposureSet = false;
881  m_deviceController->aRDrone3->sendPictureSettingsExpositionSelection(m_deviceController->aRDrone3, expo);
882 
883  // m_exposureSet is set back to true when the drone has finished his move, via a callback
884  while (!m_exposureSet) {
885  vpTime::sleepMs(1);
886  }
887  }
888  else {
889  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set exposure : drone isn't running.");
890  }
891 }
892 
908 {
909  if (isRunning() && m_deviceController != nullptr) {
910 
911  if (!isStreaming() && isLanded()) {
912  eARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE cmd_mode =
913  ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_LOW_LATENCY;
914  switch (mode) {
915  case 0:
916  cmd_mode = ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_LOW_LATENCY;
917  break;
918  case 1:
919  cmd_mode = ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_HIGH_RELIABILITY;
920  break;
921  case 2:
922  cmd_mode = ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_HIGH_RELIABILITY_LOW_FRAMERATE;
923  break;
924  default:
925  break;
926  }
927  m_streamingModeSet = false;
928  m_deviceController->aRDrone3->sendMediaStreamingVideoStreamMode(m_deviceController->aRDrone3, cmd_mode);
929 
930  // m_streamingModeSet is set back to true when the drone has finished setting the stream mode, via a callback
931  while (!m_streamingModeSet) {
932  vpTime::sleepMs(1);
933  }
934 
935  }
936  else {
937  ARSAL_PRINT(
938  ARSAL_PRINT_ERROR, "ERROR",
939  "Can't set streaming mode : drone has to be landed and not streaming in order to set streaming mode.");
940  }
941  }
942  else {
943  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set streaming mode : drone isn't running.");
944  }
945 }
946 
959 {
960  if (isRunning() && m_deviceController != nullptr) {
961 
962  if (!isStreaming() && isLanded()) {
963 
964  eARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORESOLUTIONS_TYPE cmd_mode;
965 
966  switch (mode) {
967 
968  case 0:
969  default:
970  cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORESOLUTIONS_TYPE_REC1080_STREAM480;
971  m_videoWidth = 856;
972  m_videoHeight = 480;
973  break;
974 
975  case 1:
976  cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORESOLUTIONS_TYPE_REC720_STREAM720;
977  m_videoWidth = 1280;
978  m_videoHeight = 720;
979  break;
980  }
981 
982  m_videoResolutionSet = false;
983  m_deviceController->aRDrone3->sendPictureSettingsVideoResolutions(m_deviceController->aRDrone3, cmd_mode);
984 
985  // m_videoResolutionSet is set back to true when the drone has finished setting the resolution, via a callback
986  while (!m_videoResolutionSet) {
987  vpTime::sleepMs(1);
988  }
989 
990  }
991  else {
992  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR",
993  "Can't set video resolution : drone has to be landed and not streaming in order to set streaming "
994  "parameters.");
995  }
996  }
997  else {
998  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set video resolution : drone isn't running.");
999  }
1000 }
1001 
1014 {
1015  if (isRunning() && m_deviceController != nullptr) {
1016 
1017  eARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE cmd_mode =
1018  ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_NONE;
1019  switch (mode) {
1020 
1021  case 0:
1022  cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_NONE;
1023  break;
1024  case 1:
1025  cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_ROLL;
1026  break;
1027  case 2:
1028  cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_PITCH;
1029  break;
1030  case 3:
1031  cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_ROLL_PITCH;
1032  break;
1033 
1034  default:
1035  break;
1036  }
1037  m_deviceController->aRDrone3->sendPictureSettingsVideoStabilizationMode(m_deviceController->aRDrone3, cmd_mode);
1038 
1039  }
1040  else {
1041  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set video stabilisation mode : drone isn't running.");
1042  }
1043 }
1044 
1054 {
1055  if (isRunning() && m_deviceController != nullptr) {
1056  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Starting video streaming ... ");
1057 
1058  // Sending command to the drone to start the video stream
1059  m_errorController = m_deviceController->aRDrone3->sendMediaStreamingVideoEnable(m_deviceController->aRDrone3, 1);
1060 
1061  if (m_errorController == ARCONTROLLER_OK) {
1062  m_streamingStarted = false;
1063  // Blocking until streaming is started
1064  while (!m_streamingStarted) {
1065  vpTime::sleepMs(1);
1066  }
1067  startVideoDecoding();
1068 
1069  /* We wait for the streaming to actually start (it has a delay before actually
1070  sending frames, even if it is indicated as started by the drone), or else the
1071  decoder is doesn't have time to synchronize with the stream */
1072  vpTime::sleepMs(4000);
1073 
1074  }
1075  else {
1076  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1077  }
1078 
1079  }
1080  else {
1081  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't start streaming : drone isn't running.");
1082  }
1083 }
1084 
1091 {
1092  if (m_videoDecodingStarted && m_deviceController != nullptr) {
1093  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Stopping video streaming ... ");
1094 
1095  // Sending command to the drone to stop the video stream
1096  m_errorController = m_deviceController->aRDrone3->sendMediaStreamingVideoEnable(m_deviceController->aRDrone3, 0);
1097 
1098  if (m_errorController == ARCONTROLLER_OK) {
1099 
1100  // Blocking until streaming is stopped
1101  while (getStreamingState() != ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_DISABLED) {
1102  vpTime::sleepMs(1);
1103  }
1104  vpTime::sleepMs(500); // We wait for the streaming to actually stops or else it causes segmentation fault.
1105  stopVideoDecoding();
1106 
1107  }
1108  else {
1109  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1110  }
1111 
1112  }
1113  else {
1114  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't stop streaming : streaming already stopped.");
1115  }
1116 }
1117 
1118 #endif // #ifdef VISP_HAVE_FFMPEG
1119 
1120 //*** ***//
1121 //*** Private Functions ***//
1122 //*** ***//
1123 
1128 void vpRobotBebop2::sighandler(int signo)
1129 {
1130  std::cout << "Stopping Bebop2 because of detected signal (" << signo << "): " << static_cast<char>(7);
1131  switch (signo) {
1132  case SIGINT:
1133  std::cout << "SIGINT (stopped by ^C) " << std::endl;
1134  break;
1135  case SIGBUS:
1136  std::cout << "SIGBUS (stopped due to a bus error) " << std::endl;
1137  break;
1138  case SIGSEGV:
1139  std::cout << "SIGSEGV (stopped due to a segmentation fault) " << std::endl;
1140  break;
1141  case SIGKILL:
1142  std::cout << "SIGKILL (stopped by CTRL \\) " << std::endl;
1143  break;
1144  case SIGQUIT:
1145  std::cout << "SIGQUIT " << std::endl;
1146  break;
1147  default:
1148  std::cout << signo << std::endl;
1149  }
1150 
1151  vpRobotBebop2::m_running = false;
1152 
1153  // Landing the drone
1154  if (m_deviceController != nullptr) {
1155  m_deviceController->aRDrone3->sendPilotingLanding(m_deviceController->aRDrone3);
1156  }
1157  std::exit(EXIT_FAILURE);
1158 }
1159 
1164 eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE vpRobotBebop2::getFlyingState()
1165 {
1166  if (m_deviceController != nullptr) {
1167  eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE flyingState =
1168  ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_MAX;
1169  eARCONTROLLER_ERROR error;
1170 
1171  ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary = ARCONTROLLER_ARDrone3_GetCommandElements(
1172  m_deviceController->aRDrone3, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED, &error);
1173 
1174  if (error == ARCONTROLLER_OK && elementDictionary != nullptr) {
1175  ARCONTROLLER_DICTIONARY_ARG_t *arg = nullptr;
1176  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = nullptr;
1177 
1178  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1179 
1180  if (element != nullptr) {
1181  // Suppress warnings
1182  // Get the value
1183  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE,
1184  arg);
1185 
1186  if (arg != nullptr) {
1187  // Enums are stored as I32
1188  flyingState = static_cast<eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE>(arg->value.I32);
1189  }
1190  }
1191  }
1192  return flyingState;
1193  }
1194  else {
1195  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error when checking flying state : drone isn't connected.");
1196  return ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_MAX;
1197  }
1198 }
1199 
1204 eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED vpRobotBebop2::getStreamingState()
1205 {
1206  if (m_deviceController != nullptr) {
1207  eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED streamingState =
1208  ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_MAX;
1209  eARCONTROLLER_ERROR error;
1210 
1211  ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary = ARCONTROLLER_ARDrone3_GetCommandElements(
1212  m_deviceController->aRDrone3, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED,
1213  &error);
1214 
1215  if (error == ARCONTROLLER_OK && elementDictionary != nullptr) {
1216  ARCONTROLLER_DICTIONARY_ARG_t *arg = nullptr;
1217  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = nullptr;
1218 
1219  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1220 
1221  if (element != nullptr) {
1222  // Get the value
1223  HASH_FIND_STR(element->arguments,
1224  ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED, arg);
1225 
1226  if (arg != nullptr) {
1227  // Enums are stored as I32
1228  streamingState =
1229  static_cast<eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED>(arg->value.I32);
1230  }
1231  }
1232  }
1233  return streamingState;
1234  }
1235  else {
1236  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error when checking streaming state : drone isn't connected.");
1237  return ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_MAX;
1238  }
1239 }
1240 
1245 ARDISCOVERY_Device_t *vpRobotBebop2::discoverDrone()
1246 {
1247  eARDISCOVERY_ERROR errorDiscovery = ARDISCOVERY_OK;
1248 
1249  ARDISCOVERY_Device_t *device = ARDISCOVERY_Device_New(&errorDiscovery);
1250 
1251  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Starting drone Wifi discovery ...");
1252  const char *charIpAddress = m_ipAddress.c_str();
1253  errorDiscovery =
1254  ARDISCOVERY_Device_InitWifi(device, ARDISCOVERY_PRODUCT_BEBOP_2, "bebop2", charIpAddress, m_discoveryPort);
1255 
1256  if (errorDiscovery != ARDISCOVERY_OK) {
1257  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Discovery error :%s", ARDISCOVERY_Error_ToString(errorDiscovery));
1258  }
1259  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Drone controller created.");
1260  return device;
1261 }
1262 
1269 void vpRobotBebop2::createDroneController(ARDISCOVERY_Device_t *discoveredDrone)
1270 {
1271  m_deviceController = ARCONTROLLER_Device_New(discoveredDrone, &m_errorController);
1272  if (m_errorController != ARCONTROLLER_OK) {
1273  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Creation of deviceController failed.");
1274  }
1275  ARDISCOVERY_Device_Delete(&discoveredDrone);
1276  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Device created.");
1277 }
1278 
1283 void vpRobotBebop2::setupCallbacks()
1284 {
1285  // Adding stateChanged callback, called when the state of the controller has changed
1286  m_errorController = ARCONTROLLER_Device_AddStateChangedCallback(m_deviceController, stateChangedCallback, this);
1287  if (m_errorController != ARCONTROLLER_OK) {
1288  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "add State callback failed.");
1289  }
1290 
1291  // Adding commendReceived callback, called when the a command has been received from the device
1292  m_errorController = ARCONTROLLER_Device_AddCommandReceivedCallback(m_deviceController, commandReceivedCallback, this);
1293 
1294  if (m_errorController != ARCONTROLLER_OK) {
1295  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "add Command callback failed.");
1296  }
1297 
1298 #ifdef VISP_HAVE_FFMPEG
1299  // Adding frame received callback, called when a streaming frame has been received from the device
1300  m_errorController = ARCONTROLLER_Device_SetVideoStreamCallbacks(m_deviceController, decoderConfigCallback,
1301  didReceiveFrameCallback, nullptr, this);
1302 
1303  if (m_errorController != ARCONTROLLER_OK) {
1304  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error: %s", ARCONTROLLER_Error_ToString(m_errorController));
1305  }
1306 #endif
1307  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Callbacks set up.");
1308 }
1309 
1314 void vpRobotBebop2::startController()
1315 {
1316  // Starts the controller
1317  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Connecting ...");
1318  m_errorController = ARCONTROLLER_Device_Start(m_deviceController);
1319 
1320  if (m_errorController != ARCONTROLLER_OK) {
1321  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1322  }
1323 
1324  // Waits for the stateChangedCallback to unclock the semaphore
1325  ARSAL_Sem_Wait(&(m_stateSem));
1326 
1327  // Checks the device state
1328  m_deviceState = ARCONTROLLER_Device_GetState(m_deviceController, &m_errorController);
1329 
1330  if ((m_errorController != ARCONTROLLER_OK) || (m_deviceState != ARCONTROLLER_DEVICE_STATE_RUNNING)) {
1331  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- deviceState :%d", m_deviceState);
1332  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1333  }
1334  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Controller started.");
1335 }
1336 
1337 #ifdef VISP_HAVE_FFMPEG
1343 void vpRobotBebop2::initCodec()
1344 {
1345  av_register_all();
1346  avcodec_register_all();
1347  avformat_network_init();
1348 
1349  // Finds the correct codec
1350  AVCodec *codec = avcodec_find_decoder(AV_CODEC_ID_H264);
1351  if (!codec) {
1352  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Codec not found.");
1353  return;
1354  }
1355 
1356  // Allocates memory for codec
1357  m_codecContext = avcodec_alloc_context3(codec);
1358 
1359  if (!m_codecContext) {
1360  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Failed to allocate codec context.");
1361  return;
1362  }
1363 
1364  // Sets codec parameters (TODO : should be done automaticaly by drone callback decoderConfigCallback)
1365  m_codecContext->pix_fmt = AV_PIX_FMT_YUV420P;
1366  m_codecContext->skip_frame = AVDISCARD_DEFAULT;
1367  m_codecContext->error_concealment = FF_EC_GUESS_MVS | FF_EC_DEBLOCK;
1368  m_codecContext->skip_loop_filter = AVDISCARD_DEFAULT;
1369  m_codecContext->workaround_bugs = AVMEDIA_TYPE_VIDEO;
1370  m_codecContext->codec_id = AV_CODEC_ID_H264;
1371  m_codecContext->skip_idct = AVDISCARD_DEFAULT;
1372 
1373  m_codecContext->width = m_videoWidth;
1374  m_codecContext->height = m_videoHeight;
1375 
1376  if (codec->capabilities & AV_CODEC_CAP_TRUNCATED) {
1377  m_codecContext->flags |= AV_CODEC_FLAG_TRUNCATED;
1378  }
1379  m_codecContext->flags2 |= AV_CODEC_FLAG2_CHUNKS;
1380 
1381  // Opens the codec
1382  if (avcodec_open2(m_codecContext, codec, nullptr) < 0) {
1383  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Failed to open codec.");
1384  return;
1385  }
1386 
1387  AVPixelFormat pFormat = AV_PIX_FMT_BGR24;
1388  int numBytes = av_image_get_buffer_size(pFormat, m_codecContext->width, m_codecContext->height, 1);
1389  m_buffer = static_cast<uint8_t *>(av_malloc(static_cast<unsigned long>(numBytes) * sizeof(uint8_t)));
1390 
1391  av_init_packet(&m_packet); // Packed used to send data to the decoder
1392  m_picture = av_frame_alloc(); // Frame used to receive data from the decoder
1393 
1394  m_bgr_picture_mutex.lock();
1395  m_bgr_picture = av_frame_alloc(); // Frame used to store rescaled frame received from the decoder
1396  m_bgr_picture_mutex.unlock();
1397 
1398  m_img_convert_ctx = sws_getContext(m_codecContext->width, m_codecContext->height, m_codecContext->pix_fmt,
1399  m_codecContext->width, m_codecContext->height, pFormat, SWS_BICUBIC, nullptr, nullptr,
1400  nullptr); // Used to rescale frame received from the decoder
1401 }
1402 
1408 void vpRobotBebop2::cleanUpCodec()
1409 {
1410  m_videoDecodingStarted = false;
1411  av_packet_unref(&m_packet);
1412 
1413  if (m_codecContext) {
1414  avcodec_flush_buffers(m_codecContext);
1415  avcodec_free_context(&m_codecContext);
1416  }
1417 
1418  if (m_picture) {
1419  av_frame_free(&m_picture);
1420  }
1421 
1422  if (m_bgr_picture) {
1423  m_bgr_picture_mutex.lock();
1424  av_frame_free(&m_bgr_picture);
1425  m_bgr_picture_mutex.unlock();
1426  }
1427 
1428  if (m_img_convert_ctx) {
1429  sws_freeContext(m_img_convert_ctx);
1430  }
1431  if (m_buffer) {
1432  av_free(m_buffer);
1433  }
1434 }
1435 
1441 void vpRobotBebop2::startVideoDecoding()
1442 {
1443  if (!m_videoDecodingStarted) {
1444  initCodec();
1445  m_videoDecodingStarted = true;
1446  }
1447  else {
1448  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Video decoding is already started.");
1449  }
1450 }
1451 
1457 void vpRobotBebop2::stopVideoDecoding()
1458 {
1459  if (m_videoDecodingStarted) {
1460  cleanUpCodec();
1461  }
1462  else {
1463  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Video decoding is already stopped.");
1464  }
1465 }
1466 
1474 void vpRobotBebop2::computeFrame(ARCONTROLLER_Frame_t *frame)
1475 {
1476 
1477  // Updating codec parameters from SPS and PPS buffers received from the drone in decoderConfigCallback
1478  if (m_update_codec_params && m_codec_params_data.size()) {
1479  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Updating H264 codec parameters (Buffer Size: %lu) ...",
1480  m_codec_params_data.size());
1481 
1482  m_packet.data = &m_codec_params_data[0];
1483  m_packet.size = static_cast<int>(m_codec_params_data.size());
1484 
1485  int ret = avcodec_send_packet(m_codecContext, &m_packet);
1486 
1487  if (ret == 0) {
1488 
1489  ret = avcodec_receive_frame(m_codecContext, m_picture);
1490 
1491  if (ret == 0 || ret == AVERROR(EAGAIN)) {
1492  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "H264 codec parameters updated.");
1493  }
1494  else {
1495  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Unexpected error while updating H264 parameters.");
1496  }
1497  }
1498  else {
1499  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Unexpected error while sending H264 parameters.");
1500  }
1501  m_update_codec_params = false;
1502  av_packet_unref(&m_packet);
1503  av_frame_unref(m_picture);
1504  }
1505 
1506  // Decoding frame coming from the drone
1507  m_packet.data = frame->data;
1508  m_packet.size = static_cast<int>(frame->used);
1509 
1510  int ret = avcodec_send_packet(m_codecContext, &m_packet);
1511  if (ret < 0) {
1512 
1513  char *errbuff = new char[AV_ERROR_MAX_STRING_SIZE];
1514  av_strerror(ret, errbuff, AV_ERROR_MAX_STRING_SIZE);
1515  std::string err(errbuff);
1516  delete[] errbuff;
1517  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error sending a packet for decoding : %d, %s", ret, err.c_str());
1518 
1519  }
1520  else {
1521 
1522  ret = avcodec_receive_frame(m_codecContext, m_picture);
1523 
1524  if (ret < 0) {
1525 
1526  if (ret == AVERROR(EAGAIN)) {
1527  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "AVERROR(EAGAIN)"); // Not an error, need to send data again
1528  }
1529  else if (ret == AVERROR_EOF) {
1530  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "AVERROR_EOF"); // End of file reached, should not happen with drone footage
1531  }
1532  else {
1533 
1534  char *errbuff = new char[AV_ERROR_MAX_STRING_SIZE];
1535  av_strerror(ret, errbuff, AV_ERROR_MAX_STRING_SIZE);
1536  std::string err(errbuff);
1537  delete[] errbuff;
1538  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error receiving a decoded frame : %d, %s", ret, err.c_str());
1539  }
1540  }
1541  else {
1542  m_bgr_picture_mutex.lock();
1543  av_frame_unref(m_bgr_picture);
1544  av_image_fill_arrays(m_bgr_picture->data, m_bgr_picture->linesize, m_buffer, AV_PIX_FMT_BGR24,
1545  m_codecContext->width, m_codecContext->height, 1);
1546 
1547  sws_scale(m_img_convert_ctx, (m_picture)->data, (m_picture)->linesize, 0, m_codecContext->height,
1548  (m_bgr_picture)->data, (m_bgr_picture)->linesize);
1549 
1550  m_bgr_picture_mutex.unlock();
1551  }
1552  }
1553 
1554  av_packet_unref(&m_packet);
1555 
1556  av_frame_unref(m_picture);
1557 }
1558 #endif // #ifdef VISP_HAVE_FFMPEG
1564 void vpRobotBebop2::cleanUp()
1565 {
1566  if (m_deviceController != nullptr) {
1567  // Lands the drone if not landed
1568  land();
1569 
1570 #ifdef VISP_HAVE_FFMPEG
1571  // Stops the streaming if not stopped
1572  stopStreaming();
1573 #endif
1574 
1575  // Deletes the controller
1576  m_deviceState = ARCONTROLLER_Device_GetState(m_deviceController, &m_errorController);
1577  if ((m_errorController == ARCONTROLLER_OK) && (m_deviceState != ARCONTROLLER_DEVICE_STATE_STOPPED)) {
1578 
1579  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Disconnecting ...");
1580  m_errorController = ARCONTROLLER_Device_Stop(m_deviceController);
1581 
1582  if (m_errorController == ARCONTROLLER_OK) {
1583  // Wait for the semaphore to increment, it will when the controller changes its state to 'stopped'
1584  ARSAL_Sem_Wait(&(m_stateSem));
1585  }
1586  }
1587  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Deleting device controller ...");
1588  ARCONTROLLER_Device_Delete(&m_deviceController);
1589 
1590  // Destroys the semaphore
1591  ARSAL_Sem_Destroy(&(m_stateSem));
1592 
1593  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Cleanup done.");
1594  }
1595  else {
1596 
1597  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error while cleaning up memory.");
1598  }
1599  m_running = false;
1600 }
1601 
1602 //*** ***//
1603 //*** Callbacks ***//
1604 //*** ***//
1605 
1614 void vpRobotBebop2::stateChangedCallback(eARCONTROLLER_DEVICE_STATE newState, eARCONTROLLER_ERROR error,
1615  void *customData)
1616 {
1617  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Controller state changed, new state: %d.", newState);
1618  (void)error;
1619 
1620  vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1621  switch (newState) {
1622  case ARCONTROLLER_DEVICE_STATE_STOPPED:
1623  // Stopping the programm
1624  drone->m_running = false;
1625  // Incrementing semaphore
1626  ARSAL_Sem_Post(&(drone->m_stateSem));
1627  break;
1628 
1629  case ARCONTROLLER_DEVICE_STATE_RUNNING:
1630  // Incrementing semaphore
1631  ARSAL_Sem_Post(&(drone->m_stateSem));
1632  break;
1633 
1634  default:
1635  break;
1636  }
1637 }
1638 
1639 #ifdef VISP_HAVE_FFMPEG
1649 eARCONTROLLER_ERROR vpRobotBebop2::decoderConfigCallback(ARCONTROLLER_Stream_Codec_t codec, void *customData)
1650 {
1651  vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1652 
1653  uint8_t *sps_buffer_ptr = codec.parameters.h264parameters.spsBuffer;
1654  uint32_t sps_buffer_size = static_cast<uint32_t>(codec.parameters.h264parameters.spsSize);
1655  uint8_t *pps_buffer_ptr = codec.parameters.h264parameters.ppsBuffer;
1656  uint32_t pps_buffer_size = static_cast<uint32_t>(codec.parameters.h264parameters.ppsSize);
1657 
1658  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "H264 configuration packet received: #SPS: %d #PPS: %d", sps_buffer_size,
1659  pps_buffer_size);
1660 
1661  drone->m_update_codec_params = (sps_buffer_ptr && pps_buffer_ptr && sps_buffer_size && pps_buffer_size &&
1662  (pps_buffer_size < 32) && (sps_buffer_size < 32));
1663 
1664  if (drone->m_update_codec_params) {
1665  // If codec parameters where received from the drone, we store them to pass them to the decoder in the next call of
1666  // computeFrame
1667  drone->m_codec_params_data.resize(sps_buffer_size + pps_buffer_size);
1668  std::copy(sps_buffer_ptr, sps_buffer_ptr + sps_buffer_size, drone->m_codec_params_data.begin());
1669  std::copy(pps_buffer_ptr, pps_buffer_ptr + pps_buffer_size, drone->m_codec_params_data.begin() + sps_buffer_size);
1670  }
1671  else {
1672  // If data is invalid, we clear the vector
1673  drone->m_codec_params_data.clear();
1674  }
1675  return ARCONTROLLER_OK;
1676 }
1677 
1686 eARCONTROLLER_ERROR vpRobotBebop2::didReceiveFrameCallback(ARCONTROLLER_Frame_t *frame, void *customData)
1687 {
1688  vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1689 
1690  if (frame != nullptr) {
1691 
1692  if (drone->m_videoDecodingStarted) {
1693  drone->computeFrame(frame);
1694  }
1695 
1696  }
1697  else {
1698  ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, "frame is nullptr.");
1699  }
1700 
1701  return ARCONTROLLER_OK;
1702 }
1703 #endif // #ifdef VISP_HAVE_FFMPEG
1704 
1714 void vpRobotBebop2::cmdBatteryStateChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1715  vpRobotBebop2 *drone)
1716 {
1717  ARCONTROLLER_DICTIONARY_ARG_t *arg = nullptr;
1718  ARCONTROLLER_DICTIONARY_ELEMENT_t *singleElement = nullptr;
1719 
1720  if (elementDictionary == nullptr) {
1721  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "elements is nullptr");
1722  return;
1723  }
1724 
1725  // Get the command received in the device controller
1726  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, singleElement);
1727 
1728  if (singleElement == nullptr) {
1729  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "singleElement is nullptr");
1730  return;
1731  }
1732 
1733  // Get the value
1734  HASH_FIND_STR(singleElement->arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_BATTERYSTATECHANGED_PERCENT,
1735  arg);
1736 
1737  if (arg == nullptr) {
1738  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "arg is nullptr");
1739  return;
1740  }
1741  drone->m_batteryLevel = arg->value.U8;
1742  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Battery level changed : %u percent remaining.", drone->m_batteryLevel);
1743 
1744  if (drone->m_batteryLevel <= 5) {
1745  ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, " - WARNING, very low battery level, drone will stop soon !");
1746  }
1747  else if (drone->m_batteryLevel <= 10) {
1748  ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, " - Warning, low battery level !");
1749  }
1750 }
1751 
1762 void vpRobotBebop2::cmdCameraOrientationChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1763  vpRobotBebop2 *drone)
1764 {
1765  ARCONTROLLER_DICTIONARY_ARG_t *arg = nullptr;
1766  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = nullptr;
1767  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1768  if (element != nullptr) {
1769  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2_TILT, arg);
1770 
1771  if (arg != nullptr) {
1772  drone->m_currentCameraTilt = static_cast<double>(arg->value.Float);
1773  }
1774 
1775  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2_PAN, arg);
1776  if (arg != nullptr) {
1777  drone->m_currentCameraPan = static_cast<double>(arg->value.Float);
1778  }
1779  }
1780 }
1781 
1793 void vpRobotBebop2::cmdCameraSettingsRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone)
1794 {
1795  ARCONTROLLER_DICTIONARY_ARG_t *arg = nullptr;
1796  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = nullptr;
1797  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1798  if (element != nullptr) {
1799  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_FOV,
1800  arg);
1801  if (arg != nullptr) {
1802  drone->m_cameraHorizontalFOV = static_cast<double>(arg->value.Float);
1803  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Camera horizontal FOV : %f degrees.",
1804  static_cast<double>(drone->m_cameraHorizontalFOV));
1805  }
1806  HASH_FIND_STR(element->arguments,
1807  ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_PANMAX, arg);
1808  if (arg != nullptr) {
1809  drone->m_maxCameraPan = static_cast<double>(arg->value.Float);
1810  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Max camera pan : %f degrees.",
1811  static_cast<double>(drone->m_maxCameraPan));
1812  }
1813  HASH_FIND_STR(element->arguments,
1814  ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_PANMIN, arg);
1815  if (arg != nullptr) {
1816  drone->m_minCameraPan = static_cast<double>(arg->value.Float);
1817  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Min camera pan : %f degrees.",
1818  static_cast<double>(drone->m_minCameraPan));
1819  }
1820  HASH_FIND_STR(element->arguments,
1821  ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_TILTMAX, arg);
1822  if (arg != nullptr) {
1823  drone->m_maxCameraTilt = static_cast<double>(arg->value.Float);
1824  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Max camera tilt : %f degrees.",
1825  static_cast<double>(drone->m_maxCameraTilt));
1826  }
1827  HASH_FIND_STR(element->arguments,
1828  ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_TILTMIN, arg);
1829  if (arg != nullptr) {
1830  drone->m_minCameraTilt = static_cast<double>(arg->value.Float);
1831  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Min camera tilt : %f degrees.",
1832  static_cast<double>(drone->m_minCameraTilt));
1833  }
1834  }
1835 }
1836 
1847 void vpRobotBebop2::cmdMaxPitchRollChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1848  vpRobotBebop2 *drone)
1849 {
1850  ARCONTROLLER_DICTIONARY_ARG_t *arg = nullptr;
1851  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = nullptr;
1852 
1853  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1854  if (element != nullptr) {
1855  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXTILTCHANGED_CURRENT,
1856  arg);
1857  if (arg != nullptr) {
1858  drone->m_maxTilt = static_cast<double>(arg->value.Float);
1859  }
1860  }
1861 }
1862 
1873 void vpRobotBebop2::cmdRelativeMoveEndedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone)
1874 {
1875  ARCONTROLLER_DICTIONARY_ARG_t *arg = nullptr;
1876  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = nullptr;
1877 
1878  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1879 
1880  if (element != nullptr) {
1881  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR, arg);
1882 
1883  if (arg != nullptr) {
1884  eARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR error =
1885  static_cast<eARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR>(arg->value.I32);
1886  if ((error != ARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR_OK) &&
1887  (error != ARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR_INTERRUPTED)) {
1888  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Relative move ended with error %d", error);
1889  }
1890  drone->m_relativeMoveEnded = true;
1891  }
1892  }
1893 }
1894 
1905 void vpRobotBebop2::cmdExposureSetRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone)
1906 {
1907  ARCONTROLLER_DICTIONARY_ARG_t *arg = nullptr;
1908  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = nullptr;
1909 
1910  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1911 
1912  if (element != nullptr) {
1913 
1914  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED_VALUE,
1915  arg);
1916 
1917  if (arg != nullptr) {
1918  drone->m_exposureSet = true;
1919  }
1920  }
1921 }
1922 
1932 void vpRobotBebop2::commandReceivedCallback(eARCONTROLLER_DICTIONARY_KEY commandKey,
1933  ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, void *customData)
1934 {
1935  vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1936 
1937  if (drone == nullptr)
1938  return;
1939 
1940  switch (commandKey) {
1941  case ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_BATTERYSTATECHANGED:
1942  // If the command received is a battery state changed
1943  cmdBatteryStateChangedRcv(elementDictionary, drone);
1944  break;
1945 
1946  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXTILTCHANGED:
1947  // If the command receivend is a max pitch/roll changed
1948  cmdMaxPitchRollChangedRcv(elementDictionary, drone);
1949  break;
1950 
1951  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGEVENT_MOVEBYEND:
1952  // If the command received is a relative move ended
1953  cmdRelativeMoveEndedRcv(elementDictionary, drone);
1954  break;
1955 
1956  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLATTRIMCHANGED:
1957  // If the command received is a flat trim finished
1958  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Flat trim finished ...");
1959  drone->m_flatTrimFinished = true;
1960  break;
1961 
1962  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED:
1963  // If the command received is a exposition changed
1964  cmdExposureSetRcv(elementDictionary, drone);
1965  break;
1966 
1967  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_VIDEORESOLUTIONSCHANGED:
1968  // If the command received is a resolution changed
1969  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Video resolution set ...");
1970  drone->m_videoResolutionSet = true;
1971  break;
1972 
1973  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED:
1974  // If the command received is a streaming started
1975  drone->m_streamingStarted = true;
1976  break;
1977 
1978  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOSTREAMMODECHANGED:
1979  // If the command received is a streaming mode changed
1980  drone->m_streamingModeSet = true;
1981  break;
1982 
1983  case ARCONTROLLER_DICTIONARY_KEY_COMMON_SETTINGSSTATE_RESETCHANGED:
1984  // If the command received is a settings reset
1985  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Settings reset ...");
1986  drone->m_settingsReset = true;
1987  break;
1988 
1989  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2:
1990  // If the command received is a camera orientation changed
1991  cmdCameraOrientationChangedRcv(elementDictionary, drone);
1992  break;
1993 
1994  case ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED:
1995  // If the command received is a camera information sent
1996  cmdCameraSettingsRcv(elementDictionary, drone);
1997  break;
1998 
1999  default:
2000  break;
2001  }
2002 }
2003 
2004 #undef TAG
2005 
2006 #elif !defined(VISP_BUILD_SHARED_LIBS)
2007 // Work around to avoid warning: libvisp_robot.a(vpRobotBebop2.cpp.o) has
2008 // no symbols
2009 void dummy_vpRobotBebop2() { };
2010 #endif // VISP_HAVE_ARSDK
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:341
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ fatalError
Fatal error.
Definition: vpException.h:84
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:245
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:798
Type * bitmap
points toward the bitmap
Definition: vpImage.h:139
unsigned int getHeight() const
Definition: vpImage.h:184
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)