Added fullspeed_video

Will now publish video frames as soon as received if fullspeed_video=true.  Unfortunately to do this we needed to do a few dirty hacks, implying that our architecture is no longer so suitable for the future direction of the driver. We need to talk about an architectural redesign perhaps...

launch/ardrone_aggressive.launch

@@ -65,6 +65,7 @@
 	<!-- Do we want to publish new-style navdata when received (true), or every time the ros-loop runs (false)? -->
 	<!-- (does not affect legacy navdata, which is always published at ros-loop rate) -->
 		<param name="fullspeed_navdata" value="true" />
+		<param name="fullspeed_video" value="true" />
 
 	<!-- Covariance Values (3x3 matrices reshaped to 1x9)-->
         <rosparam param="cov/imu_la">[0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1]</rosparam>

src/ardrone_driver.cpp

@@ -140,6 +140,7 @@ void ARDroneDriver::run()
                 ROS_INFO("    Legacy Mode: %s", enabled_legacy_navdata ? "On" : "Off"); //Bug: This is being inited after in the NavdataMessage*.h
                 ROS_INFO("    ROS Loop Rate: %d", looprate);
                 ROS_INFO("    Instant New Navdata Publish: %s", fullspeed_navdata ? "On" : "Off");
+                ROS_INFO("    Instant New Video Publish: %s", fullspeed_video ? "On" : "Off");
                 ROS_INFO("    Drone Send Speed: %s", ardrone_application_default_config.navdata_demo==0 ? "200Hz (navdata_demo=0)" : "15Hz (navdata_demo=1)");
                 // TODO: Enabled Navdata Demo
                 vp_os_mutex_unlock(&navdata_lock);
@@ -149,13 +150,16 @@ void ARDroneDriver::run()
                 }
             }
         } else {
-            vp_os_mutex_lock(&video_lock);
-            copy_current_frame_id = current_frame_id;
-            vp_os_mutex_unlock(&video_lock);
-            if (copy_current_frame_id != last_frame_id)
+            if(!fullspeed_video)
             {
-                last_frame_id = copy_current_frame_id;
-                publish_video();
+                vp_os_mutex_lock(&video_lock);
+                copy_current_frame_id = current_frame_id;
+                vp_os_mutex_unlock(&video_lock);
+                if (copy_current_frame_id != last_frame_id)
+                {
+                    last_frame_id = copy_current_frame_id;
+                    publish_video();
+                }
             }
 
             vp_os_mutex_lock(&navdata_lock);
@@ -366,9 +370,9 @@ void ARDroneDriver::publish_video()
         image_msg.step = D1_STREAM_WIDTH*3;
         image_msg.data.resize(D1_STREAM_WIDTH*D1_STREAM_HEIGHT*3);
 
-        vp_os_mutex_lock(&video_lock);
+        if(!fullspeed_video) vp_os_mutex_lock(&video_lock);
         std::copy(buffer, buffer+(D1_STREAM_WIDTH*D1_STREAM_HEIGHT*3), image_msg.data.begin());
-        vp_os_mutex_unlock(&video_lock);
+        if(!fullspeed_video) vp_os_mutex_unlock(&video_lock);
 
         if (cam_state == ZAP_CHANNEL_HORI)
         {
@@ -394,14 +398,14 @@ void ARDroneDriver::publish_video()
             image_msg.data.clear();
             image_msg.data.resize(D1_VERTSTREAM_WIDTH*D1_VERTSTREAM_HEIGHT*3);
             _it = image_msg.data.begin();
-            vp_os_mutex_lock(&video_lock);
+            if(!fullspeed_video) vp_os_mutex_lock(&video_lock);
             for (int row = 0; row < D1_VERTSTREAM_HEIGHT ; row++)
             {
                 int _b = row * D1_STREAM_WIDTH * 3;
                 int _e = _b + image_msg.step;
                 _it = std::copy(buffer + _b, buffer + _e, _it);
             }
-            vp_os_mutex_unlock(&video_lock);
+            if(!fullspeed_video) vp_os_mutex_unlock(&video_lock);
 
             cinfo_msg_vert.width = D1_VERTSTREAM_WIDTH;
             cinfo_msg_vert.height = D1_VERTSTREAM_HEIGHT;
@@ -424,14 +428,14 @@ void ARDroneDriver::publish_video()
             image_msg.data.clear();
             image_msg.data.resize((D1_STREAM_WIDTH - D1_MODE2_PIP_WIDTH)*D1_STREAM_HEIGHT*3);
             _it = image_msg.data.begin();
-            vp_os_mutex_lock(&video_lock);
+            if(!fullspeed_video) vp_os_mutex_lock(&video_lock);
             for (int row = 0; row < D1_STREAM_HEIGHT; row++)
             {
                 int _b = (row * D1_STREAM_WIDTH * 3) + (D1_MODE2_PIP_WIDTH * 3);
                 int _e = _b + image_msg.step;
                 _it = std::copy(buffer + _b, buffer + _e, _it);
             }
-            vp_os_mutex_unlock(&video_lock);
+            if(!fullspeed_video) vp_os_mutex_unlock(&video_lock);
 
             cinfo_msg_hori.width = D1_STREAM_WIDTH - D1_MODE2_PIP_WIDTH;
             cinfo_msg_hori.height = D1_STREAM_HEIGHT;
@@ -446,14 +450,14 @@ void ARDroneDriver::publish_video()
             image_msg.data.clear();
             image_msg.data.resize(D1_MODE2_PIP_WIDTH * D1_MODE2_PIP_HEIGHT * 3);
             _it = image_msg.data.begin();
-            vp_os_mutex_lock(&video_lock);
+            if(!fullspeed_video) vp_os_mutex_lock(&video_lock);
             for (int row = 0; row < D1_MODE2_PIP_HEIGHT; row++)
             {
                 int _b = row * D1_STREAM_WIDTH * 3;
                 int _e = _b + image_msg.step;
                 _it = std::copy(buffer + _b, buffer + _e, _it);
             }
-            vp_os_mutex_unlock(&video_lock);
+            if(!fullspeed_video) vp_os_mutex_unlock(&video_lock);
 
             cinfo_msg_vert.width = D1_MODE2_PIP_WIDTH;
             cinfo_msg_vert.height = D1_MODE2_PIP_HEIGHT;
@@ -479,9 +483,9 @@ void ARDroneDriver::publish_video()
             {
                 int _b = (row * (D1_STREAM_WIDTH * 3)) + (D1_MODE3_PIP_WIDTH * 3);
                 int _e = _b + image_msg.step;
-                vp_os_mutex_lock(&video_lock);
+                if(!fullspeed_video) vp_os_mutex_lock(&video_lock);
                 _it = std::copy(buffer + _b, buffer + _e, _it);
-                vp_os_mutex_unlock(&video_lock);
+                if(!fullspeed_video) vp_os_mutex_unlock(&video_lock);
             }
 
             cinfo_msg_vert.width = D1_VERTSTREAM_WIDTH - D1_MODE3_PIP_WIDTH;
@@ -497,13 +501,13 @@ void ARDroneDriver::publish_video()
             image_msg.data.clear();
             image_msg.data.resize(D1_MODE3_PIP_WIDTH * D1_MODE3_PIP_HEIGHT * 3);
             _it = image_msg.data.begin();
-            vp_os_mutex_lock(&video_lock);
+            if(!fullspeed_video) vp_os_mutex_lock(&video_lock);
             for (int row = 0; row < D1_MODE3_PIP_HEIGHT; row++)
             {
                 int _b = row * D1_STREAM_WIDTH * 3;
                 int _e = _b + image_msg.step;
                 _it = std::copy(buffer + _b, buffer + _e, _it);
-            }vp_os_mutex_unlock(&video_lock);
+            }if(!fullspeed_video) vp_os_mutex_unlock(&video_lock);
 
             cinfo_msg_hori.width = D1_MODE3_PIP_WIDTH;
             cinfo_msg_hori.height = D1_MODE3_PIP_HEIGHT;
@@ -544,9 +548,9 @@ void ARDroneDriver::publish_video()
         image_msg.is_bigendian = false;
         image_msg.step = D2_STREAM_WIDTH*3;
         image_msg.data.resize(D2_STREAM_WIDTH*D2_STREAM_HEIGHT*3);
-        vp_os_mutex_lock(&video_lock);
+        if(!fullspeed_video) vp_os_mutex_lock(&video_lock);
         std::copy(buffer, buffer+(D2_STREAM_WIDTH*D2_STREAM_HEIGHT*3), image_msg.data.begin());
-        vp_os_mutex_unlock(&video_lock);
+        if(!fullspeed_video) vp_os_mutex_unlock(&video_lock);
         // We only put the width and height in here.
 
 
@@ -574,17 +578,8 @@ void ARDroneDriver::publish_video()
 	
 }
 
-void ARDroneDriver::publish_navdata()
+void ARDroneDriver::publish_navdata(navdata_unpacked_t &navdata_raw, const ros::Time &navdata_receive_time)
 {
-    // Thread safe copy of interesting Navdata data
-    // TODO: This is a very expensive task, can we optimize here?
-    // maybe ignoring the copy when it is not needed.
-    vp_os_mutex_lock(&navdata_lock);
-    navdata_raw = shared_raw_navdata;
-    navdata_receive_time = shared_navdata_receive_time;
-    vp_os_mutex_unlock(&navdata_lock);
-
-
     if ((do_caliberation) && (!caliberated))
     {
         acc_samples[0].push_back(navdata_raw.navdata_phys_measures.phys_accs[ACC_X]);

src/ardrone_driver.h

@@ -1,6 +1,7 @@
 #ifndef _ARDRONE_DRIVER_H_
 #define _ARDRONE_DRIVER_H_
 
+class ARDroneDriver;
 #include <ros/ros.h>
 #include <image_transport/image_transport.h>
 #include <tf/transform_broadcaster.h>
@@ -52,8 +53,8 @@ public:
     #include "NavdataMessageDefinitions.h"
     #undef NAVDATA_STRUCTS_HEADER_PUBLIC
 
+    void publish_video();
 private:
-	void publish_video();
 	void publish_navdata();
     void publish_tf();
     bool readCovParams(std::string param_name, boost::array<double, 9> &cov_array);

src/ardrone_sdk.cpp

@@ -16,6 +16,7 @@ ARDroneDriver* rosDriver;
 
 int32_t looprate;
 bool fullspeed_navdata;
+bool fullspeed_video;
 
 int32_t should_exit;
 
@@ -61,6 +62,7 @@ extern "C" {
 
         ros::param::param("~looprate",looprate,50);
         ros::param::param("~fullspeed_navdata",fullspeed_navdata,false);
+        ros::param::param("~fullspeed_video",fullspeed_video,false);
 
         // SET SOME NON-STANDARD DEFAULT VALUES FOR THE DRIVER
         // THESE CAN BE OVERWRITTEN BY ROS PARAMETERS (below)

src/ardrone_sdk.h

@@ -47,6 +47,7 @@ extern video_decoder_config_t vec;
 
 #define NB_DRIVER_POST_STAGES   10
 extern navdata_unpacked_t shared_raw_navdata;
+extern ARDroneDriver *rosDriver;
 extern ros::Time shared_navdata_receive_time;
 
 extern vp_os_mutex_t navdata_lock;
@@ -55,6 +56,7 @@ extern vp_os_mutex_t twist_lock;
 
 extern int32_t looprate;
 extern bool fullspeed_navdata;
+extern bool fullspeed_video;
 
 extern int32_t should_exit;
 

src/video.cpp

@@ -18,6 +18,10 @@ extern "C" C_RESULT export_stage_transform( void *cfg, vp_api_io_data_t *in, vp_
     vp_os_mutex_lock(&video_lock);
 	memcpy(buffer, in->buffers[0], in->size);
     current_frame_id++;
+    if(fullspeed_video)
+    {
+    	rosDriver->publish_video();
+    }
     vp_os_mutex_unlock(&video_lock);
 //    vp_os_mutex_unlock(&video_update_lock);	
  	return (SUCCESS);

src/video.h

@@ -2,6 +2,7 @@
 #define _VIDEO_H_
 
 #include "ardrone_sdk.h"
+#include "ardrone_driver.h"
 
 // The maximum memory allocation
 #define MAX_STREAM_WIDTH 640