Mani Monajjemi
128 linhas
3.3 KiB
C
128 linhas
3.3 KiB
C
// 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_PUBLIC
|
|
void PublishNavdataTypes(const navdata_unpacked_t &n, const ros::Time &received);
|
|
#endif
|
|
|
|
#ifdef NAVDATA_STRUCTS_HEADER_PRIVATE
|
|
% for item in structs:
|
|
ros::Publisher pub_${item['struct_name']};
|
|
bool enabled_${item['struct_name']};
|
|
ardrone_autonomy::${item['struct_name']} ${item['struct_name']}_msg;
|
|
% endfor
|
|
|
|
bool enabled_legacy_navdata;
|
|
|
|
bool initialized_navdata_publishers;
|
|
#endif
|
|
|
|
#ifdef NAVDATA_STRUCTS_INITIALIZE
|
|
if(!initialized_navdata_publishers)
|
|
{
|
|
initialized_navdata_publishers = true;
|
|
|
|
ros::param::param("~enable_legacy_navdata", enabled_legacy_navdata, true);
|
|
if(enabled_legacy_navdata)
|
|
{
|
|
navdata_pub = node_handle.advertise<ardrone_autonomy::Navdata>("ardrone/navdata", 25);
|
|
imu_pub = node_handle.advertise<sensor_msgs::Imu>("ardrone/imu", 25);
|
|
mag_pub = node_handle.advertise<geometry_msgs::Vector3Stamped>("ardrone/mag", 25);
|
|
}
|
|
|
|
//-------------------------
|
|
|
|
% for item in structs:
|
|
ros::param::param("~enable_${item['struct_name']}", enabled_${item['struct_name']}, false);
|
|
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
|
|
}
|
|
#endif
|
|
|
|
#ifdef NAVDATA_STRUCTS_SOURCE
|
|
void ARDroneDriver::PublishNavdataTypes(const navdata_unpacked_t &n, const ros::Time &received)
|
|
{
|
|
if(initialized_navdata_publishers)
|
|
{
|
|
% for item in structs:
|
|
if(enabled_${item['struct_name']} && pub_${item['struct_name']}.getNumSubscribers()>0)
|
|
{
|
|
${item['struct_name']}_msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
|
|
${item['struct_name']}_msg.header.stamp = received;
|
|
${item['struct_name']}_msg.header.frame_id = droneFrameBase;
|
|
|
|
% for member in item['members']:
|
|
% if member['array_size'] is None:
|
|
{\
|
|
${format_member(item, member, None)}
|
|
${item['struct_name']}_msg.${member['name']} = m;
|
|
}
|
|
|
|
% else:
|
|
${item['struct_name']}_msg.${member['name']}.clear();
|
|
for(int i=0; i<${member['array_size']}; i++)
|
|
{\
|
|
${format_member(item, member, 'i')}
|
|
${item['struct_name']}_msg.${member['name']}.push_back(m);
|
|
}
|
|
|
|
% endif
|
|
% endfor
|
|
pub_${item['struct_name']}.publish(${item['struct_name']}_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>
|