Go to the documentation of this file.00001
00002
00003 #ifdef NAVDATA_STRUCTS_INCLUDES
00004 % for item in structs:
00005 #include "ardrone_autonomy/${item['struct_name']}.h"
00006 % endfor
00007 <% included = [] %>\
00008 % for item in structs:
00009 % for member in item['members']:
00010 % if member['include'] not in included:
00011 <% included.append(member['include']) %>\
00012 #include <${'/'.join(member['include'].split('::'))}.h>
00013 % endif
00014 % endfor
00015 % endfor
00016 #endif
00017
00018 #ifdef NAVDATA_STRUCTS_HEADER_PUBLIC
00019 void PublishNavdataTypes(const navdata_unpacked_t &n, const ros::Time &received);
00020 #endif
00021
00022 #ifdef NAVDATA_STRUCTS_HEADER_PRIVATE
00023 % for item in structs:
00024 ros::Publisher pub_${item['struct_name']};
00025 bool enabled_${item['struct_name']};
00026 ardrone_autonomy::${item['struct_name']} ${item['struct_name']}_msg;
00027 % endfor
00028
00029 bool enabled_legacy_navdata;
00030
00031 bool initialized_navdata_publishers;
00032 #endif
00033
00034 #ifdef NAVDATA_STRUCTS_INITIALIZE
00035 if(!initialized_navdata_publishers)
00036 {
00037 initialized_navdata_publishers = true;
00038
00039 ros::param::param("~enable_legacy_navdata", enabled_legacy_navdata, true);
00040 if(enabled_legacy_navdata)
00041 {
00042 navdata_pub = node_handle.advertise<ardrone_autonomy::Navdata>("ardrone/navdata", 25);
00043 imu_pub = node_handle.advertise<sensor_msgs::Imu>("ardrone/imu", 25);
00044 mag_pub = node_handle.advertise<geometry_msgs::Vector3Stamped>("ardrone/mag", 25);
00045 }
00046
00047
00048
00049 % for item in structs:
00050 ros::param::param("~enable_${item['struct_name']}", enabled_${item['struct_name']}, false);
00051 if(enabled_${item['struct_name']})
00052 {
00053 pub_${item['struct_name']} = node_handle.advertise<ardrone_autonomy::${item['struct_name']}>("ardrone/${item['struct_name']}", NAVDATA_QUEUE_SIZE);
00054 }
00055
00056
00057
00058 % endfor
00059 }
00060 #endif
00061
00062 #ifdef NAVDATA_STRUCTS_SOURCE
00063 void ARDroneDriver::PublishNavdataTypes(const navdata_unpacked_t &n, const ros::Time &received)
00064 {
00065 if(initialized_navdata_publishers)
00066 {
00067 % for item in structs:
00068 if(enabled_${item['struct_name']} && pub_${item['struct_name']}.getNumSubscribers()>0)
00069 {
00070 ${item['struct_name']}_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
00071 ${item['struct_name']}_msg.header.stamp = received;
00072 ${item['struct_name']}_msg.header.frame_id = droneFrameBase;
00073
00074 % for member in item['members']:
00075 % if member['array_size'] is None:
00076 {\
00077 ${format_member(item, member, None)}
00078 ${item['struct_name']}_msg.${member['name']} = m;
00079 }
00080
00081 % else:
00082 ${item['struct_name']}_msg.${member['name']}.clear();
00083 for(int i=0; i<${member['array_size']}; i++)
00084 {\
00085 ${format_member(item, member, 'i')}
00086 ${item['struct_name']}_msg.${member['name']}.push_back(m);
00087 }
00088
00089 % endif
00090 % endfor
00091 pub_${item['struct_name']}.publish(${item['struct_name']}_msg);
00092 }
00093
00094
00095
00096 % endfor
00097 }
00098 }
00099 #endif
00100
00101
00102
00103
00104 <%def name="format_member(item, member, i)">
00105 ${member['c_type']} c = n.${item['struct_name']}.${member['name']}${'[{}]'.format(i) if i is not None else ''};
00106 ${member['ros_type']} m;
00107 % if 'matrix33' in member['ros_type']:
00108 m.m11 = c.m11;
00109 m.m12 = c.m12;
00110 m.m13 = c.m13;
00111 m.m21 = c.m21;
00112 m.m22 = c.m22;
00113 m.m23 = c.m23;
00114 m.m31 = c.m31;
00115 m.m32 = c.m32;
00116 m.m33 = c.m33;
00117 % elif 'vector21' in member['ros_type']:
00118 m.x = c.x;
00119 m.y = c.y;
00120 % elif 'vector31' in member['ros_type']:
00121 m.x = c.x;
00122 m.y = c.y;
00123 m.z = c.z;
00124 % else:
00125 m = c;
00126 % endif
00127 </%def>