Added custom messages and handling code

We now have the ability to access any of the navdata structs which are
sent back by the drone. Still to do is to enable/disable the drone
sending based on whether we're interested in receiving the message.

src/ardrone_driver.cpp

@@ -562,6 +562,7 @@ void ARDroneDriver::publish_navdata()
 {
     // Thread safe copy of interesting Navdata data
     vp_os_mutex_lock(&navdata_lock);
+    navdata_raw = shared_raw_navdata;
     navdata_detect = shared_navdata_detect;
     navdata_phys = shared_navdata_phys;
     navdata = shared_navdata;
@@ -614,6 +615,9 @@ void ARDroneDriver::publish_navdata()
         navdata.vz -= vel_bias[2];
 
     }
+
+    PublishNavdataTypes(navdata_raw); // This is defined in the template NavdataMessageDefinitions.h template file
+
     if ((navdata_pub.getNumSubscribers() == 0) && (imu_pub.getNumSubscribers() == 0) && (mag_pub.getNumSubscribers() == 0))
         return; // why bother, no one is listening.
     const ros::Time _now = ros::Time::now();
@@ -729,6 +733,10 @@ void ARDroneDriver::publish_navdata()
     imu_pub.publish(imu_msg);
 }
 
+#define NAVDATA_STRUCTS_SOURCE
+#include "NavdataMessageDefinitions.h"
+#undef NAVDATA_STRUCTS_SOURCE
+
 void ARDroneDriver::publish_tf()
 {
     tf_base_front.stamp_ = ros::Time::now();

src/ardrone_driver.h

@@ -16,12 +16,18 @@
 #include <ardrone_tool/ardrone_version.h>
 #include <ardrone_tool/ardrone_tool.h>
 
+#define NAVDATA_STRUCTS_INCLUDES
+#include "NavdataMessageDefinitions.h"
+#undef NAVDATA_STRUCTS_INCLUDES
+
 
 #define _DEG2RAD 0.01745331111
 #define _RAD2DEG 57.2957184819
 
 #define DRIVER_USERNAME "ardrone_driver"
 #define DRIVER_APPNAME "ardrone_driver"
+#define CAMERA_QUEUE_SIZE (10)
+#define NAVDATA_QUEUE_SIZE (25)
 
 enum ROOT_FRAME
 {
@@ -104,6 +110,11 @@ private:
     navdata_magneto_t navdata_magneto;
     navdata_wind_speed_t navdata_wind;
     navdata_time_t arnavtime;
+    navdata_unpacked_t navdata_raw;
+
+    #define NAVDATA_STRUCTS_HEADER
+    #include "NavdataMessageDefinitions.h"
+    #undef NAVDATA_STRUCTS_HEADER
 
     /*
      * TF Frames