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

Merging Navdata2 to Navdata. Now Navdata msg contains Magnetometer, Pressure, Temperature and Wind sensory information.

Ver Código-Fonte
Esse commit está contido em:
Mani Monajjemi
2012-08-01 15:45:28 -07:00
pai d0f3502d94
commit e7233dcfce
5 arquivos alterados com 50 adições e 134 exclusões
Baixar Arquivo de Patch Baixar Arquivo de Diff Expandir todos os arquivos Colapsar todos os arquivos
msg/Navdata.msg
+18 -1
Ver Arquivo
@@ -1,5 +1,8 @@
Header header
# Navdata including the ARDrone 2 specifica sensors
# (magnetometer, barometer)
# 0 means no battery, 100 means full battery
float32 batteryPercent
@@ -8,6 +11,21 @@ float32 batteryPercent
# Note: 3,7 seems to discriminate type of flying (isFly = 3 | 7)
uint32 state
int32 magX
int32 magY
int32 magZ
# pressure sensor
int32 pressure
# apparently, there was a temperature sensor added as well.
int32 temp
# wind sensing...
float32 wind_speed
float32 wind_angle
float32 wind_comp_angle
# left/right tilt in degrees (rotation about the X axis)
float32 rotX
@@ -46,4 +64,3 @@ float32[] tags_distance
#time stamp
float32 tm
msg/Navdata2.msg
-66
Ver Arquivo
@@ -1,66 +0,0 @@
Header header
# Navdata including the ARDrone 2 specifica sensors
# (magnetometer, barometer)
# 0 means no battery, 100 means full battery
float32 batteryPercent
# 0: Unknown, 1: Init, 2: Landed, 3: Flying, 4: Hovering, 5: Test
# 6: Taking off, 7: Goto Fix Point, 8: Landing, 9: Looping
# Note: 3,7 seems to discriminate type of flying (isFly = 3 | 7)
uint32 state
int32 magX
int32 magY
int32 magZ
# pressure sensor
int32 pressure
# apparently, there was a temperature sensor added as well.
int32 temp
# wind sensing...
float32 wind_speed
float32 wind_angle
float32 wind_comp_angle
# left/right tilt in degrees (rotation about the X axis)
float32 rotX
# forward/backward tilt in degrees (rotation about the Y axis)
float32 rotY
# orientation in degrees (rotation about the Z axis)
float32 rotZ
# estimated altitude (cm)
int32 altd
# linear velocity (mm/sec)
float32 vx
# linear velocity (mm/sec)
float32 vy
# linear velocity (mm/sec)
float32 vz
#linear accelerations (unit: g)
float32 ax
float32 ay
float32 az
#Tags in Vision Detectoion
uint32 tags_count
uint32[] tags_type
uint32[] tags_xc
uint32[] tags_yc
uint32[] tags_width
uint32[] tags_height
float32[] tags_orientation
float32[] tags_distance
#time stamp
float32 tm
src/ardrone_driver.cpp
+25 -60
Ver Arquivo
@@ -19,8 +19,6 @@ ARDroneDriver::ARDroneDriver()
hori_pub = image_transport.advertiseCamera("ardrone/front/image_raw", 10);
vert_pub = image_transport.advertiseCamera("ardrone/bottom/image_raw", 10);
navdata_pub = node_handle.advertise<ardrone_autonomy::Navdata>("ardrone/navdata", 10);
if (IS_ARDRONE2)
navdata2_pub = node_handle.advertise<ardrone_autonomy::Navdata2>("ardrone/navdata2", 10);
toggleCam_service = node_handle.advertiseService("ardrone/togglecam", toggleCamCallback);
toggleNavdataDemo_service = node_handle.advertiseService("ardrone/togglenavdatademo", toggleNavdataDemoCallback);
setCamChannel_service = node_handle.advertiseService("ardrone/setcamchannel",setCamChannelCallback );
@@ -43,8 +41,6 @@ void ARDroneDriver::run()
{
publish_video();
publish_navdata();
if (IS_ARDRONE2)
publish_navdata2();
last_frame_id = current_frame_id;
}
ros::spinOnce();
@@ -299,6 +295,31 @@ void ARDroneDriver::publish_navdata()
msg.ay = -navdata_phys.phys_accs[ACC_Y] / 1000.0; // g
msg.az = -navdata_phys.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.pressure = navdata_pressure.Pression_meas; // typo in the SDK!
msg.temp = navdata_pressure.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;
}
else
{
msg.magX = msg.magY = msg.magZ = 0;
msg.pressure = 0.0;
msg.temp = 0.0;
msg.wind_speed = 0.0;
msg.wind_angle = 0.0;
msg.wind_comp_angle = 0.0;
}
// Tag Detection
msg.tags_count = navdata_detect.nb_detected;
for (int i = 0; i < navdata_detect.nb_detected; i++)
@@ -322,66 +343,10 @@ void ARDroneDriver::publish_navdata()
msg.tags_orientation.push_back(navdata_detect.orientation_angle[i]);
msg.tags_distance.push_back(navdata_detect.dist[i]);
}
// TODO: Ideally we would be able to figure out whether we are in an emergency state
// using the navdata.ctrl_state bitfield with the ARDRONE_EMERGENCY_MASK flag, but
// it seems to always be 0. The emergency state seems to be correlated with the
// inverse of the ARDRONE_TIMER_ELAPSED flag, but that just makes so little sense
// that I don't want to use it because it's probably wrong. So we'll just use a
// manual reset for now.
navdata_pub.publish(msg);
}
void ARDroneDriver::publish_navdata2()
{
if (navdata2_pub.getNumSubscribers() == 0)
return; // why bother, no one is listening.
ardrone_autonomy::Navdata2 msg;
msg.batteryPercent = navdata.vbat_flying_percentage;
msg.state = (navdata.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.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.magX = (int32_t)navdata_magneto.mx;
msg.magY = (int32_t)navdata_magneto.my;
msg.magZ = (int32_t)navdata_magneto.mz;
msg.pressure = navdata_pressure.Pression_meas; // typo in the SDK!
msg.temp = navdata_pressure.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;
// Tag Detection
msg.tags_count = navdata_detect.nb_detected;
for (int i = 0; i < navdata_detect.nb_detected; i++)
{
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]);
}
navdata2_pub.publish(msg);
}
void controlCHandler (int signal)
{
ros::shutdown();
src/ardrone_driver.h
+1 -4
Ver Arquivo
@@ -5,7 +5,6 @@
#include <image_transport/image_transport.h>
#include <sensor_msgs/Image.h>
#include <ardrone_autonomy/Navdata.h>
#include <ardrone_autonomy/Navdata2.h>
#include "ardrone_sdk.h"
class ARDroneDriver
@@ -19,9 +18,8 @@ public:
private:
void publish_video();
void publish_navdata();
void publish_navdata2();
ros::NodeHandle node_handle;
ros::NodeHandle node_handle;
ros::Subscriber cmd_vel_sub;
ros::Subscriber takeoff_sub;
ros::Subscriber reset_sub;
@@ -32,7 +30,6 @@ private:
image_transport::CameraPublisher vert_pub;
ros::Publisher navdata_pub;
ros::Publisher navdata2_pub;
//ros::Subscriber toggleCam_sub;
ros::ServiceServer toggleCam_service;
src/ardrone_sdk.cpp
+6 -3
Ver Arquivo
@@ -181,9 +181,12 @@ extern "C" {
navdata_phys = pnd->navdata_phys_measures;
navdata = pnd->navdata_demo;
arnavtime = pnd->navdata_time;
navdata_pressure = pnd->navdata_pressure_raw;
navdata_magneto = pnd->navdata_magneto;
navdata_wind = pnd->navdata_wind_speed;
if (IS_ARDRONE2)
{ // This is neccessary
navdata_pressure = pnd->navdata_pressure_raw;
navdata_magneto = pnd->navdata_magneto;
navdata_wind = pnd->navdata_wind_speed;
}
return C_OK;
}