40 #include <visp3/core/vpConfig.h>
42 #ifdef VISP_HAVE_ARSDK
44 #include <visp3/robot/vpRobotBebop2.h>
46 #include <visp3/core/vpExponentialMap.h>
48 #ifdef VISP_HAVE_FFMPEG
50 #include <libavcodec/avcodec.h>
51 #include <libavformat/avformat.h>
52 #include <libavutil/imgutils.h>
54 #include <visp3/core/vpImageConvert.h>
60 #define TAG "vpRobotBebop2" // For error messages of ARSDK
76 bool vpRobotBebop2::m_running =
false;
77 ARCONTROLLER_Device_t *vpRobotBebop2::m_deviceController = NULL;
115 : m_ipAddress(ipAddress), m_discoveryPort(discoveryPort)
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);
126 #ifdef VISP_HAVE_FFMPEG
127 m_codecContext = NULL;
128 m_packet = AVPacket();
130 m_bgr_picture = NULL;
131 m_img_convert_ctx = NULL;
133 m_videoDecodingStarted =
false;
136 m_batteryLevel = 100;
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;
146 m_update_codec_params =
false;
147 m_codec_params_data = std::vector<uint8_t>();
151 m_cameraHorizontalFOV = -1;
152 m_currentCameraTilt = -1;
153 m_minCameraTilt = -1;
154 m_maxCameraTilt = -1;
155 m_currentCameraPan = -1;
161 m_errorController = ARCONTROLLER_OK;
162 m_deviceState = ARCONTROLLER_DEVICE_STATE_MAX;
165 ARSAL_Sem_Init(&(m_stateSem), 0, 0);
168 ARDISCOVERY_Device_t *discoverDevice = discoverDrone();
171 createDroneController(discoverDevice);
180 if ((m_errorController != ARCONTROLLER_OK) || (m_deviceState != ARCONTROLLER_DEVICE_STATE_RUNNING)) {
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));
191 #ifdef VISP_HAVE_FFMPEG
192 setVideoResolution(0);
194 if (setDefaultSettings) {
199 #ifdef VISP_HAVE_FFMPEG
200 setVideoStabilisationMode(0);
204 setCameraOrientation(0, 0,
true);
226 m_flatTrimFinished =
false;
228 m_deviceController->aRDrone3->sendPilotingFlatTrim(m_deviceController->aRDrone3);
231 while (!m_flatTrimFinished) {
235 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't do a flat trim : drone isn't landed.");
321 if (
isRunning() && m_deviceController != NULL) {
323 m_deviceController->aRDrone3->sendCameraOrientationV2(m_deviceController->aRDrone3, static_cast<float>(tilt),
324 static_cast<float>(pan));
327 while (std::abs(tilt - m_currentCameraTilt) > 0.01 || std::abs(pan - m_currentCameraPan) > 0.01) {
333 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set camera orientation : drone isn't running.");
350 if (
isRunning() && m_deviceController != NULL) {
352 m_deviceController->aRDrone3->sendCameraOrientationV2(m_deviceController->aRDrone3, static_cast<float>(tilt),
356 while (std::abs(tilt - m_currentCameraTilt) > 0.01) {
362 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set camera tilt value : drone isn't running.");
379 if (
isRunning() && m_deviceController != NULL) {
381 m_deviceController->aRDrone3->sendCameraOrientationV2(
382 m_deviceController->aRDrone3, static_cast<float>(
getCurrentCameraTilt()), static_cast<float>(pan));
385 while (std::abs(pan - m_currentCameraPan) > 0.01) {
391 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set camera pan value : drone isn't running.");
401 if (m_deviceController == NULL) {
414 #ifdef VISP_HAVE_FFMPEG
415 return m_videoDecodingStarted;
427 return getFlyingState() == ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_HOVERING;
436 return getFlyingState() == ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_FLYING;
445 return getFlyingState() == ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_LANDED;
459 m_deviceController->aRDrone3->sendPilotingTakeOff(m_deviceController->aRDrone3);
468 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't take off : drone isn't landed.");
480 if (m_deviceController != NULL) {
481 m_deviceController->aRDrone3->sendPilotingLanding(m_deviceController->aRDrone3);
499 m_deviceController->aRDrone3->setPilotingPCMDGaz(m_deviceController->aRDrone3, static_cast<char>(value));
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));
507 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set vertical speed : drone isn't flying or hovering.");
526 m_deviceController->aRDrone3->setPilotingPCMDYaw(m_deviceController->aRDrone3, static_cast<char>(value));
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));
534 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set yaw speed : drone isn't flying or hovering.");
553 m_deviceController->aRDrone3->setPilotingPCMDPitch(m_deviceController->aRDrone3, static_cast<char>(value));
554 m_errorController = m_deviceController->aRDrone3->setPilotingPCMDFlag(m_deviceController->aRDrone3, 1);
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));
562 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set pitch value : drone isn't flying or hovering.");
581 m_deviceController->aRDrone3->setPilotingPCMDRoll(m_deviceController->aRDrone3, static_cast<char>(value));
582 m_errorController = m_deviceController->aRDrone3->setPilotingPCMDFlag(m_deviceController->aRDrone3, 1);
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));
590 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set roll value : drone isn't flying or hovering.");
602 if (m_deviceController != NULL) {
603 m_errorController = m_deviceController->aRDrone3->sendPilotingEmergency(m_deviceController->aRDrone3);
626 m_relativeMoveEnded =
false;
627 m_deviceController->aRDrone3->sendPilotingMoveBy(m_deviceController->aRDrone3, dX, dY, dZ, dPsi);
632 while (!m_relativeMoveEnded) {
637 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't move : drone isn't flying or hovering.");
656 double epsilon = (std::numeric_limits<double>::epsilon());
658 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't move : rotation around X axis should be 0.");
662 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't move : rotation around Y axis should be 0.");
667 setPosition(static_cast<float>(t[0]), static_cast<float>(t[1]), static_cast<float>(t[2]), dThetaZ, blocking);
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.");
713 ARSAL_Print_SetMinimumLevel(ARSAL_PRINT_INFO);
715 ARSAL_Print_SetMinimumLevel(ARSAL_PRINT_WARNING);
725 if (
isRunning() && m_deviceController != NULL) {
727 m_settingsReset =
false;
728 m_deviceController->common->sendSettingsReset(m_deviceController->common);
730 while (!m_settingsReset) {
735 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't reset drone settings : drone isn't running.");
752 if (
isRunning() && m_deviceController != NULL) {
753 m_deviceController->aRDrone3->sendPilotingSettingsMaxTilt(m_deviceController->aRDrone3,
754 static_cast<float>(maxTilt));
756 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set tilt value : drone isn't running.");
770 m_errorController = m_deviceController->aRDrone3->setPilotingPCMD(m_deviceController->aRDrone3, 0, 0, 0, 0, 0, 0);
778 #ifdef VISP_HAVE_FFMPEG // Functions related to video streaming and decoding requiers FFmpeg
790 if (m_videoDecodingStarted) {
792 if (m_bgr_picture->data[0] != NULL) {
793 I.
resize(static_cast<unsigned int>(m_videoHeight), static_cast<unsigned int>(m_videoWidth));
795 m_bgr_picture_mutex.lock();
798 m_bgr_picture_mutex.unlock();
800 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Error while getting current grayscale image : image data is NULL");
804 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't get current image : video streaming isn't started.");
818 if (m_videoDecodingStarted) {
820 if (m_bgr_picture->data[0] != NULL) {
821 I.
resize(static_cast<unsigned int>(m_videoHeight), static_cast<unsigned int>(m_videoWidth));
823 m_bgr_picture_mutex.lock();
826 m_bgr_picture_mutex.unlock();
828 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Error while getting current RGBa image : image data is NULL");
832 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't get current image : video streaming isn't started.");
859 if (
isRunning() && m_deviceController != NULL) {
860 expo = std::min(1.5f, std::max(-1.5f, expo));
862 m_exposureSet =
false;
863 m_deviceController->aRDrone3->sendPictureSettingsExpositionSelection(m_deviceController->aRDrone3, expo);
866 while (!m_exposureSet) {
870 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set exposure : drone isn't running.");
890 if (
isRunning() && m_deviceController != NULL) {
893 eARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE cmd_mode =
894 ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_LOW_LATENCY;
897 cmd_mode = ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_LOW_LATENCY;
900 cmd_mode = ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_HIGH_RELIABILITY;
903 cmd_mode = ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_HIGH_RELIABILITY_LOW_FRAMERATE;
908 m_streamingModeSet =
false;
909 m_deviceController->aRDrone3->sendMediaStreamingVideoStreamMode(m_deviceController->aRDrone3, cmd_mode);
912 while (!m_streamingModeSet) {
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.");
922 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set streaming mode : drone isn't running.");
939 if (
isRunning() && m_deviceController != NULL) {
943 eARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORESOLUTIONS_TYPE cmd_mode;
949 cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORESOLUTIONS_TYPE_REC1080_STREAM480;
955 cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORESOLUTIONS_TYPE_REC720_STREAM720;
961 m_videoResolutionSet =
false;
962 m_deviceController->aRDrone3->sendPictureSettingsVideoResolutions(m_deviceController->aRDrone3, cmd_mode);
965 while (!m_videoResolutionSet) {
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 "
975 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set video resolution : drone isn't running.");
992 if (
isRunning() && m_deviceController != NULL) {
994 eARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE cmd_mode =
995 ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_NONE;
999 cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_NONE;
1002 cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_ROLL;
1005 cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_PITCH;
1008 cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_ROLL_PITCH;
1014 m_deviceController->aRDrone3->sendPictureSettingsVideoStabilizationMode(m_deviceController->aRDrone3, cmd_mode);
1017 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set video stabilisation mode : drone isn't running.");
1031 if (
isRunning() && m_deviceController != NULL) {
1032 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Starting video streaming ... ");
1035 m_errorController = m_deviceController->aRDrone3->sendMediaStreamingVideoEnable(m_deviceController->aRDrone3, 1);
1037 if (m_errorController == ARCONTROLLER_OK) {
1038 m_streamingStarted =
false;
1040 while (!m_streamingStarted) {
1043 startVideoDecoding();
1051 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1055 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't start streaming : drone isn't running.");
1066 if (m_videoDecodingStarted && m_deviceController != NULL) {
1067 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Stopping video streaming ... ");
1070 m_errorController = m_deviceController->aRDrone3->sendMediaStreamingVideoEnable(m_deviceController->aRDrone3, 0);
1072 if (m_errorController == ARCONTROLLER_OK) {
1075 while (getStreamingState() != ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_DISABLED) {
1079 stopVideoDecoding();
1082 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1086 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't stop streaming : streaming already stopped.");
1090 #endif // #ifdef VISP_HAVE_FFMPEG
1100 void vpRobotBebop2::sighandler(
int signo)
1102 std::cout <<
"Stopping Bebop2 because of detected signal (" << signo <<
"): " << static_cast<char>(7);
1105 std::cout <<
"SIGINT (stopped by ^C) " << std::endl;
1108 std::cout <<
"SIGBUS (stopped due to a bus error) " << std::endl;
1111 std::cout <<
"SIGSEGV (stopped due to a segmentation fault) " << std::endl;
1114 std::cout <<
"SIGKILL (stopped by CTRL \\) " << std::endl;
1117 std::cout <<
"SIGQUIT " << std::endl;
1120 std::cout << signo << std::endl;
1123 vpRobotBebop2::m_running =
false;
1126 if (m_deviceController != NULL) {
1127 m_deviceController->aRDrone3->sendPilotingLanding(m_deviceController->aRDrone3);
1129 std::exit(EXIT_FAILURE);
1136 eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE vpRobotBebop2::getFlyingState()
1138 if (m_deviceController != NULL) {
1139 eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE flyingState =
1140 ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_MAX;
1141 eARCONTROLLER_ERROR error;
1143 ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary = ARCONTROLLER_ARDrone3_GetCommandElements(
1144 m_deviceController->aRDrone3, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED, &error);
1146 if (error == ARCONTROLLER_OK && elementDictionary != NULL) {
1147 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1148 ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1150 HASH_FIND_STR (elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1152 if (element != NULL)
1156 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE, arg);
1161 flyingState = static_cast<eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE>(arg->value.I32);
1167 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error when checking flying state : drone isn't connected.");
1168 return ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_MAX;
1176 eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED vpRobotBebop2::getStreamingState()
1178 if (m_deviceController != NULL) {
1179 eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED streamingState =
1180 ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_MAX;
1181 eARCONTROLLER_ERROR error;
1183 ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary = ARCONTROLLER_ARDrone3_GetCommandElements(
1184 m_deviceController->aRDrone3, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED,
1187 if (error == ARCONTROLLER_OK && elementDictionary != NULL) {
1188 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1189 ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1191 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1193 if (element != NULL) {
1195 HASH_FIND_STR(element->arguments,
1196 ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED, arg);
1201 static_cast<eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED>(arg->value.I32);
1205 return streamingState;
1207 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error when checking streaming state : drone isn't connected.");
1208 return ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_MAX;
1216 ARDISCOVERY_Device_t * vpRobotBebop2::discoverDrone()
1218 eARDISCOVERY_ERROR errorDiscovery = ARDISCOVERY_OK;
1220 ARDISCOVERY_Device_t * device = ARDISCOVERY_Device_New(&errorDiscovery);
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);
1226 if (errorDiscovery != ARDISCOVERY_OK)
1228 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Discovery error :%s", ARDISCOVERY_Error_ToString(errorDiscovery));
1230 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Drone controller created.");
1240 void vpRobotBebop2::createDroneController(ARDISCOVERY_Device_t * discoveredDrone)
1242 m_deviceController = ARCONTROLLER_Device_New (discoveredDrone, &m_errorController);
1243 if (m_errorController != ARCONTROLLER_OK)
1245 ARSAL_PRINT (ARSAL_PRINT_ERROR, TAG,
"Creation of deviceController failed.");
1247 ARDISCOVERY_Device_Delete (&discoveredDrone);
1248 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Device created.");
1255 void vpRobotBebop2::setupCallbacks()
1258 m_errorController = ARCONTROLLER_Device_AddStateChangedCallback(m_deviceController, stateChangedCallback,
this);
1259 if(m_errorController != ARCONTROLLER_OK)
1261 ARSAL_PRINT (ARSAL_PRINT_ERROR, TAG,
"add State callback failed.");
1265 m_errorController = ARCONTROLLER_Device_AddCommandReceivedCallback(m_deviceController, commandReceivedCallback,
this);
1267 if(m_errorController != ARCONTROLLER_OK)
1269 ARSAL_PRINT (ARSAL_PRINT_ERROR, TAG,
"add Command callback failed.");
1272 #ifdef VISP_HAVE_FFMPEG
1274 m_errorController = ARCONTROLLER_Device_SetVideoStreamCallbacks (m_deviceController, decoderConfigCallback, didReceiveFrameCallback, NULL ,
this);
1276 if(m_errorController != ARCONTROLLER_OK)
1278 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error: %s", ARCONTROLLER_Error_ToString(m_errorController));
1281 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Callbacks set up.");
1288 void vpRobotBebop2::startController()
1291 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Connecting ...");
1292 m_errorController = ARCONTROLLER_Device_Start (m_deviceController);
1294 if(m_errorController != ARCONTROLLER_OK)
1296 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1300 ARSAL_Sem_Wait (&(m_stateSem));
1303 m_deviceState = ARCONTROLLER_Device_GetState (m_deviceController, &m_errorController);
1305 if((m_errorController != ARCONTROLLER_OK) || (m_deviceState != ARCONTROLLER_DEVICE_STATE_RUNNING))
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));
1310 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Controller started.");
1313 #ifdef VISP_HAVE_FFMPEG
1319 void vpRobotBebop2::initCodec()
1322 avcodec_register_all();
1323 avformat_network_init();
1326 AVCodec *codec = avcodec_find_decoder(AV_CODEC_ID_H264);
1328 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Codec not found.");
1333 m_codecContext = avcodec_alloc_context3(codec);
1335 if (!m_codecContext) {
1336 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Failed to allocate codec context.");
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;
1349 m_codecContext->width = m_videoWidth;
1350 m_codecContext->height = m_videoHeight;
1352 if (codec->capabilities & AV_CODEC_CAP_TRUNCATED) {
1353 m_codecContext->flags |= AV_CODEC_FLAG_TRUNCATED;
1355 m_codecContext->flags2 |= AV_CODEC_FLAG2_CHUNKS;
1358 if (avcodec_open2(m_codecContext, codec, NULL) < 0) {
1359 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Failed to open codec.");
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)));
1367 av_init_packet(&m_packet);
1368 m_picture = av_frame_alloc();
1370 m_bgr_picture_mutex.lock();
1371 m_bgr_picture = av_frame_alloc();
1372 m_bgr_picture_mutex.unlock();
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,
1384 void vpRobotBebop2::cleanUpCodec()
1386 m_videoDecodingStarted =
false;
1387 av_packet_unref(&m_packet);
1389 if (m_codecContext) {
1390 avcodec_flush_buffers(m_codecContext);
1391 avcodec_free_context(&m_codecContext);
1395 av_frame_free(&m_picture);
1398 if (m_bgr_picture) {
1399 m_bgr_picture_mutex.lock();
1400 av_frame_free(&m_bgr_picture);
1401 m_bgr_picture_mutex.unlock();
1404 if (m_img_convert_ctx) {
1405 sws_freeContext(m_img_convert_ctx);
1417 void vpRobotBebop2::startVideoDecoding()
1419 if (!m_videoDecodingStarted) {
1421 m_videoDecodingStarted =
true;
1423 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Video decoding is already started.");
1432 void vpRobotBebop2::stopVideoDecoding()
1434 if (m_videoDecodingStarted) {
1437 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Video decoding is already stopped.");
1448 void vpRobotBebop2::computeFrame(ARCONTROLLER_Frame_t *frame)
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());
1456 m_packet.data = &m_codec_params_data[0];
1457 m_packet.size = static_cast<int>(m_codec_params_data.size());
1459 int ret = avcodec_send_packet(m_codecContext, &m_packet);
1463 ret = avcodec_receive_frame(m_codecContext, m_picture);
1465 if (ret == 0 || ret == AVERROR(EAGAIN)) {
1466 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"H264 codec parameters updated.");
1468 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Unexpected error while updating H264 parameters.");
1471 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Unexpected error while sending H264 parameters.");
1473 m_update_codec_params =
false;
1474 av_packet_unref(&m_packet);
1475 av_frame_unref(m_picture);
1479 m_packet.data = frame->data;
1480 m_packet.size = static_cast<int>(frame->used);
1482 int ret = avcodec_send_packet(m_codecContext, &m_packet);
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);
1489 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error sending a packet for decoding : %d, %s", ret, err.c_str());
1493 ret = avcodec_receive_frame(m_codecContext, m_picture);
1497 if (ret == AVERROR(EAGAIN)) {
1498 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"AVERROR(EAGAIN)");
1499 }
else if (ret == AVERROR_EOF) {
1500 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"AVERROR_EOF");
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);
1507 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error receiving a decoded frame : %d, %s", ret, err.c_str());
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);
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);
1518 m_bgr_picture_mutex.unlock();
1522 av_packet_unref(&m_packet);
1524 av_frame_unref(m_picture);
1526 #endif // #ifdef VISP_HAVE_FFMPEG
1532 void vpRobotBebop2::cleanUp()
1534 if (m_deviceController != NULL) {
1538 #ifdef VISP_HAVE_FFMPEG
1544 m_deviceState = ARCONTROLLER_Device_GetState(m_deviceController, &m_errorController);
1545 if ((m_errorController == ARCONTROLLER_OK) && (m_deviceState != ARCONTROLLER_DEVICE_STATE_STOPPED)) {
1547 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Disconnecting ...");
1548 m_errorController = ARCONTROLLER_Device_Stop(m_deviceController);
1550 if (m_errorController == ARCONTROLLER_OK) {
1552 ARSAL_Sem_Wait(&(m_stateSem));
1555 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Deleting device controller ...");
1556 ARCONTROLLER_Device_Delete(&m_deviceController);
1559 ARSAL_Sem_Destroy(&(m_stateSem));
1561 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Cleanup done.");
1564 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error while cleaning up memory.");
1581 void vpRobotBebop2::stateChangedCallback(eARCONTROLLER_DEVICE_STATE newState, eARCONTROLLER_ERROR error,
1584 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Controller state changed, new state: %d.", newState);
1587 vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1590 case ARCONTROLLER_DEVICE_STATE_STOPPED:
1592 drone->m_running =
false;
1594 ARSAL_Sem_Post(&(drone->m_stateSem));
1597 case ARCONTROLLER_DEVICE_STATE_RUNNING:
1599 ARSAL_Sem_Post(&(drone->m_stateSem));
1607 #ifdef VISP_HAVE_FFMPEG
1616 eARCONTROLLER_ERROR vpRobotBebop2::decoderConfigCallback(ARCONTROLLER_Stream_Codec_t codec,
void * customData)
1618 vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
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);
1625 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"H264 configuration packet received: #SPS: %d #PPS: %d", sps_buffer_size,
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));
1631 if (drone->m_update_codec_params) {
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);
1639 drone->m_codec_params_data.clear();
1641 return ARCONTROLLER_OK;
1652 eARCONTROLLER_ERROR vpRobotBebop2::didReceiveFrameCallback(ARCONTROLLER_Frame_t *frame,
void *customData)
1654 vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1656 if (frame != NULL) {
1658 if (drone->m_videoDecodingStarted) {
1659 drone->computeFrame(frame);
1664 ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG,
"frame is NULL.");
1667 return ARCONTROLLER_OK;
1669 #endif // #ifdef VISP_HAVE_FFMPEG
1681 void vpRobotBebop2::cmdBatteryStateChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1684 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1685 ARCONTROLLER_DICTIONARY_ELEMENT_t *singleElement = NULL;
1687 if (elementDictionary == NULL) {
1688 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"elements is NULL");
1693 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, singleElement);
1695 if (singleElement == NULL) {
1696 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"singleElement is NULL");
1701 HASH_FIND_STR(singleElement->arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_BATTERYSTATECHANGED_PERCENT,
1705 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"arg is NULL");
1708 drone->m_batteryLevel = arg->value.U8;
1709 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Battery level changed : %u percent remaining.", drone->m_batteryLevel);
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 !");
1728 void vpRobotBebop2::cmdCameraOrientationChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
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);
1738 drone->m_currentCameraTilt = static_cast<double>(arg->value.Float);
1741 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2_PAN, arg);
1743 drone->m_currentCameraPan = static_cast<double>(arg->value.Float);
1759 void vpRobotBebop2::cmdCameraSettingsRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
vpRobotBebop2 *drone)
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,
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));
1772 HASH_FIND_STR(element->arguments,
1773 ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_PANMAX, arg);
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));
1779 HASH_FIND_STR(element->arguments,
1780 ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_PANMIN, arg);
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));
1786 HASH_FIND_STR(element->arguments,
1787 ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_TILTMAX, arg);
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));
1793 HASH_FIND_STR(element->arguments,
1794 ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_TILTMIN, arg);
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));
1813 void vpRobotBebop2::cmdMaxPitchRollChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1816 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1817 ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
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,
1824 drone->m_maxTilt = static_cast<double>(arg->value.Float);
1839 void vpRobotBebop2::cmdRelativeMoveEndedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
vpRobotBebop2 *drone)
1841 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1842 ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1844 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1846 if (element != NULL) {
1847 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR, arg);
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);
1856 drone->m_relativeMoveEnded =
true;
1871 void vpRobotBebop2::cmdExposureSetRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
vpRobotBebop2 *drone)
1873 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1874 ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1876 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1878 if (element != NULL) {
1880 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED_VALUE,
1884 drone->m_exposureSet =
true;
1898 void vpRobotBebop2::commandReceivedCallback(eARCONTROLLER_DICTIONARY_KEY commandKey,
1899 ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
void *customData)
1901 vpRobotBebop2 *drone = static_cast<vpRobotBebop2 *>(customData);
1906 switch (commandKey) {
1907 case ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_BATTERYSTATECHANGED:
1909 cmdBatteryStateChangedRcv(elementDictionary, drone);
1912 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXTILTCHANGED:
1914 cmdMaxPitchRollChangedRcv(elementDictionary, drone);
1917 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGEVENT_MOVEBYEND:
1919 cmdRelativeMoveEndedRcv(elementDictionary, drone);
1922 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLATTRIMCHANGED:
1924 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Flat trim finished ...");
1925 drone->m_flatTrimFinished =
true;
1928 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED:
1930 cmdExposureSetRcv(elementDictionary, drone);
1933 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_VIDEORESOLUTIONSCHANGED:
1935 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Video resolution set ...");
1936 drone->m_videoResolutionSet =
true;
1939 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED:
1941 drone->m_streamingStarted =
true;
1944 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOSTREAMMODECHANGED:
1946 drone->m_streamingModeSet =
true;
1949 case ARCONTROLLER_DICTIONARY_KEY_COMMON_SETTINGSSTATE_RESETCHANGED:
1951 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Settings reset ...");
1952 drone->m_settingsReset =
true;
1955 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2:
1957 cmdCameraOrientationChangedRcv(elementDictionary, drone);
1960 case ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED:
1962 cmdCameraSettingsRcv(elementDictionary, drone);
1972 #elif !defined(VISP_BUILD_SHARED_LIBS)
1975 void dummy_vpRobotBebop2(){};
1976 #endif // VISP_HAVE_ARSDK