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