Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
48extern "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
75bool vpRobotBebop2::m_running = false;
76ARCONTROLLER_Device_t *vpRobotBebop2::m_deviceController = NULL;
77
113vpRobotBebop2::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 = NULL;
127 m_packet = AVPacket();
128 m_picture = NULL;
129 m_bgr_picture = NULL;
130 m_img_convert_ctx = NULL;
131 m_buffer = NULL;
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);
202#endif
203 setCameraOrientation(0, 0, true);
204 }
205 }
206}
207
214
222{
223 if (isRunning() && m_deviceController != NULL && 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) {
232 }
233 } else {
234 ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't do a flat trim : drone isn't landed.");
235 }
236}
237
242std::string vpRobotBebop2::getIpAddress() { return m_ipAddress; }
243
248int vpRobotBebop2::getDiscoveryPort() { return m_discoveryPort; }
249
254double vpRobotBebop2::getMaxTilt() { return m_maxTilt; }
255
262unsigned int vpRobotBebop2::getBatteryLevel() { return m_batteryLevel; }
263
268double vpRobotBebop2::getCameraHorizontalFOV() const { return m_cameraHorizontalFOV; }
269
274double vpRobotBebop2::getCurrentCameraTilt() const { return m_currentCameraTilt; }
275
280double vpRobotBebop2::getMinCameraTilt() const { return m_minCameraTilt; }
281
286double vpRobotBebop2::getMaxCameraTilt() const { return m_maxCameraTilt; }
287
292double vpRobotBebop2::getCurrentCameraPan() const { return m_currentCameraPan; }
293
298double vpRobotBebop2::getMinCameraPan() const { return m_minCameraPan; }
299
304double vpRobotBebop2::getMaxCameraPan() const { return m_maxCameraPan; }
305
318void vpRobotBebop2::setCameraOrientation(double tilt, double pan, bool blocking)
319{
320 if (isRunning() && m_deviceController != NULL) {
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) {
328 }
329 }
330
331 } else {
332 ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set camera orientation : drone isn't running.");
333 }
334}
335
347void vpRobotBebop2::setCameraTilt(double tilt, bool blocking)
348{
349 if (isRunning() && m_deviceController != NULL) {
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) {
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
376void vpRobotBebop2::setCameraPan(double pan, bool blocking)
377{
378 if (isRunning() && m_deviceController != NULL) {
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) {
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 == NULL) {
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
454void vpRobotBebop2::takeOff(bool blocking)
455{
456 if (isRunning() && isLanded() && m_deviceController != NULL) {
457
458 m_deviceController->aRDrone3->sendPilotingTakeOff(m_deviceController->aRDrone3);
459
460 if (blocking) {
461 while (!isHovering()) {
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 != NULL) {
480 m_deviceController->aRDrone3->sendPilotingLanding(m_deviceController->aRDrone3);
481 }
482}
483
495{
496 if (isRunning() && m_deviceController != NULL && (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 != NULL && (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
548{
549 if (isRunning() && m_deviceController != NULL && (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
576{
577 if (isRunning() && m_deviceController != NULL && (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 != NULL) {
602 m_errorController = m_deviceController->aRDrone3->sendPilotingEmergency(m_deviceController->aRDrone3);
603 }
604}
605
621void vpRobotBebop2::setPosition(float dX, float dY, float dZ, float dPsi, bool blocking)
622{
623 if (isRunning() && m_deviceController != NULL && (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) {
633 }
634 }
635 } else {
636 ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't move : drone isn't flying or hovering.");
637 }
638}
639
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
680void 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
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 != NULL) {
725
726 m_settingsReset = false;
727 m_deviceController->common->sendSettingsReset(m_deviceController->common);
728
729 while (!m_settingsReset) {
731 }
732
733 } else {
734 ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't reset drone settings : drone isn't running.");
735 }
736}
737
749void vpRobotBebop2::setMaxTilt(double maxTilt)
750{
751 if (isRunning() && m_deviceController != NULL) {
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 != NULL) {
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] != NULL) {
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 NULL");
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] != NULL) {
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 NULL");
828 }
829
830 } else {
831 ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't get current image : video streaming isn't started.");
832 }
833}
834
840int vpRobotBebop2::getVideoHeight() { return m_videoHeight; }
841
847int vpRobotBebop2::getVideoWidth() { return m_videoWidth; }
848
857{
858 if (isRunning() && m_deviceController != NULL) {
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) {
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 != NULL) {
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) {
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 != NULL) {
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) {
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 != NULL) {
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 != NULL) {
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 != NULL) {
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
1099void 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 != NULL) {
1126 m_deviceController->aRDrone3->sendPilotingLanding(m_deviceController->aRDrone3);
1127 }
1128 std::exit(EXIT_FAILURE);
1129}
1130
1135eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE vpRobotBebop2::getFlyingState()
1136{
1137 if (m_deviceController != NULL) {
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 != NULL) {
1146 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1147 ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1148
1149 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1150
1151 if (element != NULL) {
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 != NULL) {
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
1174eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED vpRobotBebop2::getStreamingState()
1175{
1176 if (m_deviceController != NULL) {
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 != NULL) {
1186 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1187 ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1188
1189 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1190
1191 if (element != NULL) {
1192 // Get the value
1193 HASH_FIND_STR(element->arguments,
1194 ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED, arg);
1195
1196 if (arg != NULL) {
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
1214ARDISCOVERY_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
1238void 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
1252void 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, NULL, 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
1283void 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
1312void 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, NULL) < 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, NULL, NULL,
1369 NULL); // Used to rescale frame received from the decoder
1370}
1371
1377void 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
1410void 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
1425void 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
1441void 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 comming 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
1525void vpRobotBebop2::cleanUp()
1526{
1527 if (m_deviceController != NULL) {
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
1574void 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
1609eARCONTROLLER_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
1645eARCONTROLLER_ERROR vpRobotBebop2::didReceiveFrameCallback(ARCONTROLLER_Frame_t *frame, void *customData)
1646{
1647 vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1648
1649 if (frame != NULL) {
1650
1651 if (drone->m_videoDecodingStarted) {
1652 drone->computeFrame(frame);
1653 }
1654
1655 } else {
1656 ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, "frame is NULL.");
1657 }
1658
1659 return ARCONTROLLER_OK;
1660}
1661#endif // #ifdef VISP_HAVE_FFMPEG
1662
1672void vpRobotBebop2::cmdBatteryStateChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1673 vpRobotBebop2 *drone)
1674{
1675 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1676 ARCONTROLLER_DICTIONARY_ELEMENT_t *singleElement = NULL;
1677
1678 if (elementDictionary == NULL) {
1679 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "elements is NULL");
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 == NULL) {
1687 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "singleElement is NULL");
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 == NULL) {
1696 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "arg is NULL");
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
1719void vpRobotBebop2::cmdCameraOrientationChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1720 vpRobotBebop2 *drone)
1721{
1722 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1723 ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1724 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1725 if (element != NULL) {
1726 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2_TILT, arg);
1727
1728 if (arg != NULL) {
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 != NULL) {
1734 drone->m_currentCameraPan = static_cast<double>(arg->value.Float);
1735 }
1736 }
1737}
1738
1750void vpRobotBebop2::cmdCameraSettingsRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone)
1751{
1752 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1753 ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1754 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1755 if (element != NULL) {
1756 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_FOV,
1757 arg);
1758 if (arg != NULL) {
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 != NULL) {
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 != NULL) {
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 != NULL) {
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 != NULL) {
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
1804void vpRobotBebop2::cmdMaxPitchRollChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1805 vpRobotBebop2 *drone)
1806{
1807 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1808 ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1809
1810 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1811 if (element != NULL) {
1812 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXTILTCHANGED_CURRENT,
1813 arg);
1814 if (arg != NULL) {
1815 drone->m_maxTilt = static_cast<double>(arg->value.Float);
1816 }
1817 }
1818}
1819
1830void vpRobotBebop2::cmdRelativeMoveEndedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone)
1831{
1832 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1833 ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1834
1835 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1836
1837 if (element != NULL) {
1838 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR, arg);
1839
1840 if (arg != NULL) {
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
1862void vpRobotBebop2::cmdExposureSetRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary, vpRobotBebop2 *drone)
1863{
1864 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1865 ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1866
1867 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1868
1869 if (element != NULL) {
1870
1871 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED_VALUE,
1872 arg);
1873
1874 if (arg != NULL) {
1875 drone->m_exposureSet = true;
1876 }
1877 }
1878}
1879
1889void 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 == NULL)
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
1966void dummy_vpRobotBebop2(){};
1967#endif // VISP_HAVE_ARSDK
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
Implementation of column vector and the associated operations.
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)
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition vpImage.h:795
Type * bitmap
points toward the bitmap
Definition vpImage.h:139
unsigned int getHeight() const
Definition vpImage.h:184
void setPitch(int value)
std::string getIpAddress()
double getMinCameraPan() const
void 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()
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)