diff --git a/bebop_driver/include/bebop_driver/bebop.h b/bebop_driver/include/bebop_driver/bebop.h index 2aa8fb9..b857dc7 100644 --- a/bebop_driver/include/bebop_driver/bebop.h +++ b/bebop_driver/include/bebop_driver/bebop.h @@ -40,6 +40,8 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include #include +#include "bebop_driver/bebop_video_decoder.h" + extern "C" { #include "libARSAL/ARSAL.h" @@ -180,7 +182,7 @@ class Bebop // This function is blocking and runs in the caller's thread's context // which is different from FrameReceivedCallback's context - bool GetFrontCameraFrame(std::vector& buffer, uint32_t &width, uint32_t &height) const; + bool GetFrontCameraFrame(std::vector& buffer, uint32_t &width, uint32_t &height, MetadataV2Base_t& meta_data) const; uint32_t GetFrontCameraFrameWidth() const; uint32_t GetFrontCameraFrameHeight() const; }; diff --git a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h b/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h index 03cbebc..3e227d7 100644 --- a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h +++ b/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h @@ -32,6 +32,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include #include #include +#include #include #include #include @@ -126,6 +127,7 @@ class BebopDriverNodelet : public nodelet::Nodelet ros::Subscriber toggle_recording_sub_; ros::Publisher odom_pub_; + ros::Publisher velocities_pub_; ros::Publisher camera_joint_pub_; ros::Publisher gps_fix_pub_; diff --git a/bebop_driver/include/bebop_driver/bebop_video_decoder.h b/bebop_driver/include/bebop_driver/bebop_video_decoder.h index ec86c78..989c4bc 100644 --- a/bebop_driver/include/bebop_driver/bebop_video_decoder.h +++ b/bebop_driver/include/bebop_driver/bebop_video_decoder.h @@ -57,6 +57,37 @@ extern "C" namespace bebop_driver { +struct MetadataV2Base_t +{ + uint16_t id; /**< Identifier = 0x5032 */ + uint16_t length; /**< Structure size in 32 bits words excluding the id and length + fields and including extensions */ + int32_t groundDistance; /**< Best ground distance estimation (m), Q16.16 */ + int32_t latitude; /**< Absolute latitude (deg), Q10.22 */ + int32_t longitude; /**< Absolute longitude (deg), Q10.22 */ + int32_t altitudeAndSv; /**< Absolute longitude (deg), Q10.22 */ + int16_t northSpeed; /**< North speed (m/s), Q8.8 */ + int16_t eastSpeed; /**< East speed (m/s), Q8.8 */ + int16_t downSpeed; /**< Down speed (m/s), Q8.8 */ + int16_t airSpeed; /**< Speed relative to air (m/s), negative means no data, Q8.8 */ + int16_t droneW; /**< Drone quaternion W, Q2.14 */ + int16_t droneX; /**< Drone quaternion X, Q2.14 */ + int16_t droneY; /**< Drone quaternion Y, Q2.14 */ + int16_t droneZ; /**< Drone quaternion Z, Q2.14 */ + int16_t frameW; /**< Frame view quaternion W, Q2.14 */ + int16_t frameX; /**< Frame view quaternion X, Q2.14 */ + int16_t frameY; /**< Frame view quaternion Y, Q2.14 */ + int16_t frameZ; /**< Frame view quaternion Z, Q2.14 */ + int16_t cameraPan; /**< Camera pan (rad), Q4.12 */ + int16_t cameraTilt; /**< Camera tilt (rad), Q4.12 */ + uint16_t exposureTime; /**< Frame exposure time (ms), Q8.8 */ + uint16_t gain; /**< Frame ISO gain */ + uint8_t state; /**< Bit 7 = binning, bits 6..0 = flyingState */ + uint8_t mode; /**< Bit 7 = animation, bits 6..0 = pilotingMode */ + int8_t wifiRssi; /**< Wifi RSSI (dBm) */ + uint8_t batteryPercentage; /**< Battery charge percentage */ +}; + class VideoDecoder { private: @@ -70,6 +101,7 @@ class VideoDecoder AVFrame* frame_ptr_; AVFrame* frame_rgb_ptr_; AVPacket packet_; + MetadataV2Base_t meta_data_; SwsContext* img_convert_ctx_ptr_; AVInputFormat* input_format_ptr_; uint8_t *frame_rgb_raw_ptr_; @@ -96,6 +128,7 @@ class VideoDecoder inline uint32_t GetFrameHeight() const {return codec_initialized_ ? codec_ctx_ptr_->height : 0;} inline const uint8_t* GetFrameRGBRawCstPtr() const {return frame_rgb_raw_ptr_;} + inline MetadataV2Base_t GetMetaData() const {return meta_data_;} }; } // namespace bebop_driver diff --git a/bebop_driver/src/bebop.cpp b/bebop_driver/src/bebop.cpp index d2664c1..7c1b209 100644 --- a/bebop_driver/src/bebop.cpp +++ b/bebop_driver/src/bebop.cpp @@ -41,7 +41,6 @@ extern "C" #include #include "bebop_driver/BebopArdrone3Config.h" -#include "bebop_driver/bebop_video_decoder.h" // include all callback wrappers #include "bebop_driver/autogenerated/ardrone3_state_callbacks.h" @@ -569,7 +568,7 @@ uint32_t Bebop::GetFrontCameraFrameHeight() const return video_decoder_ptr_->GetFrameHeight(); } -bool Bebop::GetFrontCameraFrame(std::vector &buffer, uint32_t& width, uint32_t& height) const +bool Bebop::GetFrontCameraFrame(std::vector &buffer, uint32_t& width, uint32_t& height, MetadataV2Base_t& meta_data) const { boost::unique_lock lock(frame_avail_mutex_); @@ -592,6 +591,8 @@ bool Bebop::GetFrontCameraFrame(std::vector &buffer, uint32_t& width, u video_decoder_ptr_->GetFrameRGBRawCstPtr() + num_bytes, buffer.begin()); + meta_data = video_decoder_ptr_->GetMetaData(); + width = video_decoder_ptr_->GetFrameWidth(); height = video_decoder_ptr_->GetFrameHeight(); is_frame_avail_ = false; diff --git a/bebop_driver/src/bebop_driver_nodelet.cpp b/bebop_driver/src/bebop_driver_nodelet.cpp index 1deca22..50e5078 100644 --- a/bebop_driver/src/bebop_driver_nodelet.cpp +++ b/bebop_driver/src/bebop_driver_nodelet.cpp @@ -35,12 +35,17 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include #include #include +#include +#include +#include +#include #include #include #include #include #include +#include "bebop_driver/bebop_video_decoder.h" #include // For AuxThread() - without the following, callback wrapper types are incomplete to the compiler @@ -148,6 +153,7 @@ void BebopDriverNodelet::onInit() toggle_recording_sub_ = nh.subscribe("record", 10, &BebopDriverNodelet::ToggleRecordingCallback, this); odom_pub_ = nh.advertise("odom", 30); + velocities_pub_ = nh.advertise("velocities", 10, true); camera_joint_pub_ = nh.advertise("joint_states", 10, true); gps_fix_pub_ = nh.advertise("fix", 10, true); @@ -438,11 +444,13 @@ void BebopDriverNodelet::CameraPublisherThread() try { sensor_msgs::ImagePtr image_msg_ptr_(new sensor_msgs::Image()); + bebop_driver::MetadataV2Base_t meta_data; + geometry_msgs::Vector3 velocity_msg_; const ros::Time t_now = ros::Time::now(); NODELET_DEBUG_STREAM("Grabbing a frame from Bebop"); // This is blocking - bebop_ptr_->GetFrontCameraFrame(image_msg_ptr_->data, frame_w, frame_h); + bebop_ptr_->GetFrontCameraFrame(image_msg_ptr_->data, frame_w, frame_h, meta_data); NODELET_DEBUG_STREAM("Frame grabbed: " << frame_w << " , " << frame_h); camera_info_msg_ptr_.reset(new sensor_msgs::CameraInfo(cinfo_manager_ptr_->getCameraInfo())); @@ -451,6 +459,40 @@ void BebopDriverNodelet::CameraPublisherThread() camera_info_msg_ptr_->width = frame_w; camera_info_msg_ptr_->height = frame_h; + // Converting Parrot Q8.8 to normal doubles + double north_speed = meta_data.northSpeed >> 8; + double east_speed = meta_data.eastSpeed >> 8; + double down_speed = meta_data.downSpeed >> 8; + + // Creating boost vector for the velocities + boost::qvm::vec velocity = {north_speed, east_speed, down_speed}; + + // Converting Parrot Q2.14 to normal doubles + double q_w = meta_data.droneW >> 14; + double q_x = meta_data.droneX >> 14; + double q_y = meta_data.droneY >> 14; + double q_z = meta_data.droneZ >> 14; + + // Compute elements of the rotation matrix from the quaternions + double c11 = q_w*q_w + q_x*q_x - q_y*q_y - q_z*q_z; + double c12 = 2*q_x*q_y - 2*q_w*q_z; + double c13 = 2*q_w*q_y + 2*q_x*q_z; + double c21 = 2*q_w*q_z + 2*q_x*q_y; + double c22 = q_w*q_w - q_x*q_x + q_y*q_y - q_z*q_z; + double c23 = 2*q_y*q_z - 2*q_w*q_x; + double c31 = 2*q_x*q_z - 2*q_w*q_y; + double c32 = 2*q_w*q_x + 2*q_y*q_z; + double c33 = q_w*q_w - q_x*q_x - q_y*q_y + q_z*q_z; + // Creating boost rotation matrix + boost::qvm::mat rotation = {c11, c12, c13, c21, c22, c23, c31, c32, c33}; + + // Transform velocities from north, east, down to x, y, z + velocity = rotation * velocity; + + velocity_msg_.x = boost::qvm::A<0>(velocity); + velocity_msg_.y = boost::qvm::A<1>(velocity); + velocity_msg_.z = boost::qvm::A<2>(velocity); + if (image_transport_pub_.getNumSubscribers() > 0) { image_msg_ptr_->encoding = "rgb8"; @@ -463,6 +505,11 @@ void BebopDriverNodelet::CameraPublisherThread() image_transport_pub_.publish(image_msg_ptr_, camera_info_msg_ptr_); } + + if (velocities_pub_.getNumSubscribers() > 0) + { + velocities_pub_.publish(velocity_msg_); + } } catch (const std::runtime_error& e) { diff --git a/bebop_driver/src/bebop_video_decoder.cpp b/bebop_driver/src/bebop_video_decoder.cpp index 63f0a82..c6111b9 100644 --- a/bebop_driver/src/bebop_video_decoder.cpp +++ b/bebop_driver/src/bebop_video_decoder.cpp @@ -30,6 +30,8 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include +#include + extern "C" { #include "libARSAL/ARSAL_Print.h" @@ -133,7 +135,7 @@ bool VideoDecoder::ReallocateBuffers() boost::lexical_cast(codec_ctx_ptr_->width) + " x " + boost::lexical_cast(codec_ctx_ptr_->width)); - const uint32_t num_bytes = avpicture_get_size(PIX_FMT_RGB24, codec_ctx_ptr_->width, codec_ctx_ptr_->width); + const uint32_t num_bytes = avpicture_get_size(AV_PIX_FMT_RGB24, codec_ctx_ptr_->width, codec_ctx_ptr_->width); frame_rgb_ptr_ = av_frame_alloc(); ThrowOnCondition(!frame_rgb_ptr_, "Can not allocate memory for frames!"); @@ -143,12 +145,12 @@ bool VideoDecoder::ReallocateBuffers() std::string("Can not allocate memory for the buffer: ") + boost::lexical_cast(num_bytes)); ThrowOnCondition(0 == avpicture_fill( - reinterpret_cast(frame_rgb_ptr_), frame_rgb_raw_ptr_, PIX_FMT_RGB24, + reinterpret_cast(frame_rgb_ptr_), frame_rgb_raw_ptr_, AV_PIX_FMT_RGB24, codec_ctx_ptr_->width, codec_ctx_ptr_->height), "Failed to initialize the picture data structure."); img_convert_ctx_ptr_ = sws_getContext(codec_ctx_ptr_->width, codec_ctx_ptr_->height, codec_ctx_ptr_->pix_fmt, - codec_ctx_ptr_->width, codec_ctx_ptr_->height, PIX_FMT_RGB24, + codec_ctx_ptr_->width, codec_ctx_ptr_->height, AV_PIX_FMT_RGB24, SWS_FAST_BILINEAR, NULL, NULL, NULL); } catch (const std::runtime_error& e) @@ -285,6 +287,11 @@ bool VideoDecoder::Decode(const ARCONTROLLER_Frame_t *bebop_frame_ptr_) packet_.data = bebop_frame_ptr_->data; packet_.size = bebop_frame_ptr_->used; + MetadataV2Base_t* meta_data_ptr_ = reinterpret_cast(bebop_frame_ptr_->metadata); + if(NULL != meta_data_ptr_) + { + meta_data_ = *meta_data_ptr_; + } const uint32_t width_prev = GetFrameWidth(); const uint32_t height_prev = GetFrameHeight();