From 1ba71f1916d1b726d6a278c89871dab35c0d2b19 Mon Sep 17 00:00:00 2001 From: Rachel Brindle Date: Thu, 26 Jul 2012 17:16:19 -0700 Subject: [PATCH 1/6] Add support for some ARDrone 2 specific sensors. Navdata no longer publishes when there is no one listening. --- msg/Navdata2.msg | 67 ++++++++++++++++++++++++++++++++++++++++ src/ardrone_driver.cpp | 70 ++++++++++++++++++++++++++++++++++++++++++ src/ardrone_driver.h | 2 ++ src/ardrone_sdk.cpp | 7 +++++ src/ardrone_sdk.h | 4 +++ 5 files changed, 150 insertions(+) create mode 100644 msg/Navdata2.msg diff --git a/msg/Navdata2.msg b/msg/Navdata2.msg new file mode 100644 index 00000000..6fe91c63 --- /dev/null +++ b/msg/Navdata2.msg @@ -0,0 +1,67 @@ +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 d2ccb62c..0a90c4f3 100644 --- a/src/ardrone_driver.cpp +++ b/src/ardrone_driver.cpp @@ -18,6 +18,7 @@ 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); + 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 ); @@ -264,6 +265,8 @@ void ARDroneDriver::publish_video() void ARDroneDriver::publish_navdata() { + if (navdata.getNumSubscribers() == 0) + return // why bother, no one is listening. ardrone_autonomy::Navdata msg; msg.batteryPercent = navdata.vbat_flying_percentage; @@ -316,6 +319,73 @@ void ARDroneDriver::publish_navdata() navdata_pub.publish(msg); } +void ARDroneDriver::publish_navdata2() +{ + if (navdata2.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++) + { + /* + * The tags_type is in raw format. In order to extract the information + * macros from ardrone_api.h is needed. + * + * #define DETECTION_MAKE_TYPE(source,tag) ( ((source)<<16) | (tag) ) + * #define DETECTION_EXTRACT_SOURCE(type) ( ((type)>>16) & 0x0FF ) + * #define DETECTION_EXTRACT_TAG(type) ( (type) & 0x0FF ) + * + * 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]); + } + // 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. + + navdata2_pub.publish(msg); +} + void controlCHandler (int signal) { ros::shutdown(); diff --git a/src/ardrone_driver.h b/src/ardrone_driver.h index af56fef0..5948dda2 100644 --- a/src/ardrone_driver.h +++ b/src/ardrone_driver.h @@ -18,6 +18,7 @@ class ARDroneDriver private: void publish_video(); void publish_navdata(); + void publish_navdata2(); ros::NodeHandle node_handle; ros::Subscriber cmd_vel_sub; @@ -30,6 +31,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 From cd28497b88bdc66c6c0fad826a22106301a08c86 Mon Sep 17 00:00:00 2001 From: Rachel Brindle Date: Thu, 26 Jul 2012 17:23:01 -0700 Subject: [PATCH 2/6] Forgot to include the navdata2 message. --- src/ardrone_driver.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ardrone_driver.h b/src/ardrone_driver.h index 5948dda2..97a26bcc 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 From cdf41f304c7175e8e222e78fd55a6f22da150bee Mon Sep 17 00:00:00 2001 From: Rachel Brindle Date: Thu, 26 Jul 2012 17:35:25 -0700 Subject: [PATCH 3/6] ARDrone 2 video no longer publishes if there are no subscribers. --- src/ardrone_driver.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/ardrone_driver.cpp b/src/ardrone_driver.cpp index 0a90c4f3..fc4e968b 100644 --- a/src/ardrone_driver.cpp +++ b/src/ardrone_driver.cpp @@ -59,6 +59,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) { /* @@ -229,6 +235,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; @@ -265,8 +273,8 @@ void ARDroneDriver::publish_video() void ARDroneDriver::publish_navdata() { - if (navdata.getNumSubscribers() == 0) - return // why bother, no one is listening. + if (navdata_pub.getNumSubscribers() == 0) + return; // why bother, no one is listening. ardrone_autonomy::Navdata msg; msg.batteryPercent = navdata.vbat_flying_percentage; @@ -321,8 +329,8 @@ void ARDroneDriver::publish_navdata() void ARDroneDriver::publish_navdata2() { - if (navdata2.getNumSubscribers() == 0) - return // why bother, no one is listening. + if (navdata2_pub.getNumSubscribers() == 0) + return; // why bother, no one is listening. ardrone_autonomy::Navdata2 msg; msg.batteryPercent = navdata.vbat_flying_percentage; From c34f24f785ee9df49a90e6b4186428ea7e91ea88 Mon Sep 17 00:00:00 2001 From: Rachel Brindle Date: Tue, 31 Jul 2012 14:48:35 -0700 Subject: [PATCH 4/6] Should publish navdata2 correctly if ARDRONE2 --- src/ardrone_driver.cpp | 5 ++++- src/ardrone_sdk.cpp | 5 +++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/ardrone_driver.cpp b/src/ardrone_driver.cpp index fc4e968b..4571fd66 100644 --- a/src/ardrone_driver.cpp +++ b/src/ardrone_driver.cpp @@ -18,7 +18,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); - navdata2_pub = node_handle.advertise("ardrone/navdata2", 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 ); @@ -40,6 +41,8 @@ void ARDroneDriver::run() { publish_video(); publish_navdata(); + if (IS_ARDRONE2) + publish_navdata2(); last_frame_id = current_frame_id; } ros::spinOnce(); diff --git a/src/ardrone_sdk.cpp b/src/ardrone_sdk.cpp index ffc1bb95..c7a43ec8 100644 --- a/src/ardrone_sdk.cpp +++ b/src/ardrone_sdk.cpp @@ -70,6 +70,11 @@ extern "C" { ardrone_application_default_config.control_yaw = (float) rosDriver->getRosParam("~control_yaw", (100.0 /180.0) * 3.1415); ardrone_application_default_config.euler_angle_max = (float) rosDriver->getRosParam("~euler_angle_max", (12.0 / 180.0) * 3.1415); ardrone_application_default_config.navdata_demo = (int) rosDriver->getRosParam("~navdata_demo", (double) 1); + if (IS_ARDRONE2) { + ardrone_application_default_config.navdata_magneto = (int) rosDriver->getRosParam("~navdata_magneto", (double) 1); + ardrone_application_default_config.navdata_pressure_raw = (int) rosDriver->getRosParam("~navdata_pressure", (double) 1); + ardrone_application_default_config.navdata_wind_speed = (int) rosDriver->getRosParam("~navdata_wind", (double) 1); + } ardrone_application_default_config.detect_type = (int) rosDriver->getRosParam("~detect_type", (double) CAD_TYPE_MULTIPLE_DETECTION_MODE); ardrone_application_default_config.enemy_colors = (int) rosDriver->getRosParam("~enemy_colors", (double) ARDRONE_DETECTION_COLOR_ORANGE_YELLOW); ardrone_application_default_config.enemy_without_shell = (bool) rosDriver->getRosParam("~enemy_without_shell", (double) 0.0); From 2f91026bbcb848ca457fc7d7eedac518f305d6a8 Mon Sep 17 00:00:00 2001 From: Brindle Date: Tue, 31 Jul 2012 21:35:48 -0700 Subject: [PATCH 5/6] foo --- src/ardrone_driver.cpp | 28 +++++++--------------------- 1 file changed, 7 insertions(+), 21 deletions(-) diff --git a/src/ardrone_driver.cpp b/src/ardrone_driver.cpp index 0a90c4f3..332971fb 100644 --- a/src/ardrone_driver.cpp +++ b/src/ardrone_driver.cpp @@ -40,6 +40,7 @@ void ARDroneDriver::run() { publish_video(); publish_navdata(); + publish_navdata2(); last_frame_id = current_frame_id; } ros::spinOnce(); @@ -265,8 +266,8 @@ void ARDroneDriver::publish_video() void ARDroneDriver::publish_navdata() { - if (navdata.getNumSubscribers() == 0) - return // why bother, no one is listening. + if (navdata_pub.getNumSubscribers() == 0) + return; // why bother, no one is listening. ardrone_autonomy::Navdata msg; msg.batteryPercent = navdata.vbat_flying_percentage; @@ -321,8 +322,8 @@ void ARDroneDriver::publish_navdata() void ARDroneDriver::publish_navdata2() { - if (navdata2.getNumSubscribers() == 0) - return // why bother, no one is listening. + if (navdata2_pub.getNumSubscribers() == 0) + return; // why bother, no one is listening. ardrone_autonomy::Navdata2 msg; msg.batteryPercent = navdata.vbat_flying_percentage; @@ -357,17 +358,6 @@ void ARDroneDriver::publish_navdata2() msg.tags_count = navdata_detect.nb_detected; for (int i = 0; i < navdata_detect.nb_detected; i++) { - /* - * The tags_type is in raw format. In order to extract the information - * macros from ardrone_api.h is needed. - * - * #define DETECTION_MAKE_TYPE(source,tag) ( ((source)<<16) | (tag) ) - * #define DETECTION_EXTRACT_SOURCE(type) ( ((type)>>16) & 0x0FF ) - * #define DETECTION_EXTRACT_TAG(type) ( (type) & 0x0FF ) - * - * 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]); @@ -376,12 +366,8 @@ void ARDroneDriver::publish_navdata2() 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. + + printf("%d\n", msg.magX); navdata2_pub.publish(msg); } From b1841cdd913eaca275005b76b998f5da36fe505f Mon Sep 17 00:00:00 2001 From: Brindle Date: Tue, 31 Jul 2012 22:39:35 -0700 Subject: [PATCH 6/6] Fixed publishing of navdata2. --- ARDroneLib/Soft/Common/navdata_keys.h | 8 ++++---- msg/Navdata2.msg | 1 - src/ardrone_driver.cpp | 6 ++---- src/ardrone_sdk.cpp | 5 ----- 4 files changed, 6 insertions(+), 14 deletions(-) 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 index 6fe91c63..3ae8b206 100644 --- a/msg/Navdata2.msg +++ b/msg/Navdata2.msg @@ -64,4 +64,3 @@ float32[] tags_distance #time stamp float32 tm - diff --git a/src/ardrone_driver.cpp b/src/ardrone_driver.cpp index af434566..98ecad2c 100644 --- a/src/ardrone_driver.cpp +++ b/src/ardrone_driver.cpp @@ -19,7 +19,7 @@ ARDroneDriver::ARDroneDriver() 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); + 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 ); @@ -377,8 +377,6 @@ void ARDroneDriver::publish_navdata2() msg.tags_distance.push_back(navdata_detect.dist[i]); } - printf("%d\n", msg.magX); - navdata2_pub.publish(msg); } @@ -396,7 +394,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_sdk.cpp b/src/ardrone_sdk.cpp index c7a43ec8..ffc1bb95 100644 --- a/src/ardrone_sdk.cpp +++ b/src/ardrone_sdk.cpp @@ -70,11 +70,6 @@ extern "C" { ardrone_application_default_config.control_yaw = (float) rosDriver->getRosParam("~control_yaw", (100.0 /180.0) * 3.1415); ardrone_application_default_config.euler_angle_max = (float) rosDriver->getRosParam("~euler_angle_max", (12.0 / 180.0) * 3.1415); ardrone_application_default_config.navdata_demo = (int) rosDriver->getRosParam("~navdata_demo", (double) 1); - if (IS_ARDRONE2) { - ardrone_application_default_config.navdata_magneto = (int) rosDriver->getRosParam("~navdata_magneto", (double) 1); - ardrone_application_default_config.navdata_pressure_raw = (int) rosDriver->getRosParam("~navdata_pressure", (double) 1); - ardrone_application_default_config.navdata_wind_speed = (int) rosDriver->getRosParam("~navdata_wind", (double) 1); - } ardrone_application_default_config.detect_type = (int) rosDriver->getRosParam("~detect_type", (double) CAD_TYPE_MULTIPLE_DETECTION_MODE); ardrone_application_default_config.enemy_colors = (int) rosDriver->getRosParam("~enemy_colors", (double) ARDRONE_DETECTION_COLOR_ORANGE_YELLOW); ardrone_application_default_config.enemy_without_shell = (bool) rosDriver->getRosParam("~enemy_without_shell", (double) 0.0);