Merge pull request #41 from mikehamer/master

Fixed bug with navdata handling & improved initialization

scripts/NavdataMessageDefinitionsTemplate.c

@@ -31,11 +31,7 @@
 	bool initialized_navdata_publishers;
 #endif
 
-#ifdef NAVDATA_STRUCTS_SOURCE
-void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t &n)
-{
-	const ros::Time now = ros::Time::now();
-
+#ifdef NAVDATA_STRUCTS_INITIALIZE
 	if(!initialized_navdata_publishers)
 	{
 		initialized_navdata_publishers = true;
@@ -61,37 +57,46 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t &n)
 
 % endfor
 	}
+#endif
 
-% for item in structs:
-	if(enabled_${item['struct_name']} && pub_${item['struct_name']}.getNumSubscribers()>0)
+#ifdef NAVDATA_STRUCTS_SOURCE
+void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t &n)
+{
+	const ros::Time now = ros::Time::now();
+
+	if(initialized_navdata_publishers)
 	{
-		${item['struct_name']}_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
-		${item['struct_name']}_msg.header.stamp = now;
-		${item['struct_name']}_msg.header.frame_id = droneFrameBase;
+% for item in structs:
+		if(enabled_${item['struct_name']} && pub_${item['struct_name']}.getNumSubscribers()>0)
+		{
+			${item['struct_name']}_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
+			${item['struct_name']}_msg.header.stamp = now;
+			${item['struct_name']}_msg.header.frame_id = droneFrameBase;
 
 % for member in item['members']:
 % if member['array_size'] is None:
-		{\
-			${format_member(item, member, None)}
-			${item['struct_name']}_msg.${member['name']} = m;
-		}
+			{\
+				${format_member(item, member, None)}
+				${item['struct_name']}_msg.${member['name']} = m;
+			}
 
 % else:
-		for(int i=0; i<${member['array_size']}; i++)
-		{\
-			${format_member(item, member, 'i')}
-			${item['struct_name']}_msg.${member['name']}.push_back(m);
-		}
+			${item['struct_name']}_msg.${member['name']}.clear();
+			for(int i=0; i<${member['array_size']}; i++)
+			{\
+				${format_member(item, member, 'i')}
+				${item['struct_name']}_msg.${member['name']}.push_back(m);
+			}
 			
 % endif
 % endfor
-		pub_${item['struct_name']}.publish(${item['struct_name']}_msg);
-	}
+			pub_${item['struct_name']}.publish(${item['struct_name']}_msg);
+		}
 
-	//-------------------------
+		//-------------------------
 
 % endfor
-
+	}
 }
 #endif
 

src/ardrone_driver.cpp

@@ -137,9 +137,10 @@ void ARDroneDriver::run()
                          ardrone_control_config.num_version_soft,
                          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("    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("    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);
                 if (ardrone_control_config.num_version_soft[0] == '0')
@@ -225,6 +226,11 @@ void ARDroneDriver::configureDrone()
     #undef ARDRONE_CONFIG_KEY_IMM_a10
     #undef ARDRONE_CONFIG_KEY_REF_a10
     #undef ARDRONE_CONFIG_KEY_STR_a10
+
+
+    #define NAVDATA_STRUCTS_INITIALIZE
+    #include "NavdataMessageDefinitions.h"
+    #undef NAVDATA_STRUCTS_INITIALIZE
 }
 
 void ARDroneDriver::resetCaliberation()