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 odo_pub = node_handle.advertise<nav_msgs::Odometry>("ardrone/odometry", 25);
00046 }
00047
00048
00049
00050 % for item in structs:
00051 ros::param::param("~enable_${item['struct_name']}", enabled_${item['struct_name']}, false);
00052 if(enabled_${item['struct_name']})
00053 {
00054 pub_${item['struct_name']} = node_handle.advertise<ardrone_autonomy::${item['struct_name']}>("ardrone/${item['struct_name']}", NAVDATA_QUEUE_SIZE);
00055 }
00056
00057
00058
00059 % endfor
00060 }
00061 #endif
00062
00063 #ifdef NAVDATA_STRUCTS_SOURCE
00064 void ARDroneDriver::PublishNavdataTypes(const navdata_unpacked_t &n, const ros::Time &received)
00065 {
00066 if(initialized_navdata_publishers)
00067 {
00068 % for item in structs:
00069 if(enabled_${item['struct_name']} && pub_${item['struct_name']}.getNumSubscribers()>0)
00070 {
00071 ${item['struct_name']}_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
00072 ${item['struct_name']}_msg.header.stamp = received;
00073 ${item['struct_name']}_msg.header.frame_id = droneFrameBase;
00074
00075 % for member in item['members']:
00076 % if member['array_size'] is None:
00077 {\
00078 ${format_member(item, member, None)}
00079 ${item['struct_name']}_msg.${member['name']} = m;
00080 }
00081
00082 % else:
00083 ${item['struct_name']}_msg.${member['name']}.clear();
00084 for(int i=0; i<${member['array_size']}; i++)
00085 {\
00086 ${format_member(item, member, 'i')}
00087 ${item['struct_name']}_msg.${member['name']}.push_back(m);
00088 }
00089
00090 % endif
00091 % endfor
00092 pub_${item['struct_name']}.publish(${item['struct_name']}_msg);
00093 }
00094
00095
00096
00097 % endfor
00098 }
00099 }
00100 #endif
00101
00102
00103
00104
00105 <%def name="format_member(item, member, i)">
00106 ${member['c_type']} c = n.${item['struct_name']}.${member['name']}${'[{}]'.format(i) if i is not None else ''};
00107 ${member['ros_type']} m;
00108 % if 'matrix33' in member['ros_type']:
00109 m.m11 = c.m11;
00110 m.m12 = c.m12;
00111 m.m13 = c.m13;
00112 m.m21 = c.m21;
00113 m.m22 = c.m22;
00114 m.m23 = c.m23;
00115 m.m31 = c.m31;
00116 m.m32 = c.m32;
00117 m.m33 = c.m33;
00118 % elif 'vector21' in member['ros_type']:
00119 m.x = c.x;
00120 m.y = c.y;
00121 % elif 'vector31' in member['ros_type']:
00122 m.x = c.x;
00123 m.y = c.y;
00124 m.z = c.z;
00125 % else:
00126 m = c;
00127 % endif
00128 </%def>