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                         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>


ardrone_autonomy
Author(s): Mani Monajjemi, Mani Monajjemi
autogenerated on Fri Dec 9 2016 03:36:59