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));
190 #ifdef VISP_HAVE_FFMPEG
193 if (setDefaultSettings) {
198 #ifdef VISP_HAVE_FFMPEG
225 m_flatTrimFinished =
false;
227 m_deviceController->aRDrone3->sendPilotingFlatTrim(m_deviceController->aRDrone3);
230 while (!m_flatTrimFinished) {
234 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't do a flat trim : drone isn't landed.");
320 if (
isRunning() && m_deviceController !=
nullptr) {
322 m_deviceController->aRDrone3->sendCameraOrientationV2(m_deviceController->aRDrone3,
static_cast<float>(tilt),
323 static_cast<float>(pan));
326 while (std::abs(tilt - m_currentCameraTilt) > 0.01 || std::abs(pan - m_currentCameraPan) > 0.01) {
332 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set camera orientation : drone isn't running.");
349 if (
isRunning() && m_deviceController !=
nullptr) {
351 m_deviceController->aRDrone3->sendCameraOrientationV2(m_deviceController->aRDrone3,
static_cast<float>(tilt),
355 while (std::abs(tilt - m_currentCameraTilt) > 0.01) {
361 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set camera tilt value : drone isn't running.");
378 if (
isRunning() && m_deviceController !=
nullptr) {
380 m_deviceController->aRDrone3->sendCameraOrientationV2(
381 m_deviceController->aRDrone3,
static_cast<float>(
getCurrentCameraTilt()),
static_cast<float>(pan));
384 while (std::abs(pan - m_currentCameraPan) > 0.01) {
390 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set camera pan value : drone isn't running.");
400 if (m_deviceController ==
nullptr) {
413 #ifdef VISP_HAVE_FFMPEG
414 return m_videoDecodingStarted;
426 return getFlyingState() == ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_HOVERING;
435 return getFlyingState() == ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_FLYING;
444 return getFlyingState() == ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_LANDED;
458 m_deviceController->aRDrone3->sendPilotingTakeOff(m_deviceController->aRDrone3);
467 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't take off : drone isn't landed.");
479 if (m_deviceController !=
nullptr) {
480 m_deviceController->aRDrone3->sendPilotingLanding(m_deviceController->aRDrone3);
498 m_deviceController->aRDrone3->setPilotingPCMDGaz(m_deviceController->aRDrone3,
static_cast<char>(value));
500 if (m_errorController != ARCONTROLLER_OK) {
501 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error when sending move command : %s",
502 ARCONTROLLER_Error_ToString(m_errorController));
506 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set vertical speed : drone isn't flying or hovering.");
525 m_deviceController->aRDrone3->setPilotingPCMDYaw(m_deviceController->aRDrone3,
static_cast<char>(value));
527 if (m_errorController != ARCONTROLLER_OK) {
528 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error when sending move command : %s",
529 ARCONTROLLER_Error_ToString(m_errorController));
533 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set yaw speed : drone isn't flying or hovering.");
552 m_deviceController->aRDrone3->setPilotingPCMDPitch(m_deviceController->aRDrone3,
static_cast<char>(value));
553 m_errorController = m_deviceController->aRDrone3->setPilotingPCMDFlag(m_deviceController->aRDrone3, 1);
555 if (m_errorController != ARCONTROLLER_OK) {
556 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error when sending move command : %s",
557 ARCONTROLLER_Error_ToString(m_errorController));
561 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set pitch value : drone isn't flying or hovering.");
580 m_deviceController->aRDrone3->setPilotingPCMDRoll(m_deviceController->aRDrone3,
static_cast<char>(value));
581 m_errorController = m_deviceController->aRDrone3->setPilotingPCMDFlag(m_deviceController->aRDrone3, 1);
583 if (m_errorController != ARCONTROLLER_OK) {
584 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error when sending move command : %s",
585 ARCONTROLLER_Error_ToString(m_errorController));
589 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set roll value : drone isn't flying or hovering.");
601 if (m_deviceController !=
nullptr) {
602 m_errorController = m_deviceController->aRDrone3->sendPilotingEmergency(m_deviceController->aRDrone3);
625 m_relativeMoveEnded =
false;
626 m_deviceController->aRDrone3->sendPilotingMoveBy(m_deviceController->aRDrone3, dX, dY, dZ, dPsi);
631 while (!m_relativeMoveEnded) {
636 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't move : drone isn't flying or hovering.");
655 double epsilon = (std::numeric_limits<double>::epsilon());
657 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't move : rotation around X axis should be 0.");
661 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't move : rotation around Y axis should be 0.");
666 setPosition(
static_cast<float>(t[0]),
static_cast<float>(t[1]),
static_cast<float>(t[2]), dThetaZ, blocking);
683 if (vel_cmd.
size() != 4) {
684 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
685 "Can't set velocity : dimension of the velocity vector should be equal to 4.");
712 ARSAL_Print_SetMinimumLevel(ARSAL_PRINT_INFO);
714 ARSAL_Print_SetMinimumLevel(ARSAL_PRINT_WARNING);
724 if (
isRunning() && m_deviceController !=
nullptr) {
726 m_settingsReset =
false;
727 m_deviceController->common->sendSettingsReset(m_deviceController->common);
729 while (!m_settingsReset) {
734 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't reset drone settings : drone isn't running.");
751 if (
isRunning() && m_deviceController !=
nullptr) {
752 m_deviceController->aRDrone3->sendPilotingSettingsMaxTilt(m_deviceController->aRDrone3,
753 static_cast<float>(maxTilt));
755 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set tilt value : drone isn't running.");
769 m_errorController = m_deviceController->aRDrone3->setPilotingPCMD(m_deviceController->aRDrone3, 0, 0, 0, 0, 0, 0);
777 #ifdef VISP_HAVE_FFMPEG
789 if (m_videoDecodingStarted) {
791 if (m_bgr_picture->data[0] !=
nullptr) {
792 I.
resize(
static_cast<unsigned int>(m_videoHeight),
static_cast<unsigned int>(m_videoWidth));
794 m_bgr_picture_mutex.lock();
797 m_bgr_picture_mutex.unlock();
799 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Error while getting current grayscale image : image data is nullptr");
803 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't get current image : video streaming isn't started.");
817 if (m_videoDecodingStarted) {
819 if (m_bgr_picture->data[0] !=
nullptr) {
820 I.
resize(
static_cast<unsigned int>(m_videoHeight),
static_cast<unsigned int>(m_videoWidth));
822 m_bgr_picture_mutex.lock();
825 m_bgr_picture_mutex.unlock();
827 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Error while getting current RGBa image : image data is nullptr");
831 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't get current image : video streaming isn't started.");
858 if (
isRunning() && m_deviceController !=
nullptr) {
859 expo = std::min(1.5f, std::max(-1.5f, expo));
861 m_exposureSet =
false;
862 m_deviceController->aRDrone3->sendPictureSettingsExpositionSelection(m_deviceController->aRDrone3, expo);
865 while (!m_exposureSet) {
869 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set exposure : drone isn't running.");
889 if (
isRunning() && m_deviceController !=
nullptr) {
892 eARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE cmd_mode =
893 ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_LOW_LATENCY;
896 cmd_mode = ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_LOW_LATENCY;
899 cmd_mode = ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_HIGH_RELIABILITY;
902 cmd_mode = ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_HIGH_RELIABILITY_LOW_FRAMERATE;
907 m_streamingModeSet =
false;
908 m_deviceController->aRDrone3->sendMediaStreamingVideoStreamMode(m_deviceController->aRDrone3, cmd_mode);
911 while (!m_streamingModeSet) {
917 ARSAL_PRINT_ERROR,
"ERROR",
918 "Can't set streaming mode : drone has to be landed and not streaming in order to set streaming mode.");
921 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set streaming mode : drone isn't running.");
938 if (
isRunning() && m_deviceController !=
nullptr) {
942 eARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORESOLUTIONS_TYPE cmd_mode;
948 cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORESOLUTIONS_TYPE_REC1080_STREAM480;
954 cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORESOLUTIONS_TYPE_REC720_STREAM720;
960 m_videoResolutionSet =
false;
961 m_deviceController->aRDrone3->sendPictureSettingsVideoResolutions(m_deviceController->aRDrone3, cmd_mode);
964 while (!m_videoResolutionSet) {
969 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
970 "Can't set video resolution : drone has to be landed and not streaming in order to set streaming "
974 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set video resolution : drone isn't running.");
991 if (
isRunning() && m_deviceController !=
nullptr) {
993 eARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE cmd_mode =
994 ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_NONE;
998 cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_NONE;
1001 cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_ROLL;
1004 cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_PITCH;
1007 cmd_mode = ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_ROLL_PITCH;
1013 m_deviceController->aRDrone3->sendPictureSettingsVideoStabilizationMode(m_deviceController->aRDrone3, cmd_mode);
1016 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't set video stabilisation mode : drone isn't running.");
1030 if (
isRunning() && m_deviceController !=
nullptr) {
1031 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Starting video streaming ... ");
1034 m_errorController = m_deviceController->aRDrone3->sendMediaStreamingVideoEnable(m_deviceController->aRDrone3, 1);
1036 if (m_errorController == ARCONTROLLER_OK) {
1037 m_streamingStarted =
false;
1039 while (!m_streamingStarted) {
1042 startVideoDecoding();
1050 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1054 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't start streaming : drone isn't running.");
1065 if (m_videoDecodingStarted && m_deviceController !=
nullptr) {
1066 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Stopping video streaming ... ");
1069 m_errorController = m_deviceController->aRDrone3->sendMediaStreamingVideoEnable(m_deviceController->aRDrone3, 0);
1071 if (m_errorController == ARCONTROLLER_OK) {
1074 while (getStreamingState() != ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_DISABLED) {
1078 stopVideoDecoding();
1081 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1085 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Can't stop streaming : streaming already stopped.");
1099 void vpRobotBebop2::sighandler(
int signo)
1101 std::cout <<
"Stopping Bebop2 because of detected signal (" << signo <<
"): " <<
static_cast<char>(7);
1104 std::cout <<
"SIGINT (stopped by ^C) " << std::endl;
1107 std::cout <<
"SIGBUS (stopped due to a bus error) " << std::endl;
1110 std::cout <<
"SIGSEGV (stopped due to a segmentation fault) " << std::endl;
1113 std::cout <<
"SIGKILL (stopped by CTRL \\) " << std::endl;
1116 std::cout <<
"SIGQUIT " << std::endl;
1119 std::cout << signo << std::endl;
1122 vpRobotBebop2::m_running =
false;
1125 if (m_deviceController !=
nullptr) {
1126 m_deviceController->aRDrone3->sendPilotingLanding(m_deviceController->aRDrone3);
1128 std::exit(EXIT_FAILURE);
1135 eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE vpRobotBebop2::getFlyingState()
1137 if (m_deviceController !=
nullptr) {
1138 eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE flyingState =
1139 ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_MAX;
1140 eARCONTROLLER_ERROR error;
1142 ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary = ARCONTROLLER_ARDrone3_GetCommandElements(
1143 m_deviceController->aRDrone3, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED, &error);
1145 if (error == ARCONTROLLER_OK && elementDictionary !=
nullptr) {
1146 ARCONTROLLER_DICTIONARY_ARG_t *arg =
nullptr;
1147 ARCONTROLLER_DICTIONARY_ELEMENT_t *element =
nullptr;
1149 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1151 if (element !=
nullptr) {
1154 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE,
1157 if (arg !=
nullptr) {
1159 flyingState =
static_cast<eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE
>(arg->value.I32);
1165 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error when checking flying state : drone isn't connected.");
1166 return ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_MAX;
1174 eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED vpRobotBebop2::getStreamingState()
1176 if (m_deviceController !=
nullptr) {
1177 eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED streamingState =
1178 ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_MAX;
1179 eARCONTROLLER_ERROR error;
1181 ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary = ARCONTROLLER_ARDrone3_GetCommandElements(
1182 m_deviceController->aRDrone3, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED,
1185 if (error == ARCONTROLLER_OK && elementDictionary !=
nullptr) {
1186 ARCONTROLLER_DICTIONARY_ARG_t *arg =
nullptr;
1187 ARCONTROLLER_DICTIONARY_ELEMENT_t *element =
nullptr;
1189 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1191 if (element !=
nullptr) {
1193 HASH_FIND_STR(element->arguments,
1194 ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED, arg);
1196 if (arg !=
nullptr) {
1199 static_cast<eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED
>(arg->value.I32);
1203 return streamingState;
1205 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error when checking streaming state : drone isn't connected.");
1206 return ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_MAX;
1214 ARDISCOVERY_Device_t *vpRobotBebop2::discoverDrone()
1216 eARDISCOVERY_ERROR errorDiscovery = ARDISCOVERY_OK;
1218 ARDISCOVERY_Device_t *device = ARDISCOVERY_Device_New(&errorDiscovery);
1220 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Starting drone Wifi discovery ...");
1221 const char *charIpAddress = m_ipAddress.c_str();
1223 ARDISCOVERY_Device_InitWifi(device, ARDISCOVERY_PRODUCT_BEBOP_2,
"bebop2", charIpAddress, m_discoveryPort);
1225 if (errorDiscovery != ARDISCOVERY_OK) {
1226 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Discovery error :%s", ARDISCOVERY_Error_ToString(errorDiscovery));
1228 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Drone controller created.");
1238 void vpRobotBebop2::createDroneController(ARDISCOVERY_Device_t *discoveredDrone)
1240 m_deviceController = ARCONTROLLER_Device_New(discoveredDrone, &m_errorController);
1241 if (m_errorController != ARCONTROLLER_OK) {
1242 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Creation of deviceController failed.");
1244 ARDISCOVERY_Device_Delete(&discoveredDrone);
1245 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Device created.");
1252 void vpRobotBebop2::setupCallbacks()
1255 m_errorController = ARCONTROLLER_Device_AddStateChangedCallback(m_deviceController, stateChangedCallback,
this);
1256 if (m_errorController != ARCONTROLLER_OK) {
1257 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"add State callback failed.");
1261 m_errorController = ARCONTROLLER_Device_AddCommandReceivedCallback(m_deviceController, commandReceivedCallback,
this);
1263 if (m_errorController != ARCONTROLLER_OK) {
1264 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"add Command callback failed.");
1267 #ifdef VISP_HAVE_FFMPEG
1269 m_errorController = ARCONTROLLER_Device_SetVideoStreamCallbacks(m_deviceController, decoderConfigCallback,
1270 didReceiveFrameCallback,
nullptr,
this);
1272 if (m_errorController != ARCONTROLLER_OK) {
1273 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error: %s", ARCONTROLLER_Error_ToString(m_errorController));
1276 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Callbacks set up.");
1283 void vpRobotBebop2::startController()
1286 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Connecting ...");
1287 m_errorController = ARCONTROLLER_Device_Start(m_deviceController);
1289 if (m_errorController != ARCONTROLLER_OK) {
1290 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1294 ARSAL_Sem_Wait(&(m_stateSem));
1297 m_deviceState = ARCONTROLLER_Device_GetState(m_deviceController, &m_errorController);
1299 if ((m_errorController != ARCONTROLLER_OK) || (m_deviceState != ARCONTROLLER_DEVICE_STATE_RUNNING)) {
1300 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- deviceState :%d", m_deviceState);
1301 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"- error :%s", ARCONTROLLER_Error_ToString(m_errorController));
1303 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Controller started.");
1306 #ifdef VISP_HAVE_FFMPEG
1312 void vpRobotBebop2::initCodec()
1315 avcodec_register_all();
1316 avformat_network_init();
1319 AVCodec *codec = avcodec_find_decoder(AV_CODEC_ID_H264);
1321 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Codec not found.");
1326 m_codecContext = avcodec_alloc_context3(codec);
1328 if (!m_codecContext) {
1329 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Failed to allocate codec context.");
1334 m_codecContext->pix_fmt = AV_PIX_FMT_YUV420P;
1335 m_codecContext->skip_frame = AVDISCARD_DEFAULT;
1336 m_codecContext->error_concealment = FF_EC_GUESS_MVS | FF_EC_DEBLOCK;
1337 m_codecContext->skip_loop_filter = AVDISCARD_DEFAULT;
1338 m_codecContext->workaround_bugs = AVMEDIA_TYPE_VIDEO;
1339 m_codecContext->codec_id = AV_CODEC_ID_H264;
1340 m_codecContext->skip_idct = AVDISCARD_DEFAULT;
1342 m_codecContext->width = m_videoWidth;
1343 m_codecContext->height = m_videoHeight;
1345 if (codec->capabilities & AV_CODEC_CAP_TRUNCATED) {
1346 m_codecContext->flags |= AV_CODEC_FLAG_TRUNCATED;
1348 m_codecContext->flags2 |= AV_CODEC_FLAG2_CHUNKS;
1351 if (avcodec_open2(m_codecContext, codec,
nullptr) < 0) {
1352 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Failed to open codec.");
1356 AVPixelFormat pFormat = AV_PIX_FMT_BGR24;
1357 int numBytes = av_image_get_buffer_size(pFormat, m_codecContext->width, m_codecContext->height, 1);
1358 m_buffer =
static_cast<uint8_t *
>(av_malloc(
static_cast<unsigned long>(numBytes) *
sizeof(uint8_t)));
1360 av_init_packet(&m_packet);
1361 m_picture = av_frame_alloc();
1363 m_bgr_picture_mutex.lock();
1364 m_bgr_picture = av_frame_alloc();
1365 m_bgr_picture_mutex.unlock();
1367 m_img_convert_ctx = sws_getContext(m_codecContext->width, m_codecContext->height, m_codecContext->pix_fmt,
1368 m_codecContext->width, m_codecContext->height, pFormat, SWS_BICUBIC,
nullptr,
nullptr,
1377 void vpRobotBebop2::cleanUpCodec()
1379 m_videoDecodingStarted =
false;
1380 av_packet_unref(&m_packet);
1382 if (m_codecContext) {
1383 avcodec_flush_buffers(m_codecContext);
1384 avcodec_free_context(&m_codecContext);
1388 av_frame_free(&m_picture);
1391 if (m_bgr_picture) {
1392 m_bgr_picture_mutex.lock();
1393 av_frame_free(&m_bgr_picture);
1394 m_bgr_picture_mutex.unlock();
1397 if (m_img_convert_ctx) {
1398 sws_freeContext(m_img_convert_ctx);
1410 void vpRobotBebop2::startVideoDecoding()
1412 if (!m_videoDecodingStarted) {
1414 m_videoDecodingStarted =
true;
1416 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Video decoding is already started.");
1425 void vpRobotBebop2::stopVideoDecoding()
1427 if (m_videoDecodingStarted) {
1430 ARSAL_PRINT(ARSAL_PRINT_ERROR,
"ERROR",
"Video decoding is already stopped.");
1441 void vpRobotBebop2::computeFrame(ARCONTROLLER_Frame_t *frame)
1445 if (m_update_codec_params && m_codec_params_data.size()) {
1446 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Updating H264 codec parameters (Buffer Size: %lu) ...",
1447 m_codec_params_data.size());
1449 m_packet.data = &m_codec_params_data[0];
1450 m_packet.size =
static_cast<int>(m_codec_params_data.size());
1452 int ret = avcodec_send_packet(m_codecContext, &m_packet);
1456 ret = avcodec_receive_frame(m_codecContext, m_picture);
1458 if (ret == 0 || ret == AVERROR(EAGAIN)) {
1459 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"H264 codec parameters updated.");
1461 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Unexpected error while updating H264 parameters.");
1464 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Unexpected error while sending H264 parameters.");
1466 m_update_codec_params =
false;
1467 av_packet_unref(&m_packet);
1468 av_frame_unref(m_picture);
1472 m_packet.data = frame->data;
1473 m_packet.size =
static_cast<int>(frame->used);
1475 int ret = avcodec_send_packet(m_codecContext, &m_packet);
1478 char *errbuff =
new char[AV_ERROR_MAX_STRING_SIZE];
1479 av_strerror(ret, errbuff, AV_ERROR_MAX_STRING_SIZE);
1480 std::string err(errbuff);
1482 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error sending a packet for decoding : %d, %s", ret, err.c_str());
1486 ret = avcodec_receive_frame(m_codecContext, m_picture);
1490 if (ret == AVERROR(EAGAIN)) {
1491 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"AVERROR(EAGAIN)");
1492 }
else if (ret == AVERROR_EOF) {
1493 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"AVERROR_EOF");
1496 char *errbuff =
new char[AV_ERROR_MAX_STRING_SIZE];
1497 av_strerror(ret, errbuff, AV_ERROR_MAX_STRING_SIZE);
1498 std::string err(errbuff);
1500 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error receiving a decoded frame : %d, %s", ret, err.c_str());
1503 m_bgr_picture_mutex.lock();
1504 av_frame_unref(m_bgr_picture);
1505 av_image_fill_arrays(m_bgr_picture->data, m_bgr_picture->linesize, m_buffer, AV_PIX_FMT_BGR24,
1506 m_codecContext->width, m_codecContext->height, 1);
1508 sws_scale(m_img_convert_ctx, (m_picture)->data, (m_picture)->linesize, 0, m_codecContext->height,
1509 (m_bgr_picture)->data, (m_bgr_picture)->linesize);
1511 m_bgr_picture_mutex.unlock();
1515 av_packet_unref(&m_packet);
1517 av_frame_unref(m_picture);
1525 void vpRobotBebop2::cleanUp()
1527 if (m_deviceController !=
nullptr) {
1531 #ifdef VISP_HAVE_FFMPEG
1537 m_deviceState = ARCONTROLLER_Device_GetState(m_deviceController, &m_errorController);
1538 if ((m_errorController == ARCONTROLLER_OK) && (m_deviceState != ARCONTROLLER_DEVICE_STATE_STOPPED)) {
1540 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Disconnecting ...");
1541 m_errorController = ARCONTROLLER_Device_Stop(m_deviceController);
1543 if (m_errorController == ARCONTROLLER_OK) {
1545 ARSAL_Sem_Wait(&(m_stateSem));
1548 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Deleting device controller ...");
1549 ARCONTROLLER_Device_Delete(&m_deviceController);
1552 ARSAL_Sem_Destroy(&(m_stateSem));
1554 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"- Cleanup done.");
1557 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Error while cleaning up memory.");
1574 void vpRobotBebop2::stateChangedCallback(eARCONTROLLER_DEVICE_STATE newState, eARCONTROLLER_ERROR error,
1577 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Controller state changed, new state: %d.", newState);
1582 case ARCONTROLLER_DEVICE_STATE_STOPPED:
1584 drone->m_running =
false;
1586 ARSAL_Sem_Post(&(drone->m_stateSem));
1589 case ARCONTROLLER_DEVICE_STATE_RUNNING:
1591 ARSAL_Sem_Post(&(drone->m_stateSem));
1599 #ifdef VISP_HAVE_FFMPEG
1609 eARCONTROLLER_ERROR vpRobotBebop2::decoderConfigCallback(ARCONTROLLER_Stream_Codec_t codec,
void *customData)
1613 uint8_t *sps_buffer_ptr = codec.parameters.h264parameters.spsBuffer;
1614 uint32_t sps_buffer_size =
static_cast<uint32_t
>(codec.parameters.h264parameters.spsSize);
1615 uint8_t *pps_buffer_ptr = codec.parameters.h264parameters.ppsBuffer;
1616 uint32_t pps_buffer_size =
static_cast<uint32_t
>(codec.parameters.h264parameters.ppsSize);
1618 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"H264 configuration packet received: #SPS: %d #PPS: %d", sps_buffer_size,
1621 drone->m_update_codec_params = (sps_buffer_ptr && pps_buffer_ptr && sps_buffer_size && pps_buffer_size &&
1622 (pps_buffer_size < 32) && (sps_buffer_size < 32));
1624 if (drone->m_update_codec_params) {
1627 drone->m_codec_params_data.resize(sps_buffer_size + pps_buffer_size);
1628 std::copy(sps_buffer_ptr, sps_buffer_ptr + sps_buffer_size, drone->m_codec_params_data.begin());
1629 std::copy(pps_buffer_ptr, pps_buffer_ptr + pps_buffer_size, drone->m_codec_params_data.begin() + sps_buffer_size);
1632 drone->m_codec_params_data.clear();
1634 return ARCONTROLLER_OK;
1645 eARCONTROLLER_ERROR vpRobotBebop2::didReceiveFrameCallback(ARCONTROLLER_Frame_t *frame,
void *customData)
1649 if (frame !=
nullptr) {
1651 if (drone->m_videoDecodingStarted) {
1652 drone->computeFrame(frame);
1656 ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG,
"frame is nullptr.");
1659 return ARCONTROLLER_OK;
1672 void vpRobotBebop2::cmdBatteryStateChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1675 ARCONTROLLER_DICTIONARY_ARG_t *arg =
nullptr;
1676 ARCONTROLLER_DICTIONARY_ELEMENT_t *singleElement =
nullptr;
1678 if (elementDictionary ==
nullptr) {
1679 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"elements is nullptr");
1684 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, singleElement);
1686 if (singleElement ==
nullptr) {
1687 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"singleElement is nullptr");
1692 HASH_FIND_STR(singleElement->arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_BATTERYSTATECHANGED_PERCENT,
1695 if (arg ==
nullptr) {
1696 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"arg is nullptr");
1699 drone->m_batteryLevel = arg->value.U8;
1700 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Battery level changed : %u percent remaining.", drone->m_batteryLevel);
1702 if (drone->m_batteryLevel <= 5) {
1703 ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG,
" - WARNING, very low battery level, drone will stop soon !");
1704 }
else if (drone->m_batteryLevel <= 10) {
1705 ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG,
" - Warning, low battery level !");
1719 void vpRobotBebop2::cmdCameraOrientationChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1722 ARCONTROLLER_DICTIONARY_ARG_t *arg =
nullptr;
1723 ARCONTROLLER_DICTIONARY_ELEMENT_t *element =
nullptr;
1724 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1725 if (element !=
nullptr) {
1726 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2_TILT, arg);
1728 if (arg !=
nullptr) {
1729 drone->m_currentCameraTilt =
static_cast<double>(arg->value.Float);
1732 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2_PAN, arg);
1733 if (arg !=
nullptr) {
1734 drone->m_currentCameraPan =
static_cast<double>(arg->value.Float);
1750 void vpRobotBebop2::cmdCameraSettingsRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
vpRobotBebop2 *drone)
1752 ARCONTROLLER_DICTIONARY_ARG_t *arg =
nullptr;
1753 ARCONTROLLER_DICTIONARY_ELEMENT_t *element =
nullptr;
1754 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1755 if (element !=
nullptr) {
1756 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_FOV,
1758 if (arg !=
nullptr) {
1759 drone->m_cameraHorizontalFOV =
static_cast<double>(arg->value.Float);
1760 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Camera horizontal FOV : %f degrees.",
1761 static_cast<double>(drone->m_cameraHorizontalFOV));
1763 HASH_FIND_STR(element->arguments,
1764 ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_PANMAX, arg);
1765 if (arg !=
nullptr) {
1766 drone->m_maxCameraPan =
static_cast<double>(arg->value.Float);
1767 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Max camera pan : %f degrees.",
1768 static_cast<double>(drone->m_maxCameraPan));
1770 HASH_FIND_STR(element->arguments,
1771 ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_PANMIN, arg);
1772 if (arg !=
nullptr) {
1773 drone->m_minCameraPan =
static_cast<double>(arg->value.Float);
1774 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Min camera pan : %f degrees.",
1775 static_cast<double>(drone->m_minCameraPan));
1777 HASH_FIND_STR(element->arguments,
1778 ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_TILTMAX, arg);
1779 if (arg !=
nullptr) {
1780 drone->m_maxCameraTilt =
static_cast<double>(arg->value.Float);
1781 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Max camera tilt : %f degrees.",
1782 static_cast<double>(drone->m_maxCameraTilt));
1784 HASH_FIND_STR(element->arguments,
1785 ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED_TILTMIN, arg);
1786 if (arg !=
nullptr) {
1787 drone->m_minCameraTilt =
static_cast<double>(arg->value.Float);
1788 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
" - Min camera tilt : %f degrees.",
1789 static_cast<double>(drone->m_minCameraTilt));
1804 void vpRobotBebop2::cmdMaxPitchRollChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
1807 ARCONTROLLER_DICTIONARY_ARG_t *arg =
nullptr;
1808 ARCONTROLLER_DICTIONARY_ELEMENT_t *element =
nullptr;
1810 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1811 if (element !=
nullptr) {
1812 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXTILTCHANGED_CURRENT,
1814 if (arg !=
nullptr) {
1815 drone->m_maxTilt =
static_cast<double>(arg->value.Float);
1830 void vpRobotBebop2::cmdRelativeMoveEndedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
vpRobotBebop2 *drone)
1832 ARCONTROLLER_DICTIONARY_ARG_t *arg =
nullptr;
1833 ARCONTROLLER_DICTIONARY_ELEMENT_t *element =
nullptr;
1835 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1837 if (element !=
nullptr) {
1838 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR, arg);
1840 if (arg !=
nullptr) {
1841 eARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR error =
1842 static_cast<eARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR
>(arg->value.I32);
1843 if ((error != ARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR_OK) &&
1844 (error != ARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR_INTERRUPTED)) {
1845 ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG,
"Relative move ended with error %d", error);
1847 drone->m_relativeMoveEnded =
true;
1862 void vpRobotBebop2::cmdExposureSetRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
vpRobotBebop2 *drone)
1864 ARCONTROLLER_DICTIONARY_ARG_t *arg =
nullptr;
1865 ARCONTROLLER_DICTIONARY_ELEMENT_t *element =
nullptr;
1867 HASH_FIND_STR(elementDictionary, ARCONTROLLER_DICTIONARY_SINGLE_KEY, element);
1869 if (element !=
nullptr) {
1871 HASH_FIND_STR(element->arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED_VALUE,
1874 if (arg !=
nullptr) {
1875 drone->m_exposureSet =
true;
1889 void vpRobotBebop2::commandReceivedCallback(eARCONTROLLER_DICTIONARY_KEY commandKey,
1890 ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary,
void *customData)
1894 if (drone ==
nullptr)
1897 switch (commandKey) {
1898 case ARCONTROLLER_DICTIONARY_KEY_COMMON_COMMONSTATE_BATTERYSTATECHANGED:
1900 cmdBatteryStateChangedRcv(elementDictionary, drone);
1903 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXTILTCHANGED:
1905 cmdMaxPitchRollChangedRcv(elementDictionary, drone);
1908 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGEVENT_MOVEBYEND:
1910 cmdRelativeMoveEndedRcv(elementDictionary, drone);
1913 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_FLATTRIMCHANGED:
1915 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Flat trim finished ...");
1916 drone->m_flatTrimFinished =
true;
1919 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED:
1921 cmdExposureSetRcv(elementDictionary, drone);
1924 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_VIDEORESOLUTIONSCHANGED:
1926 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Video resolution set ...");
1927 drone->m_videoResolutionSet =
true;
1930 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED:
1932 drone->m_streamingStarted =
true;
1935 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOSTREAMMODECHANGED:
1937 drone->m_streamingModeSet =
true;
1940 case ARCONTROLLER_DICTIONARY_KEY_COMMON_SETTINGSSTATE_RESETCHANGED:
1942 ARSAL_PRINT(ARSAL_PRINT_INFO, TAG,
"Settings reset ...");
1943 drone->m_settingsReset =
true;
1946 case ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_CAMERASTATE_ORIENTATIONV2:
1948 cmdCameraOrientationChangedRcv(elementDictionary, drone);
1951 case ARCONTROLLER_DICTIONARY_KEY_COMMON_CAMERASETTINGSSTATE_CAMERASETTINGSCHANGED:
1953 cmdCameraSettingsRcv(elementDictionary, drone);
1963 #elif !defined(VISP_BUILD_SHARED_LIBS)
1966 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)