diff --git a/ARDroneLib/Soft/Common/navdata_keys.h b/ARDroneLib/Soft/Common/navdata_keys.h index 049567e6..fdc941d7 100644 --- a/ARDroneLib/Soft/Common/navdata_keys.h +++ b/ARDroneLib/Soft/Common/navdata_keys.h @@ -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 ) diff --git a/msg/Navdata2.msg b/msg/Navdata2.msg new file mode 100644 index 00000000..3ae8b206 --- /dev/null +++ b/msg/Navdata2.msg @@ -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 diff --git a/src/ardrone_driver.cpp b/src/ardrone_driver.cpp index 31c14293..9f229d7e 100644 --- a/src/ardrone_driver.cpp +++ b/src/ardrone_driver.cpp @@ -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/navdata", 10); + if (IS_ARDRONE2) + navdata2_pub = node_handle.advertise("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); diff --git a/src/ardrone_driver.h b/src/ardrone_driver.h index 99e0bf8e..ed6cd474 100644 --- a/src/ardrone_driver.h +++ b/src/ardrone_driver.h @@ -5,6 +5,7 @@ #include #include #include +#include #include "ardrone_sdk.h" class ARDroneDriver @@ -18,6 +19,7 @@ class ARDroneDriver private: void publish_video(); void publish_navdata(); + void publish_navdata2(); ros::NodeHandle node_handle; ros::Subscriber cmd_vel_sub; @@ -30,6 +32,7 @@ class ARDroneDriver image_transport::CameraPublisher vert_pub; ros::Publisher navdata_pub; + ros::Publisher navdata2_pub; //ros::Subscriber toggleCam_sub; ros::ServiceServer toggleCam_service; diff --git a/src/ardrone_sdk.cpp b/src/ardrone_sdk.cpp index 1517c185..ffc1bb95 100644 --- a/src/ardrone_sdk.cpp +++ b/src/ardrone_sdk.cpp @@ -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; } diff --git a/src/ardrone_sdk.h b/src/ardrone_sdk.h index ff8e41c2..5519de41 100644 --- a/src/ardrone_sdk.h +++ b/src/ardrone_sdk.h @@ -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