Full-Speed Navdata & Loop-Rate

Two new parameters have been added:
* `fullspeed_navdata`, which controls whether new-style navdata is published when received, or at `looprate`
* `looprate`, which controls the speed of the internal ros-loop. Note that legacy navdata is always published at `looprate`

Also made various speed improvements to the navdata processing pipeline to enable this faster processing.

Also fixed a problem with `enable_legacy_navdata`, whereby it wasn't read correctly and thus had no effect. Also mentioning it here because I forgot to mention it when I added it ages ago.

Also updated and commented the launch file to reflect these changes.

Regarding testing, I've flown with this for three batteries, with `fullspeed_navdata=true`, `looprate=50`, while rosbagging everything and experienced no unusual behaviour.

Conflicts:

	src/ardrone_driver.h

launch/ardrone_aggressive.launch

@@ -1,6 +1,8 @@
 <!-- This is a sample lanuch file, please change it based on your needs -->
 <launch>
 	<node name="ardrone_driver" pkg="ardrone_autonomy" type="ardrone_driver" output="screen" clear_params="true">
+	    
+	<!-- Modifies the drone's onboard parameters. If not specified, drone default will be used (consult SDK or ardrone_autonomy wiki) -->
 	    <param name="outdoor" value="1" />
 	    <param name="flight_without_shell" value="1" />
 	    <param name="navdata_demo" value="0" />
@@ -24,6 +26,10 @@
 	    <param name="do_imu_caliberation" value="false" />
 	    <param name="tf_prefix" value="mydrone" />
 
+	<!-- Enables the standard /ardrone/navdata, imu and mag topics. If not specified, defaults to TRUE -->
+	    <param name="enable_legacy_navdata" value="true" />
+	    
+	<!-- Enables the new-style, full information navdata packets. If not specified, defaults to FALSE -->
 	    <param name="enable_navdata_demo" value="true" />
 		<param name="enable_navdata_time" value="true" />
 		<param name="enable_navdata_raw_measures" value="true" />
@@ -53,7 +59,14 @@
 		<param name="enable_navdata_wifi" value="true" />
 		<param name="enable_navdata_zimmu_3000" value="true" />
 
-        <!-- Covariance Values (3x3 matrices reshaped to 1x9)-->
+	<!-- Tunes the speed at which the ros loop runs, and thus, the rate at which navdata is published -->
+		<param name="looprate" value="50" />
+
+	<!-- 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" />
+
+	<!-- 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>
         <rosparam param="cov/imu_av">[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]</rosparam>
         <rosparam param="cov/imu_or">[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 100000.0]</rosparam>

scripts/NavdataMessageDefinitionsTemplate.c

@@ -15,7 +15,11 @@
 % endfor	
 #endif
 
-#ifdef NAVDATA_STRUCTS_HEADER
+#ifdef NAVDATA_STRUCTS_HEADER_PUBLIC
+	void PublishNavdataTypes(navdata_unpacked_t &n);
+#endif
+
+#ifdef NAVDATA_STRUCTS_HEADER_PRIVATE
 % for item in structs:
 	ros::Publisher pub_${item['struct_name']};
 	bool enabled_${item['struct_name']};
@@ -25,11 +29,10 @@
 	bool enabled_legacy_navdata;
 
 	bool initialized_navdata_publishers;
-	void PublishNavdataTypes(navdata_unpacked_t n);
 #endif
 
 #ifdef NAVDATA_STRUCTS_SOURCE
-void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
+void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t &n)
 {
 	const ros::Time now = ros::Time::now();
 
@@ -37,9 +40,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 	{
 		initialized_navdata_publishers = true;
 
-		enabled_legacy_navdata = true;
+		ros::param::param("~enable_legacy_navdata", enabled_legacy_navdata, true);
-		ros::param::get("~enable_${item['struct_name']}", enabled_${item['struct_name']});
-
 		if(enabled_legacy_navdata)
 		{
 			navdata_pub = node_handle.advertise<ardrone_autonomy::Navdata>("ardrone/navdata", 25);
@@ -50,9 +51,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		//-------------------------
 
 % for item in structs:
-		enabled_${item['struct_name']} = false;
+		ros::param::param("~enable_${item['struct_name']}", enabled_${item['struct_name']}, false);
-		ros::param::get("~enable_${item['struct_name']}", enabled_${item['struct_name']});
-
 		if(enabled_${item['struct_name']})
 		{
 			pub_${item['struct_name']} = node_handle.advertise<ardrone_autonomy::${item['struct_name']}>("ardrone/${item['struct_name']}", NAVDATA_QUEUE_SIZE);

src/NavdataMessageDefinitions.h

@@ -40,7 +40,11 @@
 #include <ardrone_autonomy/matrix33.h>
 #endif
 
-#ifdef NAVDATA_STRUCTS_HEADER
+#ifdef NAVDATA_STRUCTS_HEADER_PUBLIC
+	void PublishNavdataTypes(navdata_unpacked_t &n);
+#endif
+
+#ifdef NAVDATA_STRUCTS_HEADER_PRIVATE
 	ros::Publisher pub_navdata_demo;
 	bool enabled_navdata_demo;
 	ardrone_autonomy::navdata_demo navdata_demo_msg;
@@ -129,11 +133,10 @@
 	bool enabled_legacy_navdata;
 
 	bool initialized_navdata_publishers;
-	void PublishNavdataTypes(navdata_unpacked_t n);
 #endif
 
 #ifdef NAVDATA_STRUCTS_SOURCE
-void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
+void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t &n)
 {
 	const ros::Time now = ros::Time::now();
 
@@ -141,9 +144,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 	{
 		initialized_navdata_publishers = true;
 
-		enabled_legacy_navdata = true;
+		ros::param::param("~enable_legacy_navdata", enabled_legacy_navdata, true);
-		ros::param::get("~enable_navdata_zimmu_3000", enabled_navdata_zimmu_3000);
-
 		if(enabled_legacy_navdata)
 		{
 			navdata_pub = node_handle.advertise<ardrone_autonomy::Navdata>("ardrone/navdata", 25);
@@ -153,9 +154,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_demo = false;
+		ros::param::param("~enable_navdata_demo", enabled_navdata_demo, false);
-		ros::param::get("~enable_navdata_demo", enabled_navdata_demo);
-
 		if(enabled_navdata_demo)
 		{
 			pub_navdata_demo = node_handle.advertise<ardrone_autonomy::navdata_demo>("ardrone/navdata_demo", NAVDATA_QUEUE_SIZE);
@@ -163,9 +162,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_time = false;
+		ros::param::param("~enable_navdata_time", enabled_navdata_time, false);
-		ros::param::get("~enable_navdata_time", enabled_navdata_time);
-
 		if(enabled_navdata_time)
 		{
 			pub_navdata_time = node_handle.advertise<ardrone_autonomy::navdata_time>("ardrone/navdata_time", NAVDATA_QUEUE_SIZE);
@@ -173,9 +170,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_raw_measures = false;
+		ros::param::param("~enable_navdata_raw_measures", enabled_navdata_raw_measures, false);
-		ros::param::get("~enable_navdata_raw_measures", enabled_navdata_raw_measures);
-
 		if(enabled_navdata_raw_measures)
 		{
 			pub_navdata_raw_measures = node_handle.advertise<ardrone_autonomy::navdata_raw_measures>("ardrone/navdata_raw_measures", NAVDATA_QUEUE_SIZE);
@@ -183,9 +178,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_phys_measures = false;
+		ros::param::param("~enable_navdata_phys_measures", enabled_navdata_phys_measures, false);
-		ros::param::get("~enable_navdata_phys_measures", enabled_navdata_phys_measures);
-
 		if(enabled_navdata_phys_measures)
 		{
 			pub_navdata_phys_measures = node_handle.advertise<ardrone_autonomy::navdata_phys_measures>("ardrone/navdata_phys_measures", NAVDATA_QUEUE_SIZE);
@@ -193,9 +186,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_gyros_offsets = false;
+		ros::param::param("~enable_navdata_gyros_offsets", enabled_navdata_gyros_offsets, false);
-		ros::param::get("~enable_navdata_gyros_offsets", enabled_navdata_gyros_offsets);
-
 		if(enabled_navdata_gyros_offsets)
 		{
 			pub_navdata_gyros_offsets = node_handle.advertise<ardrone_autonomy::navdata_gyros_offsets>("ardrone/navdata_gyros_offsets", NAVDATA_QUEUE_SIZE);
@@ -203,9 +194,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_euler_angles = false;
+		ros::param::param("~enable_navdata_euler_angles", enabled_navdata_euler_angles, false);
-		ros::param::get("~enable_navdata_euler_angles", enabled_navdata_euler_angles);
-
 		if(enabled_navdata_euler_angles)
 		{
 			pub_navdata_euler_angles = node_handle.advertise<ardrone_autonomy::navdata_euler_angles>("ardrone/navdata_euler_angles", NAVDATA_QUEUE_SIZE);
@@ -213,9 +202,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_references = false;
+		ros::param::param("~enable_navdata_references", enabled_navdata_references, false);
-		ros::param::get("~enable_navdata_references", enabled_navdata_references);
-
 		if(enabled_navdata_references)
 		{
 			pub_navdata_references = node_handle.advertise<ardrone_autonomy::navdata_references>("ardrone/navdata_references", NAVDATA_QUEUE_SIZE);
@@ -223,9 +210,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_trims = false;
+		ros::param::param("~enable_navdata_trims", enabled_navdata_trims, false);
-		ros::param::get("~enable_navdata_trims", enabled_navdata_trims);
-
 		if(enabled_navdata_trims)
 		{
 			pub_navdata_trims = node_handle.advertise<ardrone_autonomy::navdata_trims>("ardrone/navdata_trims", NAVDATA_QUEUE_SIZE);
@@ -233,9 +218,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_rc_references = false;
+		ros::param::param("~enable_navdata_rc_references", enabled_navdata_rc_references, false);
-		ros::param::get("~enable_navdata_rc_references", enabled_navdata_rc_references);
-
 		if(enabled_navdata_rc_references)
 		{
 			pub_navdata_rc_references = node_handle.advertise<ardrone_autonomy::navdata_rc_references>("ardrone/navdata_rc_references", NAVDATA_QUEUE_SIZE);
@@ -243,9 +226,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_pwm = false;
+		ros::param::param("~enable_navdata_pwm", enabled_navdata_pwm, false);
-		ros::param::get("~enable_navdata_pwm", enabled_navdata_pwm);
-
 		if(enabled_navdata_pwm)
 		{
 			pub_navdata_pwm = node_handle.advertise<ardrone_autonomy::navdata_pwm>("ardrone/navdata_pwm", NAVDATA_QUEUE_SIZE);
@@ -253,9 +234,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_altitude = false;
+		ros::param::param("~enable_navdata_altitude", enabled_navdata_altitude, false);
-		ros::param::get("~enable_navdata_altitude", enabled_navdata_altitude);
-
 		if(enabled_navdata_altitude)
 		{
 			pub_navdata_altitude = node_handle.advertise<ardrone_autonomy::navdata_altitude>("ardrone/navdata_altitude", NAVDATA_QUEUE_SIZE);
@@ -263,9 +242,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_vision_raw = false;
+		ros::param::param("~enable_navdata_vision_raw", enabled_navdata_vision_raw, false);
-		ros::param::get("~enable_navdata_vision_raw", enabled_navdata_vision_raw);
-
 		if(enabled_navdata_vision_raw)
 		{
 			pub_navdata_vision_raw = node_handle.advertise<ardrone_autonomy::navdata_vision_raw>("ardrone/navdata_vision_raw", NAVDATA_QUEUE_SIZE);
@@ -273,9 +250,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_vision_of = false;
+		ros::param::param("~enable_navdata_vision_of", enabled_navdata_vision_of, false);
-		ros::param::get("~enable_navdata_vision_of", enabled_navdata_vision_of);
-
 		if(enabled_navdata_vision_of)
 		{
 			pub_navdata_vision_of = node_handle.advertise<ardrone_autonomy::navdata_vision_of>("ardrone/navdata_vision_of", NAVDATA_QUEUE_SIZE);
@@ -283,9 +258,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_vision = false;
+		ros::param::param("~enable_navdata_vision", enabled_navdata_vision, false);
-		ros::param::get("~enable_navdata_vision", enabled_navdata_vision);
-
 		if(enabled_navdata_vision)
 		{
 			pub_navdata_vision = node_handle.advertise<ardrone_autonomy::navdata_vision>("ardrone/navdata_vision", NAVDATA_QUEUE_SIZE);
@@ -293,9 +266,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_vision_perf = false;
+		ros::param::param("~enable_navdata_vision_perf", enabled_navdata_vision_perf, false);
-		ros::param::get("~enable_navdata_vision_perf", enabled_navdata_vision_perf);
-
 		if(enabled_navdata_vision_perf)
 		{
 			pub_navdata_vision_perf = node_handle.advertise<ardrone_autonomy::navdata_vision_perf>("ardrone/navdata_vision_perf", NAVDATA_QUEUE_SIZE);
@@ -303,9 +274,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_trackers_send = false;
+		ros::param::param("~enable_navdata_trackers_send", enabled_navdata_trackers_send, false);
-		ros::param::get("~enable_navdata_trackers_send", enabled_navdata_trackers_send);
-
 		if(enabled_navdata_trackers_send)
 		{
 			pub_navdata_trackers_send = node_handle.advertise<ardrone_autonomy::navdata_trackers_send>("ardrone/navdata_trackers_send", NAVDATA_QUEUE_SIZE);
@@ -313,9 +282,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_vision_detect = false;
+		ros::param::param("~enable_navdata_vision_detect", enabled_navdata_vision_detect, false);
-		ros::param::get("~enable_navdata_vision_detect", enabled_navdata_vision_detect);
-
 		if(enabled_navdata_vision_detect)
 		{
 			pub_navdata_vision_detect = node_handle.advertise<ardrone_autonomy::navdata_vision_detect>("ardrone/navdata_vision_detect", NAVDATA_QUEUE_SIZE);
@@ -323,9 +290,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_watchdog = false;
+		ros::param::param("~enable_navdata_watchdog", enabled_navdata_watchdog, false);
-		ros::param::get("~enable_navdata_watchdog", enabled_navdata_watchdog);
-
 		if(enabled_navdata_watchdog)
 		{
 			pub_navdata_watchdog = node_handle.advertise<ardrone_autonomy::navdata_watchdog>("ardrone/navdata_watchdog", NAVDATA_QUEUE_SIZE);
@@ -333,9 +298,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_adc_data_frame = false;
+		ros::param::param("~enable_navdata_adc_data_frame", enabled_navdata_adc_data_frame, false);
-		ros::param::get("~enable_navdata_adc_data_frame", enabled_navdata_adc_data_frame);
-
 		if(enabled_navdata_adc_data_frame)
 		{
 			pub_navdata_adc_data_frame = node_handle.advertise<ardrone_autonomy::navdata_adc_data_frame>("ardrone/navdata_adc_data_frame", NAVDATA_QUEUE_SIZE);
@@ -343,9 +306,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_video_stream = false;
+		ros::param::param("~enable_navdata_video_stream", enabled_navdata_video_stream, false);
-		ros::param::get("~enable_navdata_video_stream", enabled_navdata_video_stream);
-
 		if(enabled_navdata_video_stream)
 		{
 			pub_navdata_video_stream = node_handle.advertise<ardrone_autonomy::navdata_video_stream>("ardrone/navdata_video_stream", NAVDATA_QUEUE_SIZE);
@@ -353,9 +314,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_games = false;
+		ros::param::param("~enable_navdata_games", enabled_navdata_games, false);
-		ros::param::get("~enable_navdata_games", enabled_navdata_games);
-
 		if(enabled_navdata_games)
 		{
 			pub_navdata_games = node_handle.advertise<ardrone_autonomy::navdata_games>("ardrone/navdata_games", NAVDATA_QUEUE_SIZE);
@@ -363,9 +322,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_pressure_raw = false;
+		ros::param::param("~enable_navdata_pressure_raw", enabled_navdata_pressure_raw, false);
-		ros::param::get("~enable_navdata_pressure_raw", enabled_navdata_pressure_raw);
-
 		if(enabled_navdata_pressure_raw)
 		{
 			pub_navdata_pressure_raw = node_handle.advertise<ardrone_autonomy::navdata_pressure_raw>("ardrone/navdata_pressure_raw", NAVDATA_QUEUE_SIZE);
@@ -373,9 +330,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_magneto = false;
+		ros::param::param("~enable_navdata_magneto", enabled_navdata_magneto, false);
-		ros::param::get("~enable_navdata_magneto", enabled_navdata_magneto);
-
 		if(enabled_navdata_magneto)
 		{
 			pub_navdata_magneto = node_handle.advertise<ardrone_autonomy::navdata_magneto>("ardrone/navdata_magneto", NAVDATA_QUEUE_SIZE);
@@ -383,9 +338,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_wind_speed = false;
+		ros::param::param("~enable_navdata_wind_speed", enabled_navdata_wind_speed, false);
-		ros::param::get("~enable_navdata_wind_speed", enabled_navdata_wind_speed);
-
 		if(enabled_navdata_wind_speed)
 		{
 			pub_navdata_wind_speed = node_handle.advertise<ardrone_autonomy::navdata_wind_speed>("ardrone/navdata_wind_speed", NAVDATA_QUEUE_SIZE);
@@ -393,9 +346,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_kalman_pressure = false;
+		ros::param::param("~enable_navdata_kalman_pressure", enabled_navdata_kalman_pressure, false);
-		ros::param::get("~enable_navdata_kalman_pressure", enabled_navdata_kalman_pressure);
-
 		if(enabled_navdata_kalman_pressure)
 		{
 			pub_navdata_kalman_pressure = node_handle.advertise<ardrone_autonomy::navdata_kalman_pressure>("ardrone/navdata_kalman_pressure", NAVDATA_QUEUE_SIZE);
@@ -403,9 +354,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_hdvideo_stream = false;
+		ros::param::param("~enable_navdata_hdvideo_stream", enabled_navdata_hdvideo_stream, false);
-		ros::param::get("~enable_navdata_hdvideo_stream", enabled_navdata_hdvideo_stream);
-
 		if(enabled_navdata_hdvideo_stream)
 		{
 			pub_navdata_hdvideo_stream = node_handle.advertise<ardrone_autonomy::navdata_hdvideo_stream>("ardrone/navdata_hdvideo_stream", NAVDATA_QUEUE_SIZE);
@@ -413,9 +362,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_wifi = false;
+		ros::param::param("~enable_navdata_wifi", enabled_navdata_wifi, false);
-		ros::param::get("~enable_navdata_wifi", enabled_navdata_wifi);
-
 		if(enabled_navdata_wifi)
 		{
 			pub_navdata_wifi = node_handle.advertise<ardrone_autonomy::navdata_wifi>("ardrone/navdata_wifi", NAVDATA_QUEUE_SIZE);
@@ -423,9 +370,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 
 		//-------------------------
 
-		enabled_navdata_zimmu_3000 = false;
+		ros::param::param("~enable_navdata_zimmu_3000", enabled_navdata_zimmu_3000, false);
-		ros::param::get("~enable_navdata_zimmu_3000", enabled_navdata_zimmu_3000);
-
 		if(enabled_navdata_zimmu_3000)
 		{
 			pub_navdata_zimmu_3000 = node_handle.advertise<ardrone_autonomy::navdata_zimmu_3000>("ardrone/navdata_zimmu_3000", NAVDATA_QUEUE_SIZE);

src/ardrone_driver.cpp

@@ -114,7 +114,7 @@ ARDroneDriver::~ARDroneDriver()
 
 void ARDroneDriver::run()
 {
-    ros::Rate loop_rate(50);
+    ros::Rate loop_rate(looprate);
     ros::Time startTime = ros::Time::now();
     static int freq_dev = 0;
 	while (node_handle.ok())
@@ -133,7 +133,7 @@ void ARDroneDriver::run()
                          ardrone_control_config.ardrone_name,
                          (IS_ARDRONE1) ? 1 : 2,
                          ardrone_control_config.num_version_soft,
-                         shared_navdata.vbat_flying_percentage);
+                         shared_raw_navdata.navdata_demo.vbat_flying_percentage);
                 vp_os_mutex_unlock(&navdata_lock);
                 if (ardrone_control_config.num_version_soft[0] == '0')
                 {
@@ -559,30 +559,20 @@ 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;
-    arnavtime = shared_arnavtime;
-    if (IS_ARDRONE2)
-    { // This is neccessary
-        navdata_pressure = shared_navdata_pressure;
-        navdata_magneto = shared_navdata_magneto;
-        navdata_wind = shared_navdata_wind;
-    }
     vp_os_mutex_unlock(&navdata_lock);
 
 
     if ((do_caliberation) && (!caliberated))
     {
-        acc_samples[0].push_back(navdata_phys.phys_accs[ACC_X]);
+        acc_samples[0].push_back(navdata_raw.navdata_phys_measures.phys_accs[ACC_X]);
-        acc_samples[1].push_back(navdata_phys.phys_accs[ACC_Y]);
+        acc_samples[1].push_back(navdata_raw.navdata_phys_measures.phys_accs[ACC_Y]);
-        acc_samples[2].push_back(navdata_phys.phys_accs[ACC_Z]);
+        acc_samples[2].push_back(navdata_raw.navdata_phys_measures.phys_accs[ACC_Z]);
-        gyro_samples[0].push_back(navdata_phys.phys_gyros[GYRO_X]);
+        gyro_samples[0].push_back(navdata_raw.navdata_phys_measures.phys_gyros[GYRO_X]);
-        gyro_samples[1].push_back(navdata_phys.phys_gyros[GYRO_Y]);
+        gyro_samples[1].push_back(navdata_raw.navdata_phys_measures.phys_gyros[GYRO_Y]);
-        gyro_samples[2].push_back(navdata_phys.phys_gyros[GYRO_Z]);
+        gyro_samples[2].push_back(navdata_raw.navdata_phys_measures.phys_gyros[GYRO_Z]);
-        vel_samples[0].push_back(navdata.vx);
+        vel_samples[0].push_back(navdata_raw.navdata_demo.vx);
-        vel_samples[1].push_back(navdata.vy);
+        vel_samples[1].push_back(navdata_raw.navdata_demo.vy);
-        vel_samples[2].push_back(navdata.vz);
+        vel_samples[2].push_back(navdata_raw.navdata_demo.vz);
         if (acc_samples[0].size() == max_num_samples)
         {
             for (int j = 0; j < 3; j++)
@@ -594,7 +584,7 @@ void ARDroneDriver::publish_navdata()
             ROS_INFO("Bias in linear acceleration (mg): [%4.4lf, %4.4lf, %4.4lf]", acc_bias[0], acc_bias[1], acc_bias[2]);
             ROS_INFO("Bias in angular velocity (deg/s): [%4.4lf, %4.4lf, %4.4lf]", gyro_bias[0], gyro_bias[1], gyro_bias[2]);
             ROS_INFO("Bias in linear velocity (mm/s): [%4.4lf, %4.4lf, %4.4lf]", vel_bias[0], vel_bias[1], vel_bias[2]);
-            ROS_INFO("Above values (except z-axis accel) will be substracted from actual IMU data in `navdata` and `imu` topic.");
+            ROS_INFO("Above values (except z-axis accel) will be substracted from actual IMU data in `navdata_raw.navdata_demo` and `imu` topic.");
             ROS_INFO("This feature can be disabled using `do_imu_caliberation` parameter. Recaliberate using `imu_recalib` service.");
             caliberated = true;
         }
@@ -603,18 +593,21 @@ void ARDroneDriver::publish_navdata()
     {
         for (int j = 0; j < 3; j++)
         {
-            if (j != 2) navdata_phys.phys_accs[j] -= acc_bias[j];
+            if (j != 2) navdata_raw.navdata_phys_measures.phys_accs[j] -= acc_bias[j];
-            navdata_phys.phys_gyros[j] -= gyro_bias[j];
+            navdata_raw.navdata_phys_measures.phys_gyros[j] -= gyro_bias[j];
         }
-        navdata.vx -= vel_bias[0];
+        navdata_raw.navdata_demo.vx -= vel_bias[0];
-        navdata.vy -= vel_bias[1];
+        navdata_raw.navdata_demo.vy -= vel_bias[1];
-        navdata.vz -= vel_bias[2];
+        navdata_raw.navdata_demo.vz -= vel_bias[2];
 
     }
 
-    PublishNavdataTypes(navdata_raw); // This is defined in the template NavdataMessageDefinitions.h template file
+    if(!fullspeed_navdata) // only transmit this data in the loop if we're transmitting at loop speed, rather than full speed
+    {
+        PublishNavdataTypes(navdata_raw); // 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))
+    if (!enabled_legacy_navdata || ((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();
 
@@ -622,37 +615,37 @@ void ARDroneDriver::publish_navdata()
 
     msg.header.stamp = _now;
     msg.header.frame_id = droneFrameBase;
-    msg.batteryPercent = navdata.vbat_flying_percentage;
+    msg.batteryPercent = navdata_raw.navdata_demo.vbat_flying_percentage;
-    msg.state = (navdata.ctrl_state >> 16);
+    msg.state = (navdata_raw.navdata_demo.ctrl_state >> 16);
     
     // positive means counterclockwise rotation around axis
-    msg.rotX = navdata.phi / 1000.0; // tilt left/right
+    msg.rotX = navdata_raw.navdata_demo.phi / 1000.0; // tilt left/right
-    msg.rotY = -navdata.theta / 1000.0; // tilt forward/backward
+    msg.rotY = -navdata_raw.navdata_demo.theta / 1000.0; // tilt forward/backward
-    msg.rotZ = -navdata.psi / 1000.0; // orientation
+    msg.rotZ = -navdata_raw.navdata_demo.psi / 1000.0; // orientation
 
-    msg.altd = navdata.altitude; // cm
+    msg.altd = navdata_raw.navdata_demo.altitude; // cm
-    msg.vx = navdata.vx; // mm/sec
+    msg.vx = navdata_raw.navdata_demo.vx; // mm/sec
-    msg.vy = -navdata.vy; // mm/sec
+    msg.vy = -navdata_raw.navdata_demo.vy; // mm/sec
-    msg.vz = -navdata.vz; // mm/sec
+    msg.vz = -navdata_raw.navdata_demo.vz; // mm/sec
-    msg.tm = arnavtime.time;
+    msg.tm = navdata_raw.navdata_time.time;
-    msg.ax = navdata_phys.phys_accs[ACC_X] / 1000.0; // g
+    msg.ax = navdata_raw.navdata_phys_measures.phys_accs[ACC_X] / 1000.0; // g
-    msg.ay = -navdata_phys.phys_accs[ACC_Y] / 1000.0; // g
+    msg.ay = -navdata_raw.navdata_phys_measures.phys_accs[ACC_Y] / 1000.0; // g
-    msg.az = -navdata_phys.phys_accs[ACC_Z] / 1000.0; // g
+    msg.az = -navdata_raw.navdata_phys_measures.phys_accs[ACC_Z] / 1000.0; // g
 
     // New stuff
 
     if (IS_ARDRONE2)
     {
-        msg.magX = (int32_t)navdata_magneto.mx;
+        msg.magX = (int32_t)navdata_raw.navdata_magneto.mx;
-        msg.magY = (int32_t)navdata_magneto.my;
+        msg.magY = (int32_t)navdata_raw.navdata_magneto.my;
-        msg.magZ = (int32_t)navdata_magneto.mz;
+        msg.magZ = (int32_t)navdata_raw.navdata_magneto.mz;
 
-        msg.pressure = navdata_pressure.Pression_meas; // typo in the SDK!
+        msg.pressure = navdata_raw.navdata_pressure_raw.Pression_meas; // typo in the SDK!
-        msg.temp = navdata_pressure.Temperature_meas;
+        msg.temp = navdata_raw.navdata_pressure_raw.Temperature_meas;
 
-        msg.wind_speed = navdata_wind.wind_speed;
+        msg.wind_speed = navdata_raw.navdata_wind_speed.wind_speed;
-        msg.wind_angle = navdata_wind.wind_angle;
+        msg.wind_angle = navdata_raw.navdata_wind_speed.wind_angle;
-        msg.wind_comp_angle = navdata_wind.wind_compensation_phi;
+        msg.wind_comp_angle = navdata_raw.navdata_wind_speed.wind_compensation_phi;
     }
     else
     {
@@ -665,8 +658,8 @@ void ARDroneDriver::publish_navdata()
     }
 
     // Tag Detection
-    msg.tags_count = navdata_detect.nb_detected;
+    msg.tags_count = navdata_raw.navdata_vision_detect.nb_detected;
-    for (int i = 0; i < navdata_detect.nb_detected; i++)
+    for (int i = 0; i < navdata_raw.navdata_vision_detect.nb_detected; i++)
     {
         /*
          * The tags_type is in raw format. In order to extract the information
@@ -679,13 +672,13 @@ void ARDroneDriver::publish_navdata()
          * Please also note that the xc, yc, width and height are in [0,1000] range
          * and must get converted back based on image resolution.
          */
-        msg.tags_type.push_back(navdata_detect.type[i]);
+        msg.tags_type.push_back(navdata_raw.navdata_vision_detect.type[i]);
-        msg.tags_xc.push_back(navdata_detect.xc[i]);
+        msg.tags_xc.push_back(navdata_raw.navdata_vision_detect.xc[i]);
-        msg.tags_yc.push_back(navdata_detect.yc[i]);
+        msg.tags_yc.push_back(navdata_raw.navdata_vision_detect.yc[i]);
-        msg.tags_width.push_back(navdata_detect.width[i]);
+        msg.tags_width.push_back(navdata_raw.navdata_vision_detect.width[i]);
-        msg.tags_height.push_back(navdata_detect.height[i]);
+        msg.tags_height.push_back(navdata_raw.navdata_vision_detect.height[i]);
-        msg.tags_orientation.push_back(navdata_detect.orientation_angle[i]);
+        msg.tags_orientation.push_back(navdata_raw.navdata_vision_detect.orientation_angle[i]);
-        msg.tags_distance.push_back(navdata_detect.dist[i]);
+        msg.tags_distance.push_back(navdata_raw.navdata_vision_detect.dist[i]);
     }
 
     /* IMU */
@@ -704,9 +697,9 @@ void ARDroneDriver::publish_navdata()
 
     // IMU - Gyro (Gyro is being sent in deg/sec)
     // TODO: Should Gyro be added to Navdata?
-    imu_msg.angular_velocity.x = navdata_phys.phys_gyros[GYRO_X] * DEG_TO_RAD;
+    imu_msg.angular_velocity.x = navdata_raw.navdata_phys_measures.phys_gyros[GYRO_X] * DEG_TO_RAD;
-    imu_msg.angular_velocity.y = -navdata_phys.phys_gyros[GYRO_Y] * DEG_TO_RAD;
+    imu_msg.angular_velocity.y = -navdata_raw.navdata_phys_measures.phys_gyros[GYRO_Y] * DEG_TO_RAD;
-    imu_msg.angular_velocity.z = -navdata_phys.phys_gyros[GYRO_Z] * DEG_TO_RAD;
+    imu_msg.angular_velocity.z = -navdata_raw.navdata_phys_measures.phys_gyros[GYRO_Z] * DEG_TO_RAD;
 
     mag_msg.header.frame_id = droneFrameBase;
     mag_msg.header.stamp = _now;

src/ardrone_driver.h

@@ -48,6 +48,10 @@ public:
     double getRosParam(char* param, double defaultVal);
     bool imuReCalibCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response &response);
 
+    #define NAVDATA_STRUCTS_HEADER_PUBLIC
+    #include "NavdataMessageDefinitions.h"
+    #undef NAVDATA_STRUCTS_HEADER_PUBLIC
+
 private:
 	void publish_video();
 	void publish_navdata();
@@ -104,19 +108,12 @@ private:
     std::string droneFrameId;
 
     // Navdata copy
-    navdata_demo_t navdata;
-    navdata_phys_measures_t navdata_phys;
-    navdata_vision_detect_t navdata_detect;
-    navdata_pressure_raw_t navdata_pressure;
-    navdata_magneto_t navdata_magneto;
-    navdata_wind_speed_t navdata_wind;
-    navdata_time_t arnavtime;
     navdata_unpacked_t navdata_raw;
 
     // Load auto-generated declarations for full navdata
-    #define NAVDATA_STRUCTS_HEADER
+    #define NAVDATA_STRUCTS_HEADER_PRIVATE
     #include "NavdataMessageDefinitions.h"
-    #undef NAVDATA_STRUCTS_HEADER
+    #undef NAVDATA_STRUCTS_HEADER_PRIVATE
 
     /*
      * TF Frames

src/ardrone_sdk.cpp

@@ -3,13 +3,6 @@
 #include "teleop_twist.h"
 #include "ardrone_driver.h"
 
-navdata_demo_t shared_navdata;
-navdata_phys_measures_t shared_navdata_phys;
-navdata_vision_detect_t shared_navdata_detect;
-navdata_pressure_raw_t shared_navdata_pressure;
-navdata_magneto_t shared_navdata_magneto;
-navdata_wind_speed_t shared_navdata_wind;
-navdata_time_t shared_arnavtime;
 navdata_unpacked_t shared_raw_navdata;
 
 vp_os_mutex_t navdata_lock;
@@ -20,6 +13,9 @@ long int current_navdata_id = 0;
 
 ARDroneDriver* rosDriver;
 
+int32_t looprate;
+bool fullspeed_navdata;
+
 int32_t should_exit;
 
 extern "C" {
@@ -62,6 +58,8 @@ extern "C" {
             printf("Something must be really wrong with the SDK!");
         }
 
+        ros::param::param("~looprate",looprate,50);
+        ros::param::param("~fullspeed_navdata",fullspeed_navdata,false);
 
         // SET SOME NON-STANDARD DEFAULT VALUES FOR THE DRIVER
         // THESE CAN BE OVERWRITTEN BY ROS PARAMETERS (below)
@@ -227,18 +225,12 @@ extern "C" {
     C_RESULT navdata_custom_process(const navdata_unpacked_t * const pnd) {
         vp_os_mutex_lock(&navdata_lock);
         shared_raw_navdata = *pnd;
-        shared_navdata_detect = pnd->navdata_vision_detect;
+        if(fullspeed_navdata)
-        shared_navdata_phys = pnd->navdata_phys_measures;
+        {
-        shared_navdata = pnd->navdata_demo;
+            rosDriver->PublishNavdataTypes(shared_raw_navdata); //if we're publishing navdata at full speed, publish!
-        shared_arnavtime = pnd->navdata_time;
-        if (IS_ARDRONE2)
-        { // This is neccessary
-            shared_navdata_pressure = pnd->navdata_pressure_raw;
-            shared_navdata_magneto = pnd->navdata_magneto;
-            shared_navdata_wind = pnd->navdata_wind_speed;
         }
-        vp_os_mutex_unlock(&navdata_lock);
         current_navdata_id++;
+        vp_os_mutex_unlock(&navdata_lock);
 		return C_OK;
 	}
 

src/ardrone_sdk.h

@@ -45,19 +45,15 @@ extern video_decoder_config_t vec;
 
 
 #define NB_DRIVER_POST_STAGES   10
-extern navdata_vision_detect_t shared_navdata_detect;
-extern navdata_phys_measures_t shared_navdata_phys;
-extern navdata_demo_t shared_navdata;
-extern navdata_time_t shared_arnavtime;
-extern navdata_pressure_raw_t shared_navdata_pressure;
-extern navdata_magneto_t shared_navdata_magneto;
-extern navdata_wind_speed_t shared_navdata_wind;
 extern navdata_unpacked_t shared_raw_navdata;
 
 extern vp_os_mutex_t navdata_lock;
 extern vp_os_mutex_t video_lock;
 extern vp_os_mutex_t twist_lock;
 
+extern int32_t looprate;
+extern bool fullspeed_navdata;
+
 extern int32_t should_exit;
 
 #endif