bebop.cpp
Go to the documentation of this file.
00001 
00025 #include <stdexcept>
00026 #include <cmath>
00027 #include <string>
00028 #include <algorithm>
00029 #include <vector>
00030 
00031 #include <boost/bind.hpp>
00032 #include <boost/thread/locks.hpp>
00033 #include <boost/make_shared.hpp>
00034 
00035 extern "C"
00036 {
00037   #include "libARCommands/ARCommands.h"
00038   #include "libARDiscovery/ARDiscovery.h"
00039   #include <libARController/ARController.h>
00040 }
00041 
00042 #include <bebop_driver/bebop.h>
00043 #include "bebop_driver/BebopArdrone3Config.h"
00044 #include "bebop_driver/bebop_video_decoder.h"
00045 
00046 // include all callback wrappers
00047 #include "bebop_driver/autogenerated/ardrone3_state_callbacks.h"
00048 #include "bebop_driver/autogenerated/common_state_callbacks.h"
00049 #include "bebop_driver/autogenerated/ardrone3_setting_callbacks.h"
00050 
00051 /*
00052  * Bebop coordinate systems
00053  *
00054  * Velocities:
00055  *
00056  * +x : East
00057  * +y : South
00058  * +z : Down
00059  *
00060  * Attitude:
00061  *
00062  * +x   : forward
00063  * +y   : right
00064  * +z   : down
00065  * +yaw : CW
00066  *
00067  * ROS coordinate system (REP 105)
00068  *
00069  * +x   : forward
00070  * +y   : left
00071  * +z   : up
00072  * +yaw : CCW
00073  *
00074  * Move function (setPilotingPCMD, conforms with Attiude, except for gaz)
00075  *
00076  * +roll  : right
00077  * +pitch : forward
00078  * +gaz   : up
00079  * +yaw   : CW
00080  *
00081  */
00082 
00083 namespace bebop_driver
00084 {
00085 
00086 const char* Bebop::LOG_TAG = "BebopSDK";
00087 
00088 void Bebop::StateChangedCallback(eARCONTROLLER_DEVICE_STATE new_state, eARCONTROLLER_ERROR error, void *bebop_void_ptr)
00089 {
00090   // TODO(mani-monaj): Log error
00091   Bebop* bebop_ptr_ = static_cast<Bebop*>(bebop_void_ptr);
00092 
00093   switch (new_state)
00094   {
00095   case ARCONTROLLER_DEVICE_STATE_STOPPED:
00096     ARSAL_Sem_Post(&(bebop_ptr_->state_sem_));
00097     break;
00098   case ARCONTROLLER_DEVICE_STATE_RUNNING:
00099     ARSAL_Sem_Post(&(bebop_ptr_->state_sem_));
00100     break;
00101   }
00102 }
00103 
00104 void Bebop::CommandReceivedCallback(eARCONTROLLER_DICTIONARY_KEY cmd_key,
00105                                     ARCONTROLLER_DICTIONARY_ELEMENT_t *element_dict_ptr,
00106                                     void *bebop_void_ptr)
00107 {
00108   static long int lwp_id = util::GetLWPId();
00109   static bool lwp_id_printed = false;
00110 
00111   if (!lwp_id_printed)
00112   {
00113     ARSAL_PRINT(ARSAL_PRINT_INFO, LOG_TAG, "Command Received Callback LWP id is: %ld", lwp_id);
00114     lwp_id_printed = true;
00115   }
00116   Bebop* bebop_ptr = static_cast<Bebop*>(bebop_void_ptr);
00117   if (!bebop_ptr->IsConnected()) return;
00118 
00119   ARCONTROLLER_DICTIONARY_ELEMENT_t *single_element_ptr = NULL;
00120 
00121   if (element_dict_ptr)
00122   {
00123     // We are only interested in single key dictionaries
00124     HASH_FIND_STR(element_dict_ptr, ARCONTROLLER_DICTIONARY_SINGLE_KEY, single_element_ptr);
00125 
00126     if (single_element_ptr)
00127     {
00128       callback_map_t::iterator it = bebop_ptr->callback_map_.find(cmd_key);
00129       if (it != bebop_ptr->callback_map_.end())
00130       {
00131         // TODO(mani-monaj): Check if we can find the time from the packets
00132         it->second->Update(element_dict_ptr->arguments, ros::Time::now());
00133       }
00134     }
00135   }
00136 }
00137 
00138 // This callback is called within the same context as FrameReceivedCallback()
00139 eARCONTROLLER_ERROR Bebop::DecoderConfigCallback(ARCONTROLLER_Stream_Codec_t codec, void *bebop_void_ptr)
00140 {
00141   Bebop* bebop_ptr = static_cast<Bebop*>(bebop_void_ptr);
00142   if (codec.type == ARCONTROLLER_STREAM_CODEC_TYPE_H264)
00143   {
00144     ARSAL_PRINT(ARSAL_PRINT_INFO, LOG_TAG, "H264 configuration packet received: #SPS: %d #PPS: %d (MP4? %d)",
00145                 codec.parameters.h264parameters.spsSize,
00146                 codec.parameters.h264parameters.ppsSize,
00147                 codec.parameters.h264parameters.isMP4Compliant);
00148 
00149     if (!bebop_ptr->video_decoder_ptr_->SetH264Params(
00150           codec.parameters.h264parameters.spsBuffer,
00151           codec.parameters.h264parameters.spsSize,
00152           codec.parameters.h264parameters.ppsBuffer,
00153           codec.parameters.h264parameters.ppsSize))
00154     {
00155       return ARCONTROLLER_ERROR;
00156     }
00157   }
00158   else
00159   {
00160     ARSAL_PRINT(ARSAL_PRINT_WARNING, LOG_TAG, "Codec type is not H264");
00161     return ARCONTROLLER_ERROR;
00162   }
00163 
00164   return ARCONTROLLER_OK;
00165 }
00166 
00167 // This Callback runs in ARCONTROLLER_Stream_ReaderThreadRun context and blocks it until it returns
00168 eARCONTROLLER_ERROR Bebop::FrameReceivedCallback(ARCONTROLLER_Frame_t *frame, void *bebop_void_ptr)
00169 {
00170   static long int lwp_id = util::GetLWPId();
00171   static bool lwp_id_printed = false;
00172   if (!lwp_id_printed)
00173   {
00174     ARSAL_PRINT(ARSAL_PRINT_INFO, LOG_TAG, "Frame Recv & Decode LWP id: %ld", lwp_id);
00175     lwp_id_printed = true;
00176   }
00177 
00178   if (!frame)
00179   {
00180     ARSAL_PRINT(ARSAL_PRINT_WARNING, LOG_TAG, "Received frame is NULL");
00181     return ARCONTROLLER_ERROR_NO_VIDEO;
00182   }
00183 
00184   Bebop* bebop_ptr = static_cast<Bebop*>(bebop_void_ptr);
00185   if (!bebop_ptr->IsConnected()) return ARCONTROLLER_ERROR;
00186 
00187   {
00188     boost::unique_lock<boost::mutex> lock(bebop_ptr->frame_avail_mutex_);
00189     if (bebop_ptr->is_frame_avail_)
00190     {
00191       ARSAL_PRINT(ARSAL_PRINT_WARNING, LOG_TAG, "Previous frame might have been missed.");
00192     }
00193 
00194     if (!bebop_ptr->video_decoder_ptr_->Decode(frame))
00195     {
00196       ARSAL_PRINT(ARSAL_PRINT_ERROR, LOG_TAG, "Video decode failed");
00197     }
00198     else
00199     {
00200       bebop_ptr->is_frame_avail_ = true;
00201       // ARSAL_PRINT(ARSAL_PRINT_INFO, LOG_TAG, "FRAME IS READY");
00202       bebop_ptr->frame_avail_cond_.notify_one();
00203     }
00204   }
00205 
00206   return ARCONTROLLER_OK;
00207 }
00208 
00209 
00210 Bebop::Bebop(ARSAL_Print_Callback_t custom_print_callback):
00211   is_connected_(false),
00212   is_streaming_started_(false),
00213   device_ptr_(NULL),
00214   device_controller_ptr_(NULL),
00215   error_(ARCONTROLLER_OK),
00216   device_state_(ARCONTROLLER_DEVICE_STATE_MAX),
00217   video_decoder_ptr_(new bebop_driver::VideoDecoder()),
00218   is_frame_avail_(false)
00219 //  out_file("/tmp/ts.txt")
00220 {
00221   // Redirect all calls to AR_PRINT_* to this function if provided
00222   if (custom_print_callback)
00223     ARSAL_Print_SetCallback(custom_print_callback);
00224 
00225   ARSAL_PRINT(ARSAL_PRINT_INFO, LOG_TAG, "Bebop Cnstr()");
00226 }
00227 
00228 Bebop::~Bebop()
00229 {
00230   // This is the last resort, the program must run Cleanup() fo
00231   // proper disconnection and free
00232   if (device_ptr_) ARDISCOVERY_Device_Delete(&device_ptr_);
00233   if (device_controller_ptr_) ARCONTROLLER_Device_Delete(&device_controller_ptr_);
00234 }
00235 
00236 void Bebop::Connect(ros::NodeHandle& nh, ros::NodeHandle& priv_nh, const std::string& bebop_ip)
00237 {
00238   try
00239   {
00240     if (is_connected_) throw std::runtime_error("Already inited");
00241 
00242     // TODO(mani-monaj): Error checking;
00243     ARSAL_Sem_Init(&state_sem_, 0, 0);
00244 
00245     eARDISCOVERY_ERROR error_discovery = ARDISCOVERY_OK;
00246     device_ptr_ = ARDISCOVERY_Device_New(&error_discovery);
00247 
00248     if (error_discovery != ARDISCOVERY_OK)
00249     {
00250       throw std::runtime_error("Discovery failed: " + std::string(ARDISCOVERY_Error_ToString(error_discovery)));
00251     }
00252 
00253     // set/save ip
00254     bebop_ip_ = bebop_ip;
00255 
00256     // TODO(mani-monaj): Make ip and port params
00257     error_discovery = ARDISCOVERY_Device_InitWifi(device_ptr_,
00258                                                   ARDISCOVERY_PRODUCT_ARDRONE, "Bebop",
00259                                                   bebop_ip_.c_str(), 44444);
00260 
00261     if (error_discovery != ARDISCOVERY_OK)
00262     {
00263       throw std::runtime_error("Discovery failed: " + std::string(ARDISCOVERY_Error_ToString(error_discovery)));
00264     }
00265 
00266     device_controller_ptr_ = ARCONTROLLER_Device_New(device_ptr_, &error_);
00267     ThrowOnCtrlError(error_, "Creation of device controller failed: ");
00268 
00269     ARDISCOVERY_Device_Delete(&device_ptr_);
00270 
00271       // callback_map is not being modified after this initial update
00272 #define UPDTAE_CALLBACK_MAP
00273       #include "bebop_driver/autogenerated/ardrone3_state_callback_includes.h"
00274       #include "bebop_driver/autogenerated/common_state_callback_includes.h"
00275       #include "bebop_driver/autogenerated/ardrone3_setting_callback_includes.h"
00276 #undef UPDTAE_CALLBACK_MAP
00277 
00278     ThrowOnCtrlError(ARCONTROLLER_Device_Start(device_controller_ptr_), "Controller device start failed");
00279 
00280     ThrowOnCtrlError(
00281           ARCONTROLLER_Device_AddStateChangedCallback(
00282             device_controller_ptr_, Bebop::StateChangedCallback, reinterpret_cast<void*>(this)),
00283           "Registering state callback failed");
00284     ThrowOnCtrlError(
00285           ARCONTROLLER_Device_AddCommandReceivedCallback(
00286             device_controller_ptr_, Bebop::CommandReceivedCallback, reinterpret_cast<void*>(this)),
00287           "Registering command callback failed");
00288 
00289 //    ThrowOnCtrlError(
00290 //          ARCONTROLLER_Device_SetVideoStreamMP4Compliant(
00291 //            device_controller_ptr_, 1),
00292 //          "Enforcing MP4 compliancy failed");
00293 
00294     // The forth argument is frame timeout callback
00295     ThrowOnCtrlError(
00296           ARCONTROLLER_Device_SetVideoStreamCallbacks(
00297             device_controller_ptr_,  Bebop::DecoderConfigCallback, Bebop::FrameReceivedCallback,
00298             NULL , reinterpret_cast<void*>(this)),
00299           "Registering video callback failed");
00300 
00301     // This semaphore is touched inside the StateCallback
00302     ARSAL_Sem_Wait(&state_sem_);
00303 
00304     device_state_ = ARCONTROLLER_Device_GetState(device_controller_ptr_, &error_);
00305     if ((error_ != ARCONTROLLER_OK) || (device_state_ != ARCONTROLLER_DEVICE_STATE_RUNNING))
00306     {
00307       throw std::runtime_error("Waiting for device failed: " + std::string(ARCONTROLLER_Error_ToString(error_)));
00308     }
00309 
00310     // Enforce termination of video streaming ... (use Start/Stop streaming to enable/disable this)
00311     ThrowOnCtrlError(device_controller_ptr_->aRDrone3->sendMediaStreamingVideoEnable(
00312                        device_controller_ptr_->aRDrone3, 0), "Stopping video stream failed.");
00313   }
00314   catch (const std::runtime_error& e)
00315   {
00316     Cleanup();
00317     throw e;
00318   }
00319 
00320   is_connected_ = true;
00321   ARSAL_PRINT(ARSAL_PRINT_INFO, LOG_TAG, "BebopSDK inited, lwp_id: %ld", util::GetLWPId());
00322 }
00323 
00324 void Bebop::Cleanup()
00325 {
00326   ARSAL_PRINT(ARSAL_PRINT_INFO, LOG_TAG, "Bebop Cleanup()");
00327   if (device_controller_ptr_)
00328   {
00329     device_state_ = ARCONTROLLER_Device_GetState(device_controller_ptr_, &error_);
00330     if ((error_ == ARCONTROLLER_OK) && (device_state_ != ARCONTROLLER_DEVICE_STATE_STOPPED))
00331     {
00332       ARSAL_PRINT(ARSAL_PRINT_INFO, LOG_TAG, "Disconnecting ...");
00333       error_ = ARCONTROLLER_Device_Stop(device_controller_ptr_);
00334       if (error_ == ARCONTROLLER_OK)
00335       {
00336         ARSAL_Sem_Wait(&state_sem_);
00337       }
00338     }
00339     ARCONTROLLER_Device_Delete(&device_controller_ptr_);
00340   }
00341   is_connected_ = false;
00342   ARSAL_Sem_Destroy(&state_sem_);
00343 }
00344 
00345 void Bebop::StartStreaming()
00346 {
00347   if (is_streaming_started_)
00348   {
00349     ARSAL_PRINT(ARSAL_PRINT_WARNING, LOG_TAG, "Video streaming is already started ...");
00350     return;
00351   }
00352   try
00353   {
00354     ThrowOnInternalError("Starting video stream failed");
00355     // Start video streaming
00356     ThrowOnCtrlError(device_controller_ptr_->aRDrone3->sendMediaStreamingVideoEnable(
00357                        device_controller_ptr_->aRDrone3, 1), "Starting video stream failed.");
00358     is_streaming_started_ = true;
00359     ARSAL_PRINT(ARSAL_PRINT_WARNING, LOG_TAG, "Video streaming started ...");
00360   }
00361   catch (const std::runtime_error& e)
00362   {
00363     ARSAL_PRINT(ARSAL_PRINT_INFO, LOG_TAG, "Failed to start video streaming ...");
00364     is_streaming_started_ = false;
00365     throw e;
00366   }
00367 }
00368 
00369 void Bebop::StopStreaming()
00370 {
00371   if (!is_streaming_started_) return;
00372   try
00373   {
00374     ThrowOnInternalError("Stopping video stream failed");
00375     // Stop video streaming
00376     ThrowOnCtrlError(device_controller_ptr_->aRDrone3->sendMediaStreamingVideoEnable(
00377                        device_controller_ptr_->aRDrone3, 0), "Stopping video stream failed.");
00378     is_streaming_started_ = false;
00379   }
00380   catch (const std::runtime_error& e)
00381   {
00382     ARSAL_PRINT(ARSAL_PRINT_ERROR, LOG_TAG, "Failed to stop video streaming ...");
00383   }
00384 }
00385 
00386 void Bebop::Disconnect()
00387 {
00388   if (!is_connected_) return;
00389   if (is_streaming_started_) StopStreaming();
00390   Cleanup();
00391   ARSAL_PRINT(ARSAL_PRINT_INFO, LOG_TAG, "Disconnected from Bebop ...");
00392 }
00393 
00394 void Bebop::SetDate(const std::string &date)
00395 {
00396   ThrowOnInternalError("Setting Date Failed");
00397   ThrowOnCtrlError(
00398         device_controller_ptr_->common->sendCommonCurrentDate(device_controller_ptr_->common, const_cast<char*>(date.c_str())),
00399         "Setting Date Failed");
00400 }
00401 
00402 void Bebop::RequestAllSettings()
00403 {
00404   ThrowOnInternalError("Request Settings Failed");
00405   ThrowOnCtrlError(
00406         device_controller_ptr_->common->sendSettingsAllSettings(device_controller_ptr_->common),
00407         "Request Settings Failed");
00408 }
00409 
00410 void Bebop::ResetAllSettings()
00411 {
00412   ThrowOnInternalError("Reset Settings Failed");
00413   ThrowOnCtrlError(
00414         device_controller_ptr_->common->sendSettingsReset(device_controller_ptr_->common),
00415         "Reset Settings Failed");
00416 
00417   ARSAL_PRINT(ARSAL_PRINT_INFO, LOG_TAG, "All settings of the drone have been reset to default values.");
00418 }
00419 
00420 void Bebop::UpdateSettings(const BebopArdrone3Config &config)
00421 {
00422   ThrowOnInternalError("Update Settings Failed");
00423 
00424   // For all callback_map members
00425   // 1) Check if they are derived from AbstractSetting type
00426   // 1.1) Pass the config objects to them
00427 //  boost::lock_guard<boost::mutex> lock(callback_map_mutex_);
00428   for (callback_map_t::iterator it = callback_map_.begin(); it != callback_map_.end(); it++)
00429   {
00430     // Convert the base class pointer (AbstractCommand) to the derived class pointer (AbstractSetting)
00431     // In case of State classes, do nothing
00432     boost::shared_ptr<cb::AbstractSetting> setting_ptr = boost::dynamic_pointer_cast<cb::AbstractSetting>(it->second);
00433     if (setting_ptr)
00434     {
00435       setting_ptr->UpdateBebopFromROS(config, device_controller_ptr_);
00436     }
00437   }
00438 }
00439 
00440 void Bebop::Takeoff()
00441 {
00442   ThrowOnInternalError("Takeoff failed");
00443   ThrowOnCtrlError(
00444         device_controller_ptr_->aRDrone3->sendPilotingTakeOff(device_controller_ptr_->aRDrone3),
00445         "Takeoff failed");
00446 }
00447 
00448 void Bebop::Land()
00449 {
00450   ThrowOnInternalError("Land failed");
00451   ThrowOnCtrlError(
00452         device_controller_ptr_->aRDrone3->sendPilotingLanding(device_controller_ptr_->aRDrone3),
00453         "Land failed");
00454 }
00455 
00456 void Bebop::Emergency()
00457 {
00458   ThrowOnInternalError("Emergency failed");
00459   ThrowOnCtrlError(
00460         device_controller_ptr_->aRDrone3->sendPilotingEmergency(device_controller_ptr_->aRDrone3),
00461         "Emergency failed");
00462 }
00463 
00464 void Bebop::FlatTrim()
00465 {
00466   ThrowOnInternalError("FlatTrim failed");
00467   ThrowOnCtrlError(
00468         device_controller_ptr_->aRDrone3->sendPilotingFlatTrim(device_controller_ptr_->aRDrone3),
00469         "FlatTrim failed");
00470 }
00471 
00472 void Bebop::NavigateHome(const bool &start_stop)
00473 {
00474   ThrowOnInternalError("Navigate home failed");
00475   ThrowOnCtrlError(
00476         device_controller_ptr_->aRDrone3->sendPilotingNavigateHome(
00477           device_controller_ptr_->aRDrone3, start_stop ? 1 : 0),
00478         "Navigate home failed");
00479 }
00480 
00481 void Bebop::StartAutonomousFlight(const std::string &filepath)
00482 {
00483   ThrowOnInternalError("Start autonomous flight failed");
00484   ThrowOnCtrlError(
00485         device_controller_ptr_->common->sendMavlinkStart(device_controller_ptr_->common, const_cast<char*>(filepath.c_str()), (eARCOMMANDS_COMMON_MAVLINK_START_TYPE)0),
00486         "Start autonomous flight failed");
00487 }
00488 
00489 void Bebop::PauseAutonomousFlight()
00490 {
00491   ThrowOnInternalError("Pause autonomous flight failed");
00492   ThrowOnCtrlError(
00493         device_controller_ptr_->common->sendMavlinkPause(device_controller_ptr_->common),
00494         "Pause autonomous flight failed");
00495 }
00496 
00497 void Bebop::StopAutonomousFlight()
00498 {
00499   ThrowOnInternalError("Stop autonomous flight failed");
00500   ThrowOnCtrlError(
00501         device_controller_ptr_->common->sendMavlinkStop(device_controller_ptr_->common),
00502         "Stop autonomous flight failed");
00503 }
00504 
00505 void Bebop::AnimationFlip(const uint8_t &anim_id)
00506 {
00507   ThrowOnInternalError("Animation failed");
00508   if (anim_id >= ARCOMMANDS_ARDRONE3_ANIMATIONS_FLIP_DIRECTION_MAX)
00509   {
00510     throw std::runtime_error("Inavlid animation id");
00511   }
00512   ThrowOnCtrlError(
00513         device_controller_ptr_->aRDrone3->sendAnimationsFlip(
00514           device_controller_ptr_->aRDrone3, static_cast<eARCOMMANDS_ARDRONE3_ANIMATIONS_FLIP_DIRECTION>(
00515             anim_id % ARCOMMANDS_ARDRONE3_ANIMATIONS_FLIP_DIRECTION_MAX)),
00516         "Navigate home failed");
00517 }
00518 
00519 void Bebop::Move(const double &roll, const double &pitch, const double &gaz_speed, const double &yaw_speed)
00520 {
00521   // TODO(mani-monaj): Bound check
00522   ThrowOnInternalError("Move failure");
00523 
00524   // If roll or pitch value are non-zero, enabel roll/pitch flag
00525   const bool do_rp = !((fabs(roll) < 0.001) && (fabs(pitch) < 0.001));
00526 
00527   // If all values are zero, hover
00528   const bool do_hover = !do_rp && (fabs(yaw_speed) < 0.001) && (fabs(gaz_speed) < 0.001);
00529 
00530   if (do_hover)
00531   {
00532     ARSAL_PRINT(ARSAL_PRINT_DEBUG, LOG_TAG, "STOP");
00533     ThrowOnCtrlError(
00534           device_controller_ptr_->aRDrone3->setPilotingPCMD(
00535             device_controller_ptr_->aRDrone3,
00536             0, 0, 0, 0, 0, 0));
00537   }
00538   else
00539   {
00540     ThrowOnCtrlError(
00541           device_controller_ptr_->aRDrone3->setPilotingPCMD(
00542             device_controller_ptr_->aRDrone3,
00543             do_rp,
00544             static_cast<int8_t>(roll * 100.0),
00545             static_cast<int8_t>(pitch * 100.0),
00546             static_cast<int8_t>(yaw_speed * 100.0),
00547             static_cast<int8_t>(gaz_speed * 100.0),
00548             0));
00549   }
00550 }
00551 
00552 // in degrees
00553 void Bebop::MoveCamera(const double &tilt, const double &pan)
00554 {
00555   ThrowOnInternalError("Camera Move Failure");
00556   ThrowOnCtrlError(device_controller_ptr_->aRDrone3->setCameraOrientation(
00557                      device_controller_ptr_->aRDrone3,
00558                      static_cast<int8_t>(tilt),
00559                      static_cast<int8_t>(pan)));
00560 }
00561 
00562 uint32_t Bebop::GetFrontCameraFrameWidth() const
00563 {
00564   return video_decoder_ptr_->GetFrameWidth();
00565 }
00566 
00567 uint32_t Bebop::GetFrontCameraFrameHeight() const
00568 {
00569   return video_decoder_ptr_->GetFrameHeight();
00570 }
00571 
00572 bool Bebop::GetFrontCameraFrame(std::vector<uint8_t> &buffer, uint32_t& width, uint32_t& height) const
00573 {
00574   boost::unique_lock<boost::mutex> lock(frame_avail_mutex_);
00575 
00576   ARSAL_PRINT(ARSAL_PRINT_DEBUG, LOG_TAG, "Waiting for frame to become available ...");
00577   while (!is_frame_avail_)
00578   {
00579     frame_avail_cond_.wait(lock);
00580   }
00581 
00582 //  ARSAL_PRINT(ARSAL_PRINT_INFO, LOG_TAG, "COPY STARTED");
00583   const uint32_t num_bytes = video_decoder_ptr_->GetFrameWidth() * video_decoder_ptr_->GetFrameHeight() * 3;
00584   if (num_bytes == 0)
00585   {
00586     ARSAL_PRINT(ARSAL_PRINT_WARNING, LOG_TAG, "No picture size");
00587     return false;
00588   }
00589   buffer.resize(num_bytes);
00590   // New frame is ready
00591   std::copy(video_decoder_ptr_->GetFrameRGBRawCstPtr(),
00592             video_decoder_ptr_->GetFrameRGBRawCstPtr() + num_bytes,
00593             buffer.begin());
00594 
00595   width = video_decoder_ptr_->GetFrameWidth();
00596   height = video_decoder_ptr_->GetFrameHeight();
00597   is_frame_avail_ = false;
00598 //  ARSAL_PRINT(ARSAL_PRINT_INFO, LOG_TAG, "COPY ENDED");
00599   return true;
00600 }
00601 
00602 void Bebop::TakeSnapshot()
00603 {
00604   ThrowOnInternalError("Snapshot Failure");
00605   ThrowOnCtrlError(
00606     device_controller_ptr_->aRDrone3->sendMediaRecordPictureV2(
00607           device_controller_ptr_->aRDrone3));
00608 }
00609 
00610 void Bebop::SetExposure(const float& exposure)
00611 {
00612   ThrowOnInternalError("Failed to set exposure");
00613   // TODO fairf4x: Check bounds ?
00614   ThrowOnCtrlError(
00615     device_controller_ptr_->aRDrone3->sendPictureSettingsExpositionSelection(device_controller_ptr_->aRDrone3, (float)exposure)
00616   );
00617 }
00618 
00619 void Bebop::ToggleVideoRecording(const bool start)
00620 {
00621   ThrowOnInternalError("Video Toggle Failure");
00622   ThrowOnCtrlError(device_controller_ptr_->aRDrone3->sendMediaRecordVideoV2(
00623                      device_controller_ptr_->aRDrone3,
00624                      start ? ARCOMMANDS_ARDRONE3_MEDIARECORD_VIDEOV2_RECORD_START :
00625                              ARCOMMANDS_ARDRONE3_MEDIARECORD_VIDEOV2_RECORD_STOP));
00626 }
00627 
00628 void Bebop::ThrowOnInternalError(const std::string &message)
00629 {
00630   if (!is_connected_ || !device_controller_ptr_)
00631   {
00632     throw std::runtime_error(message);
00633   }
00634 }
00635 
00636 void Bebop::ThrowOnCtrlError(const eARCONTROLLER_ERROR &error, const std::string &message)
00637 {
00638   if (error != ARCONTROLLER_OK)
00639   {
00640     throw std::runtime_error(message + std::string(ARCONTROLLER_Error_ToString(error)));
00641   }
00642 }
00643 
00644 }  // namespace bebop_driver


bebop_driver
Author(s): Mani Monajjemi
autogenerated on Mon Aug 21 2017 02:36:39