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

Merge pull request #2 from younata/master

Ver Código-Fonte 
[Still Experimental] Added magnetometer, pressure, temperature, and wind speed information to new message navdata2.
Esse commit está contido em:
Mani Monajjemi
2012-08-01 12:13:08 -07:00
pai eaee5ab16d cb746ed9f6
commit d0f3502d94
6 arquivos alterados com 149 adições e 5 exclusões
Baixar Arquivo de Patch Baixar Arquivo de Diff Expandir todos os arquivos Colapsar todos os arquivos
ARDroneLib/Soft/Common/navdata_keys.h
+4 -4
Ver Arquivo
@@ -31,10 +31,10 @@ NAVDATA_OPTION( navdata_watchdog_t , navdata_watchdog , NAVDATA_WATC
NAVDATA_OPTION( navdata_adc_data_frame_t, navdata_adc_data_frame , NAVDATA_ADC_DATA_FRAME_TAG )
NAVDATA_OPTION( navdata_video_stream_t, navdata_video_stream , NAVDATA_VIDEO_STREAM_TAG )
NAVDATA_OPTION( navdata_games_t, navdata_games , NAVDATA_GAMES_TAG )
NAVDATA_OPTION( navdata_pressure_raw_t, navdata_pressure_raw , NAVDATA_PRESSURE_RAW_TAG )
NAVDATA_OPTION( navdata_magneto_t, navdata_magneto , NAVDATA_MAGNETO_TAG )
NAVDATA_OPTION( navdata_wind_speed_t, navdata_wind_speed , NAVDATA_WIND_TAG )
NAVDATA_OPTION( navdata_kalman_pressure_t,navdata_kalman_pressure , NAVDATA_KALMAN_PRESSURE_TAG )
NAVDATA_OPTION( navdata_pressure_raw_t, navdata_pressure_raw , NAVDATA_PRESSURE_RAW_TAG )
NAVDATA_OPTION( navdata_magneto_t, navdata_magneto , NAVDATA_MAGNETO_TAG )
NAVDATA_OPTION( navdata_wind_speed_t, navdata_wind_speed , NAVDATA_WIND_TAG )
NAVDATA_OPTION( navdata_kalman_pressure_t,navdata_kalman_pressure , NAVDATA_KALMAN_PRESSURE_TAG )
NAVDATA_OPTION( navdata_hdvideo_stream_t ,navdata_hdvideo_stream , NAVDATA_HDVIDEO_STREAM_TAG )
NAVDATA_OPTION( navdata_wifi_t ,navdata_wifi , NAVDATA_WIFI_TAG )
msg/Navdata2.msg
+66
Ver Arquivo
@@ -0,0 +1,66 @@
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
+65 -1
Ver Arquivo
@@ -19,6 +19,8 @@ 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 );
@@ -41,6 +43,8 @@ void ARDroneDriver::run()
{
publish_video();
publish_navdata();
if (IS_ARDRONE2)
publish_navdata2();
last_frame_id = current_frame_id;
}
ros::spinOnce();
@@ -60,6 +64,12 @@ double ARDroneDriver::getRosParam(char* param, double defaultVal)
void ARDroneDriver::publish_video()
{
if (image_pub.getNumSubscribers() == 0)
return;
if (hori_pub.getNumSubscribers() == 0)
return;
if (vert_pub.getNumSubscribers() == 0)
return;
if (IS_ARDRONE1)
{
/*
@@ -230,6 +240,8 @@ void ARDroneDriver::publish_video()
*/
if (IS_ARDRONE2)
{
if (hori_pub.getNumSubscribers() == 0 || vert_pub.getNumSubscribers() == 0)
return;
sensor_msgs::Image image_msg;
sensor_msgs::CameraInfo cinfo_msg;
sensor_msgs::Image::_data_type::iterator _it;
@@ -266,6 +278,8 @@ void ARDroneDriver::publish_video()
void ARDroneDriver::publish_navdata()
{
if (navdata_pub.getNumSubscribers() == 0)
return; // why bother, no one is listening.
ardrone_autonomy::Navdata msg;
msg.batteryPercent = navdata.vbat_flying_percentage;
@@ -318,6 +332,56 @@ void ARDroneDriver::publish_navdata()
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();
@@ -332,7 +396,7 @@ int main(int argc, char** argv)
{
// We need to implement our own Signal handler instead of ROS to shutdown
// the SDK threads correctly.
ros::init(argc, argv, "ardrone_driver", ros::init_options::NoSigintHandler);
signal (SIGABRT, &controlCHandler);
src/ardrone_driver.h
+3
Ver Arquivo
@@ -5,6 +5,7 @@
#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
@@ -18,6 +19,7 @@ public:
private:
void publish_video();
void publish_navdata();
void publish_navdata2();
ros::NodeHandle node_handle;
ros::Subscriber cmd_vel_sub;
@@ -30,6 +32,7 @@ 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
+7
Ver Arquivo
@@ -7,6 +7,10 @@ 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;
ARDroneDriver* rosDriver;
@@ -177,6 +181,9 @@ 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;
return C_OK;
}
src/ardrone_sdk.h
+4
Ver Arquivo
@@ -44,6 +44,10 @@ extern navdata_phys_measures_t navdata_phys;
extern navdata_demo_t navdata;
extern navdata_time_t arnavtime;
extern navdata_pressure_raw_t navdata_pressure;
extern navdata_magneto_t navdata_magneto;
extern navdata_wind_speed_t navdata_wind;
extern int32_t should_exit;
#endif