Video metadata & Publish velocities

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 <boost/thread/mutex.hpp>
 #include <boost/thread/condition_variable.hpp>
 
+#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<uint8_t>& buffer, uint32_t &width, uint32_t &height) const;
+  bool GetFrontCameraFrame(std::vector<uint8_t>& buffer, uint32_t &width, uint32_t &height, MetadataV2Base_t& meta_data) const;
   uint32_t GetFrontCameraFrameWidth() const;
   uint32_t GetFrontCameraFrameHeight() const;
 };

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 <ros/timer.h>
 #include <nodelet/nodelet.h>
 #include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Vector3.h>
 #include <std_msgs/Empty.h>
 #include <std_msgs/UInt8.h>
 #include <std_msgs/Float32.h>
@@ -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_;
 

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

bebop_driver/src/bebop.cpp

@@ -41,7 +41,6 @@ extern "C"
 
 #include <bebop_driver/bebop.h>
 #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<uint8_t> &buffer, uint32_t& width, uint32_t& height) const
+bool Bebop::GetFrontCameraFrame(std::vector<uint8_t> &buffer, uint32_t& width, uint32_t& height, MetadataV2Base_t& meta_data) const
 {
   boost::unique_lock<boost::mutex> lock(frame_avail_mutex_);
 
@@ -592,6 +591,8 @@ bool Bebop::GetFrontCameraFrame(std::vector<uint8_t> &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;

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 <boost/bind.hpp>
 #include <boost/make_shared.hpp>
 #include <boost/date_time/posix_time/posix_time.hpp>
+#include <boost/qvm/mat.hpp>
+#include <boost/qvm/vec.hpp>
+#include <boost/qvm/vec_access.hpp>
+#include <boost/qvm/vec_mat_operations.hpp>
 #include <cmath>
 #include <algorithm>
 #include <string>
 #include <cstdio>
 
 #include <bebop_driver/bebop_driver_nodelet.h>
+#include "bebop_driver/bebop_video_decoder.h"
 #include <bebop_driver/BebopArdrone3Config.h>
 
 // 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<nav_msgs::Odometry>("odom", 30);
+  velocities_pub_ = nh.advertise<geometry_msgs::Vector3>("velocities", 10, true);
   camera_joint_pub_ = nh.advertise<sensor_msgs::JointState>("joint_states", 10, true);
   gps_fix_pub_ = nh.advertise<sensor_msgs::NavSatFix>("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<double, 3> 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<double, 3, 3> 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)
     {

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 <boost/lexical_cast.hpp>
 
+#include <nodelet/nodelet.h>
+
 extern "C"
 {
   #include "libARSAL/ARSAL_Print.h"
@@ -133,7 +135,7 @@ bool VideoDecoder::ReallocateBuffers()
                      boost::lexical_cast<std::string>(codec_ctx_ptr_->width) +
                      " x " + boost::lexical_cast<std::string>(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<std::string>(num_bytes));
     ThrowOnCondition(0 == avpicture_fill(
-                       reinterpret_cast<AVPicture*>(frame_rgb_ptr_), frame_rgb_raw_ptr_, PIX_FMT_RGB24,
+                       reinterpret_cast<AVPicture*>(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<MetadataV2Base_t*>(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();