Added enable_legacy_navdata (true by default)

This flag allows us to turn off the legacy navdata/imu/mag topics,
favouring usage of the actual packets

scripts/NavdataMessageDefinitionsTemplate.c

@@ -19,8 +19,10 @@
 % for item in structs:
 	ros::Publisher pub_${item['struct_name']};
 	bool enabled_${item['struct_name']};
-
 % endfor
+
+	bool enabled_legacy_navdata;
+
 	bool initialized_navdata_publishers;
 	void PublishNavdataTypes(navdata_unpacked_t n);
 #endif
@@ -32,6 +34,18 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 	{
 		initialized_navdata_publishers = true;
 
+		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);
+			imu_pub = node_handle.advertise<sensor_msgs::Imu>("ardrone/imu", 25);
+			mag_pub = node_handle.advertise<geometry_msgs::Vector3Stamped>("ardrone/mag", 25);
+		}
+
+		//-------------------------
+
 % for item in structs:
 		enabled_${item['struct_name']} = false;
 		ros::param::get("~enable_${item['struct_name']}", enabled_${item['struct_name']});
@@ -41,6 +55,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_${item['struct_name']} = node_handle.advertise<ardrone_autonomy::${item['struct_name']}>("ardrone/${item['struct_name']}", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 % endfor
 	}
 
@@ -69,6 +85,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_${item['struct_name']}.publish(msg);
 	}
 
+	//-------------------------
+
 % endfor
 
 }

src/NavdataMessageDefinitions.h

@@ -43,88 +43,63 @@
 #ifdef NAVDATA_STRUCTS_HEADER
 	ros::Publisher pub_navdata_demo;
 	bool enabled_navdata_demo;
-
 	ros::Publisher pub_navdata_time;
 	bool enabled_navdata_time;
-
 	ros::Publisher pub_navdata_raw_measures;
 	bool enabled_navdata_raw_measures;
-
 	ros::Publisher pub_navdata_phys_measures;
 	bool enabled_navdata_phys_measures;
-
 	ros::Publisher pub_navdata_gyros_offsets;
 	bool enabled_navdata_gyros_offsets;
-
 	ros::Publisher pub_navdata_euler_angles;
 	bool enabled_navdata_euler_angles;
-
 	ros::Publisher pub_navdata_references;
 	bool enabled_navdata_references;
-
 	ros::Publisher pub_navdata_trims;
 	bool enabled_navdata_trims;
-
 	ros::Publisher pub_navdata_rc_references;
 	bool enabled_navdata_rc_references;
-
 	ros::Publisher pub_navdata_pwm;
 	bool enabled_navdata_pwm;
-
 	ros::Publisher pub_navdata_altitude;
 	bool enabled_navdata_altitude;
-
 	ros::Publisher pub_navdata_vision_raw;
 	bool enabled_navdata_vision_raw;
-
 	ros::Publisher pub_navdata_vision_of;
 	bool enabled_navdata_vision_of;
-
 	ros::Publisher pub_navdata_vision;
 	bool enabled_navdata_vision;
-
 	ros::Publisher pub_navdata_vision_perf;
 	bool enabled_navdata_vision_perf;
-
 	ros::Publisher pub_navdata_trackers_send;
 	bool enabled_navdata_trackers_send;
-
 	ros::Publisher pub_navdata_vision_detect;
 	bool enabled_navdata_vision_detect;
-
 	ros::Publisher pub_navdata_watchdog;
 	bool enabled_navdata_watchdog;
-
 	ros::Publisher pub_navdata_adc_data_frame;
 	bool enabled_navdata_adc_data_frame;
-
 	ros::Publisher pub_navdata_video_stream;
 	bool enabled_navdata_video_stream;
-
 	ros::Publisher pub_navdata_games;
 	bool enabled_navdata_games;
-
 	ros::Publisher pub_navdata_pressure_raw;
 	bool enabled_navdata_pressure_raw;
-
 	ros::Publisher pub_navdata_magneto;
 	bool enabled_navdata_magneto;
-
 	ros::Publisher pub_navdata_wind_speed;
 	bool enabled_navdata_wind_speed;
-
 	ros::Publisher pub_navdata_kalman_pressure;
 	bool enabled_navdata_kalman_pressure;
-
 	ros::Publisher pub_navdata_hdvideo_stream;
 	bool enabled_navdata_hdvideo_stream;
-
 	ros::Publisher pub_navdata_wifi;
 	bool enabled_navdata_wifi;
-
 	ros::Publisher pub_navdata_zimmu_3000;
 	bool enabled_navdata_zimmu_3000;
 
+	bool enabled_legacy_navdata;
+
 	bool initialized_navdata_publishers;
 	void PublishNavdataTypes(navdata_unpacked_t n);
 #endif
@@ -136,6 +111,18 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 	{
 		initialized_navdata_publishers = true;
 
+		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);
+			imu_pub = node_handle.advertise<sensor_msgs::Imu>("ardrone/imu", 25);
+			mag_pub = node_handle.advertise<geometry_msgs::Vector3Stamped>("ardrone/mag", 25);
+		}
+
+		//-------------------------
+
 		enabled_navdata_demo = false;
 		ros::param::get("~enable_navdata_demo", enabled_navdata_demo);
 
@@ -144,6 +131,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_demo = node_handle.advertise<ardrone_autonomy::navdata_demo>("ardrone/navdata_demo", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_time = false;
 		ros::param::get("~enable_navdata_time", enabled_navdata_time);
 
@@ -152,6 +141,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_time = node_handle.advertise<ardrone_autonomy::navdata_time>("ardrone/navdata_time", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_raw_measures = false;
 		ros::param::get("~enable_navdata_raw_measures", enabled_navdata_raw_measures);
 
@@ -160,6 +151,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_raw_measures = node_handle.advertise<ardrone_autonomy::navdata_raw_measures>("ardrone/navdata_raw_measures", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_phys_measures = false;
 		ros::param::get("~enable_navdata_phys_measures", enabled_navdata_phys_measures);
 
@@ -168,6 +161,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_phys_measures = node_handle.advertise<ardrone_autonomy::navdata_phys_measures>("ardrone/navdata_phys_measures", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_gyros_offsets = false;
 		ros::param::get("~enable_navdata_gyros_offsets", enabled_navdata_gyros_offsets);
 
@@ -176,6 +171,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_gyros_offsets = node_handle.advertise<ardrone_autonomy::navdata_gyros_offsets>("ardrone/navdata_gyros_offsets", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_euler_angles = false;
 		ros::param::get("~enable_navdata_euler_angles", enabled_navdata_euler_angles);
 
@@ -184,6 +181,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_euler_angles = node_handle.advertise<ardrone_autonomy::navdata_euler_angles>("ardrone/navdata_euler_angles", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_references = false;
 		ros::param::get("~enable_navdata_references", enabled_navdata_references);
 
@@ -192,6 +191,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_references = node_handle.advertise<ardrone_autonomy::navdata_references>("ardrone/navdata_references", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_trims = false;
 		ros::param::get("~enable_navdata_trims", enabled_navdata_trims);
 
@@ -200,6 +201,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_trims = node_handle.advertise<ardrone_autonomy::navdata_trims>("ardrone/navdata_trims", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_rc_references = false;
 		ros::param::get("~enable_navdata_rc_references", enabled_navdata_rc_references);
 
@@ -208,6 +211,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_rc_references = node_handle.advertise<ardrone_autonomy::navdata_rc_references>("ardrone/navdata_rc_references", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_pwm = false;
 		ros::param::get("~enable_navdata_pwm", enabled_navdata_pwm);
 
@@ -216,6 +221,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_pwm = node_handle.advertise<ardrone_autonomy::navdata_pwm>("ardrone/navdata_pwm", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_altitude = false;
 		ros::param::get("~enable_navdata_altitude", enabled_navdata_altitude);
 
@@ -224,6 +231,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_altitude = node_handle.advertise<ardrone_autonomy::navdata_altitude>("ardrone/navdata_altitude", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_vision_raw = false;
 		ros::param::get("~enable_navdata_vision_raw", enabled_navdata_vision_raw);
 
@@ -232,6 +241,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_vision_raw = node_handle.advertise<ardrone_autonomy::navdata_vision_raw>("ardrone/navdata_vision_raw", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_vision_of = false;
 		ros::param::get("~enable_navdata_vision_of", enabled_navdata_vision_of);
 
@@ -240,6 +251,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_vision_of = node_handle.advertise<ardrone_autonomy::navdata_vision_of>("ardrone/navdata_vision_of", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_vision = false;
 		ros::param::get("~enable_navdata_vision", enabled_navdata_vision);
 
@@ -248,6 +261,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_vision = node_handle.advertise<ardrone_autonomy::navdata_vision>("ardrone/navdata_vision", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_vision_perf = false;
 		ros::param::get("~enable_navdata_vision_perf", enabled_navdata_vision_perf);
 
@@ -256,6 +271,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_vision_perf = node_handle.advertise<ardrone_autonomy::navdata_vision_perf>("ardrone/navdata_vision_perf", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_trackers_send = false;
 		ros::param::get("~enable_navdata_trackers_send", enabled_navdata_trackers_send);
 
@@ -264,6 +281,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_trackers_send = node_handle.advertise<ardrone_autonomy::navdata_trackers_send>("ardrone/navdata_trackers_send", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_vision_detect = false;
 		ros::param::get("~enable_navdata_vision_detect", enabled_navdata_vision_detect);
 
@@ -272,6 +291,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_vision_detect = node_handle.advertise<ardrone_autonomy::navdata_vision_detect>("ardrone/navdata_vision_detect", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_watchdog = false;
 		ros::param::get("~enable_navdata_watchdog", enabled_navdata_watchdog);
 
@@ -280,6 +301,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_watchdog = node_handle.advertise<ardrone_autonomy::navdata_watchdog>("ardrone/navdata_watchdog", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_adc_data_frame = false;
 		ros::param::get("~enable_navdata_adc_data_frame", enabled_navdata_adc_data_frame);
 
@@ -288,6 +311,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_adc_data_frame = node_handle.advertise<ardrone_autonomy::navdata_adc_data_frame>("ardrone/navdata_adc_data_frame", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_video_stream = false;
 		ros::param::get("~enable_navdata_video_stream", enabled_navdata_video_stream);
 
@@ -296,6 +321,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_video_stream = node_handle.advertise<ardrone_autonomy::navdata_video_stream>("ardrone/navdata_video_stream", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_games = false;
 		ros::param::get("~enable_navdata_games", enabled_navdata_games);
 
@@ -304,6 +331,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_games = node_handle.advertise<ardrone_autonomy::navdata_games>("ardrone/navdata_games", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_pressure_raw = false;
 		ros::param::get("~enable_navdata_pressure_raw", enabled_navdata_pressure_raw);
 
@@ -312,6 +341,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_pressure_raw = node_handle.advertise<ardrone_autonomy::navdata_pressure_raw>("ardrone/navdata_pressure_raw", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_magneto = false;
 		ros::param::get("~enable_navdata_magneto", enabled_navdata_magneto);
 
@@ -320,6 +351,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_magneto = node_handle.advertise<ardrone_autonomy::navdata_magneto>("ardrone/navdata_magneto", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_wind_speed = false;
 		ros::param::get("~enable_navdata_wind_speed", enabled_navdata_wind_speed);
 
@@ -328,6 +361,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_wind_speed = node_handle.advertise<ardrone_autonomy::navdata_wind_speed>("ardrone/navdata_wind_speed", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_kalman_pressure = false;
 		ros::param::get("~enable_navdata_kalman_pressure", enabled_navdata_kalman_pressure);
 
@@ -336,6 +371,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_kalman_pressure = node_handle.advertise<ardrone_autonomy::navdata_kalman_pressure>("ardrone/navdata_kalman_pressure", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_hdvideo_stream = false;
 		ros::param::get("~enable_navdata_hdvideo_stream", enabled_navdata_hdvideo_stream);
 
@@ -344,6 +381,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_hdvideo_stream = node_handle.advertise<ardrone_autonomy::navdata_hdvideo_stream>("ardrone/navdata_hdvideo_stream", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_wifi = false;
 		ros::param::get("~enable_navdata_wifi", enabled_navdata_wifi);
 
@@ -352,6 +391,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_wifi = node_handle.advertise<ardrone_autonomy::navdata_wifi>("ardrone/navdata_wifi", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 		enabled_navdata_zimmu_3000 = false;
 		ros::param::get("~enable_navdata_zimmu_3000", enabled_navdata_zimmu_3000);
 
@@ -360,6 +401,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 			pub_navdata_zimmu_3000 = node_handle.advertise<ardrone_autonomy::navdata_zimmu_3000>("ardrone/navdata_zimmu_3000", NAVDATA_QUEUE_SIZE);
 		}
 
+		//-------------------------
+
 	}
 
 	if(enabled_navdata_demo && pub_navdata_demo.getNumSubscribers()>0)
@@ -474,6 +517,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_demo.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_time && pub_navdata_time.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_time msg;
@@ -506,6 +551,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_time.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_raw_measures && pub_navdata_raw_measures.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_raw_measures msg;
@@ -652,6 +699,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_raw_measures.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_phys_measures && pub_navdata_phys_measures.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_phys_measures msg;
@@ -734,6 +783,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_phys_measures.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_gyros_offsets && pub_navdata_gyros_offsets.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_gyros_offsets msg;
@@ -767,6 +818,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_gyros_offsets.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_euler_angles && pub_navdata_euler_angles.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_euler_angles msg;
@@ -807,6 +860,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_euler_angles.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_references && pub_navdata_references.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_references msg;
@@ -999,6 +1054,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_references.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_trims && pub_navdata_trims.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_trims msg;
@@ -1047,6 +1104,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_trims.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_rc_references && pub_navdata_rc_references.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_rc_references msg;
@@ -1111,6 +1170,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_rc_references.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_pwm && pub_navdata_pwm.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_pwm msg;
@@ -1335,6 +1396,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_pwm.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_altitude && pub_navdata_altitude.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_altitude msg;
@@ -1442,6 +1505,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_altitude.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_vision_raw && pub_navdata_vision_raw.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_vision_raw msg;
@@ -1490,6 +1555,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_vision_raw.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_vision_of && pub_navdata_vision_of.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_vision_of msg;
@@ -1532,6 +1599,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_vision_of.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_vision && pub_navdata_vision.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_vision msg;
@@ -1718,6 +1787,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_vision.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_vision_perf && pub_navdata_vision_perf.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_vision_perf msg;
@@ -1791,6 +1862,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_vision_perf.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_trackers_send && pub_navdata_trackers_send.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_trackers_send msg;
@@ -1834,6 +1907,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_trackers_send.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_vision_detect && pub_navdata_vision_detect.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_vision_detect msg;
@@ -1966,6 +2041,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_vision_detect.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_watchdog && pub_navdata_watchdog.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_watchdog msg;
@@ -1990,6 +2067,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_watchdog.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_adc_data_frame && pub_navdata_adc_data_frame.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_adc_data_frame msg;
@@ -2031,6 +2110,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_adc_data_frame.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_video_stream && pub_navdata_video_stream.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_video_stream msg;
@@ -2159,6 +2240,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_video_stream.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_games && pub_navdata_games.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_games msg;
@@ -2199,6 +2282,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_games.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_pressure_raw && pub_navdata_pressure_raw.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_pressure_raw msg;
@@ -2255,6 +2340,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_pressure_raw.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_magneto && pub_navdata_magneto.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_magneto msg;
@@ -2397,6 +2484,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_magneto.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_wind_speed && pub_navdata_wind_speed.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_wind_speed msg;
@@ -2525,6 +2614,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_wind_speed.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_kalman_pressure && pub_navdata_kalman_pressure.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_kalman_pressure msg;
@@ -2685,6 +2776,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_kalman_pressure.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_hdvideo_stream && pub_navdata_hdvideo_stream.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_hdvideo_stream msg;
@@ -2765,6 +2858,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_hdvideo_stream.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_wifi && pub_navdata_wifi.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_wifi msg;
@@ -2797,6 +2892,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_wifi.publish(msg);
 	}
 
+	//-------------------------
+
 	if(enabled_navdata_zimmu_3000 && pub_navdata_zimmu_3000.getNumSubscribers()>0)
 	{
 		ardrone_autonomy::navdata_zimmu_3000 msg;
@@ -2837,6 +2934,8 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
 		pub_navdata_zimmu_3000.publish(msg);
 	}
 
+	//-------------------------
+
 
 }
 #endif

src/ardrone_driver.cpp

@@ -20,9 +20,6 @@ ARDroneDriver::ARDroneDriver()
     image_pub = image_transport.advertiseCamera("ardrone/image_raw", 10);
     hori_pub = image_transport.advertiseCamera("ardrone/front/image_raw", 10);
     vert_pub = image_transport.advertiseCamera("ardrone/bottom/image_raw", 10);
-    navdata_pub = node_handle.advertise<ardrone_autonomy::Navdata>("ardrone/navdata", 25);
-    imu_pub = node_handle.advertise<sensor_msgs::Imu>("ardrone/imu", 25);
-    mag_pub = node_handle.advertise<geometry_msgs::Vector3Stamped>("ardrone/mag", 25);
     toggleCam_service = node_handle.advertiseService("ardrone/togglecam", toggleCamCallback);
     setCamChannel_service = node_handle.advertiseService("ardrone/setcamchannel",setCamChannelCallback );
     setLedAnimation_service = node_handle.advertiseService("ardrone/setledanimation", setLedAnimationCallback);
@@ -618,7 +615,7 @@ void ARDroneDriver::publish_navdata()
 
     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))
+    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();
 

src/ardrone_sdk.cpp

@@ -70,6 +70,8 @@ extern "C" {
         command_always_send = false;   //constantly sends navdata messages to the drone, even if the messages haven't changed
         ros::param::param("~command_disable_hover", command_disable_hover, false);
         ros::param::param("~command_always_send"  , command_always_send,   false);
+        ROS_INFO("Hovering is %s",(command_disable_hover?"DISABLED!":"Enabled."));
+        ROS_INFO("Will %s send duplicate commands.",(command_always_send?"ALWAYS":"not"));
 
 
         // SET SOME NON-STANDARD DEFAULT VALUES FOR THE DRIVER