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