NavdataMessageDefinitionsTemplate.c

Go to the documentation of this file.

// 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);
                        odo_pub = node_handle.advertise<nav_msgs::Odometry>("ardrone/odometry", 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>