fullspeed_navdata now affects publishing of /ardrone/navdata,imu,mag topics as well.

This update also includes a few speed improvements for the case where fullspeed_navdata==true.

scripts/NavdataMessageDefinitionsTemplate.c

@@ -16,7 +16,7 @@
 #endif
 
 #ifdef NAVDATA_STRUCTS_HEADER_PUBLIC
-	void PublishNavdataTypes(navdata_unpacked_t &n, ros::Time &received);
+	void PublishNavdataTypes(const navdata_unpacked_t &n, const ros::Time &received);
 #endif
 
 #ifdef NAVDATA_STRUCTS_HEADER_PRIVATE

src/NavdataMessageDefinitions.h

@@ -41,7 +41,7 @@
 #endif
 
 #ifdef NAVDATA_STRUCTS_HEADER_PUBLIC
-	void PublishNavdataTypes(navdata_unpacked_t &n, ros::Time &received);
+	void PublishNavdataTypes(const navdata_unpacked_t &n, const ros::Time &received);
 #endif
 
 #ifdef NAVDATA_STRUCTS_HEADER_PRIVATE
@@ -378,7 +378,7 @@
 #endif
 
 #ifdef NAVDATA_STRUCTS_SOURCE
-void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t &n, ros::Time &received)
+void ARDroneDriver::PublishNavdataTypes(const navdata_unpacked_t &n, const ros::Time &received)
 {
 	if(initialized_navdata_publishers)
 	{

src/ardrone_driver.cpp

@@ -135,7 +135,7 @@ void ARDroneDriver::run()
                          ardrone_control_config.ardrone_name,
                          (IS_ARDRONE1) ? 1 : 2,
                          ardrone_control_config.num_version_soft,
-                         shared_raw_navdata.navdata_demo.vbat_flying_percentage);
+                         shared_raw_navdata->navdata_demo.vbat_flying_percentage);
                 ROS_INFO("Navdata Publish Settings:");
                 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);
@@ -162,13 +162,26 @@ void ARDroneDriver::run()
                 }
             }
 
-            vp_os_mutex_lock(&navdata_lock);
-            copy_current_navdata_id = current_navdata_id;
-            vp_os_mutex_unlock(&navdata_lock);
-            if (copy_current_navdata_id != last_navdata_id)
+            if(!fullspeed_navdata)
             {
-                last_navdata_id = copy_current_navdata_id;
-                publish_navdata();                
+                vp_os_mutex_lock(&navdata_lock);
+                copy_current_navdata_id = current_navdata_id;
+                vp_os_mutex_unlock(&navdata_lock);
+                if (copy_current_navdata_id != last_navdata_id)
+                {
+                    vp_os_mutex_lock(&navdata_lock);
+                    last_navdata_id = copy_current_navdata_id;
+                    
+                    // 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.
+                    navdata_unpacked_t navdata_raw = *shared_raw_navdata;
+                    ros::Time navdata_receive_time = shared_navdata_receive_time;
+                    vp_os_mutex_unlock(&navdata_lock);
+
+                    PublishNavdataTypes(navdata_raw, navdata_receive_time); // This function is defined in the template NavdataMessageDefinitions.h template file
+                    publish_navdata(navdata_raw, navdata_receive_time);                
+                }
             }
             if (freq_dev == 0) publish_tf();
 
@@ -620,11 +633,6 @@ void ARDroneDriver::publish_navdata(navdata_unpacked_t &navdata_raw, const ros::
 
     }
 
-    if(!fullspeed_navdata) // only transmit this data in the loop if we're transmitting at loop speed, rather than full speed
-    {
-        PublishNavdataTypes(navdata_raw, navdata_receive_time); // This function is defined in the template NavdataMessageDefinitions.h template file
-    }
-
     if (!enabled_legacy_navdata || ((navdata_pub.getNumSubscribers() == 0) && (imu_pub.getNumSubscribers() == 0) && (mag_pub.getNumSubscribers() == 0)))
         return; // why bother, no one is listening.
 

src/ardrone_driver.h

@@ -2,6 +2,7 @@
 #define _ARDRONE_DRIVER_H_
 
 class ARDroneDriver;
+
 #include <ros/ros.h>
 #include <image_transport/image_transport.h>
 #include <tf/transform_broadcaster.h>
@@ -54,8 +55,9 @@ public:
     #undef NAVDATA_STRUCTS_HEADER_PUBLIC
 
     void publish_video();
+    void publish_navdata(navdata_unpacked_t &navdata_raw, const ros::Time &navdata_receive_time);
+
 private:
-	void publish_navdata();
     void publish_tf();
     bool readCovParams(std::string param_name, boost::array<double, 9> &cov_array);
     double calcAverage(std::vector<double> &vec);
@@ -111,10 +113,6 @@ private:
     bool inited;
     std::string droneFrameId;
 
-    // Navdata copy
-    navdata_unpacked_t navdata_raw;
-    ros::Time navdata_receive_time;
-
     // Load auto-generated declarations for full navdata
     #define NAVDATA_STRUCTS_HEADER_PRIVATE
     #include "NavdataMessageDefinitions.h"

src/ardrone_sdk.cpp

@@ -3,7 +3,7 @@
 #include "teleop_twist.h"
 
 
-navdata_unpacked_t shared_raw_navdata;
+navdata_unpacked_t *shared_raw_navdata;
 ros::Time shared_navdata_receive_time;
 
 vp_os_mutex_t navdata_lock;
@@ -227,14 +227,15 @@ extern "C" {
 
     C_RESULT navdata_custom_process(const navdata_unpacked_t * const pnd) {
         vp_os_mutex_lock(&navdata_lock);
-        // TODO: This is expensive, too (1908 Bytes)!
-        shared_raw_navdata = *pnd;
         shared_navdata_receive_time = ros::Time::now();
+        shared_raw_navdata = (navdata_unpacked_t*)pnd;
 
         if(fullspeed_navdata)
         {
-            rosDriver->PublishNavdataTypes(shared_raw_navdata,shared_navdata_receive_time); //if we're publishing navdata at full speed, publish!
+            rosDriver->PublishNavdataTypes(*shared_raw_navdata, shared_navdata_receive_time); //if we're publishing navdata at full speed, publish!
+            rosDriver->publish_navdata(*shared_raw_navdata, shared_navdata_receive_time);
         }
+
         current_navdata_id++;
         vp_os_mutex_unlock(&navdata_lock);
 		return C_OK;

src/ardrone_sdk.h

@@ -46,8 +46,10 @@ extern video_decoder_config_t vec;
 #include "ardrone_driver.h"
 
 #define NB_DRIVER_POST_STAGES   10
-extern navdata_unpacked_t shared_raw_navdata;
+
 extern ARDroneDriver *rosDriver;
+
+extern navdata_unpacked_t *shared_raw_navdata;
 extern ros::Time shared_navdata_receive_time;
 
 extern vp_os_mutex_t navdata_lock;