Full-Speed Navdata & Loop-Rate
Two new parameters have been added: * `fullspeed_navdata`, which controls whether new-style navdata is published when received, or at `looprate` * `looprate`, which controls the speed of the internal ros-loop. Note that legacy navdata is always published at `looprate` Also made various speed improvements to the navdata processing pipeline to enable this faster processing. Also fixed a problem with `enable_legacy_navdata`, whereby it wasn't read correctly and thus had no effect. Also mentioning it here because I forgot to mention it when I added it ages ago. Also updated and commented the launch file to reflect these changes. Regarding testing, I've flown with this for three batteries, with `fullspeed_navdata=true`, `looprate=50`, while rosbagging everything and experienced no unusual behaviour. Conflicts: src/ardrone_driver.h
Esse commit está contido em:
Mike Hamer
@@ -1,6 +1,8 @@
|
||||
<!-- This is a sample lanuch file, please change it based on your needs -->
|
||||
<launch>
|
||||
<node name="ardrone_driver" pkg="ardrone_autonomy" type="ardrone_driver" output="screen" clear_params="true">
|
||||
|
||||
<!-- Modifies the drone's onboard parameters. If not specified, drone default will be used (consult SDK or ardrone_autonomy wiki) -->
|
||||
<param name="outdoor" value="1" />
|
||||
<param name="flight_without_shell" value="1" />
|
||||
<param name="navdata_demo" value="0" />
|
||||
@@ -24,6 +26,10 @@
|
||||
<param name="do_imu_caliberation" value="false" />
|
||||
<param name="tf_prefix" value="mydrone" />
|
||||
|
||||
<!-- Enables the standard /ardrone/navdata, imu and mag topics. If not specified, defaults to TRUE -->
|
||||
<param name="enable_legacy_navdata" value="true" />
|
||||
|
||||
<!-- Enables the new-style, full information navdata packets. If not specified, defaults to FALSE -->
|
||||
<param name="enable_navdata_demo" value="true" />
|
||||
<param name="enable_navdata_time" value="true" />
|
||||
<param name="enable_navdata_raw_measures" value="true" />
|
||||
@@ -53,7 +59,14 @@
|
||||
<param name="enable_navdata_wifi" value="true" />
|
||||
<param name="enable_navdata_zimmu_3000" value="true" />
|
||||
|
||||
<!-- Covariance Values (3x3 matrices reshaped to 1x9)-->
|
||||
<!-- Tunes the speed at which the ros loop runs, and thus, the rate at which navdata is published -->
|
||||
<param name="looprate" value="50" />
|
||||
|
||||
<!-- Do we want to publish new-style navdata when received (true), or every time the ros-loop runs (false)? -->
|
||||
<!-- (does not affect legacy navdata, which is always published at ros-loop rate) -->
|
||||
<param name="fullspeed_navdata" value="true" />
|
||||
|
||||
<!-- Covariance Values (3x3 matrices reshaped to 1x9)-->
|
||||
<rosparam param="cov/imu_la">[0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1]</rosparam>
|
||||
<rosparam param="cov/imu_av">[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]</rosparam>
|
||||
<rosparam param="cov/imu_or">[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 100000.0]</rosparam>
|
||||
|
||||
@@ -15,7 +15,11 @@
|
||||
% endfor
|
||||
#endif
|
||||
|
||||
#ifdef NAVDATA_STRUCTS_HEADER
|
||||
#ifdef NAVDATA_STRUCTS_HEADER_PUBLIC
|
||||
void PublishNavdataTypes(navdata_unpacked_t &n);
|
||||
#endif
|
||||
|
||||
#ifdef NAVDATA_STRUCTS_HEADER_PRIVATE
|
||||
% for item in structs:
|
||||
ros::Publisher pub_${item['struct_name']};
|
||||
bool enabled_${item['struct_name']};
|
||||
@@ -25,11 +29,10 @@
|
||||
bool enabled_legacy_navdata;
|
||||
|
||||
bool initialized_navdata_publishers;
|
||||
void PublishNavdataTypes(navdata_unpacked_t n);
|
||||
#endif
|
||||
|
||||
#ifdef NAVDATA_STRUCTS_SOURCE
|
||||
void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t &n)
|
||||
{
|
||||
const ros::Time now = ros::Time::now();
|
||||
|
||||
@@ -37,9 +40,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
{
|
||||
initialized_navdata_publishers = true;
|
||||
|
||||
enabled_legacy_navdata = true;
|
||||
ros::param::get("~enable_${item['struct_name']}", enabled_${item['struct_name']});
|
||||
|
||||
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);
|
||||
@@ -50,9 +51,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
//-------------------------
|
||||
|
||||
% for item in structs:
|
||||
enabled_${item['struct_name']} = false;
|
||||
ros::param::get("~enable_${item['struct_name']}", enabled_${item['struct_name']});
|
||||
|
||||
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);
|
||||
|
||||
@@ -40,7 +40,11 @@
|
||||
#include <ardrone_autonomy/matrix33.h>
|
||||
#endif
|
||||
|
||||
#ifdef NAVDATA_STRUCTS_HEADER
|
||||
#ifdef NAVDATA_STRUCTS_HEADER_PUBLIC
|
||||
void PublishNavdataTypes(navdata_unpacked_t &n);
|
||||
#endif
|
||||
|
||||
#ifdef NAVDATA_STRUCTS_HEADER_PRIVATE
|
||||
ros::Publisher pub_navdata_demo;
|
||||
bool enabled_navdata_demo;
|
||||
ardrone_autonomy::navdata_demo navdata_demo_msg;
|
||||
@@ -129,11 +133,10 @@
|
||||
bool enabled_legacy_navdata;
|
||||
|
||||
bool initialized_navdata_publishers;
|
||||
void PublishNavdataTypes(navdata_unpacked_t n);
|
||||
#endif
|
||||
|
||||
#ifdef NAVDATA_STRUCTS_SOURCE
|
||||
void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t &n)
|
||||
{
|
||||
const ros::Time now = ros::Time::now();
|
||||
|
||||
@@ -141,9 +144,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
{
|
||||
initialized_navdata_publishers = true;
|
||||
|
||||
enabled_legacy_navdata = true;
|
||||
ros::param::get("~enable_navdata_zimmu_3000", enabled_navdata_zimmu_3000);
|
||||
|
||||
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);
|
||||
@@ -153,9 +154,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_demo = false;
|
||||
ros::param::get("~enable_navdata_demo", enabled_navdata_demo);
|
||||
|
||||
ros::param::param("~enable_navdata_demo", enabled_navdata_demo, false);
|
||||
if(enabled_navdata_demo)
|
||||
{
|
||||
pub_navdata_demo = node_handle.advertise<ardrone_autonomy::navdata_demo>("ardrone/navdata_demo", NAVDATA_QUEUE_SIZE);
|
||||
@@ -163,9 +162,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_time = false;
|
||||
ros::param::get("~enable_navdata_time", enabled_navdata_time);
|
||||
|
||||
ros::param::param("~enable_navdata_time", enabled_navdata_time, false);
|
||||
if(enabled_navdata_time)
|
||||
{
|
||||
pub_navdata_time = node_handle.advertise<ardrone_autonomy::navdata_time>("ardrone/navdata_time", NAVDATA_QUEUE_SIZE);
|
||||
@@ -173,9 +170,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_raw_measures = false;
|
||||
ros::param::get("~enable_navdata_raw_measures", enabled_navdata_raw_measures);
|
||||
|
||||
ros::param::param("~enable_navdata_raw_measures", enabled_navdata_raw_measures, false);
|
||||
if(enabled_navdata_raw_measures)
|
||||
{
|
||||
pub_navdata_raw_measures = node_handle.advertise<ardrone_autonomy::navdata_raw_measures>("ardrone/navdata_raw_measures", NAVDATA_QUEUE_SIZE);
|
||||
@@ -183,9 +178,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_phys_measures = false;
|
||||
ros::param::get("~enable_navdata_phys_measures", enabled_navdata_phys_measures);
|
||||
|
||||
ros::param::param("~enable_navdata_phys_measures", enabled_navdata_phys_measures, false);
|
||||
if(enabled_navdata_phys_measures)
|
||||
{
|
||||
pub_navdata_phys_measures = node_handle.advertise<ardrone_autonomy::navdata_phys_measures>("ardrone/navdata_phys_measures", NAVDATA_QUEUE_SIZE);
|
||||
@@ -193,9 +186,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_gyros_offsets = false;
|
||||
ros::param::get("~enable_navdata_gyros_offsets", enabled_navdata_gyros_offsets);
|
||||
|
||||
ros::param::param("~enable_navdata_gyros_offsets", enabled_navdata_gyros_offsets, false);
|
||||
if(enabled_navdata_gyros_offsets)
|
||||
{
|
||||
pub_navdata_gyros_offsets = node_handle.advertise<ardrone_autonomy::navdata_gyros_offsets>("ardrone/navdata_gyros_offsets", NAVDATA_QUEUE_SIZE);
|
||||
@@ -203,9 +194,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_euler_angles = false;
|
||||
ros::param::get("~enable_navdata_euler_angles", enabled_navdata_euler_angles);
|
||||
|
||||
ros::param::param("~enable_navdata_euler_angles", enabled_navdata_euler_angles, false);
|
||||
if(enabled_navdata_euler_angles)
|
||||
{
|
||||
pub_navdata_euler_angles = node_handle.advertise<ardrone_autonomy::navdata_euler_angles>("ardrone/navdata_euler_angles", NAVDATA_QUEUE_SIZE);
|
||||
@@ -213,9 +202,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_references = false;
|
||||
ros::param::get("~enable_navdata_references", enabled_navdata_references);
|
||||
|
||||
ros::param::param("~enable_navdata_references", enabled_navdata_references, false);
|
||||
if(enabled_navdata_references)
|
||||
{
|
||||
pub_navdata_references = node_handle.advertise<ardrone_autonomy::navdata_references>("ardrone/navdata_references", NAVDATA_QUEUE_SIZE);
|
||||
@@ -223,9 +210,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_trims = false;
|
||||
ros::param::get("~enable_navdata_trims", enabled_navdata_trims);
|
||||
|
||||
ros::param::param("~enable_navdata_trims", enabled_navdata_trims, false);
|
||||
if(enabled_navdata_trims)
|
||||
{
|
||||
pub_navdata_trims = node_handle.advertise<ardrone_autonomy::navdata_trims>("ardrone/navdata_trims", NAVDATA_QUEUE_SIZE);
|
||||
@@ -233,9 +218,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_rc_references = false;
|
||||
ros::param::get("~enable_navdata_rc_references", enabled_navdata_rc_references);
|
||||
|
||||
ros::param::param("~enable_navdata_rc_references", enabled_navdata_rc_references, false);
|
||||
if(enabled_navdata_rc_references)
|
||||
{
|
||||
pub_navdata_rc_references = node_handle.advertise<ardrone_autonomy::navdata_rc_references>("ardrone/navdata_rc_references", NAVDATA_QUEUE_SIZE);
|
||||
@@ -243,9 +226,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_pwm = false;
|
||||
ros::param::get("~enable_navdata_pwm", enabled_navdata_pwm);
|
||||
|
||||
ros::param::param("~enable_navdata_pwm", enabled_navdata_pwm, false);
|
||||
if(enabled_navdata_pwm)
|
||||
{
|
||||
pub_navdata_pwm = node_handle.advertise<ardrone_autonomy::navdata_pwm>("ardrone/navdata_pwm", NAVDATA_QUEUE_SIZE);
|
||||
@@ -253,9 +234,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_altitude = false;
|
||||
ros::param::get("~enable_navdata_altitude", enabled_navdata_altitude);
|
||||
|
||||
ros::param::param("~enable_navdata_altitude", enabled_navdata_altitude, false);
|
||||
if(enabled_navdata_altitude)
|
||||
{
|
||||
pub_navdata_altitude = node_handle.advertise<ardrone_autonomy::navdata_altitude>("ardrone/navdata_altitude", NAVDATA_QUEUE_SIZE);
|
||||
@@ -263,9 +242,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_vision_raw = false;
|
||||
ros::param::get("~enable_navdata_vision_raw", enabled_navdata_vision_raw);
|
||||
|
||||
ros::param::param("~enable_navdata_vision_raw", enabled_navdata_vision_raw, false);
|
||||
if(enabled_navdata_vision_raw)
|
||||
{
|
||||
pub_navdata_vision_raw = node_handle.advertise<ardrone_autonomy::navdata_vision_raw>("ardrone/navdata_vision_raw", NAVDATA_QUEUE_SIZE);
|
||||
@@ -273,9 +250,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_vision_of = false;
|
||||
ros::param::get("~enable_navdata_vision_of", enabled_navdata_vision_of);
|
||||
|
||||
ros::param::param("~enable_navdata_vision_of", enabled_navdata_vision_of, false);
|
||||
if(enabled_navdata_vision_of)
|
||||
{
|
||||
pub_navdata_vision_of = node_handle.advertise<ardrone_autonomy::navdata_vision_of>("ardrone/navdata_vision_of", NAVDATA_QUEUE_SIZE);
|
||||
@@ -283,9 +258,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_vision = false;
|
||||
ros::param::get("~enable_navdata_vision", enabled_navdata_vision);
|
||||
|
||||
ros::param::param("~enable_navdata_vision", enabled_navdata_vision, false);
|
||||
if(enabled_navdata_vision)
|
||||
{
|
||||
pub_navdata_vision = node_handle.advertise<ardrone_autonomy::navdata_vision>("ardrone/navdata_vision", NAVDATA_QUEUE_SIZE);
|
||||
@@ -293,9 +266,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_vision_perf = false;
|
||||
ros::param::get("~enable_navdata_vision_perf", enabled_navdata_vision_perf);
|
||||
|
||||
ros::param::param("~enable_navdata_vision_perf", enabled_navdata_vision_perf, false);
|
||||
if(enabled_navdata_vision_perf)
|
||||
{
|
||||
pub_navdata_vision_perf = node_handle.advertise<ardrone_autonomy::navdata_vision_perf>("ardrone/navdata_vision_perf", NAVDATA_QUEUE_SIZE);
|
||||
@@ -303,9 +274,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_trackers_send = false;
|
||||
ros::param::get("~enable_navdata_trackers_send", enabled_navdata_trackers_send);
|
||||
|
||||
ros::param::param("~enable_navdata_trackers_send", enabled_navdata_trackers_send, false);
|
||||
if(enabled_navdata_trackers_send)
|
||||
{
|
||||
pub_navdata_trackers_send = node_handle.advertise<ardrone_autonomy::navdata_trackers_send>("ardrone/navdata_trackers_send", NAVDATA_QUEUE_SIZE);
|
||||
@@ -313,9 +282,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_vision_detect = false;
|
||||
ros::param::get("~enable_navdata_vision_detect", enabled_navdata_vision_detect);
|
||||
|
||||
ros::param::param("~enable_navdata_vision_detect", enabled_navdata_vision_detect, false);
|
||||
if(enabled_navdata_vision_detect)
|
||||
{
|
||||
pub_navdata_vision_detect = node_handle.advertise<ardrone_autonomy::navdata_vision_detect>("ardrone/navdata_vision_detect", NAVDATA_QUEUE_SIZE);
|
||||
@@ -323,9 +290,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_watchdog = false;
|
||||
ros::param::get("~enable_navdata_watchdog", enabled_navdata_watchdog);
|
||||
|
||||
ros::param::param("~enable_navdata_watchdog", enabled_navdata_watchdog, false);
|
||||
if(enabled_navdata_watchdog)
|
||||
{
|
||||
pub_navdata_watchdog = node_handle.advertise<ardrone_autonomy::navdata_watchdog>("ardrone/navdata_watchdog", NAVDATA_QUEUE_SIZE);
|
||||
@@ -333,9 +298,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_adc_data_frame = false;
|
||||
ros::param::get("~enable_navdata_adc_data_frame", enabled_navdata_adc_data_frame);
|
||||
|
||||
ros::param::param("~enable_navdata_adc_data_frame", enabled_navdata_adc_data_frame, false);
|
||||
if(enabled_navdata_adc_data_frame)
|
||||
{
|
||||
pub_navdata_adc_data_frame = node_handle.advertise<ardrone_autonomy::navdata_adc_data_frame>("ardrone/navdata_adc_data_frame", NAVDATA_QUEUE_SIZE);
|
||||
@@ -343,9 +306,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_video_stream = false;
|
||||
ros::param::get("~enable_navdata_video_stream", enabled_navdata_video_stream);
|
||||
|
||||
ros::param::param("~enable_navdata_video_stream", enabled_navdata_video_stream, false);
|
||||
if(enabled_navdata_video_stream)
|
||||
{
|
||||
pub_navdata_video_stream = node_handle.advertise<ardrone_autonomy::navdata_video_stream>("ardrone/navdata_video_stream", NAVDATA_QUEUE_SIZE);
|
||||
@@ -353,9 +314,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_games = false;
|
||||
ros::param::get("~enable_navdata_games", enabled_navdata_games);
|
||||
|
||||
ros::param::param("~enable_navdata_games", enabled_navdata_games, false);
|
||||
if(enabled_navdata_games)
|
||||
{
|
||||
pub_navdata_games = node_handle.advertise<ardrone_autonomy::navdata_games>("ardrone/navdata_games", NAVDATA_QUEUE_SIZE);
|
||||
@@ -363,9 +322,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_pressure_raw = false;
|
||||
ros::param::get("~enable_navdata_pressure_raw", enabled_navdata_pressure_raw);
|
||||
|
||||
ros::param::param("~enable_navdata_pressure_raw", enabled_navdata_pressure_raw, false);
|
||||
if(enabled_navdata_pressure_raw)
|
||||
{
|
||||
pub_navdata_pressure_raw = node_handle.advertise<ardrone_autonomy::navdata_pressure_raw>("ardrone/navdata_pressure_raw", NAVDATA_QUEUE_SIZE);
|
||||
@@ -373,9 +330,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_magneto = false;
|
||||
ros::param::get("~enable_navdata_magneto", enabled_navdata_magneto);
|
||||
|
||||
ros::param::param("~enable_navdata_magneto", enabled_navdata_magneto, false);
|
||||
if(enabled_navdata_magneto)
|
||||
{
|
||||
pub_navdata_magneto = node_handle.advertise<ardrone_autonomy::navdata_magneto>("ardrone/navdata_magneto", NAVDATA_QUEUE_SIZE);
|
||||
@@ -383,9 +338,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_wind_speed = false;
|
||||
ros::param::get("~enable_navdata_wind_speed", enabled_navdata_wind_speed);
|
||||
|
||||
ros::param::param("~enable_navdata_wind_speed", enabled_navdata_wind_speed, false);
|
||||
if(enabled_navdata_wind_speed)
|
||||
{
|
||||
pub_navdata_wind_speed = node_handle.advertise<ardrone_autonomy::navdata_wind_speed>("ardrone/navdata_wind_speed", NAVDATA_QUEUE_SIZE);
|
||||
@@ -393,9 +346,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_kalman_pressure = false;
|
||||
ros::param::get("~enable_navdata_kalman_pressure", enabled_navdata_kalman_pressure);
|
||||
|
||||
ros::param::param("~enable_navdata_kalman_pressure", enabled_navdata_kalman_pressure, false);
|
||||
if(enabled_navdata_kalman_pressure)
|
||||
{
|
||||
pub_navdata_kalman_pressure = node_handle.advertise<ardrone_autonomy::navdata_kalman_pressure>("ardrone/navdata_kalman_pressure", NAVDATA_QUEUE_SIZE);
|
||||
@@ -403,9 +354,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_hdvideo_stream = false;
|
||||
ros::param::get("~enable_navdata_hdvideo_stream", enabled_navdata_hdvideo_stream);
|
||||
|
||||
ros::param::param("~enable_navdata_hdvideo_stream", enabled_navdata_hdvideo_stream, false);
|
||||
if(enabled_navdata_hdvideo_stream)
|
||||
{
|
||||
pub_navdata_hdvideo_stream = node_handle.advertise<ardrone_autonomy::navdata_hdvideo_stream>("ardrone/navdata_hdvideo_stream", NAVDATA_QUEUE_SIZE);
|
||||
@@ -413,9 +362,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_wifi = false;
|
||||
ros::param::get("~enable_navdata_wifi", enabled_navdata_wifi);
|
||||
|
||||
ros::param::param("~enable_navdata_wifi", enabled_navdata_wifi, false);
|
||||
if(enabled_navdata_wifi)
|
||||
{
|
||||
pub_navdata_wifi = node_handle.advertise<ardrone_autonomy::navdata_wifi>("ardrone/navdata_wifi", NAVDATA_QUEUE_SIZE);
|
||||
@@ -423,9 +370,7 @@ void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n)
|
||||
|
||||
//-------------------------
|
||||
|
||||
enabled_navdata_zimmu_3000 = false;
|
||||
ros::param::get("~enable_navdata_zimmu_3000", enabled_navdata_zimmu_3000);
|
||||
|
||||
ros::param::param("~enable_navdata_zimmu_3000", enabled_navdata_zimmu_3000, false);
|
||||
if(enabled_navdata_zimmu_3000)
|
||||
{
|
||||
pub_navdata_zimmu_3000 = node_handle.advertise<ardrone_autonomy::navdata_zimmu_3000>("ardrone/navdata_zimmu_3000", NAVDATA_QUEUE_SIZE);
|
||||
|
||||
+55
-62
@@ -114,7 +114,7 @@ ARDroneDriver::~ARDroneDriver()
|
||||
|
||||
void ARDroneDriver::run()
|
||||
{
|
||||
ros::Rate loop_rate(50);
|
||||
ros::Rate loop_rate(looprate);
|
||||
ros::Time startTime = ros::Time::now();
|
||||
static int freq_dev = 0;
|
||||
while (node_handle.ok())
|
||||
@@ -133,7 +133,7 @@ void ARDroneDriver::run()
|
||||
ardrone_control_config.ardrone_name,
|
||||
(IS_ARDRONE1) ? 1 : 2,
|
||||
ardrone_control_config.num_version_soft,
|
||||
shared_navdata.vbat_flying_percentage);
|
||||
shared_raw_navdata.navdata_demo.vbat_flying_percentage);
|
||||
vp_os_mutex_unlock(&navdata_lock);
|
||||
if (ardrone_control_config.num_version_soft[0] == '0')
|
||||
{
|
||||
@@ -559,30 +559,20 @@ 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;
|
||||
arnavtime = shared_arnavtime;
|
||||
if (IS_ARDRONE2)
|
||||
{ // This is neccessary
|
||||
navdata_pressure = shared_navdata_pressure;
|
||||
navdata_magneto = shared_navdata_magneto;
|
||||
navdata_wind = shared_navdata_wind;
|
||||
}
|
||||
vp_os_mutex_unlock(&navdata_lock);
|
||||
|
||||
|
||||
if ((do_caliberation) && (!caliberated))
|
||||
{
|
||||
acc_samples[0].push_back(navdata_phys.phys_accs[ACC_X]);
|
||||
acc_samples[1].push_back(navdata_phys.phys_accs[ACC_Y]);
|
||||
acc_samples[2].push_back(navdata_phys.phys_accs[ACC_Z]);
|
||||
gyro_samples[0].push_back(navdata_phys.phys_gyros[GYRO_X]);
|
||||
gyro_samples[1].push_back(navdata_phys.phys_gyros[GYRO_Y]);
|
||||
gyro_samples[2].push_back(navdata_phys.phys_gyros[GYRO_Z]);
|
||||
vel_samples[0].push_back(navdata.vx);
|
||||
vel_samples[1].push_back(navdata.vy);
|
||||
vel_samples[2].push_back(navdata.vz);
|
||||
acc_samples[0].push_back(navdata_raw.navdata_phys_measures.phys_accs[ACC_X]);
|
||||
acc_samples[1].push_back(navdata_raw.navdata_phys_measures.phys_accs[ACC_Y]);
|
||||
acc_samples[2].push_back(navdata_raw.navdata_phys_measures.phys_accs[ACC_Z]);
|
||||
gyro_samples[0].push_back(navdata_raw.navdata_phys_measures.phys_gyros[GYRO_X]);
|
||||
gyro_samples[1].push_back(navdata_raw.navdata_phys_measures.phys_gyros[GYRO_Y]);
|
||||
gyro_samples[2].push_back(navdata_raw.navdata_phys_measures.phys_gyros[GYRO_Z]);
|
||||
vel_samples[0].push_back(navdata_raw.navdata_demo.vx);
|
||||
vel_samples[1].push_back(navdata_raw.navdata_demo.vy);
|
||||
vel_samples[2].push_back(navdata_raw.navdata_demo.vz);
|
||||
if (acc_samples[0].size() == max_num_samples)
|
||||
{
|
||||
for (int j = 0; j < 3; j++)
|
||||
@@ -594,7 +584,7 @@ void ARDroneDriver::publish_navdata()
|
||||
ROS_INFO("Bias in linear acceleration (mg): [%4.4lf, %4.4lf, %4.4lf]", acc_bias[0], acc_bias[1], acc_bias[2]);
|
||||
ROS_INFO("Bias in angular velocity (deg/s): [%4.4lf, %4.4lf, %4.4lf]", gyro_bias[0], gyro_bias[1], gyro_bias[2]);
|
||||
ROS_INFO("Bias in linear velocity (mm/s): [%4.4lf, %4.4lf, %4.4lf]", vel_bias[0], vel_bias[1], vel_bias[2]);
|
||||
ROS_INFO("Above values (except z-axis accel) will be substracted from actual IMU data in `navdata` and `imu` topic.");
|
||||
ROS_INFO("Above values (except z-axis accel) will be substracted from actual IMU data in `navdata_raw.navdata_demo` and `imu` topic.");
|
||||
ROS_INFO("This feature can be disabled using `do_imu_caliberation` parameter. Recaliberate using `imu_recalib` service.");
|
||||
caliberated = true;
|
||||
}
|
||||
@@ -603,18 +593,21 @@ void ARDroneDriver::publish_navdata()
|
||||
{
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
if (j != 2) navdata_phys.phys_accs[j] -= acc_bias[j];
|
||||
navdata_phys.phys_gyros[j] -= gyro_bias[j];
|
||||
if (j != 2) navdata_raw.navdata_phys_measures.phys_accs[j] -= acc_bias[j];
|
||||
navdata_raw.navdata_phys_measures.phys_gyros[j] -= gyro_bias[j];
|
||||
}
|
||||
navdata.vx -= vel_bias[0];
|
||||
navdata.vy -= vel_bias[1];
|
||||
navdata.vz -= vel_bias[2];
|
||||
navdata_raw.navdata_demo.vx -= vel_bias[0];
|
||||
navdata_raw.navdata_demo.vy -= vel_bias[1];
|
||||
navdata_raw.navdata_demo.vz -= vel_bias[2];
|
||||
|
||||
}
|
||||
|
||||
PublishNavdataTypes(navdata_raw); // This is defined in the template NavdataMessageDefinitions.h template file
|
||||
if(!fullspeed_navdata) // only transmit this data in the loop if we're transmitting at loop speed, rather than full speed
|
||||
{
|
||||
PublishNavdataTypes(navdata_raw); // This function is defined in the template NavdataMessageDefinitions.h template file
|
||||
}
|
||||
|
||||
if (!enabled_legacy_navdata || (navdata_pub.getNumSubscribers() == 0) && (imu_pub.getNumSubscribers() == 0) && (mag_pub.getNumSubscribers() == 0))
|
||||
if (!enabled_legacy_navdata || ((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();
|
||||
|
||||
@@ -622,37 +615,37 @@ void ARDroneDriver::publish_navdata()
|
||||
|
||||
msg.header.stamp = _now;
|
||||
msg.header.frame_id = droneFrameBase;
|
||||
msg.batteryPercent = navdata.vbat_flying_percentage;
|
||||
msg.state = (navdata.ctrl_state >> 16);
|
||||
msg.batteryPercent = navdata_raw.navdata_demo.vbat_flying_percentage;
|
||||
msg.state = (navdata_raw.navdata_demo.ctrl_state >> 16);
|
||||
|
||||
// positive means counterclockwise rotation around axis
|
||||
msg.rotX = navdata.phi / 1000.0; // tilt left/right
|
||||
msg.rotY = -navdata.theta / 1000.0; // tilt forward/backward
|
||||
msg.rotZ = -navdata.psi / 1000.0; // orientation
|
||||
msg.rotX = navdata_raw.navdata_demo.phi / 1000.0; // tilt left/right
|
||||
msg.rotY = -navdata_raw.navdata_demo.theta / 1000.0; // tilt forward/backward
|
||||
msg.rotZ = -navdata_raw.navdata_demo.psi / 1000.0; // orientation
|
||||
|
||||
msg.altd = navdata.altitude; // cm
|
||||
msg.vx = navdata.vx; // mm/sec
|
||||
msg.vy = -navdata.vy; // mm/sec
|
||||
msg.vz = -navdata.vz; // mm/sec
|
||||
msg.tm = arnavtime.time;
|
||||
msg.ax = navdata_phys.phys_accs[ACC_X] / 1000.0; // g
|
||||
msg.ay = -navdata_phys.phys_accs[ACC_Y] / 1000.0; // g
|
||||
msg.az = -navdata_phys.phys_accs[ACC_Z] / 1000.0; // g
|
||||
msg.altd = navdata_raw.navdata_demo.altitude; // cm
|
||||
msg.vx = navdata_raw.navdata_demo.vx; // mm/sec
|
||||
msg.vy = -navdata_raw.navdata_demo.vy; // mm/sec
|
||||
msg.vz = -navdata_raw.navdata_demo.vz; // mm/sec
|
||||
msg.tm = navdata_raw.navdata_time.time;
|
||||
msg.ax = navdata_raw.navdata_phys_measures.phys_accs[ACC_X] / 1000.0; // g
|
||||
msg.ay = -navdata_raw.navdata_phys_measures.phys_accs[ACC_Y] / 1000.0; // g
|
||||
msg.az = -navdata_raw.navdata_phys_measures.phys_accs[ACC_Z] / 1000.0; // g
|
||||
|
||||
// New stuff
|
||||
|
||||
if (IS_ARDRONE2)
|
||||
{
|
||||
msg.magX = (int32_t)navdata_magneto.mx;
|
||||
msg.magY = (int32_t)navdata_magneto.my;
|
||||
msg.magZ = (int32_t)navdata_magneto.mz;
|
||||
msg.magX = (int32_t)navdata_raw.navdata_magneto.mx;
|
||||
msg.magY = (int32_t)navdata_raw.navdata_magneto.my;
|
||||
msg.magZ = (int32_t)navdata_raw.navdata_magneto.mz;
|
||||
|
||||
msg.pressure = navdata_pressure.Pression_meas; // typo in the SDK!
|
||||
msg.temp = navdata_pressure.Temperature_meas;
|
||||
msg.pressure = navdata_raw.navdata_pressure_raw.Pression_meas; // typo in the SDK!
|
||||
msg.temp = navdata_raw.navdata_pressure_raw.Temperature_meas;
|
||||
|
||||
msg.wind_speed = navdata_wind.wind_speed;
|
||||
msg.wind_angle = navdata_wind.wind_angle;
|
||||
msg.wind_comp_angle = navdata_wind.wind_compensation_phi;
|
||||
msg.wind_speed = navdata_raw.navdata_wind_speed.wind_speed;
|
||||
msg.wind_angle = navdata_raw.navdata_wind_speed.wind_angle;
|
||||
msg.wind_comp_angle = navdata_raw.navdata_wind_speed.wind_compensation_phi;
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -665,8 +658,8 @@ void ARDroneDriver::publish_navdata()
|
||||
}
|
||||
|
||||
// Tag Detection
|
||||
msg.tags_count = navdata_detect.nb_detected;
|
||||
for (int i = 0; i < navdata_detect.nb_detected; i++)
|
||||
msg.tags_count = navdata_raw.navdata_vision_detect.nb_detected;
|
||||
for (int i = 0; i < navdata_raw.navdata_vision_detect.nb_detected; i++)
|
||||
{
|
||||
/*
|
||||
* The tags_type is in raw format. In order to extract the information
|
||||
@@ -679,13 +672,13 @@ void ARDroneDriver::publish_navdata()
|
||||
* Please also note that the xc, yc, width and height are in [0,1000] range
|
||||
* and must get converted back based on image resolution.
|
||||
*/
|
||||
msg.tags_type.push_back(navdata_detect.type[i]);
|
||||
msg.tags_xc.push_back(navdata_detect.xc[i]);
|
||||
msg.tags_yc.push_back(navdata_detect.yc[i]);
|
||||
msg.tags_width.push_back(navdata_detect.width[i]);
|
||||
msg.tags_height.push_back(navdata_detect.height[i]);
|
||||
msg.tags_orientation.push_back(navdata_detect.orientation_angle[i]);
|
||||
msg.tags_distance.push_back(navdata_detect.dist[i]);
|
||||
msg.tags_type.push_back(navdata_raw.navdata_vision_detect.type[i]);
|
||||
msg.tags_xc.push_back(navdata_raw.navdata_vision_detect.xc[i]);
|
||||
msg.tags_yc.push_back(navdata_raw.navdata_vision_detect.yc[i]);
|
||||
msg.tags_width.push_back(navdata_raw.navdata_vision_detect.width[i]);
|
||||
msg.tags_height.push_back(navdata_raw.navdata_vision_detect.height[i]);
|
||||
msg.tags_orientation.push_back(navdata_raw.navdata_vision_detect.orientation_angle[i]);
|
||||
msg.tags_distance.push_back(navdata_raw.navdata_vision_detect.dist[i]);
|
||||
}
|
||||
|
||||
/* IMU */
|
||||
@@ -704,9 +697,9 @@ void ARDroneDriver::publish_navdata()
|
||||
|
||||
// IMU - Gyro (Gyro is being sent in deg/sec)
|
||||
// TODO: Should Gyro be added to Navdata?
|
||||
imu_msg.angular_velocity.x = navdata_phys.phys_gyros[GYRO_X] * DEG_TO_RAD;
|
||||
imu_msg.angular_velocity.y = -navdata_phys.phys_gyros[GYRO_Y] * DEG_TO_RAD;
|
||||
imu_msg.angular_velocity.z = -navdata_phys.phys_gyros[GYRO_Z] * DEG_TO_RAD;
|
||||
imu_msg.angular_velocity.x = navdata_raw.navdata_phys_measures.phys_gyros[GYRO_X] * DEG_TO_RAD;
|
||||
imu_msg.angular_velocity.y = -navdata_raw.navdata_phys_measures.phys_gyros[GYRO_Y] * DEG_TO_RAD;
|
||||
imu_msg.angular_velocity.z = -navdata_raw.navdata_phys_measures.phys_gyros[GYRO_Z] * DEG_TO_RAD;
|
||||
|
||||
mag_msg.header.frame_id = droneFrameBase;
|
||||
mag_msg.header.stamp = _now;
|
||||
|
||||
@@ -48,6 +48,10 @@ public:
|
||||
double getRosParam(char* param, double defaultVal);
|
||||
bool imuReCalibCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response &response);
|
||||
|
||||
#define NAVDATA_STRUCTS_HEADER_PUBLIC
|
||||
#include "NavdataMessageDefinitions.h"
|
||||
#undef NAVDATA_STRUCTS_HEADER_PUBLIC
|
||||
|
||||
private:
|
||||
void publish_video();
|
||||
void publish_navdata();
|
||||
@@ -104,19 +108,12 @@ private:
|
||||
std::string droneFrameId;
|
||||
|
||||
// Navdata copy
|
||||
navdata_demo_t navdata;
|
||||
navdata_phys_measures_t navdata_phys;
|
||||
navdata_vision_detect_t navdata_detect;
|
||||
navdata_pressure_raw_t navdata_pressure;
|
||||
navdata_magneto_t navdata_magneto;
|
||||
navdata_wind_speed_t navdata_wind;
|
||||
navdata_time_t arnavtime;
|
||||
navdata_unpacked_t navdata_raw;
|
||||
|
||||
// Load auto-generated declarations for full navdata
|
||||
#define NAVDATA_STRUCTS_HEADER
|
||||
#define NAVDATA_STRUCTS_HEADER_PRIVATE
|
||||
#include "NavdataMessageDefinitions.h"
|
||||
#undef NAVDATA_STRUCTS_HEADER
|
||||
#undef NAVDATA_STRUCTS_HEADER_PRIVATE
|
||||
|
||||
/*
|
||||
* TF Frames
|
||||
|
||||
+9
-17
@@ -3,13 +3,6 @@
|
||||
#include "teleop_twist.h"
|
||||
#include "ardrone_driver.h"
|
||||
|
||||
navdata_demo_t shared_navdata;
|
||||
navdata_phys_measures_t shared_navdata_phys;
|
||||
navdata_vision_detect_t shared_navdata_detect;
|
||||
navdata_pressure_raw_t shared_navdata_pressure;
|
||||
navdata_magneto_t shared_navdata_magneto;
|
||||
navdata_wind_speed_t shared_navdata_wind;
|
||||
navdata_time_t shared_arnavtime;
|
||||
navdata_unpacked_t shared_raw_navdata;
|
||||
|
||||
vp_os_mutex_t navdata_lock;
|
||||
@@ -20,6 +13,9 @@ long int current_navdata_id = 0;
|
||||
|
||||
ARDroneDriver* rosDriver;
|
||||
|
||||
int32_t looprate;
|
||||
bool fullspeed_navdata;
|
||||
|
||||
int32_t should_exit;
|
||||
|
||||
extern "C" {
|
||||
@@ -62,6 +58,8 @@ extern "C" {
|
||||
printf("Something must be really wrong with the SDK!");
|
||||
}
|
||||
|
||||
ros::param::param("~looprate",looprate,50);
|
||||
ros::param::param("~fullspeed_navdata",fullspeed_navdata,false);
|
||||
|
||||
// SET SOME NON-STANDARD DEFAULT VALUES FOR THE DRIVER
|
||||
// THESE CAN BE OVERWRITTEN BY ROS PARAMETERS (below)
|
||||
@@ -227,18 +225,12 @@ extern "C" {
|
||||
C_RESULT navdata_custom_process(const navdata_unpacked_t * const pnd) {
|
||||
vp_os_mutex_lock(&navdata_lock);
|
||||
shared_raw_navdata = *pnd;
|
||||
shared_navdata_detect = pnd->navdata_vision_detect;
|
||||
shared_navdata_phys = pnd->navdata_phys_measures;
|
||||
shared_navdata = pnd->navdata_demo;
|
||||
shared_arnavtime = pnd->navdata_time;
|
||||
if (IS_ARDRONE2)
|
||||
{ // This is neccessary
|
||||
shared_navdata_pressure = pnd->navdata_pressure_raw;
|
||||
shared_navdata_magneto = pnd->navdata_magneto;
|
||||
shared_navdata_wind = pnd->navdata_wind_speed;
|
||||
if(fullspeed_navdata)
|
||||
{
|
||||
rosDriver->PublishNavdataTypes(shared_raw_navdata); //if we're publishing navdata at full speed, publish!
|
||||
}
|
||||
vp_os_mutex_unlock(&navdata_lock);
|
||||
current_navdata_id++;
|
||||
vp_os_mutex_unlock(&navdata_lock);
|
||||
return C_OK;
|
||||
}
|
||||
|
||||
|
||||
+3
-7
@@ -45,19 +45,15 @@ extern video_decoder_config_t vec;
|
||||
|
||||
|
||||
#define NB_DRIVER_POST_STAGES 10
|
||||
extern navdata_vision_detect_t shared_navdata_detect;
|
||||
extern navdata_phys_measures_t shared_navdata_phys;
|
||||
extern navdata_demo_t shared_navdata;
|
||||
extern navdata_time_t shared_arnavtime;
|
||||
extern navdata_pressure_raw_t shared_navdata_pressure;
|
||||
extern navdata_magneto_t shared_navdata_magneto;
|
||||
extern navdata_wind_speed_t shared_navdata_wind;
|
||||
extern navdata_unpacked_t shared_raw_navdata;
|
||||
|
||||
extern vp_os_mutex_t navdata_lock;
|
||||
extern vp_os_mutex_t video_lock;
|
||||
extern vp_os_mutex_t twist_lock;
|
||||
|
||||
extern int32_t looprate;
|
||||
extern bool fullspeed_navdata;
|
||||
|
||||
extern int32_t should_exit;
|
||||
|
||||
#endif
|
||||
|
||||
Referência em uma Nova Issue
Bloquear um usuário