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"
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
194 if (setDefaultSettings) {
199 #ifdef VISP_HAVE_FFMPEG
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
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.");
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) {
1155 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE,
1160 flyingState =
static_cast<eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE
>(arg->value.I32);
1166 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error when checking flying state : drone isn't connected.");
1167 return ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_MAX;
1175 eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED vpRobotBebop2::getStreamingState()
1177 if (m_deviceController != NULL) {
1178 eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED streamingState =
1179 ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_MAX;
1180 eARCONTROLLER_ERROR error;
1182 ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary = ARCONTROLLER_ARDrone3_GetCommandElements(
1183 m_deviceController->aRDrone3, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED,
1186 if (error == ARCONTROLLER_OK && elementDictionary != NULL) {
1187 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1188 ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1190 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1192 if (element != NULL) {
1194 HASH_FIND_STR(element->arguments,
1195 ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED, arg);
1200 static_cast<eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED
>(arg->value.I32);
1204 return streamingState;
1206 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error when checking streaming state : drone isn't connected.");
1207 return ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_MAX;
1215 ARDISCOVERY_Device_t *vpRobotBebop2::discoverDrone()
1217 eARDISCOVERY_ERROR errorDiscovery = ARDISCOVERY_OK;
1219 ARDISCOVERY_Device_t *device = ARDISCOVERY_Device_New(&errorDiscovery);
1221 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Starting drone Wifi discovery ...");
1222 const char *charIpAddress = m_ipAddress.c_str();
1224 ARDISCOVERY_Device_InitWifi(device, ARDISCOVERY_PRODUCT_BEBOP_2,
"bebop2", charIpAddress, m_discoveryPort);
1226 if (errorDiscovery != ARDISCOVERY_OK) {
1227 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Discovery error :%s", ARDISCOVERY_Error_ToString(errorDiscovery));
1229 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Drone controller created.");
1239 void vpRobotBebop2::createDroneController(ARDISCOVERY_Device_t *discoveredDrone)
1241 m_deviceController = ARCONTROLLER_Device_New(discoveredDrone, &m_errorController);
1242 if (m_errorController != ARCONTROLLER_OK) {
1243 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Creation of deviceController failed.");
1245 ARDISCOVERY_Device_Delete(&discoveredDrone);
1246 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Device created.");
1253 void vpRobotBebop2::setupCallbacks()
1256 m_errorController = ARCONTROLLER_Device_AddStateChangedCallback(m_deviceController, stateChangedCallback,
this);
1257 if (m_errorController != ARCONTROLLER_OK) {
1258 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"add State callback failed.");
1262 m_errorController = ARCONTROLLER_Device_AddCommandReceivedCallback(m_deviceController, commandReceivedCallback,
this);
1264 if (m_errorController != ARCONTROLLER_OK) {
1265 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"add Command callback failed.");
1268 #ifdef VISP_HAVE_FFMPEG
1270 m_errorController = ARCONTROLLER_Device_SetVideoStreamCallbacks(m_deviceController, decoderConfigCallback,
1271 didReceiveFrameCallback, NULL,
this);
1273 if (m_errorController != ARCONTROLLER_OK) {
1274 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error: %s", ARCONTROLLER_Error_ToString(m_errorController));
1277 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Callbacks set up.");
1284 void vpRobotBebop2::startController()
1287 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Connecting ...");
1288 m_errorController = ARCONTROLLER_Device_Start(m_deviceController);
1290 if (m_errorController != ARCONTROLLER_OK) {
1291 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1295 ARSAL_Sem_Wait(&(m_stateSem));
1298 m_deviceState = ARCONTROLLER_Device_GetState(m_deviceController, &m_errorController);
1300 if ((m_errorController != ARCONTROLLER_OK) || (m_deviceState != ARCONTROLLER_DEVICE_STATE_RUNNING)) {
1301 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- deviceState :%d", m_deviceState);
1302 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1304 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Controller started.");
1307 #ifdef VISP_HAVE_FFMPEG
1313 void vpRobotBebop2::initCodec()
1316 avcodec_register_all();
1317 avformat_network_init();
1320 AVCodec *codec = avcodec_find_decoder(AV_CODEC_ID_H264);
1322 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Codec not found.");
1327 m_codecContext = avcodec_alloc_context3(codec);
1329 if (!m_codecContext) {
1330 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Failed to allocate codec context.");
1335 m_codecContext->pix_fmt = AV_PIX_FMT_YUV420P;
1336 m_codecContext->skip_frame = AVDISCARD_DEFAULT;
1337 m_codecContext->error_concealment = FF_EC_GUESS_MVS | FF_EC_DEBLOCK;
1338 m_codecContext->skip_loop_filter = AVDISCARD_DEFAULT;
1339 m_codecContext->workaround_bugs = AVMEDIA_TYPE_VIDEO;
1340 m_codecContext->codec_id = AV_CODEC_ID_H264;
1341 m_codecContext->skip_idct = AVDISCARD_DEFAULT;
1343 m_codecContext->width = m_videoWidth;
1344 m_codecContext->height = m_videoHeight;
1346 if (codec->capabilities & AV_CODEC_CAP_TRUNCATED) {
1347 m_codecContext->flags |= AV_CODEC_FLAG_TRUNCATED;
1349 m_codecContext->flags2 |= AV_CODEC_FLAG2_CHUNKS;
1352 if (avcodec_open2(m_codecContext, codec, NULL) < 0) {
1353 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Failed to open codec.");
1357 AVPixelFormat pFormat = AV_PIX_FMT_BGR24;
1358 int numBytes = av_image_get_buffer_size(pFormat, m_codecContext->width, m_codecContext->height, 1);
1359 m_buffer =
static_cast<uint8_t *
>(av_malloc(
static_cast<unsigned long>(numBytes) *
sizeof(uint8_t)));
1361 av_init_packet(&m_packet);
1362 m_picture = av_frame_alloc();
1364 m_bgr_picture_mutex.lock();
1365 m_bgr_picture = av_frame_alloc();
1366 m_bgr_picture_mutex.unlock();
1368 m_img_convert_ctx = sws_getContext(m_codecContext->width, m_codecContext->height, m_codecContext->pix_fmt,
1369 m_codecContext->width, m_codecContext->height, pFormat, SWS_BICUBIC, NULL, NULL,
1378 void vpRobotBebop2::cleanUpCodec()
1380 m_videoDecodingStarted =
false;
1381 av_packet_unref(&m_packet);
1383 if (m_codecContext) {
1384 avcodec_flush_buffers(m_codecContext);
1385 avcodec_free_context(&m_codecContext);
1389 av_frame_free(&m_picture);
1392 if (m_bgr_picture) {
1393 m_bgr_picture_mutex.lock();
1394 av_frame_free(&m_bgr_picture);
1395 m_bgr_picture_mutex.unlock();
1398 if (m_img_convert_ctx) {
1399 sws_freeContext(m_img_convert_ctx);
1411 void vpRobotBebop2::startVideoDecoding()
1413 if (!m_videoDecodingStarted) {
1415 m_videoDecodingStarted =
true;
1417 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Video decoding is already started.");
1426 void vpRobotBebop2::stopVideoDecoding()
1428 if (m_videoDecodingStarted) {
1431 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Video decoding is already stopped.");
1442 void vpRobotBebop2::computeFrame(ARCONTROLLER_Frame_t *frame)
1446 if (m_update_codec_params && m_codec_params_data.size()) {
1447 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Updating H264 codec parameters (Buffer Size: %lu) ...",
1448 m_codec_params_data.size());
1450 m_packet.data = &m_codec_params_data[0];
1451 m_packet.size =
static_cast<int>(m_codec_params_data.size());
1453 int ret = avcodec_send_packet(m_codecContext, &m_packet);
1457 ret = avcodec_receive_frame(m_codecContext, m_picture);
1459 if (ret == 0 || ret == AVERROR(EAGAIN)) {
1460 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"H264 codec parameters updated.");
1462 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Unexpected error while updating H264 parameters.");
1465 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Unexpected error while sending H264 parameters.");
1467 m_update_codec_params =
false;
1468 av_packet_unref(&m_packet);
1469 av_frame_unref(m_picture);
1473 m_packet.data = frame->data;
1474 m_packet.size =
static_cast<int>(frame->used);
1476 int ret = avcodec_send_packet(m_codecContext, &m_packet);
1479 char *errbuff =
new char[AV_ERROR_MAX_STRING_SIZE];
1480 av_strerror(ret, errbuff, AV_ERROR_MAX_STRING_SIZE);
1481 std::string err(errbuff);
1483 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error sending a packet for decoding : %d, %s", ret, err.c_str());
1487 ret = avcodec_receive_frame(m_codecContext, m_picture);
1491 if (ret == AVERROR(EAGAIN)) {
1492 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"AVERROR(EAGAIN)");
1493 }
else if (ret == AVERROR_EOF) {
1494 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"AVERROR_EOF");
1497 char *errbuff =
new char[AV_ERROR_MAX_STRING_SIZE];
1498 av_strerror(ret, errbuff, AV_ERROR_MAX_STRING_SIZE);
1499 std::string err(errbuff);
1501 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error receiving a decoded frame : %d, %s", ret, err.c_str());
1504 m_bgr_picture_mutex.lock();
1505 av_frame_unref(m_bgr_picture);
1506 av_image_fill_arrays(m_bgr_picture->data, m_bgr_picture->linesize, m_buffer, AV_PIX_FMT_BGR24,
1507 m_codecContext->width, m_codecContext->height, 1);
1509 sws_scale(m_img_convert_ctx, (m_picture)->data, (m_picture)->linesize, 0, m_codecContext->height,
1510 (m_bgr_picture)->data, (m_bgr_picture)->linesize);
1512 m_bgr_picture_mutex.unlock();
1516 av_packet_unref(&m_packet);
1518 av_frame_unref(m_picture);
1526 void vpRobotBebop2::cleanUp()
1528 if (m_deviceController != NULL) {
1532 #ifdef VISP_HAVE_FFMPEG
1538 m_deviceState = ARCONTROLLER_Device_GetState(m_deviceController, &m_errorController);
1539 if ((m_errorController == ARCONTROLLER_OK) && (m_deviceState != ARCONTROLLER_DEVICE_STATE_STOPPED)) {
1541 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Disconnecting ...");
1542 m_errorController = ARCONTROLLER_Device_Stop(m_deviceController);
1544 if (m_errorController == ARCONTROLLER_OK) {
1546 ARSAL_Sem_Wait(&(m_stateSem));
1549 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Deleting device controller ...");
1550 ARCONTROLLER_Device_Delete(&m_deviceController);
1553 ARSAL_Sem_Destroy(&(m_stateSem));
1555 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Cleanup done.");
1558 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error while cleaning up memory.");
1575 void vpRobotBebop2::stateChangedCallback(eARCONTROLLER_DEVICE_STATE newState, eARCONTROLLER_ERROR error,
1578 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Controller state changed, new state: %d.", newState);
1583 case ARCONTROLLER_DEVICE_STATE_STOPPED:
1585 drone->m_running =
false;
1587 ARSAL_Sem_Post(&(drone->m_stateSem));
1590 case ARCONTROLLER_DEVICE_STATE_RUNNING:
1592 ARSAL_Sem_Post(&(drone->m_stateSem));
1600 #ifdef VISP_HAVE_FFMPEG
1610 eARCONTROLLER_ERROR vpRobotBebop2::decoderConfigCallback(ARCONTROLLER_Stream_Codec_t codec,
void *customData)
1614 uint8_t *sps_buffer_ptr = codec.parameters.h264parameters.spsBuffer;
1615 uint32_t sps_buffer_size =
static_cast<uint32_t
>(codec.parameters.h264parameters.spsSize);
1616 uint8_t *pps_buffer_ptr = codec.parameters.h264parameters.ppsBuffer;
1617 uint32_t pps_buffer_size =
static_cast<uint32_t
>(codec.parameters.h264parameters.ppsSize);
1619 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"H264 configuration packet received: #SPS: %d #PPS: %d", sps_buffer_size,
1622 drone->m_update_codec_params = (sps_buffer_ptr && pps_buffer_ptr && sps_buffer_size && pps_buffer_size &&
1623 (pps_buffer_size < 32) && (sps_buffer_size < 32));
1625 if (drone->m_update_codec_params) {
1628 drone->m_codec_params_data.resize(sps_buffer_size + pps_buffer_size);
1629 std::copy(sps_buffer_ptr, sps_buffer_ptr + sps_buffer_size, drone->m_codec_params_data.begin());
1630 std::copy(pps_buffer_ptr, pps_buffer_ptr + pps_buffer_size, drone->m_codec_params_data.begin() + sps_buffer_size);
1633 drone->m_codec_params_data.clear();
1635 return ARCONTROLLER_OK;
1646 eARCONTROLLER_ERROR vpRobotBebop2::didReceiveFrameCallback(ARCONTROLLER_Frame_t *frame,
void *customData)
1650 if (frame != NULL) {
1652 if (drone->m_videoDecodingStarted) {
1653 drone->computeFrame(frame);
1657 ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG,
"frame is NULL.");
1660 return ARCONTROLLER_OK;
1673 void vpRobotBebop2::cmdBatteryStateChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1676 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1677 ARCONTROLLER_DICTIONARY_ELEMENT_t *singleElement = NULL;
1679 if (elementDictionary == NULL) {
1680 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"elements is NULL");
1685 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, singleElement);
1687 if (singleElement == NULL) {
1688 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"singleElement is NULL");
1693 HASH_FIND_STR(singleElement->arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_BATTERYSTATECHANGED_PERCENT,
1697 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"arg is NULL");
1700 drone->m_batteryLevel = arg->value.U8;
1701 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Battery level changed : %u percent remaining.", drone->m_batteryLevel);
1703 if (drone->m_batteryLevel <= 5) {
1704 ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG,
" - WARNING, very low battery level, drone will stop soon !");
1705 }
else if (drone->m_batteryLevel <= 10) {
1706 ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG,
" - Warning, low battery level !");
1720 void vpRobotBebop2::cmdCameraOrientationChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1723 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1724 ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1725 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1726 if (element != NULL) {
1727 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2_TILT, arg);
1730 drone->m_currentCameraTilt =
static_cast<double>(arg->value.Float);
1733 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2_PAN, arg);
1735 drone->m_currentCameraPan =
static_cast<double>(arg->value.Float);
1751 void vpRobotBebop2::cmdCameraSettingsRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
vpRobotBebop2 *drone)
1753 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1754 ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1755 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1756 if (element != NULL) {
1757 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_FOV,
1760 drone->m_cameraHorizontalFOV =
static_cast<double>(arg->value.Float);
1761 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Camera horizontal FOV : %f degrees.",
1762 static_cast<double>(drone->m_cameraHorizontalFOV));
1764 HASH_FIND_STR(element->arguments,
1765 ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_PANMAX, arg);
1767 drone->m_maxCameraPan =
static_cast<double>(arg->value.Float);
1768 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Max camera pan : %f degrees.",
1769 static_cast<double>(drone->m_maxCameraPan));
1771 HASH_FIND_STR(element->arguments,
1772 ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_PANMIN, arg);
1774 drone->m_minCameraPan =
static_cast<double>(arg->value.Float);
1775 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Min camera pan : %f degrees.",
1776 static_cast<double>(drone->m_minCameraPan));
1778 HASH_FIND_STR(element->arguments,
1779 ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_TILTMAX, arg);
1781 drone->m_maxCameraTilt =
static_cast<double>(arg->value.Float);
1782 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Max camera tilt : %f degrees.",
1783 static_cast<double>(drone->m_maxCameraTilt));
1785 HASH_FIND_STR(element->arguments,
1786 ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_TILTMIN, arg);
1788 drone->m_minCameraTilt =
static_cast<double>(arg->value.Float);
1789 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Min camera tilt : %f degrees.",
1790 static_cast<double>(drone->m_minCameraTilt));
1805 void vpRobotBebop2::cmdMaxPitchRollChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1808 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1809 ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1811 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1812 if (element != NULL) {
1813 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXTILTCHANGED_CURRENT,
1816 drone->m_maxTilt =
static_cast<double>(arg->value.Float);
1831 void vpRobotBebop2::cmdRelativeMoveEndedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
vpRobotBebop2 *drone)
1833 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1834 ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1836 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1838 if (element != NULL) {
1839 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR, arg);
1842 eARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR error =
1843 static_cast<eARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR
>(arg->value.I32);
1844 if ((error != ARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR_OK) &&
1845 (error != ARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR_INTERRUPTED)) {
1846 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Relative move ended with error %d", error);
1848 drone->m_relativeMoveEnded =
true;
1863 void vpRobotBebop2::cmdExposureSetRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
vpRobotBebop2 *drone)
1865 ARCONTROLLER_DICTIONARY_ARG_t *arg = NULL;
1866 ARCONTROLLER_DICTIONARY_ELEMENT_t *element = NULL;
1868 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1870 if (element != NULL) {
1872 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED_VALUE,
1876 drone->m_exposureSet =
true;
1890 void vpRobotBebop2::commandReceivedCallback(eARCONTROLLER_DICTIONARY_KEY commandKey,
1891 ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
void *customData)
1898 switch (commandKey) {
1899 case ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_BATTERYSTATECHANGED:
1901 cmdBatteryStateChangedRcv(elementDictionary, drone);
1904 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXTILTCHANGED:
1906 cmdMaxPitchRollChangedRcv(elementDictionary, drone);
1909 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGEVENT_MOVEBYEND:
1911 cmdRelativeMoveEndedRcv(elementDictionary, drone);
1914 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLATTRIMCHANGED:
1916 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Flat trim finished ...");
1917 drone->m_flatTrimFinished =
true;
1920 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED:
1922 cmdExposureSetRcv(elementDictionary, drone);
1925 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_VIDEORESOLUTIONSCHANGED:
1927 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Video resolution set ...");
1928 drone->m_videoResolutionSet =
true;
1931 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED:
1933 drone->m_streamingStarted =
true;
1936 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOSTREAMMODECHANGED:
1938 drone->m_streamingModeSet =
true;
1941 case ARCONTROLLER_DICTIONARY_KEY_COMMON_SETTINGSSTATE_RESETCHANGED:
1943 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Settings reset ...");
1944 drone->m_settingsReset =
true;
1947 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2:
1949 cmdCameraOrientationChangedRcv(elementDictionary, drone);
1952 case ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED:
1954 cmdCameraSettingsRcv(elementDictionary, drone);
1964 #elif !defined(VISP_BUILD_SHARED_LIBS)
1967 void dummy_vpRobotBebop2(){};
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
error that can be emited by ViSP classes.
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
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Type * bitmap
points toward the bitmap
unsigned int getHeight() const
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)
double getMinCameraTilt() const
double getMaxCameraTilt() const
void setVerbose(bool verbose)
void setVerticalSpeed(int value)
void getGrayscaleImage(vpImage< unsigned char > &I)
void setStreamingMode(int mode)
unsigned int getBatteryLevel()
void getRGBaImage(vpImage< vpRGBa > &I)
void setYawSpeed(int value)
void setCameraPan(double pan, bool blocking=false)
void takeOff(bool blocking=true)
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)