39 #include <visp3/core/vpConfig.h>
41 #ifdef VISP_HAVE_ARSDK
43 #include <visp3/robot/vpRobotBebop2.h>
45 #include <visp3/core/vpExponentialMap.h>
47 #ifdef VISP_HAVE_FFMPEG
49 #include <libavcodec/avcodec.h>
50 #include <libavformat/avformat.h>
51 #include <libavutil/imgutils.h>
53 #include <visp3/core/vpImageConvert.h>
59 #define TAG "vpRobotBebop2"
75 bool vpRobotBebop2::m_running =
false;
76 ARCONTROLLER_Device_t *vpRobotBebop2::m_deviceController =
nullptr;
114 : m_ipAddress(ipAddress), m_discoveryPort(discoveryPort)
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);
125 #ifdef VISP_HAVE_FFMPEG
126 m_codecContext =
nullptr;
127 m_packet = AVPacket();
129 m_bgr_picture =
nullptr;
130 m_img_convert_ctx =
nullptr;
132 m_videoDecodingStarted =
false;
135 m_batteryLevel = 100;
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;
145 m_update_codec_params =
false;
146 m_codec_params_data = std::vector<uint8_t>();
150 m_cameraHorizontalFOV = -1;
151 m_currentCameraTilt = -1;
152 m_minCameraTilt = -1;
153 m_maxCameraTilt = -1;
154 m_currentCameraPan = -1;
160 m_errorController = ARCONTROLLER_OK;
161 m_deviceState = ARCONTROLLER_DEVICE_STATE_MAX;
164 ARSAL_Sem_Init(&(m_stateSem), 0, 0);
167 ARDISCOVERY_Device_t *discoverDevice = discoverDrone();
170 createDroneController(discoverDevice);
179 if ((m_errorController != ARCONTROLLER_OK) || (m_deviceState != ARCONTROLLER_DEVICE_STATE_RUNNING)) {
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));
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) {
236 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't do a flat trim : drone isn't landed.");
322 if (
isRunning() && m_deviceController !=
nullptr) {
324 m_deviceController->aRDrone3->sendCameraOrientationV2(m_deviceController->aRDrone3,
static_cast<float>(tilt),
325 static_cast<float>(pan));
328 while (std::abs(tilt - m_currentCameraTilt) > 0.01 || std::abs(pan - m_currentCameraPan) > 0.01) {
335 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set camera orientation : drone isn't running.");
352 if (
isRunning() && m_deviceController !=
nullptr) {
354 m_deviceController->aRDrone3->sendCameraOrientationV2(m_deviceController->aRDrone3,
static_cast<float>(tilt),
358 while (std::abs(tilt - m_currentCameraTilt) > 0.01) {
365 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set camera tilt value : drone isn't running.");
382 if (
isRunning() && m_deviceController !=
nullptr) {
384 m_deviceController->aRDrone3->sendCameraOrientationV2(
385 m_deviceController->aRDrone3,
static_cast<float>(
getCurrentCameraTilt()),
static_cast<float>(pan));
388 while (std::abs(pan - m_currentCameraPan) > 0.01) {
395 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set camera pan value : drone isn't running.");
405 if (m_deviceController ==
nullptr) {
419 #ifdef VISP_HAVE_FFMPEG
420 return m_videoDecodingStarted;
432 return getFlyingState() == ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_HOVERING;
441 return getFlyingState() == ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_FLYING;
450 return getFlyingState() == ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_LANDED;
464 m_deviceController->aRDrone3->sendPilotingTakeOff(m_deviceController->aRDrone3);
474 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't take off : drone isn't landed.");
486 if (m_deviceController !=
nullptr) {
487 m_deviceController->aRDrone3->sendPilotingLanding(m_deviceController->aRDrone3);
505 m_deviceController->aRDrone3->setPilotingPCMDGaz(m_deviceController->aRDrone3,
static_cast<char>(value));
507 if (m_errorController != ARCONTROLLER_OK) {
508 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error when sending move command : %s",
509 ARCONTROLLER_Error_ToString(m_errorController));
514 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set vertical speed : drone isn't flying or hovering.");
533 m_deviceController->aRDrone3->setPilotingPCMDYaw(m_deviceController->aRDrone3,
static_cast<char>(value));
535 if (m_errorController != ARCONTROLLER_OK) {
536 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error when sending move command : %s",
537 ARCONTROLLER_Error_ToString(m_errorController));
542 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set yaw speed : drone isn't flying or hovering.");
561 m_deviceController->aRDrone3->setPilotingPCMDPitch(m_deviceController->aRDrone3,
static_cast<char>(value));
562 m_errorController = m_deviceController->aRDrone3->setPilotingPCMDFlag(m_deviceController->aRDrone3, 1);
564 if (m_errorController != ARCONTROLLER_OK) {
565 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error when sending move command : %s",
566 ARCONTROLLER_Error_ToString(m_errorController));
571 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set pitch value : drone isn't flying or hovering.");
590 m_deviceController->aRDrone3->setPilotingPCMDRoll(m_deviceController->aRDrone3,
static_cast<char>(value));
591 m_errorController = m_deviceController->aRDrone3->setPilotingPCMDFlag(m_deviceController->aRDrone3, 1);
593 if (m_errorController != ARCONTROLLER_OK) {
594 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error when sending move command : %s",
595 ARCONTROLLER_Error_ToString(m_errorController));
600 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set roll value : drone isn't flying or hovering.");
612 if (m_deviceController !=
nullptr) {
613 m_errorController = m_deviceController->aRDrone3->sendPilotingEmergency(m_deviceController->aRDrone3);
636 m_relativeMoveEnded =
false;
637 m_deviceController->aRDrone3->sendPilotingMoveBy(m_deviceController->aRDrone3, dX, dY, dZ, dPsi);
642 while (!m_relativeMoveEnded) {
648 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't move : drone isn't flying or hovering.");
667 double epsilon = (std::numeric_limits<double>::epsilon());
669 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't move : rotation around X axis should be 0.");
673 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't move : rotation around Y axis should be 0.");
678 setPosition(
static_cast<float>(t[0]),
static_cast<float>(t[1]),
static_cast<float>(t[2]), dThetaZ, blocking);
695 if (vel_cmd.
size() != 4) {
696 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
697 "Can't set velocity : dimension of the velocity vector should be equal to 4.");
724 ARSAL_Print_SetMinimumLevel(ARSAL_PRINT_INFO);
727 ARSAL_Print_SetMinimumLevel(ARSAL_PRINT_WARNING);
737 if (
isRunning() && m_deviceController !=
nullptr) {
739 m_settingsReset =
false;
740 m_deviceController->common->sendSettingsReset(m_deviceController->common);
742 while (!m_settingsReset) {
748 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't reset drone settings : drone isn't running.");
765 if (
isRunning() && m_deviceController !=
nullptr) {
766 m_deviceController->aRDrone3->sendPilotingSettingsMaxTilt(m_deviceController->aRDrone3,
767 static_cast<float>(maxTilt));
770 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set tilt value : drone isn't running.");
784 m_errorController = m_deviceController->aRDrone3->setPilotingPCMD(m_deviceController->aRDrone3, 0, 0, 0, 0, 0, 0);
792 #ifdef VISP_HAVE_FFMPEG
804 if (m_videoDecodingStarted) {
806 if (m_bgr_picture->data[0] !=
nullptr) {
807 I.
resize(
static_cast<unsigned int>(m_videoHeight),
static_cast<unsigned int>(m_videoWidth));
809 m_bgr_picture_mutex.lock();
812 m_bgr_picture_mutex.unlock();
815 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Error while getting current grayscale image : image data is nullptr");
820 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't get current image : video streaming isn't started.");
834 if (m_videoDecodingStarted) {
836 if (m_bgr_picture->data[0] !=
nullptr) {
837 I.
resize(
static_cast<unsigned int>(m_videoHeight),
static_cast<unsigned int>(m_videoWidth));
839 m_bgr_picture_mutex.lock();
842 m_bgr_picture_mutex.unlock();
845 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Error while getting current RGBa image : image data is nullptr");
850 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't get current image : video streaming isn't started.");
877 if (
isRunning() && m_deviceController !=
nullptr) {
878 expo = std::min<float>(1.5f, std::max<float>(-1.5f, expo));
880 m_exposureSet =
false;
881 m_deviceController->aRDrone3->sendPictureSettingsExpositionSelection(m_deviceController->aRDrone3, expo);
884 while (!m_exposureSet) {
889 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set exposure : drone isn't running.");
909 if (
isRunning() && m_deviceController !=
nullptr) {
912 eARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE cmd_mode =
913 ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_LOW_LATENCY;
916 cmd_mode = ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_LOW_LATENCY;
919 cmd_mode = ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_HIGH_RELIABILITY;
922 cmd_mode = ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_HIGH_RELIABILITY_LOW_FRAMERATE;
927 m_streamingModeSet =
false;
928 m_deviceController->aRDrone3->sendMediaStreamingVideoStreamMode(m_deviceController->aRDrone3, cmd_mode);
931 while (!m_streamingModeSet) {
938 ARSAL_PRINT_ERROR,
"ERROR",
939 "Can't set streaming mode : drone has to be landed and not streaming in order to set streaming mode.");
943 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set streaming mode : drone isn't running.");
960 if (
isRunning() && m_deviceController !=
nullptr) {
964 eARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORESOLUTIONS_TYPE cmd_mode;
970 cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORESOLUTIONS_TYPE_REC1080_STREAM480;
976 cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORESOLUTIONS_TYPE_REC720_STREAM720;
982 m_videoResolutionSet =
false;
983 m_deviceController->aRDrone3->sendPictureSettingsVideoResolutions(m_deviceController->aRDrone3, cmd_mode);
986 while (!m_videoResolutionSet) {
992 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
993 "Can't set video resolution : drone has to be landed and not streaming in order to set streaming "
998 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set video resolution : drone isn't running.");
1015 if (
isRunning() && m_deviceController !=
nullptr) {
1017 eARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE cmd_mode =
1018 ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_NONE;
1022 cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_NONE;
1025 cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_ROLL;
1028 cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_PITCH;
1031 cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_ROLL_PITCH;
1037 m_deviceController->aRDrone3->sendPictureSettingsVideoStabilizationMode(m_deviceController->aRDrone3, cmd_mode);
1041 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set video stabilisation mode : drone isn't running.");
1055 if (
isRunning() && m_deviceController !=
nullptr) {
1056 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Starting video streaming ... ");
1059 m_errorController = m_deviceController->aRDrone3->sendMediaStreamingVideoEnable(m_deviceController->aRDrone3, 1);
1061 if (m_errorController == ARCONTROLLER_OK) {
1062 m_streamingStarted =
false;
1064 while (!m_streamingStarted) {
1067 startVideoDecoding();
1076 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1081 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't start streaming : drone isn't running.");
1092 if (m_videoDecodingStarted && m_deviceController !=
nullptr) {
1093 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Stopping video streaming ... ");
1096 m_errorController = m_deviceController->aRDrone3->sendMediaStreamingVideoEnable(m_deviceController->aRDrone3, 0);
1098 if (m_errorController == ARCONTROLLER_OK) {
1101 while (getStreamingState() != ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_DISABLED) {
1105 stopVideoDecoding();
1109 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1114 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't stop streaming : streaming already stopped.");
1128 void vpRobotBebop2::sighandler(
int signo)
1130 std::cout <<
"Stopping Bebop2 because of detected signal (" << signo <<
"): " <<
static_cast<char>(7);
1133 std::cout <<
"SIGINT (stopped by ^C) " << std::endl;
1136 std::cout <<
"SIGBUS (stopped due to a bus error) " << std::endl;
1139 std::cout <<
"SIGSEGV (stopped due to a segmentation fault) " << std::endl;
1142 std::cout <<
"SIGKILL (stopped by CTRL \\) " << std::endl;
1145 std::cout <<
"SIGQUIT " << std::endl;
1148 std::cout << signo << std::endl;
1151 vpRobotBebop2::m_running =
false;
1154 if (m_deviceController !=
nullptr) {
1155 m_deviceController->aRDrone3->sendPilotingLanding(m_deviceController->aRDrone3);
1157 std::exit(EXIT_FAILURE);
1164 eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE vpRobotBebop2::getFlyingState()
1166 if (m_deviceController !=
nullptr) {
1167 eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE flyingState =
1168 ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_MAX;
1169 eARCONTROLLER_ERROR error;
1171 ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary = ARCONTROLLER_ARDrone3_GetCommandElements(
1172 m_deviceController->aRDrone3, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED, &error);
1174 if (error == ARCONTROLLER_OK && elementDictionary !=
nullptr) {
1175 ARCONTROLLER_DICTIONARY_ARG_t *arg =
nullptr;
1176 ARCONTROLLER_DICTIONARY_ELEMENT_t *element =
nullptr;
1178 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1180 if (element !=
nullptr) {
1183 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE,
1186 if (arg !=
nullptr) {
1188 flyingState =
static_cast<eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE
>(arg->value.I32);
1195 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error when checking flying state : drone isn't connected.");
1196 return ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_MAX;
1204 eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED vpRobotBebop2::getStreamingState()
1206 if (m_deviceController !=
nullptr) {
1207 eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED streamingState =
1208 ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_MAX;
1209 eARCONTROLLER_ERROR error;
1211 ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary = ARCONTROLLER_ARDrone3_GetCommandElements(
1212 m_deviceController->aRDrone3, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED,
1215 if (error == ARCONTROLLER_OK && elementDictionary !=
nullptr) {
1216 ARCONTROLLER_DICTIONARY_ARG_t *arg =
nullptr;
1217 ARCONTROLLER_DICTIONARY_ELEMENT_t *element =
nullptr;
1219 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1221 if (element !=
nullptr) {
1223 HASH_FIND_STR(element->arguments,
1224 ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED, arg);
1226 if (arg !=
nullptr) {
1229 static_cast<eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED
>(arg->value.I32);
1233 return streamingState;
1236 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error when checking streaming state : drone isn't connected.");
1237 return ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_MAX;
1245 ARDISCOVERY_Device_t *vpRobotBebop2::discoverDrone()
1247 eARDISCOVERY_ERROR errorDiscovery = ARDISCOVERY_OK;
1249 ARDISCOVERY_Device_t *device = ARDISCOVERY_Device_New(&errorDiscovery);
1251 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Starting drone Wifi discovery ...");
1252 const char *charIpAddress = m_ipAddress.c_str();
1254 ARDISCOVERY_Device_InitWifi(device, ARDISCOVERY_PRODUCT_BEBOP_2,
"bebop2", charIpAddress, m_discoveryPort);
1256 if (errorDiscovery != ARDISCOVERY_OK) {
1257 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Discovery error :%s", ARDISCOVERY_Error_ToString(errorDiscovery));
1259 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Drone controller created.");
1269 void vpRobotBebop2::createDroneController(ARDISCOVERY_Device_t *discoveredDrone)
1271 m_deviceController = ARCONTROLLER_Device_New(discoveredDrone, &m_errorController);
1272 if (m_errorController != ARCONTROLLER_OK) {
1273 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Creation of deviceController failed.");
1275 ARDISCOVERY_Device_Delete(&discoveredDrone);
1276 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Device created.");
1283 void vpRobotBebop2::setupCallbacks()
1286 m_errorController = ARCONTROLLER_Device_AddStateChangedCallback(m_deviceController, stateChangedCallback,
this);
1287 if (m_errorController != ARCONTROLLER_OK) {
1288 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"add State callback failed.");
1292 m_errorController = ARCONTROLLER_Device_AddCommandReceivedCallback(m_deviceController, commandReceivedCallback,
this);
1294 if (m_errorController != ARCONTROLLER_OK) {
1295 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"add Command callback failed.");
1298 #ifdef VISP_HAVE_FFMPEG
1300 m_errorController = ARCONTROLLER_Device_SetVideoStreamCallbacks(m_deviceController, decoderConfigCallback,
1301 didReceiveFrameCallback,
nullptr,
this);
1303 if (m_errorController != ARCONTROLLER_OK) {
1304 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error: %s", ARCONTROLLER_Error_ToString(m_errorController));
1307 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Callbacks set up.");
1314 void vpRobotBebop2::startController()
1317 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Connecting ...");
1318 m_errorController = ARCONTROLLER_Device_Start(m_deviceController);
1320 if (m_errorController != ARCONTROLLER_OK) {
1321 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1325 ARSAL_Sem_Wait(&(m_stateSem));
1328 m_deviceState = ARCONTROLLER_Device_GetState(m_deviceController, &m_errorController);
1330 if ((m_errorController != ARCONTROLLER_OK) || (m_deviceState != ARCONTROLLER_DEVICE_STATE_RUNNING)) {
1331 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- deviceState :%d", m_deviceState);
1332 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1334 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Controller started.");
1337 #ifdef VISP_HAVE_FFMPEG
1343 void vpRobotBebop2::initCodec()
1346 avcodec_register_all();
1347 avformat_network_init();
1350 AVCodec *codec = avcodec_find_decoder(AV_CODEC_ID_H264);
1352 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Codec not found.");
1357 m_codecContext = avcodec_alloc_context3(codec);
1359 if (!m_codecContext) {
1360 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Failed to allocate codec context.");
1365 m_codecContext->pix_fmt = AV_PIX_FMT_YUV420P;
1366 m_codecContext->skip_frame = AVDISCARD_DEFAULT;
1367 m_codecContext->error_concealment = FF_EC_GUESS_MVS | FF_EC_DEBLOCK;
1368 m_codecContext->skip_loop_filter = AVDISCARD_DEFAULT;
1369 m_codecContext->workaround_bugs = AVMEDIA_TYPE_VIDEO;
1370 m_codecContext->codec_id = AV_CODEC_ID_H264;
1371 m_codecContext->skip_idct = AVDISCARD_DEFAULT;
1373 m_codecContext->width = m_videoWidth;
1374 m_codecContext->height = m_videoHeight;
1376 if (codec->capabilities & AV_CODEC_CAP_TRUNCATED) {
1377 m_codecContext->flags |= AV_CODEC_FLAG_TRUNCATED;
1379 m_codecContext->flags2 |= AV_CODEC_FLAG2_CHUNKS;
1382 if (avcodec_open2(m_codecContext, codec,
nullptr) < 0) {
1383 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Failed to open codec.");
1387 AVPixelFormat pFormat = AV_PIX_FMT_BGR24;
1388 int numBytes = av_image_get_buffer_size(pFormat, m_codecContext->width, m_codecContext->height, 1);
1389 m_buffer =
static_cast<uint8_t *
>(av_malloc(
static_cast<unsigned long>(numBytes) *
sizeof(uint8_t)));
1391 av_init_packet(&m_packet);
1392 m_picture = av_frame_alloc();
1394 m_bgr_picture_mutex.lock();
1395 m_bgr_picture = av_frame_alloc();
1396 m_bgr_picture_mutex.unlock();
1398 m_img_convert_ctx = sws_getContext(m_codecContext->width, m_codecContext->height, m_codecContext->pix_fmt,
1399 m_codecContext->width, m_codecContext->height, pFormat, SWS_BICUBIC,
nullptr,
nullptr,
1408 void vpRobotBebop2::cleanUpCodec()
1410 m_videoDecodingStarted =
false;
1411 av_packet_unref(&m_packet);
1413 if (m_codecContext) {
1414 avcodec_flush_buffers(m_codecContext);
1415 avcodec_free_context(&m_codecContext);
1419 av_frame_free(&m_picture);
1422 if (m_bgr_picture) {
1423 m_bgr_picture_mutex.lock();
1424 av_frame_free(&m_bgr_picture);
1425 m_bgr_picture_mutex.unlock();
1428 if (m_img_convert_ctx) {
1429 sws_freeContext(m_img_convert_ctx);
1441 void vpRobotBebop2::startVideoDecoding()
1443 if (!m_videoDecodingStarted) {
1445 m_videoDecodingStarted =
true;
1448 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Video decoding is already started.");
1457 void vpRobotBebop2::stopVideoDecoding()
1459 if (m_videoDecodingStarted) {
1463 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Video decoding is already stopped.");
1474 void vpRobotBebop2::computeFrame(ARCONTROLLER_Frame_t *frame)
1478 if (m_update_codec_params && m_codec_params_data.size()) {
1479 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Updating H264 codec parameters (Buffer Size: %lu) ...",
1480 m_codec_params_data.size());
1482 m_packet.data = &m_codec_params_data[0];
1483 m_packet.size =
static_cast<int>(m_codec_params_data.size());
1485 int ret = avcodec_send_packet(m_codecContext, &m_packet);
1489 ret = avcodec_receive_frame(m_codecContext, m_picture);
1491 if (ret == 0 || ret == AVERROR(EAGAIN)) {
1492 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"H264 codec parameters updated.");
1495 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Unexpected error while updating H264 parameters.");
1499 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Unexpected error while sending H264 parameters.");
1501 m_update_codec_params =
false;
1502 av_packet_unref(&m_packet);
1503 av_frame_unref(m_picture);
1507 m_packet.data = frame->data;
1508 m_packet.size =
static_cast<int>(frame->used);
1510 int ret = avcodec_send_packet(m_codecContext, &m_packet);
1513 char *errbuff =
new char[AV_ERROR_MAX_STRING_SIZE];
1514 av_strerror(ret, errbuff, AV_ERROR_MAX_STRING_SIZE);
1515 std::string err(errbuff);
1517 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error sending a packet for decoding : %d, %s", ret, err.c_str());
1522 ret = avcodec_receive_frame(m_codecContext, m_picture);
1526 if (ret == AVERROR(EAGAIN)) {
1527 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"AVERROR(EAGAIN)");
1529 else if (ret == AVERROR_EOF) {
1530 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"AVERROR_EOF");
1534 char *errbuff =
new char[AV_ERROR_MAX_STRING_SIZE];
1535 av_strerror(ret, errbuff, AV_ERROR_MAX_STRING_SIZE);
1536 std::string err(errbuff);
1538 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error receiving a decoded frame : %d, %s", ret, err.c_str());
1542 m_bgr_picture_mutex.lock();
1543 av_frame_unref(m_bgr_picture);
1544 av_image_fill_arrays(m_bgr_picture->data, m_bgr_picture->linesize, m_buffer, AV_PIX_FMT_BGR24,
1545 m_codecContext->width, m_codecContext->height, 1);
1547 sws_scale(m_img_convert_ctx, (m_picture)->data, (m_picture)->linesize, 0, m_codecContext->height,
1548 (m_bgr_picture)->data, (m_bgr_picture)->linesize);
1550 m_bgr_picture_mutex.unlock();
1554 av_packet_unref(&m_packet);
1556 av_frame_unref(m_picture);
1564 void vpRobotBebop2::cleanUp()
1566 if (m_deviceController !=
nullptr) {
1570 #ifdef VISP_HAVE_FFMPEG
1576 m_deviceState = ARCONTROLLER_Device_GetState(m_deviceController, &m_errorController);
1577 if ((m_errorController == ARCONTROLLER_OK) && (m_deviceState != ARCONTROLLER_DEVICE_STATE_STOPPED)) {
1579 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Disconnecting ...");
1580 m_errorController = ARCONTROLLER_Device_Stop(m_deviceController);
1582 if (m_errorController == ARCONTROLLER_OK) {
1584 ARSAL_Sem_Wait(&(m_stateSem));
1587 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Deleting device controller ...");
1588 ARCONTROLLER_Device_Delete(&m_deviceController);
1591 ARSAL_Sem_Destroy(&(m_stateSem));
1593 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Cleanup done.");
1597 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error while cleaning up memory.");
1614 void vpRobotBebop2::stateChangedCallback(eARCONTROLLER_DEVICE_STATE newState, eARCONTROLLER_ERROR error,
1617 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Controller state changed, new state: %d.", newState);
1622 case ARCONTROLLER_DEVICE_STATE_STOPPED:
1624 drone->m_running =
false;
1626 ARSAL_Sem_Post(&(drone->m_stateSem));
1629 case ARCONTROLLER_DEVICE_STATE_RUNNING:
1631 ARSAL_Sem_Post(&(drone->m_stateSem));
1639 #ifdef VISP_HAVE_FFMPEG
1649 eARCONTROLLER_ERROR vpRobotBebop2::decoderConfigCallback(ARCONTROLLER_Stream_Codec_t codec,
void *customData)
1653 uint8_t *sps_buffer_ptr = codec.parameters.h264parameters.spsBuffer;
1654 uint32_t sps_buffer_size =
static_cast<uint32_t
>(codec.parameters.h264parameters.spsSize);
1655 uint8_t *pps_buffer_ptr = codec.parameters.h264parameters.ppsBuffer;
1656 uint32_t pps_buffer_size =
static_cast<uint32_t
>(codec.parameters.h264parameters.ppsSize);
1658 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"H264 configuration packet received: #SPS: %d #PPS: %d", sps_buffer_size,
1661 drone->m_update_codec_params = (sps_buffer_ptr && pps_buffer_ptr && sps_buffer_size && pps_buffer_size &&
1662 (pps_buffer_size < 32) && (sps_buffer_size < 32));
1664 if (drone->m_update_codec_params) {
1667 drone->m_codec_params_data.resize(sps_buffer_size + pps_buffer_size);
1668 std::copy(sps_buffer_ptr, sps_buffer_ptr + sps_buffer_size, drone->m_codec_params_data.begin());
1669 std::copy(pps_buffer_ptr, pps_buffer_ptr + pps_buffer_size, drone->m_codec_params_data.begin() + sps_buffer_size);
1673 drone->m_codec_params_data.clear();
1675 return ARCONTROLLER_OK;
1686 eARCONTROLLER_ERROR vpRobotBebop2::didReceiveFrameCallback(ARCONTROLLER_Frame_t *frame,
void *customData)
1690 if (frame !=
nullptr) {
1692 if (drone->m_videoDecodingStarted) {
1693 drone->computeFrame(frame);
1698 ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG,
"frame is nullptr.");
1701 return ARCONTROLLER_OK;
1714 void vpRobotBebop2::cmdBatteryStateChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1717 ARCONTROLLER_DICTIONARY_ARG_t *arg =
nullptr;
1718 ARCONTROLLER_DICTIONARY_ELEMENT_t *singleElement =
nullptr;
1720 if (elementDictionary ==
nullptr) {
1721 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"elements is nullptr");
1726 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, singleElement);
1728 if (singleElement ==
nullptr) {
1729 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"singleElement is nullptr");
1734 HASH_FIND_STR(singleElement->arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_BATTERYSTATECHANGED_PERCENT,
1737 if (arg ==
nullptr) {
1738 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"arg is nullptr");
1741 drone->m_batteryLevel = arg->value.U8;
1742 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Battery level changed : %u percent remaining.", drone->m_batteryLevel);
1744 if (drone->m_batteryLevel <= 5) {
1745 ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG,
" - WARNING, very low battery level, drone will stop soon !");
1747 else if (drone->m_batteryLevel <= 10) {
1748 ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG,
" - Warning, low battery level !");
1762 void vpRobotBebop2::cmdCameraOrientationChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1765 ARCONTROLLER_DICTIONARY_ARG_t *arg =
nullptr;
1766 ARCONTROLLER_DICTIONARY_ELEMENT_t *element =
nullptr;
1767 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1768 if (element !=
nullptr) {
1769 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2_TILT, arg);
1771 if (arg !=
nullptr) {
1772 drone->m_currentCameraTilt =
static_cast<double>(arg->value.Float);
1775 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2_PAN, arg);
1776 if (arg !=
nullptr) {
1777 drone->m_currentCameraPan =
static_cast<double>(arg->value.Float);
1793 void vpRobotBebop2::cmdCameraSettingsRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
vpRobotBebop2 *drone)
1795 ARCONTROLLER_DICTIONARY_ARG_t *arg =
nullptr;
1796 ARCONTROLLER_DICTIONARY_ELEMENT_t *element =
nullptr;
1797 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1798 if (element !=
nullptr) {
1799 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_FOV,
1801 if (arg !=
nullptr) {
1802 drone->m_cameraHorizontalFOV =
static_cast<double>(arg->value.Float);
1803 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Camera horizontal FOV : %f degrees.",
1804 static_cast<double>(drone->m_cameraHorizontalFOV));
1806 HASH_FIND_STR(element->arguments,
1807 ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_PANMAX, arg);
1808 if (arg !=
nullptr) {
1809 drone->m_maxCameraPan =
static_cast<double>(arg->value.Float);
1810 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Max camera pan : %f degrees.",
1811 static_cast<double>(drone->m_maxCameraPan));
1813 HASH_FIND_STR(element->arguments,
1814 ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_PANMIN, arg);
1815 if (arg !=
nullptr) {
1816 drone->m_minCameraPan =
static_cast<double>(arg->value.Float);
1817 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Min camera pan : %f degrees.",
1818 static_cast<double>(drone->m_minCameraPan));
1820 HASH_FIND_STR(element->arguments,
1821 ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_TILTMAX, arg);
1822 if (arg !=
nullptr) {
1823 drone->m_maxCameraTilt =
static_cast<double>(arg->value.Float);
1824 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Max camera tilt : %f degrees.",
1825 static_cast<double>(drone->m_maxCameraTilt));
1827 HASH_FIND_STR(element->arguments,
1828 ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_TILTMIN, arg);
1829 if (arg !=
nullptr) {
1830 drone->m_minCameraTilt =
static_cast<double>(arg->value.Float);
1831 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Min camera tilt : %f degrees.",
1832 static_cast<double>(drone->m_minCameraTilt));
1847 void vpRobotBebop2::cmdMaxPitchRollChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1850 ARCONTROLLER_DICTIONARY_ARG_t *arg =
nullptr;
1851 ARCONTROLLER_DICTIONARY_ELEMENT_t *element =
nullptr;
1853 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1854 if (element !=
nullptr) {
1855 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXTILTCHANGED_CURRENT,
1857 if (arg !=
nullptr) {
1858 drone->m_maxTilt =
static_cast<double>(arg->value.Float);
1873 void vpRobotBebop2::cmdRelativeMoveEndedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
vpRobotBebop2 *drone)
1875 ARCONTROLLER_DICTIONARY_ARG_t *arg =
nullptr;
1876 ARCONTROLLER_DICTIONARY_ELEMENT_t *element =
nullptr;
1878 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1880 if (element !=
nullptr) {
1881 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR, arg);
1883 if (arg !=
nullptr) {
1884 eARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR error =
1885 static_cast<eARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR
>(arg->value.I32);
1886 if ((error != ARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR_OK) &&
1887 (error != ARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR_INTERRUPTED)) {
1888 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Relative move ended with error %d", error);
1890 drone->m_relativeMoveEnded =
true;
1905 void vpRobotBebop2::cmdExposureSetRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
vpRobotBebop2 *drone)
1907 ARCONTROLLER_DICTIONARY_ARG_t *arg =
nullptr;
1908 ARCONTROLLER_DICTIONARY_ELEMENT_t *element =
nullptr;
1910 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1912 if (element !=
nullptr) {
1914 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED_VALUE,
1917 if (arg !=
nullptr) {
1918 drone->m_exposureSet =
true;
1932 void vpRobotBebop2::commandReceivedCallback(eARCONTROLLER_DICTIONARY_KEY commandKey,
1933 ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
void *customData)
1937 if (drone ==
nullptr)
1940 switch (commandKey) {
1941 case ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_BATTERYSTATECHANGED:
1943 cmdBatteryStateChangedRcv(elementDictionary, drone);
1946 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXTILTCHANGED:
1948 cmdMaxPitchRollChangedRcv(elementDictionary, drone);
1951 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGEVENT_MOVEBYEND:
1953 cmdRelativeMoveEndedRcv(elementDictionary, drone);
1956 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLATTRIMCHANGED:
1958 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Flat trim finished ...");
1959 drone->m_flatTrimFinished =
true;
1962 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED:
1964 cmdExposureSetRcv(elementDictionary, drone);
1967 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_VIDEORESOLUTIONSCHANGED:
1969 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Video resolution set ...");
1970 drone->m_videoResolutionSet =
true;
1973 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED:
1975 drone->m_streamingStarted =
true;
1978 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOSTREAMMODECHANGED:
1980 drone->m_streamingModeSet =
true;
1983 case ARCONTROLLER_DICTIONARY_KEY_COMMON_SETTINGSSTATE_RESETCHANGED:
1985 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Settings reset ...");
1986 drone->m_settingsReset =
true;
1989 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2:
1991 cmdCameraOrientationChangedRcv(elementDictionary, drone);
1994 case ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED:
1996 cmdCameraSettingsRcv(elementDictionary, drone);
2006 #elif !defined(VISP_BUILD_SHARED_LIBS)
2009 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 emitted 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)