NavdataMessageDefinitionsTemplate.c
Go to the documentation of this file.
00001 // Autogenerated from source-files using the CreateNavdataFormat.py script
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>


ardrone_autonomy
Author(s): Mani Monajjemi
autogenerated on Mon Jan 6 2014 11:03:00