Added custom messages and handling code

We now have the ability to access any of the navdata structs which are
sent back by the drone. Still to do is to enable/disable the drone
sending based on whether we're interested in receiving the message.

msg/navdata_adc_data_frame.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 uint32 version
 uint8[] data_frame

msg/navdata_altitude.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 int32 altitude_vision
 float32 altitude_vz
@@ -7,7 +8,7 @@ int32 altitude_ref
 int32 altitude_raw
 float32 obs_accZ
 float32 obs_alt
-ardrone_autonomy::vector31 obs_x
+vector31 obs_x
 uint32 obs_state
-ardrone_autonomy::vector21 est_vb
+vector21 est_vb
 uint32 est_state

msg/navdata_demo.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 uint32 ctrl_state
 uint32 vbat_flying_percentage

msg/navdata_euler_angles.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 float32 theta_a
 float32 phi_a

msg/navdata_games.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 uint32 double_tap_counter
 uint32 finish_line_counter

msg/navdata_gyros_offsets.msg

@@ -1,4 +1,5 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 float32[] offset_g

msg/navdata_hdvideo_stream.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 uint32 hdvideo_state
 uint32 storage_fifo_nb_packets

msg/navdata_kalman_pressure.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 float32 offset_pressure
 float32 est_z

msg/navdata_magneto.msg

@@ -1,12 +1,13 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 int16 mx
 int16 my
 int16 mz
-ardrone_autonomy::vector31 magneto_raw
+vector31 magneto_raw
-ardrone_autonomy::vector31 magneto_rectified
+vector31 magneto_rectified
-ardrone_autonomy::vector31 magneto_offset
+vector31 magneto_offset
 float32 heading_unwrapped
 float32 heading_gyro_unwrapped
 float32 heading_fusion_unwrapped

msg/navdata_phys_measures.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 float32 accs_temp
 uint16 gyro_temp

msg/navdata_pressure_raw.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 int32 up
 int16 ut

msg/navdata_pwm.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 uint8 motor1
 uint8 motor2
@@ -25,5 +26,4 @@ uint16 current_motor1
 uint16 current_motor2
 uint16 current_motor3
 uint16 current_motor4
-float32 altitude_prop
 float32 altitude_der

msg/navdata_raw_measures.msg

@@ -1,7 +1,7 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
-uint16[] raw_accs
 int16[] raw_gyros
 int16[] raw_gyros_110
 uint32 vbat_raw
@@ -13,7 +13,6 @@ uint16 us_courbe_temps
 uint16 us_courbe_valeur
 uint16 us_courbe_ref
 uint16 flag_echo_ini
-uint16 frame_number
 uint16 nb_echo
 uint32 sum_echo
 int32 alt_temp_raw

msg/navdata_rc_references.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 int32 rc_ref_pitch
 int32 rc_ref_roll

msg/navdata_references.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 int32 ref_theta
 int32 ref_phi

msg/navdata_time.msg

@@ -1,4 +1,5 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 uint32 time

msg/navdata_trackers_send.msg

@@ -1,3 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
+int32[] locked
+vector21[] point

msg/navdata_trims.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 float32 angular_rates_trim_r
 float32 euler_angles_trim_theta

msg/navdata_video_stream.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 uint8 quant
 uint32 frame_size
@@ -8,12 +9,9 @@ uint32 atcmd_ref_seq
 uint32 atcmd_mean_ref_gap
 float32 atcmd_var_ref_gap
 uint32 atcmd_ref_quality
-uint32 out_bitrate
 uint32 desired_bitrate
-int32 data1
 int32 data2
 int32 data3
 int32 data4
 int32 data5
-uint32 tcp_queue_level
 uint32 fifo_queue_level

msg/navdata_vision.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 uint32 vision_state
 int32 vision_misc
@@ -13,7 +14,7 @@ float32 phi_capture
 float32 psi_capture
 int32 altitude_capture
 uint32 time_capture
-ardrone_autonomy::vector31 body_v
+vector31 body_v
 float32 delta_phi
 float32 delta_theta
 float32 delta_psi

msg/navdata_vision_detect.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 uint32 nb_detected
 uint32[] type
@@ -9,6 +10,6 @@ uint32[] width
 uint32[] height
 uint32[] dist
 float32[] orientation_angle
-ardrone_autonomy::matrix33[] rotation
+matrix33[] rotation
-ardrone_autonomy::vector31[] translation
+vector31[] translation
 uint32[] camera_source

msg/navdata_vision_of.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 float32[] of_dx
 float32[] of_dy

msg/navdata_vision_perf.msg

@@ -1,7 +1,7 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
-float32 time_szo
 float32 time_corners
 float32 time_compute
 float32 time_tracking

msg/navdata_vision_raw.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 float32 vision_tx_raw
 float32 vision_ty_raw

msg/navdata_watchdog.msg

@@ -1,4 +1,4 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
-int32 watchdog

msg/navdata_wifi.msg

@@ -1,4 +1,5 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 uint32 link_quality

msg/navdata_wind_speed.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 float32 wind_speed
 float32 wind_angle

msg/navdata_zimmu_3000.msg

@@ -1,5 +1,6 @@
 Header header
-float32 drone_timeuint16 tag
+float64 drone_time
+uint16 tag
 uint16 size
 int32 vzimmuLSB
 float32 vzfind

scripts/CreateNavdataFormat.py

@@ -1,4 +1,5 @@
 import re
+from mako.template import Template
 
 PATH_NAVDATA_KEYS = '../ARDroneLib/Soft/Common/navdata_keys.h'
 PATH_NAVDATA_COMMON = '../ARDroneLib/Soft/Common/navdata_common.h'
@@ -29,20 +30,24 @@ contents = ''
 with open(PATH_NAVDATA_COMMON,'r') as navdata_common:
 	contents = navdata_common.read()
 
-ros_datatypes = {'uint32_t':'uint32',
+ros_datatypes = {'uint32_t'		:('std_msgs::UInt32','uint32_t'),
-				 'uint16_t':'uint16',
+				 'uint16_t'		:('std_msgs::UInt16','uint16_t'),
-				 'uint8_t' :'uint8',
+				 'uint8_t' 		:('std_msgs::UInt8','uint8_t'),
-				 'int32_t':'int32',
+				 'int32_t'		:('std_msgs::Int32','int32_t'),
-				 'int16_t':'int16',
+				 'int16_t'		:('std_msgs::Int16','int16_t'),
-				 'int8_t' :'int8',
+				 'int8_t' 		:('std_msgs::Int8','int8_t'),
-				 'float64_t':'float64',
+				 'float64_t'	:('std_msgs::Float64','float64_t'),
-				 'float32_t':'float32',
+				 'float32_t'	:('std_msgs::Float32','float32_t'),
-				 'matrix33_t':'ardrone_autonomy::matrix33',
+				 'matrix33_t'	:('ardrone_autonomy::matrix33','ardrone_autonomy::matrix33'),
-				 'vector31_t':'ardrone_autonomy::vector31',
+				 'vector31_t'	:('ardrone_autonomy::vector31','ardrone_autonomy::vector31'),
-				 'vector21_t':'ardrone_autonomy::vector21',
+				 'vector21_t'	:('ardrone_autonomy::vector21','ardrone_autonomy::vector21'),
-				 'velocities_t':'ardrone_autonomy::vector31',
+				 'screen_point_t'	:('ardrone_autonomy::vector21','ardrone_autonomy::vector21'),
-				 'char':'uint8',
+				 'velocities_t'	:('ardrone_autonomy::vector31','ardrone_autonomy::vector31'),
-				 'bool_t':'int32'}
+				 'char'			:('std_msgs::UInt8','char'),
+				 'bool_t'		:('std_msgs::Int32','bool_t')}
+
+
+
 
 structs = []
 print 'Parsing Navdata Struct Contents'
@@ -58,33 +63,50 @@ for (struct,name,_) in keys:
 	structcontents = re.search(rg,contents)
 	if structcontents:
 		print '-- '+name
-		allmembers = re.findall(r'\s*(\w+(?:\s*?\*)?)\s*(\w+)\s*(?:\[(\w+)\])?\s*;(.*)',structcontents.groups('inside')[0])
+		allmembers = re.findall(r'(.*?)\s*(\w+(?:\s*?\*)?)\s*(\w+)\s*(?:\[([\w\s*/+-]+)\])?\s*;(.*)',structcontents.groups('inside')[0])
-		members = [(t,n,s) for (t,n,s,c) in allmembers if 'Deprecated' not in c]
+		members = [(t,n,s) for (b,t,n,s,c) in allmembers if 'Deprecated' not in c and '//' not in b]
 	structs.append((struct,name,members))
 
 
+
+
+
 print 'Saving Custom ROS Message Definitions'
 for (struct,name,members) in structs:
 	with open('../msg/'+name+'.msg','w') as f:
 		f.write('Header header\n')
-		f.write('float64 drone_time')
+		f.write('float64 drone_time\n')
 		for (t,n,s) in members:
-			dt = ros_datatypes[t]
+			dt = ros_datatypes[t][0]
+			dt = dt.rpartition('::')[2].lower()
 			if s!='':
 				f.write('{0}[] {1}\n'.format(dt,n))
 			else:
 				f.write('{0} {1}\n'.format(dt,n))
 
-print 'Saving Struct Preprocessor File'
+
+
+
+print 'Generating C Source'
+
+items = []
+for (struct_type, struct_name, members) in structs:
+	item = {}
+	item['struct_name'] = struct_name
+	item['members'] = []
+	for (c_type,name,size) in members:
+		member = {}
+		member['name'] = name
+		member['c_type'] = c_type
+		member['include'] = ros_datatypes[c_type][0]
+		member['ros_type'] = ros_datatypes[c_type][1]
+		member['array_size'] = size if size!='' else None
+		item['members'].append(member)
+	items.append(item)
+
+template = Template(filename='NavdataMessageDefinitionsTemplate.c');
+
 with open('../src/NavdataMessageDefinitions.h','w') as f:
-	for (struct,name,members) in structs:
+	f.write(template.render(structs=items))
-		f.write('NavdataStructStart({0},{1})\n'.format(struct,name))
-		for (t,n,s) in members:
-			dt = ros_datatypes[t]
-			if s!='':
-				f.write('NavdataStructArray({0},{1},{2},{3},{4},{5})\n'.format(struct,name,t,dt,s,n))
-			else:
-				f.write('NavdataStructMember({0},{1},{2},{3},{4})\n'.format(struct,name,t,dt,n))
-		f.write('NavdataStructEnd({0},{1})\n\n'.format(struct,name))
 
 print 'You should now run `rosmake ardrone_autonomy` to build the custom messages.'

scripts/NavdataMessageDefinitionsTemplate.c

@@ -0,0 +1,103 @@
+// Autogenerated from source-files using the CreateNavdataFormat.py script
+
+#ifdef NAVDATA_STRUCTS_INCLUDES
+% for item in structs:
+#include "ardrone_autonomy/${item['struct_name']}.h"
+% endfor
+<% included = [] %>\
+% for item in structs:
+% for member in item['members']:
+% if member['include'] not in included:
+<% included.append(member['include']) %>\
+#include <${'/'.join(member['include'].split('::'))}.h>
+% endif
+% endfor
+% endfor	
+#endif
+
+#ifdef NAVDATA_STRUCTS_HEADER
+% for item in structs:
+	ros::Publisher pub_${item['struct_name']};
+	bool enabled_${item['struct_name']};
+
+% endfor
+	bool initialized_navdata_publishers;
+	void PublishNavdataTypes(navdata_unpacked_t n);
+#endif
+
+#ifdef NAVDATA_STRUCTS_SOURCE
+void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
+{
+	if(!initialized_navdata_publishers)
+	{
+		initialized_navdata_publishers = true;
+
+% for item in structs:
+		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);
+		}
+
+% endfor
+	}
+
+% for item in structs:
+	if(enabled_${item['struct_name']} && pub_${item['struct_name']}.getNumSubscribers()>0)
+	{
+		ardrone_autonomy::${item['struct_name']} msg;
+		msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
+
+% for member in item['members']:
+% if member['array_size'] is None:
+		{\
+			${format_member(item, member, None)}
+			msg.${member['name']} = m;
+		}
+
+% else:
+		for(int i=0; i<${member['array_size']}; i++)
+		{\
+			${format_member(item, member, 'i')}
+			msg.${member['name']}.push_back(m);
+		}
+		
+% endif
+% endfor
+		pub_${item['struct_name']}.publish(msg);
+	}
+
+% endfor
+
+}
+#endif
+
+
+
+
+<%def name="format_member(item, member, i)">
+			${member['c_type']} c = n.${item['struct_name']}.${member['name']}${'[{}]'.format(i) if i is not None else ''};
+			${member['ros_type']} m;
+% if 'matrix33' in member['ros_type']:
+			m.m11 = c.m11;
+			m.m12 = c.m12;
+			m.m13 = c.m13;
+			m.m21 = c.m21;
+			m.m22 = c.m22;
+			m.m23 = c.m23;
+			m.m31 = c.m31;
+			m.m32 = c.m32;
+			m.m33 = c.m33;
+% elif 'vector21' in member['ros_type']:
+			m.x = c.x;
+			m.y = c.y;
+% elif 'vector31' in member['ros_type']:
+			m.x = c.x;
+			m.y = c.y;
+			m.z = c.z;
+% else:
+			m = c;
+% endif
+</%def>

src/ardrone_driver.cpp

@@ -562,6 +562,7 @@ 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;
@@ -614,6 +615,9 @@ void ARDroneDriver::publish_navdata()
         navdata.vz -= vel_bias[2];
 
     }
+
+    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))
         return; // why bother, no one is listening.
     const ros::Time _now = ros::Time::now();
@@ -729,6 +733,10 @@ void ARDroneDriver::publish_navdata()
     imu_pub.publish(imu_msg);
 }
 
+#define NAVDATA_STRUCTS_SOURCE
+#include "NavdataMessageDefinitions.h"
+#undef NAVDATA_STRUCTS_SOURCE
+
 void ARDroneDriver::publish_tf()
 {
     tf_base_front.stamp_ = ros::Time::now();

src/ardrone_driver.h

@@ -16,12 +16,18 @@
 #include <ardrone_tool/ardrone_version.h>
 #include <ardrone_tool/ardrone_tool.h>
 
+#define NAVDATA_STRUCTS_INCLUDES
+#include "NavdataMessageDefinitions.h"
+#undef NAVDATA_STRUCTS_INCLUDES
+
 
 #define _DEG2RAD 0.01745331111
 #define _RAD2DEG 57.2957184819
 
 #define DRIVER_USERNAME "ardrone_driver"
 #define DRIVER_APPNAME "ardrone_driver"
+#define CAMERA_QUEUE_SIZE (10)
+#define NAVDATA_QUEUE_SIZE (25)
 
 enum ROOT_FRAME
 {
@@ -104,6 +110,11 @@ private:
     navdata_magneto_t navdata_magneto;
     navdata_wind_speed_t navdata_wind;
     navdata_time_t arnavtime;
+    navdata_unpacked_t navdata_raw;
+
+    #define NAVDATA_STRUCTS_HEADER
+    #include "NavdataMessageDefinitions.h"
+    #undef NAVDATA_STRUCTS_HEADER
 
     /*
      * TF Frames