wellton/ardrone_autonomy
1
0
Fork 0
Código Issues Pull Requests Ações Pacotes Projetos Versões Wiki Atividade

Added custom messages and handling code

Ver Código-Fonte 
We now have the ability to access any of the navdata structs which are
sent back by the drone. Still to do is to enable/disable the drone
sending based on whether we're interested in receiving the message.
Esse commit está contido em:
Mike Hamer
2012-11-21 12:53:33 +01:00
pai 2fc267c869
commit ca5fadf778
33 arquivos alterados com 3056 adições e 410 exclusões
Baixar Arquivo de Patch Baixar Arquivo de Diff Expandir todos os arquivos Colapsar todos os arquivos
msg/navdata_adc_data_frame.msg
+2 -1
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
uint32 version
uint8[] data_frame
msg/navdata_altitude.msg
+4 -3
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
int32 altitude_vision
float32 altitude_vz
@@ -7,7 +8,7 @@ int32 altitude_ref
int32 altitude_raw
float32 obs_accZ
float32 obs_alt
ardrone_autonomy::vector31 obs_x
vector31 obs_x
uint32 obs_state
ardrone_autonomy::vector21 est_vb
vector21 est_vb
uint32 est_state
msg/navdata_demo.msg
+2 -1
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
uint32 ctrl_state
uint32 vbat_flying_percentage
msg/navdata_euler_angles.msg
+2 -1
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
float32 theta_a
float32 phi_a
msg/navdata_games.msg
+2 -1
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
uint32 double_tap_counter
uint32 finish_line_counter
msg/navdata_gyros_offsets.msg
+2 -1
Ver Arquivo
@@ -1,4 +1,5 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
float32[] offset_g
msg/navdata_hdvideo_stream.msg
+2 -1
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
uint32 hdvideo_state
uint32 storage_fifo_nb_packets
msg/navdata_kalman_pressure.msg
+2 -1
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
float32 offset_pressure
float32 est_z
msg/navdata_magneto.msg
+5 -4
Ver Arquivo
@@ -1,12 +1,13 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
int16 mx
int16 my
int16 mz
ardrone_autonomy::vector31 magneto_raw
ardrone_autonomy::vector31 magneto_rectified
ardrone_autonomy::vector31 magneto_offset
vector31 magneto_raw
vector31 magneto_rectified
vector31 magneto_offset
float32 heading_unwrapped
float32 heading_gyro_unwrapped
float32 heading_fusion_unwrapped
msg/navdata_phys_measures.msg
+2 -1
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
float32 accs_temp
uint16 gyro_temp
msg/navdata_pressure_raw.msg
+2 -1
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
int32 up
int16 ut
msg/navdata_pwm.msg
+2 -2
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
uint8 motor1
uint8 motor2
@@ -25,5 +26,4 @@ uint16 current_motor1
uint16 current_motor2
uint16 current_motor3
uint16 current_motor4
float32 altitude_prop
float32 altitude_der
msg/navdata_raw_measures.msg
+2 -3
Ver Arquivo
@@ -1,7 +1,7 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
uint16[] raw_accs
int16[] raw_gyros
int16[] raw_gyros_110
uint32 vbat_raw
@@ -13,7 +13,6 @@ uint16 us_courbe_temps
uint16 us_courbe_valeur
uint16 us_courbe_ref
uint16 flag_echo_ini
uint16 frame_number
uint16 nb_echo
uint32 sum_echo
int32 alt_temp_raw
msg/navdata_rc_references.msg
+2 -1
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
int32 rc_ref_pitch
int32 rc_ref_roll
msg/navdata_references.msg
+2 -1
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
int32 ref_theta
int32 ref_phi
msg/navdata_time.msg
+2 -1
Ver Arquivo
@@ -1,4 +1,5 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
uint32 time
msg/navdata_trackers_send.msg
+4 -1
Ver Arquivo
@@ -1,3 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
int32[] locked
vector21[] point
msg/navdata_trims.msg
+2 -1
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
float32 angular_rates_trim_r
float32 euler_angles_trim_theta
msg/navdata_video_stream.msg
+2 -4
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
uint8 quant
uint32 frame_size
@@ -8,12 +9,9 @@ uint32 atcmd_ref_seq
uint32 atcmd_mean_ref_gap
float32 atcmd_var_ref_gap
uint32 atcmd_ref_quality
uint32 out_bitrate
uint32 desired_bitrate
int32 data1
int32 data2
int32 data3
int32 data4
int32 data5
uint32 tcp_queue_level
uint32 fifo_queue_level
msg/navdata_vision.msg
+3 -2
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
uint32 vision_state
int32 vision_misc
@@ -13,7 +14,7 @@ float32 phi_capture
float32 psi_capture
int32 altitude_capture
uint32 time_capture
ardrone_autonomy::vector31 body_v
vector31 body_v
float32 delta_phi
float32 delta_theta
float32 delta_psi
msg/navdata_vision_detect.msg
+4 -3
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
uint32 nb_detected
uint32[] type
@@ -9,6 +10,6 @@ uint32[] width
uint32[] height
uint32[] dist
float32[] orientation_angle
ardrone_autonomy::matrix33[] rotation
ardrone_autonomy::vector31[] translation
matrix33[] rotation
vector31[] translation
uint32[] camera_source
msg/navdata_vision_of.msg
+2 -1
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
float32[] of_dx
float32[] of_dy
msg/navdata_vision_perf.msg
+2 -2
Ver Arquivo
@@ -1,7 +1,7 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
float32 time_szo
float32 time_corners
float32 time_compute
float32 time_tracking
msg/navdata_vision_raw.msg
+2 -1
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
float32 vision_tx_raw
float32 vision_ty_raw
msg/navdata_watchdog.msg
+2 -2
Ver Arquivo
@@ -1,4 +1,4 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
int32 watchdog
msg/navdata_wifi.msg
+2 -1
Ver Arquivo
@@ -1,4 +1,5 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
uint32 link_quality
msg/navdata_wind_speed.msg
+2 -1
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
float32 wind_speed
float32 wind_angle
msg/navdata_zimmu_3000.msg
+2 -1
Ver Arquivo
@@ -1,5 +1,6 @@
Header header
float32 drone_timeuint16 tag
float64 drone_time
uint16 tag
uint16 size
int32 vzimmuLSB
float32 vzfind
scripts/CreateNavdataFormat.py
+50 -28
Ver Arquivo
@@ -1,4 +1,5 @@
import re
from mako.template import Template
PATH_NAVDATA_KEYS = '../ARDroneLib/Soft/Common/navdata_keys.h'
PATH_NAVDATA_COMMON = '../ARDroneLib/Soft/Common/navdata_common.h'
@@ -29,20 +30,24 @@ contents = ''
with open(PATH_NAVDATA_COMMON,'r') as navdata_common:
contents = navdata_common.read()
ros_datatypes = {'uint32_t':'uint32',
'uint16_t':'uint16',
'uint8_t' :'uint8',
'int32_t':'int32',
'int16_t':'int16',
'int8_t' :'int8',
'float64_t':'float64',
'float32_t':'float32',
'matrix33_t':'ardrone_autonomy::matrix33',
'vector31_t':'ardrone_autonomy::vector31',
'vector21_t':'ardrone_autonomy::vector21',
'velocities_t':'ardrone_autonomy::vector31',
'char':'uint8',
'bool_t':'int32'}
ros_datatypes = {'uint32_t' :('std_msgs::UInt32','uint32_t'),
'uint16_t' :('std_msgs::UInt16','uint16_t'),
'uint8_t' :('std_msgs::UInt8','uint8_t'),
'int32_t' :('std_msgs::Int32','int32_t'),
'int16_t' :('std_msgs::Int16','int16_t'),
'int8_t' :('std_msgs::Int8','int8_t'),
'float64_t' :('std_msgs::Float64','float64_t'),
'float32_t' :('std_msgs::Float32','float32_t'),
'matrix33_t' :('ardrone_autonomy::matrix33','ardrone_autonomy::matrix33'),
'vector31_t' :('ardrone_autonomy::vector31','ardrone_autonomy::vector31'),
'vector21_t' :('ardrone_autonomy::vector21','ardrone_autonomy::vector21'),
'screen_point_t' :('ardrone_autonomy::vector21','ardrone_autonomy::vector21'),
'velocities_t' :('ardrone_autonomy::vector31','ardrone_autonomy::vector31'),
'char' :('std_msgs::UInt8','char'),
'bool_t' :('std_msgs::Int32','bool_t')}
structs = []
print 'Parsing Navdata Struct Contents'
@@ -58,33 +63,50 @@ for (struct,name,_) in keys:
structcontents = re.search(rg,contents)
if structcontents:
print '-- '+name
allmembers = re.findall(r'\s*(\w+(?:\s*?\*)?)\s*(\w+)\s*(?:\[(\w+)\])?\s*;(.*)',structcontents.groups('inside')[0])
members = [(t,n,s) for (t,n,s,c) in allmembers if 'Deprecated' not in c]
allmembers = re.findall(r'(.*?)\s*(\w+(?:\s*?\*)?)\s*(\w+)\s*(?:\[([\w\s*/+-]+)\])?\s*;(.*)',structcontents.groups('inside')[0])
members = [(t,n,s) for (b,t,n,s,c) in allmembers if 'Deprecated' not in c and '//' not in b]
structs.append((struct,name,members))
print 'Saving Custom ROS Message Definitions'
for (struct,name,members) in structs:
with open('../msg/'+name+'.msg','w') as f:
f.write('Header header\n')
f.write('float64 drone_time')
f.write('float64 drone_time\n')
for (t,n,s) in members:
dt = ros_datatypes[t]
dt = ros_datatypes[t][0]
dt = dt.rpartition('::')[2].lower()
if s!='':
f.write('{0}[] {1}\n'.format(dt,n))
else:
f.write('{0} {1}\n'.format(dt,n))
print 'Saving Struct Preprocessor File'
print 'Generating C Source'
items = []
for (struct_type, struct_name, members) in structs:
item = {}
item['struct_name'] = struct_name
item['members'] = []
for (c_type,name,size) in members:
member = {}
member['name'] = name
member['c_type'] = c_type
member['include'] = ros_datatypes[c_type][0]
member['ros_type'] = ros_datatypes[c_type][1]
member['array_size'] = size if size!='' else None
item['members'].append(member)
items.append(item)
template = Template(filename='NavdataMessageDefinitionsTemplate.c');
with open('../src/NavdataMessageDefinitions.h','w') as f:
for (struct,name,members) in structs:
f.write('NavdataStructStart({0},{1})\n'.format(struct,name))
for (t,n,s) in members:
dt = ros_datatypes[t]
if s!='':
f.write('NavdataStructArray({0},{1},{2},{3},{4},{5})\n'.format(struct,name,t,dt,s,n))
else:
f.write('NavdataStructMember({0},{1},{2},{3},{4})\n'.format(struct,name,t,dt,n))
f.write('NavdataStructEnd({0},{1})\n\n'.format(struct,name))
f.write(template.render(structs=items))
print 'You should now run `rosmake ardrone_autonomy` to build the custom messages.'
scripts/NavdataMessageDefinitionsTemplate.c
+103
Ver Arquivo
@@ -0,0 +1,103 @@
// 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
% for item in structs:
ros::Publisher pub_${item['struct_name']};
bool enabled_${item['struct_name']};
% endfor
bool initialized_navdata_publishers;
void PublishNavdataTypes(navdata_unpacked_t n);
#endif
#ifdef NAVDATA_STRUCTS_SOURCE
void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
{
if(!initialized_navdata_publishers)
{
initialized_navdata_publishers = true;
% for item in structs:
enabled_${item['struct_name']} = false;
ros::param::get("~enable_${item['struct_name']}", enabled_${item['struct_name']});
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
}
% for item in structs:
if(enabled_${item['struct_name']} && pub_${item['struct_name']}.getNumSubscribers()>0)
{
ardrone_autonomy::${item['struct_name']} msg;
msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0;
% for member in item['members']:
% if member['array_size'] is None:
{\
${format_member(item, member, None)}
msg.${member['name']} = m;
}
% else:
for(int i=0; i<${member['array_size']}; i++)
{\
${format_member(item, member, 'i')}
msg.${member['name']}.push_back(m);
}
% endif
% endfor
pub_${item['struct_name']}.publish(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>
src/NavdataMessageDefinitions.h
+2818 -338
Ver Arquivo
Diferenças do arquivo suprimidas por serem muito extensas Carregar Diff
src/ardrone_driver.cpp
+8
Ver Arquivo
@@ -562,6 +562,7 @@ void ARDroneDriver::publish_navdata()
{
// Thread safe copy of interesting Navdata data
vp_os_mutex_lock(&navdata_lock);
navdata_raw = shared_raw_navdata;
navdata_detect = shared_navdata_detect;
navdata_phys = shared_navdata_phys;
navdata = shared_navdata;
@@ -614,6 +615,9 @@ void ARDroneDriver::publish_navdata()
navdata.vz -= vel_bias[2];
}
PublishNavdataTypes(navdata_raw); // This is defined in the template NavdataMessageDefinitions.h template file
if ((navdata_pub.getNumSubscribers() == 0) && (imu_pub.getNumSubscribers() == 0) && (mag_pub.getNumSubscribers() == 0))
return; // why bother, no one is listening.
const ros::Time _now = ros::Time::now();
@@ -729,6 +733,10 @@ void ARDroneDriver::publish_navdata()
imu_pub.publish(imu_msg);
}
#define NAVDATA_STRUCTS_SOURCE
#include "NavdataMessageDefinitions.h"
#undef NAVDATA_STRUCTS_SOURCE
void ARDroneDriver::publish_tf()
{
tf_base_front.stamp_ = ros::Time::now();
src/ardrone_driver.h
+11
Ver Arquivo
@@ -16,12 +16,18 @@
#include <ardrone_tool/ardrone_version.h>
#include <ardrone_tool/ardrone_tool.h>
#define NAVDATA_STRUCTS_INCLUDES
#include "NavdataMessageDefinitions.h"
#undef NAVDATA_STRUCTS_INCLUDES
#define _DEG2RAD 0.01745331111
#define _RAD2DEG 57.2957184819
#define DRIVER_USERNAME "ardrone_driver"
#define DRIVER_APPNAME "ardrone_driver"
#define CAMERA_QUEUE_SIZE (10)
#define NAVDATA_QUEUE_SIZE (25)
enum ROOT_FRAME
{
@@ -104,6 +110,11 @@ private:
navdata_magneto_t navdata_magneto;
navdata_wind_speed_t navdata_wind;
navdata_time_t arnavtime;
navdata_unpacked_t navdata_raw;
#define NAVDATA_STRUCTS_HEADER
#include "NavdataMessageDefinitions.h"
#undef NAVDATA_STRUCTS_HEADER
/*
* TF Frames