Visual Servoing Platform  version 3.6.1 under development (2023-12-06)
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  } else {
188  m_running = true;
189 
190 #ifdef VISP_HAVE_FFMPEG
192 #endif
193  if (setDefaultSettings) {
195 
196  setMaxTilt(10);
197 
198 #ifdef VISP_HAVE_FFMPEG
200  setExposure(0);
201  setStreamingMode(0);
202 #endif
203  setCameraOrientation(0, 0, true);
204  }
205  }
206 }
207 
214 
222 {
223  if (isRunning() && m_deviceController != nullptr && isLanded()) {
224 
225  m_flatTrimFinished = false;
226 
227  m_deviceController->aRDrone3->sendPilotingFlatTrim(m_deviceController->aRDrone3);
228 
229  // m_flatTrimFinished is set back to true when the drone has finished the calibration, via a callback
230  while (!m_flatTrimFinished) {
231  vpTime::sleepMs(1);
232  }
233  } else {
234  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't do a flat trim : drone isn't landed.");
235  }
236 }
237 
242 std::string vpRobotBebop2::getIpAddress() { return m_ipAddress; }
243 
248 int vpRobotBebop2::getDiscoveryPort() { return m_discoveryPort; }
249 
254 double vpRobotBebop2::getMaxTilt() { return m_maxTilt; }
255 
262 unsigned int vpRobotBebop2::getBatteryLevel() { return m_batteryLevel; }
263 
268 double vpRobotBebop2::getCameraHorizontalFOV() const { return m_cameraHorizontalFOV; }
269 
274 double vpRobotBebop2::getCurrentCameraTilt() const { return m_currentCameraTilt; }
275 
280 double vpRobotBebop2::getMinCameraTilt() const { return m_minCameraTilt; }
281 
286 double vpRobotBebop2::getMaxCameraTilt() const { return m_maxCameraTilt; }
287 
292 double vpRobotBebop2::getCurrentCameraPan() const { return m_currentCameraPan; }
293 
298 double vpRobotBebop2::getMinCameraPan() const { return m_minCameraPan; }
299 
304 double vpRobotBebop2::getMaxCameraPan() const { return m_maxCameraPan; }
305 
318 void vpRobotBebop2::setCameraOrientation(double tilt, double pan, bool blocking)
319 {
320  if (isRunning() && m_deviceController != nullptr) {
321 
322  m_deviceController->aRDrone3->sendCameraOrientationV2(m_deviceController->aRDrone3, static_cast<float>(tilt),
323  static_cast<float>(pan));
324 
325  if (blocking) {
326  while (std::abs(tilt - m_currentCameraTilt) > 0.01 || std::abs(pan - m_currentCameraPan) > 0.01) {
327  vpTime::sleepMs(1);
328  }
329  }
330 
331  } else {
332  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set camera orientation : drone isn't running.");
333  }
334 }
335 
347 void vpRobotBebop2::setCameraTilt(double tilt, bool blocking)
348 {
349  if (isRunning() && m_deviceController != nullptr) {
350 
351  m_deviceController->aRDrone3->sendCameraOrientationV2(m_deviceController->aRDrone3, static_cast<float>(tilt),
352  static_cast<float>(getCurrentCameraPan()));
353 
354  if (blocking) {
355  while (std::abs(tilt - m_currentCameraTilt) > 0.01) {
356  vpTime::sleepMs(1);
357  }
358  }
359 
360  } else {
361  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set camera tilt value : drone isn't running.");
362  }
363 }
364 
376 void vpRobotBebop2::setCameraPan(double pan, bool blocking)
377 {
378  if (isRunning() && m_deviceController != nullptr) {
379 
380  m_deviceController->aRDrone3->sendCameraOrientationV2(
381  m_deviceController->aRDrone3, static_cast<float>(getCurrentCameraTilt()), static_cast<float>(pan));
382 
383  if (blocking) {
384  while (std::abs(pan - m_currentCameraPan) > 0.01) {
385  vpTime::sleepMs(1);
386  }
387  }
388 
389  } else {
390  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set camera pan value : drone isn't running.");
391  }
392 }
393 
399 {
400  if (m_deviceController == nullptr) {
401  return false;
402  } else {
403  return m_running;
404  }
405 }
406 
412 {
413 #ifdef VISP_HAVE_FFMPEG
414  return m_videoDecodingStarted;
415 #else
416  return false;
417 #endif
418 }
419 
425 {
426  return getFlyingState() == ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_HOVERING;
427 }
428 
434 {
435  return getFlyingState() == ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_FLYING;
436 }
437 
443 {
444  return getFlyingState() == ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_LANDED;
445 }
446 
454 void vpRobotBebop2::takeOff(bool blocking)
455 {
456  if (isRunning() && isLanded() && m_deviceController != nullptr) {
457 
458  m_deviceController->aRDrone3->sendPilotingTakeOff(m_deviceController->aRDrone3);
459 
460  if (blocking) {
461  while (!isHovering()) {
462  vpTime::sleepMs(1);
463  }
464  }
465 
466  } else {
467  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't take off : drone isn't landed.");
468  }
469 }
470 
478 {
479  if (m_deviceController != nullptr) {
480  m_deviceController->aRDrone3->sendPilotingLanding(m_deviceController->aRDrone3);
481  }
482 }
483 
495 {
496  if (isRunning() && m_deviceController != nullptr && (isFlying() || isHovering())) {
497  m_errorController =
498  m_deviceController->aRDrone3->setPilotingPCMDGaz(m_deviceController->aRDrone3, static_cast<char>(value));
499 
500  if (m_errorController != ARCONTROLLER_OK) {
501  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error when sending move command : %s",
502  ARCONTROLLER_Error_ToString(m_errorController));
503  }
504 
505  } else {
506  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set vertical speed : drone isn't flying or hovering.");
507  }
508 }
509 
521 {
522  if (isRunning() && m_deviceController != nullptr && (isFlying() || isHovering())) {
523 
524  m_errorController =
525  m_deviceController->aRDrone3->setPilotingPCMDYaw(m_deviceController->aRDrone3, static_cast<char>(value));
526 
527  if (m_errorController != ARCONTROLLER_OK) {
528  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error when sending move command : %s",
529  ARCONTROLLER_Error_ToString(m_errorController));
530  }
531 
532  } else {
533  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set yaw speed : drone isn't flying or hovering.");
534  }
535 }
536 
547 void vpRobotBebop2::setPitch(int value)
548 {
549  if (isRunning() && m_deviceController != nullptr && (isFlying() || isHovering())) {
550 
551  m_errorController =
552  m_deviceController->aRDrone3->setPilotingPCMDPitch(m_deviceController->aRDrone3, static_cast<char>(value));
553  m_errorController = m_deviceController->aRDrone3->setPilotingPCMDFlag(m_deviceController->aRDrone3, 1);
554 
555  if (m_errorController != ARCONTROLLER_OK) {
556  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error when sending move command : %s",
557  ARCONTROLLER_Error_ToString(m_errorController));
558  }
559 
560  } else {
561  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set pitch value : drone isn't flying or hovering.");
562  }
563 }
564 
575 void vpRobotBebop2::setRoll(int value)
576 {
577  if (isRunning() && m_deviceController != nullptr && (isFlying() || isHovering())) {
578 
579  m_errorController =
580  m_deviceController->aRDrone3->setPilotingPCMDRoll(m_deviceController->aRDrone3, static_cast<char>(value));
581  m_errorController = m_deviceController->aRDrone3->setPilotingPCMDFlag(m_deviceController->aRDrone3, 1);
582 
583  if (m_errorController != ARCONTROLLER_OK) {
584  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error when sending move command : %s",
585  ARCONTROLLER_Error_ToString(m_errorController));
586  }
587 
588  } else {
589  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set roll value : drone isn't flying or hovering.");
590  }
591 }
592 
600 {
601  if (m_deviceController != nullptr) {
602  m_errorController = m_deviceController->aRDrone3->sendPilotingEmergency(m_deviceController->aRDrone3);
603  }
604 }
605 
621 void vpRobotBebop2::setPosition(float dX, float dY, float dZ, float dPsi, bool blocking)
622 {
623  if (isRunning() && m_deviceController != nullptr && (isFlying() || isHovering())) {
624 
625  m_relativeMoveEnded = false;
626  m_deviceController->aRDrone3->sendPilotingMoveBy(m_deviceController->aRDrone3, dX, dY, dZ, dPsi);
627 
628  if (blocking) {
629 
630  // m_relativeMoveEnded is set back to true when the drone has finished his move, via a callback
631  while (!m_relativeMoveEnded) {
632  vpTime::sleepMs(1);
633  }
634  }
635  } else {
636  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't move : drone isn't flying or hovering.");
637  }
638 }
639 
653 void vpRobotBebop2::setPosition(const vpHomogeneousMatrix &M, bool blocking)
654 {
655  double epsilon = (std::numeric_limits<double>::epsilon());
656  if (std::abs(M.getRotationMatrix().getThetaUVector()[0]) >= epsilon) {
657  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't move : rotation around X axis should be 0.");
658  return;
659  }
660  if (std::abs(M.getRotationMatrix().getThetaUVector()[1]) >= epsilon) {
661  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't move : rotation around Y axis should be 0.");
662  return;
663  }
664  float dThetaZ = static_cast<float>(M.getRotationMatrix().getThetaUVector()[2]);
666  setPosition(static_cast<float>(t[0]), static_cast<float>(t[1]), static_cast<float>(t[2]), dThetaZ, blocking);
667 }
668 
680 void vpRobotBebop2::setVelocity(const vpColVector &vel_cmd, double delta_t)
681 {
682 
683  if (vel_cmd.size() != 4) {
684  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR",
685  "Can't set velocity : dimension of the velocity vector should be equal to 4.");
686  stopMoving();
687  return;
688  }
689 
690  vpColVector ve(6);
691 
692  ve[0] = vel_cmd[0];
693  ve[1] = vel_cmd[1];
694  ve[2] = vel_cmd[2];
695  ve[5] = vel_cmd[3];
696 
698  setPosition(M, false);
699 }
700 
709 void vpRobotBebop2::setVerbose(bool verbose)
710 {
711  if (verbose) {
712  ARSAL_Print_SetMinimumLevel(ARSAL_PRINT_INFO);
713  } else {
714  ARSAL_Print_SetMinimumLevel(ARSAL_PRINT_WARNING);
715  }
716 }
717 
723 {
724  if (isRunning() && m_deviceController != nullptr) {
725 
726  m_settingsReset = false;
727  m_deviceController->common->sendSettingsReset(m_deviceController->common);
728 
729  while (!m_settingsReset) {
730  vpTime::sleepMs(1);
731  }
732 
733  } else {
734  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't reset drone settings : drone isn't running.");
735  }
736 }
737 
749 void vpRobotBebop2::setMaxTilt(double maxTilt)
750 {
751  if (isRunning() && m_deviceController != nullptr) {
752  m_deviceController->aRDrone3->sendPilotingSettingsMaxTilt(m_deviceController->aRDrone3,
753  static_cast<float>(maxTilt));
754  } else {
755  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set tilt value : drone isn't running.");
756  }
757 }
758 
767 {
768  if (isRunning() && !isLanded() && m_deviceController != nullptr) {
769  m_errorController = m_deviceController->aRDrone3->setPilotingPCMD(m_deviceController->aRDrone3, 0, 0, 0, 0, 0, 0);
770  }
771 }
772 
773 //*** ***//
774 //*** Streaming functions ***//
775 //*** ***//
776 
777 #ifdef VISP_HAVE_FFMPEG // Functions related to video streaming and decoding requiers FFmpeg
778 
788 {
789  if (m_videoDecodingStarted) {
790 
791  if (m_bgr_picture->data[0] != nullptr) {
792  I.resize(static_cast<unsigned int>(m_videoHeight), static_cast<unsigned int>(m_videoWidth));
793 
794  m_bgr_picture_mutex.lock();
795  vpImageConvert::BGRToGrey(m_bgr_picture->data[0], reinterpret_cast<unsigned char *>(I.bitmap), I.getWidth(),
796  I.getHeight());
797  m_bgr_picture_mutex.unlock();
798  } else {
799  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Error while getting current grayscale image : image data is nullptr");
800  }
801 
802  } else {
803  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't get current image : video streaming isn't started.");
804  }
805 }
806 
816 {
817  if (m_videoDecodingStarted) {
818 
819  if (m_bgr_picture->data[0] != nullptr) {
820  I.resize(static_cast<unsigned int>(m_videoHeight), static_cast<unsigned int>(m_videoWidth));
821 
822  m_bgr_picture_mutex.lock();
823  vpImageConvert::BGRToRGBa(m_bgr_picture->data[0], reinterpret_cast<unsigned char *>(I.bitmap), I.getWidth(),
824  I.getHeight());
825  m_bgr_picture_mutex.unlock();
826  } else {
827  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Error while getting current RGBa image : image data is nullptr");
828  }
829 
830  } else {
831  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't get current image : video streaming isn't started.");
832  }
833 }
834 
840 int vpRobotBebop2::getVideoHeight() { return m_videoHeight; }
841 
847 int vpRobotBebop2::getVideoWidth() { return m_videoWidth; }
848 
857 {
858  if (isRunning() && m_deviceController != nullptr) {
859  expo = std::min(1.5f, std::max(-1.5f, expo));
860 
861  m_exposureSet = false;
862  m_deviceController->aRDrone3->sendPictureSettingsExpositionSelection(m_deviceController->aRDrone3, expo);
863 
864  // m_exposureSet is set back to true when the drone has finished his move, via a callback
865  while (!m_exposureSet) {
866  vpTime::sleepMs(1);
867  }
868  } else {
869  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set exposure : drone isn't running.");
870  }
871 }
872 
888 {
889  if (isRunning() && m_deviceController != nullptr) {
890 
891  if (!isStreaming() && isLanded()) {
892  eARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE cmd_mode =
893  ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_LOW_LATENCY;
894  switch (mode) {
895  case 0:
896  cmd_mode = ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_LOW_LATENCY;
897  break;
898  case 1:
899  cmd_mode = ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_HIGH_RELIABILITY;
900  break;
901  case 2:
902  cmd_mode = ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_HIGH_RELIABILITY_LOW_FRAMERATE;
903  break;
904  default:
905  break;
906  }
907  m_streamingModeSet = false;
908  m_deviceController->aRDrone3->sendMediaStreamingVideoStreamMode(m_deviceController->aRDrone3, cmd_mode);
909 
910  // m_streamingModeSet is set back to true when the drone has finished setting the stream mode, via a callback
911  while (!m_streamingModeSet) {
912  vpTime::sleepMs(1);
913  }
914 
915  } else {
916  ARSAL_PRINT(
917  ARSAL_PRINT_ERROR, "ERROR",
918  "Can't set streaming mode : drone has to be landed and not streaming in order to set streaming mode.");
919  }
920  } else {
921  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set streaming mode : drone isn't running.");
922  }
923 }
924 
937 {
938  if (isRunning() && m_deviceController != nullptr) {
939 
940  if (!isStreaming() && isLanded()) {
941 
942  eARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORESOLUTIONS_TYPE cmd_mode;
943 
944  switch (mode) {
945 
946  case 0:
947  default:
948  cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORESOLUTIONS_TYPE_REC1080_STREAM480;
949  m_videoWidth = 856;
950  m_videoHeight = 480;
951  break;
952 
953  case 1:
954  cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORESOLUTIONS_TYPE_REC720_STREAM720;
955  m_videoWidth = 1280;
956  m_videoHeight = 720;
957  break;
958  }
959 
960  m_videoResolutionSet = false;
961  m_deviceController->aRDrone3->sendPictureSettingsVideoResolutions(m_deviceController->aRDrone3, cmd_mode);
962 
963  // m_videoResolutionSet is set back to true when the drone has finished setting the resolution, via a callback
964  while (!m_videoResolutionSet) {
965  vpTime::sleepMs(1);
966  }
967 
968  } else {
969  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR",
970  "Can't set video resolution : drone has to be landed and not streaming in order to set streaming "
971  "parameters.");
972  }
973  } else {
974  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set video resolution : drone isn't running.");
975  }
976 }
977 
990 {
991  if (isRunning() && m_deviceController != nullptr) {
992 
993  eARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE cmd_mode =
994  ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_NONE;
995  switch (mode) {
996 
997  case 0:
998  cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_NONE;
999  break;
1000  case 1:
1001  cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_ROLL;
1002  break;
1003  case 2:
1004  cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_PITCH;
1005  break;
1006  case 3:
1007  cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_ROLL_PITCH;
1008  break;
1009 
1010  default:
1011  break;
1012  }
1013  m_deviceController->aRDrone3->sendPictureSettingsVideoStabilizationMode(m_deviceController->aRDrone3, cmd_mode);
1014 
1015  } else {
1016  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set video stabilisation mode : drone isn't running.");
1017  }
1018 }
1019 
1029 {
1030  if (isRunning() && m_deviceController != nullptr) {
1031  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Starting video streaming ... ");
1032 
1033  // Sending command to the drone to start the video stream
1034  m_errorController = m_deviceController->aRDrone3->sendMediaStreamingVideoEnable(m_deviceController->aRDrone3, 1);
1035 
1036  if (m_errorController == ARCONTROLLER_OK) {
1037  m_streamingStarted = false;
1038  // Blocking until streaming is started
1039  while (!m_streamingStarted) {
1040  vpTime::sleepMs(1);
1041  }
1042  startVideoDecoding();
1043 
1044  /* We wait for the streaming to actually start (it has a delay before actually
1045  sending frames, even if it is indicated as started by the drone), or else the
1046  decoder is doesn't have time to synchronize with the stream */
1047  vpTime::sleepMs(4000);
1048 
1049  } else {
1050  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1051  }
1052 
1053  } else {
1054  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't start streaming : drone isn't running.");
1055  }
1056 }
1057 
1064 {
1065  if (m_videoDecodingStarted && m_deviceController != nullptr) {
1066  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Stopping video streaming ... ");
1067 
1068  // Sending command to the drone to stop the video stream
1069  m_errorController = m_deviceController->aRDrone3->sendMediaStreamingVideoEnable(m_deviceController->aRDrone3, 0);
1070 
1071  if (m_errorController == ARCONTROLLER_OK) {
1072 
1073  // Blocking until streaming is stopped
1074  while (getStreamingState() != ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_DISABLED) {
1075  vpTime::sleepMs(1);
1076  }
1077  vpTime::sleepMs(500); // We wait for the streaming to actually stops or else it causes segmentation fault.
1078  stopVideoDecoding();
1079 
1080  } else {
1081  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1082  }
1083 
1084  } else {
1085  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't stop streaming : streaming already stopped.");
1086  }
1087 }
1088 
1089 #endif // #ifdef VISP_HAVE_FFMPEG
1090 
1091 //*** ***//
1092 //*** Private Functions ***//
1093 //*** ***//
1094 
1099 void vpRobotBebop2::sighandler(int signo)
1100 {
1101  std::cout << "Stopping Bebop2 because of detected signal (" << signo << "): " << static_cast<char>(7);
1102  switch (signo) {
1103  case SIGINT:
1104  std::cout << "SIGINT (stopped by ^C) " << std::endl;
1105  break;
1106  case SIGBUS:
1107  std::cout << "SIGBUS (stopped due to a bus error) " << std::endl;
1108  break;
1109  case SIGSEGV:
1110  std::cout << "SIGSEGV (stopped due to a segmentation fault) " << std::endl;
1111  break;
1112  case SIGKILL:
1113  std::cout << "SIGKILL (stopped by CTRL \\) " << std::endl;
1114  break;
1115  case SIGQUIT:
1116  std::cout << "SIGQUIT " << std::endl;
1117  break;
1118  default:
1119  std::cout << signo << std::endl;
1120  }
1121 
1122  vpRobotBebop2::m_running = false;
1123 
1124  // Landing the drone
1125  if (m_deviceController != nullptr) {
1126  m_deviceController->aRDrone3->sendPilotingLanding(m_deviceController->aRDrone3);
1127  }
1128  std::exit(EXIT_FAILURE);
1129 }
1130 
1135 eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE vpRobotBebop2::getFlyingState()
1136 {
1137  if (m_deviceController != nullptr) {
1138  eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE flyingState =
1139  ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_MAX;
1140  eARCONTROLLER_ERROR error;
1141 
1142  ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary = ARCONTROLLER_ARDrone3_GetCommandElements(
1143  m_deviceController->aRDrone3, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED, &error);
1144 
1145  if (error == ARCONTROLLER_OK && elementDictionary != nullptr) {
1146  ARCONTROLLER_DICTIONARY_ARG_t *arg = nullptr;
1147  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = nullptr;
1148 
1149  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1150 
1151  if (element != nullptr) {
1152  // Suppress warnings
1153  // Get the value
1154  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE,
1155  arg);
1156 
1157  if (arg != nullptr) {
1158  // Enums are stored as I32
1159  flyingState = static_cast<eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE>(arg->value.I32);
1160  }
1161  }
1162  }
1163  return flyingState;
1164  } else {
1165  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error when checking flying state : drone isn't connected.");
1166  return ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_MAX;
1167  }
1168 }
1169 
1174 eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED vpRobotBebop2::getStreamingState()
1175 {
1176  if (m_deviceController != nullptr) {
1177  eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED streamingState =
1178  ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_MAX;
1179  eARCONTROLLER_ERROR error;
1180 
1181  ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary = ARCONTROLLER_ARDrone3_GetCommandElements(
1182  m_deviceController->aRDrone3, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED,
1183  &error);
1184 
1185  if (error == ARCONTROLLER_OK && elementDictionary != nullptr) {
1186  ARCONTROLLER_DICTIONARY_ARG_t *arg = nullptr;
1187  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = nullptr;
1188 
1189  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1190 
1191  if (element != nullptr) {
1192  // Get the value
1193  HASH_FIND_STR(element->arguments,
1194  ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED, arg);
1195 
1196  if (arg != nullptr) {
1197  // Enums are stored as I32
1198  streamingState =
1199  static_cast<eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED>(arg->value.I32);
1200  }
1201  }
1202  }
1203  return streamingState;
1204  } else {
1205  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error when checking streaming state : drone isn't connected.");
1206  return ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_MAX;
1207  }
1208 }
1209 
1214 ARDISCOVERY_Device_t *vpRobotBebop2::discoverDrone()
1215 {
1216  eARDISCOVERY_ERROR errorDiscovery = ARDISCOVERY_OK;
1217 
1218  ARDISCOVERY_Device_t *device = ARDISCOVERY_Device_New(&errorDiscovery);
1219 
1220  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Starting drone Wifi discovery ...");
1221  const char *charIpAddress = m_ipAddress.c_str();
1222  errorDiscovery =
1223  ARDISCOVERY_Device_InitWifi(device, ARDISCOVERY_PRODUCT_BEBOP_2, "bebop2", charIpAddress, m_discoveryPort);
1224 
1225  if (errorDiscovery != ARDISCOVERY_OK) {
1226  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Discovery error :%s", ARDISCOVERY_Error_ToString(errorDiscovery));
1227  }
1228  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Drone controller created.");
1229  return device;
1230 }
1231 
1238 void vpRobotBebop2::createDroneController(ARDISCOVERY_Device_t *discoveredDrone)
1239 {
1240  m_deviceController = ARCONTROLLER_Device_New(discoveredDrone, &m_errorController);
1241  if (m_errorController != ARCONTROLLER_OK) {
1242  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Creation of deviceController failed.");
1243  }
1244  ARDISCOVERY_Device_Delete(&discoveredDrone);
1245  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Device created.");
1246 }
1247 
1252 void vpRobotBebop2::setupCallbacks()
1253 {
1254  // Adding stateChanged callback, called when the state of the controller has changed
1255  m_errorController = ARCONTROLLER_Device_AddStateChangedCallback(m_deviceController, stateChangedCallback, this);
1256  if (m_errorController != ARCONTROLLER_OK) {
1257  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "add State callback failed.");
1258  }
1259 
1260  // Adding commendReceived callback, called when the a command has been received from the device
1261  m_errorController = ARCONTROLLER_Device_AddCommandReceivedCallback(m_deviceController, commandReceivedCallback, this);
1262 
1263  if (m_errorController != ARCONTROLLER_OK) {
1264  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "add Command callback failed.");
1265  }
1266 
1267 #ifdef VISP_HAVE_FFMPEG
1268  // Adding frame received callback, called when a streaming frame has been received from the device
1269  m_errorController = ARCONTROLLER_Device_SetVideoStreamCallbacks(m_deviceController, decoderConfigCallback,
1270  didReceiveFrameCallback, nullptr, this);
1271 
1272  if (m_errorController != ARCONTROLLER_OK) {
1273  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error: %s", ARCONTROLLER_Error_ToString(m_errorController));
1274  }
1275 #endif
1276  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Callbacks set up.");
1277 }
1278 
1283 void vpRobotBebop2::startController()
1284 {
1285  // Starts the controller
1286  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Connecting ...");
1287  m_errorController = ARCONTROLLER_Device_Start(m_deviceController);
1288 
1289  if (m_errorController != ARCONTROLLER_OK) {
1290  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1291  }
1292 
1293  // Waits for the stateChangedCallback to unclock the semaphore
1294  ARSAL_Sem_Wait(&(m_stateSem));
1295 
1296  // Checks the device state
1297  m_deviceState = ARCONTROLLER_Device_GetState(m_deviceController, &m_errorController);
1298 
1299  if ((m_errorController != ARCONTROLLER_OK) || (m_deviceState != ARCONTROLLER_DEVICE_STATE_RUNNING)) {
1300  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- deviceState :%d", m_deviceState);
1301  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1302  }
1303  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Controller started.");
1304 }
1305 
1306 #ifdef VISP_HAVE_FFMPEG
1312 void vpRobotBebop2::initCodec()
1313 {
1314  av_register_all();
1315  avcodec_register_all();
1316  avformat_network_init();
1317 
1318  // Finds the correct codec
1319  AVCodec *codec = avcodec_find_decoder(AV_CODEC_ID_H264);
1320  if (!codec) {
1321  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Codec not found.");
1322  return;
1323  }
1324 
1325  // Allocates memory for codec
1326  m_codecContext = avcodec_alloc_context3(codec);
1327 
1328  if (!m_codecContext) {
1329  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Failed to allocate codec context.");
1330  return;
1331  }
1332 
1333  // Sets codec parameters (TODO : should be done automaticaly by drone callback decoderConfigCallback)
1334  m_codecContext->pix_fmt = AV_PIX_FMT_YUV420P;
1335  m_codecContext->skip_frame = AVDISCARD_DEFAULT;
1336  m_codecContext->error_concealment = FF_EC_GUESS_MVS | FF_EC_DEBLOCK;
1337  m_codecContext->skip_loop_filter = AVDISCARD_DEFAULT;
1338  m_codecContext->workaround_bugs = AVMEDIA_TYPE_VIDEO;
1339  m_codecContext->codec_id = AV_CODEC_ID_H264;
1340  m_codecContext->skip_idct = AVDISCARD_DEFAULT;
1341 
1342  m_codecContext->width = m_videoWidth;
1343  m_codecContext->height = m_videoHeight;
1344 
1345  if (codec->capabilities & AV_CODEC_CAP_TRUNCATED) {
1346  m_codecContext->flags |= AV_CODEC_FLAG_TRUNCATED;
1347  }
1348  m_codecContext->flags2 |= AV_CODEC_FLAG2_CHUNKS;
1349 
1350  // Opens the codec
1351  if (avcodec_open2(m_codecContext, codec, nullptr) < 0) {
1352  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Failed to open codec.");
1353  return;
1354  }
1355 
1356  AVPixelFormat pFormat = AV_PIX_FMT_BGR24;
1357  int numBytes = av_image_get_buffer_size(pFormat, m_codecContext->width, m_codecContext->height, 1);
1358  m_buffer = static_cast<uint8_t *>(av_malloc(static_cast<unsigned long>(numBytes) * sizeof(uint8_t)));
1359 
1360  av_init_packet(&m_packet); // Packed used to send data to the decoder
1361  m_picture = av_frame_alloc(); // Frame used to receive data from the decoder
1362 
1363  m_bgr_picture_mutex.lock();
1364  m_bgr_picture = av_frame_alloc(); // Frame used to store rescaled frame received from the decoder
1365  m_bgr_picture_mutex.unlock();
1366 
1367  m_img_convert_ctx = sws_getContext(m_codecContext->width, m_codecContext->height, m_codecContext->pix_fmt,
1368  m_codecContext->width, m_codecContext->height, pFormat, SWS_BICUBIC, nullptr, nullptr,
1369  nullptr); // Used to rescale frame received from the decoder
1370 }
1371 
1377 void vpRobotBebop2::cleanUpCodec()
1378 {
1379  m_videoDecodingStarted = false;
1380  av_packet_unref(&m_packet);
1381 
1382  if (m_codecContext) {
1383  avcodec_flush_buffers(m_codecContext);
1384  avcodec_free_context(&m_codecContext);
1385  }
1386 
1387  if (m_picture) {
1388  av_frame_free(&m_picture);
1389  }
1390 
1391  if (m_bgr_picture) {
1392  m_bgr_picture_mutex.lock();
1393  av_frame_free(&m_bgr_picture);
1394  m_bgr_picture_mutex.unlock();
1395  }
1396 
1397  if (m_img_convert_ctx) {
1398  sws_freeContext(m_img_convert_ctx);
1399  }
1400  if (m_buffer) {
1401  av_free(m_buffer);
1402  }
1403 }
1404 
1410 void vpRobotBebop2::startVideoDecoding()
1411 {
1412  if (!m_videoDecodingStarted) {
1413  initCodec();
1414  m_videoDecodingStarted = true;
1415  } else {
1416  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Video decoding is already started.");
1417  }
1418 }
1419 
1425 void vpRobotBebop2::stopVideoDecoding()
1426 {
1427  if (m_videoDecodingStarted) {
1428  cleanUpCodec();
1429  } else {
1430  ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Video decoding is already stopped.");
1431  }
1432 }
1433 
1441 void vpRobotBebop2::computeFrame(ARCONTROLLER_Frame_t *frame)
1442 {
1443 
1444  // Updating codec parameters from SPS and PPS buffers received from the drone in decoderConfigCallback
1445  if (m_update_codec_params && m_codec_params_data.size()) {
1446  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Updating H264 codec parameters (Buffer Size: %lu) ...",
1447  m_codec_params_data.size());
1448 
1449  m_packet.data = &m_codec_params_data[0];
1450  m_packet.size = static_cast<int>(m_codec_params_data.size());
1451 
1452  int ret = avcodec_send_packet(m_codecContext, &m_packet);
1453 
1454  if (ret == 0) {
1455 
1456  ret = avcodec_receive_frame(m_codecContext, m_picture);
1457 
1458  if (ret == 0 || ret == AVERROR(EAGAIN)) {
1459  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "H264 codec parameters updated.");
1460  } else {
1461  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Unexpected error while updating H264 parameters.");
1462  }
1463  } else {
1464  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Unexpected error while sending H264 parameters.");
1465  }
1466  m_update_codec_params = false;
1467  av_packet_unref(&m_packet);
1468  av_frame_unref(m_picture);
1469  }
1470 
1471  // Decoding frame coming from the drone
1472  m_packet.data = frame->data;
1473  m_packet.size = static_cast<int>(frame->used);
1474 
1475  int ret = avcodec_send_packet(m_codecContext, &m_packet);
1476  if (ret < 0) {
1477 
1478  char *errbuff = new char[AV_ERROR_MAX_STRING_SIZE];
1479  av_strerror(ret, errbuff, AV_ERROR_MAX_STRING_SIZE);
1480  std::string err(errbuff);
1481  delete[] errbuff;
1482  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error sending a packet for decoding : %d, %s", ret, err.c_str());
1483 
1484  } else {
1485 
1486  ret = avcodec_receive_frame(m_codecContext, m_picture);
1487 
1488  if (ret < 0) {
1489 
1490  if (ret == AVERROR(EAGAIN)) {
1491  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "AVERROR(EAGAIN)"); // Not an error, need to send data again
1492  } else if (ret == AVERROR_EOF) {
1493  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "AVERROR_EOF"); // End of file reached, should not happen with drone footage
1494  } else {
1495 
1496  char *errbuff = new char[AV_ERROR_MAX_STRING_SIZE];
1497  av_strerror(ret, errbuff, AV_ERROR_MAX_STRING_SIZE);
1498  std::string err(errbuff);
1499  delete[] errbuff;
1500  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error receiving a decoded frame : %d, %s", ret, err.c_str());
1501  }
1502  } else {
1503  m_bgr_picture_mutex.lock();
1504  av_frame_unref(m_bgr_picture);
1505  av_image_fill_arrays(m_bgr_picture->data, m_bgr_picture->linesize, m_buffer, AV_PIX_FMT_BGR24,
1506  m_codecContext->width, m_codecContext->height, 1);
1507 
1508  sws_scale(m_img_convert_ctx, (m_picture)->data, (m_picture)->linesize, 0, m_codecContext->height,
1509  (m_bgr_picture)->data, (m_bgr_picture)->linesize);
1510 
1511  m_bgr_picture_mutex.unlock();
1512  }
1513  }
1514 
1515  av_packet_unref(&m_packet);
1516 
1517  av_frame_unref(m_picture);
1518 }
1519 #endif // #ifdef VISP_HAVE_FFMPEG
1525 void vpRobotBebop2::cleanUp()
1526 {
1527  if (m_deviceController != nullptr) {
1528  // Lands the drone if not landed
1529  land();
1530 
1531 #ifdef VISP_HAVE_FFMPEG
1532  // Stops the streaming if not stopped
1533  stopStreaming();
1534 #endif
1535 
1536  // Deletes the controller
1537  m_deviceState = ARCONTROLLER_Device_GetState(m_deviceController, &m_errorController);
1538  if ((m_errorController == ARCONTROLLER_OK) && (m_deviceState != ARCONTROLLER_DEVICE_STATE_STOPPED)) {
1539 
1540  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Disconnecting ...");
1541  m_errorController = ARCONTROLLER_Device_Stop(m_deviceController);
1542 
1543  if (m_errorController == ARCONTROLLER_OK) {
1544  // Wait for the semaphore to increment, it will when the controller changes its state to 'stopped'
1545  ARSAL_Sem_Wait(&(m_stateSem));
1546  }
1547  }
1548  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Deleting device controller ...");
1549  ARCONTROLLER_Device_Delete(&m_deviceController);
1550 
1551  // Destroys the semaphore
1552  ARSAL_Sem_Destroy(&(m_stateSem));
1553 
1554  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Cleanup done.");
1555  } else {
1556 
1557  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error while cleaning up memory.");
1558  }
1559  m_running = false;
1560 }
1561 
1562 //*** ***//
1563 //*** Callbacks ***//
1564 //*** ***//
1565 
1574 void vpRobotBebop2::stateChangedCallback(eARCONTROLLER_DEVICE_STATE newState, eARCONTROLLER_ERROR error,
1575  void *customData)
1576 {
1577  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Controller state changed, new state: %d.", newState);
1578  (void)error;
1579 
1580  vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1581  switch (newState) {
1582  case ARCONTROLLER_DEVICE_STATE_STOPPED:
1583  // Stopping the programm
1584  drone->m_running = false;
1585  // Incrementing semaphore
1586  ARSAL_Sem_Post(&(drone->m_stateSem));
1587  break;
1588 
1589  case ARCONTROLLER_DEVICE_STATE_RUNNING:
1590  // Incrementing semaphore
1591  ARSAL_Sem_Post(&(drone->m_stateSem));
1592  break;
1593 
1594  default:
1595  break;
1596  }
1597 }
1598 
1599 #ifdef VISP_HAVE_FFMPEG
1609 eARCONTROLLER_ERROR vpRobotBebop2::decoderConfigCallback(ARCONTROLLER_Stream_Codec_t codec, void *customData)
1610 {
1611  vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1612 
1613  uint8_t *sps_buffer_ptr = codec.parameters.h264parameters.spsBuffer;
1614  uint32_t sps_buffer_size = static_cast<uint32_t>(codec.parameters.h264parameters.spsSize);
1615  uint8_t *pps_buffer_ptr = codec.parameters.h264parameters.ppsBuffer;
1616  uint32_t pps_buffer_size = static_cast<uint32_t>(codec.parameters.h264parameters.ppsSize);
1617 
1618  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "H264 configuration packet received: #SPS: %d #PPS: %d", sps_buffer_size,
1619  pps_buffer_size);
1620 
1621  drone->m_update_codec_params = (sps_buffer_ptr && pps_buffer_ptr && sps_buffer_size && pps_buffer_size &&
1622  (pps_buffer_size < 32) && (sps_buffer_size < 32));
1623 
1624  if (drone->m_update_codec_params) {
1625  // If codec parameters where received from the drone, we store them to pass them to the decoder in the next call of
1626  // computeFrame
1627  drone->m_codec_params_data.resize(sps_buffer_size + pps_buffer_size);
1628  std::copy(sps_buffer_ptr, sps_buffer_ptr + sps_buffer_size, drone->m_codec_params_data.begin());
1629  std::copy(pps_buffer_ptr, pps_buffer_ptr + pps_buffer_size, drone->m_codec_params_data.begin() + sps_buffer_size);
1630  } else {
1631  // If data is invalid, we clear the vector
1632  drone->m_codec_params_data.clear();
1633  }
1634  return ARCONTROLLER_OK;
1635 }
1636 
1645 eARCONTROLLER_ERROR vpRobotBebop2::didReceiveFrameCallback(ARCONTROLLER_Frame_t *frame, void *customData)
1646 {
1647  vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1648 
1649  if (frame != nullptr) {
1650 
1651  if (drone->m_videoDecodingStarted) {
1652  drone->computeFrame(frame);
1653  }
1654 
1655  } else {
1656  ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, "frame is nullptr.");
1657  }
1658 
1659  return ARCONTROLLER_OK;
1660 }
1661 #endif // #ifdef VISP_HAVE_FFMPEG
1662 
1672 void vpRobotBebop2::cmdBatteryStateChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1673  vpRobotBebop2 *drone)
1674 {
1675  ARCONTROLLER_DICTIONARY_ARG_t *arg = nullptr;
1676  ARCONTROLLER_DICTIONARY_ELEMENT_t *singleElement = nullptr;
1677 
1678  if (elementDictionary == nullptr) {
1679  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "elements is nullptr");
1680  return;
1681  }
1682 
1683  // Get the command received in the device controller
1684  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, singleElement);
1685 
1686  if (singleElement == nullptr) {
1687  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "singleElement is nullptr");
1688  return;
1689  }
1690 
1691  // Get the value
1692  HASH_FIND_STR(singleElement->arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_BATTERYSTATECHANGED_PERCENT,
1693  arg);
1694 
1695  if (arg == nullptr) {
1696  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "arg is nullptr");
1697  return;
1698  }
1699  drone->m_batteryLevel = arg->value.U8;
1700  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Battery level changed : %u percent remaining.", drone->m_batteryLevel);
1701 
1702  if (drone->m_batteryLevel <= 5) {
1703  ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, " - WARNING, very low battery level, drone will stop soon !");
1704  } else if (drone->m_batteryLevel <= 10) {
1705  ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, " - Warning, low battery level !");
1706  }
1707 }
1708 
1719 void vpRobotBebop2::cmdCameraOrientationChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1720  vpRobotBebop2 *drone)
1721 {
1722  ARCONTROLLER_DICTIONARY_ARG_t *arg = nullptr;
1723  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = nullptr;
1724  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1725  if (element != nullptr) {
1726  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2_TILT, arg);
1727 
1728  if (arg != nullptr) {
1729  drone->m_currentCameraTilt = static_cast<double>(arg->value.Float);
1730  }
1731 
1732  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2_PAN, arg);
1733  if (arg != nullptr) {
1734  drone->m_currentCameraPan = static_cast<double>(arg->value.Float);
1735  }
1736  }
1737 }
1738 
1750 void vpRobotBebop2::cmdCameraSettingsRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone)
1751 {
1752  ARCONTROLLER_DICTIONARY_ARG_t *arg = nullptr;
1753  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = nullptr;
1754  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1755  if (element != nullptr) {
1756  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_FOV,
1757  arg);
1758  if (arg != nullptr) {
1759  drone->m_cameraHorizontalFOV = static_cast<double>(arg->value.Float);
1760  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Camera horizontal FOV : %f degrees.",
1761  static_cast<double>(drone->m_cameraHorizontalFOV));
1762  }
1763  HASH_FIND_STR(element->arguments,
1764  ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_PANMAX, arg);
1765  if (arg != nullptr) {
1766  drone->m_maxCameraPan = static_cast<double>(arg->value.Float);
1767  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Max camera pan : %f degrees.",
1768  static_cast<double>(drone->m_maxCameraPan));
1769  }
1770  HASH_FIND_STR(element->arguments,
1771  ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_PANMIN, arg);
1772  if (arg != nullptr) {
1773  drone->m_minCameraPan = static_cast<double>(arg->value.Float);
1774  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Min camera pan : %f degrees.",
1775  static_cast<double>(drone->m_minCameraPan));
1776  }
1777  HASH_FIND_STR(element->arguments,
1778  ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_TILTMAX, arg);
1779  if (arg != nullptr) {
1780  drone->m_maxCameraTilt = static_cast<double>(arg->value.Float);
1781  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Max camera tilt : %f degrees.",
1782  static_cast<double>(drone->m_maxCameraTilt));
1783  }
1784  HASH_FIND_STR(element->arguments,
1785  ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_TILTMIN, arg);
1786  if (arg != nullptr) {
1787  drone->m_minCameraTilt = static_cast<double>(arg->value.Float);
1788  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Min camera tilt : %f degrees.",
1789  static_cast<double>(drone->m_minCameraTilt));
1790  }
1791  }
1792 }
1793 
1804 void vpRobotBebop2::cmdMaxPitchRollChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1805  vpRobotBebop2 *drone)
1806 {
1807  ARCONTROLLER_DICTIONARY_ARG_t *arg = nullptr;
1808  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = nullptr;
1809 
1810  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1811  if (element != nullptr) {
1812  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXTILTCHANGED_CURRENT,
1813  arg);
1814  if (arg != nullptr) {
1815  drone->m_maxTilt = static_cast<double>(arg->value.Float);
1816  }
1817  }
1818 }
1819 
1830 void vpRobotBebop2::cmdRelativeMoveEndedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone)
1831 {
1832  ARCONTROLLER_DICTIONARY_ARG_t *arg = nullptr;
1833  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = nullptr;
1834 
1835  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1836 
1837  if (element != nullptr) {
1838  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR, arg);
1839 
1840  if (arg != nullptr) {
1841  eARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR error =
1842  static_cast<eARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR>(arg->value.I32);
1843  if ((error != ARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR_OK) &&
1844  (error != ARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR_INTERRUPTED)) {
1845  ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Relative move ended with error %d", error);
1846  }
1847  drone->m_relativeMoveEnded = true;
1848  }
1849  }
1850 }
1851 
1862 void vpRobotBebop2::cmdExposureSetRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone)
1863 {
1864  ARCONTROLLER_DICTIONARY_ARG_t *arg = nullptr;
1865  ARCONTROLLER_DICTIONARY_ELEMENT_t *element = nullptr;
1866 
1867  HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1868 
1869  if (element != nullptr) {
1870 
1871  HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED_VALUE,
1872  arg);
1873 
1874  if (arg != nullptr) {
1875  drone->m_exposureSet = true;
1876  }
1877  }
1878 }
1879 
1889 void vpRobotBebop2::commandReceivedCallback(eARCONTROLLER_DICTIONARY_KEY commandKey,
1890  ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, void *customData)
1891 {
1892  vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1893 
1894  if (drone == nullptr)
1895  return;
1896 
1897  switch (commandKey) {
1898  case ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_BATTERYSTATECHANGED:
1899  // If the command received is a battery state changed
1900  cmdBatteryStateChangedRcv(elementDictionary, drone);
1901  break;
1902 
1903  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXTILTCHANGED:
1904  // If the command receivend is a max pitch/roll changed
1905  cmdMaxPitchRollChangedRcv(elementDictionary, drone);
1906  break;
1907 
1908  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGEVENT_MOVEBYEND:
1909  // If the command received is a relative move ended
1910  cmdRelativeMoveEndedRcv(elementDictionary, drone);
1911  break;
1912 
1913  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLATTRIMCHANGED:
1914  // If the command received is a flat trim finished
1915  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Flat trim finished ...");
1916  drone->m_flatTrimFinished = true;
1917  break;
1918 
1919  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED:
1920  // If the command received is a exposition changed
1921  cmdExposureSetRcv(elementDictionary, drone);
1922  break;
1923 
1924  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_VIDEORESOLUTIONSCHANGED:
1925  // If the command received is a resolution changed
1926  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Video resolution set ...");
1927  drone->m_videoResolutionSet = true;
1928  break;
1929 
1930  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED:
1931  // If the command received is a streaming started
1932  drone->m_streamingStarted = true;
1933  break;
1934 
1935  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOSTREAMMODECHANGED:
1936  // If the command received is a streaming mode changed
1937  drone->m_streamingModeSet = true;
1938  break;
1939 
1940  case ARCONTROLLER_DICTIONARY_KEY_COMMON_SETTINGSSTATE_RESETCHANGED:
1941  // If the command received is a settings reset
1942  ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "Settings reset ...");
1943  drone->m_settingsReset = true;
1944  break;
1945 
1946  case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2:
1947  // If the command received is a camera orientation changed
1948  cmdCameraOrientationChangedRcv(elementDictionary, drone);
1949  break;
1950 
1951  case ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED:
1952  // If the command received is a camera information sent
1953  cmdCameraSettingsRcv(elementDictionary, drone);
1954  break;
1955 
1956  default:
1957  break;
1958  }
1959 }
1960 
1961 #undef TAG
1962 
1963 #elif !defined(VISP_BUILD_SHARED_LIBS)
1964 // Work around to avoid warning: libvisp_robot.a(vpRobotBebop2.cpp.o) has
1965 // no symbols
1966 void dummy_vpRobotBebop2(){};
1967 #endif // VISP_HAVE_ARSDK
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:269
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:240
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:793
Type * bitmap
points toward the bitmap
Definition: vpImage.h:139
unsigned int getHeight() const
Definition: vpImage.h:182
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)