From 60dd3eee673ebf2fd174643ebcebb0b1bc75822b Mon Sep 17 00:00:00 2001 From: Mani Monajjemi Date: Tue, 4 Dec 2012 18:05:32 -0800 Subject: [PATCH] Support for flight animations added through `setflightanimation` service. --- README.md | 22 +++++++++++++++++++++- src/ardrone_driver.cpp | 1 + src/ardrone_driver.h | 1 + src/teleop_twist.cpp | 13 +++++++++++++ src/teleop_twist.h | 2 ++ srv/FlightAnim.srv | 30 ++++++++++++++++++++++++++++++ 6 files changed, 68 insertions(+), 1 deletion(-) create mode 100644 srv/FlightAnim.srv diff --git a/README.md b/README.md index fc0270f..e2f9418 100644 --- a/README.md +++ b/README.md @@ -168,7 +168,7 @@ Calling `ardrone/setledanimation` service will invoke one of 14 pre-defined LED * `float32 freq`: The frequency of the animation in Hz * `uint8 duration`: The duration of the animation in Seconds. -The `type` parameter will map [in order] to one of these animations: +The `type` parameter will map [in order] to one of these animations (check `srv/LedAnim.srv` for more details): BLINK_GREEN_RED, BLINK_GREEN, BLINK_RED, BLINK_ORANGE, SNAKE_GREEN_RED, FIRE, STANDARD, RED, GREEN, RED_SNAKE,BLANK, @@ -176,6 +176,26 @@ The `type` parameter will map [in order] to one of these animations: You can test these animations in command line using commands like `rosservice call /ardrone/setledanimation 1 4 5` +### Flight Animations + +Calling `ardrone/setflightanimation` service will execute one of 20 pre-defined flight animations for the drone. The paramaters are: + +* `uint8 type`: The type of flight animation, a number in range [0..19] +* `uint16 duration`: The duration of the animation. Use 0 for default duration (recommended) + +The `type` paramater will map [in order] to one of these pre-defined animations (check `srv/FlightAnim.srv` for more details): + + ARDRONE_ANIM_PHI_M30_DEG, ARDRONE_ANIM_PHI_30_DEG, ARDRONE_ANIM_THETA_M30_DEG, ARDRONE_ANIM_THETA_30_DEG, + ARDRONE_ANIM_THETA_20DEG_YAW_200DEG, ARDRONE_ANIM_THETA_20DEG_YAW_M200DEG, ARDRONE_ANIM_TURNAROUND, + ARDRONE_ANIM_TURNAROUND_GODOWN, ARDRONE_ANIM_YAW_SHAKE, ARDRONE_ANIM_YAW_DANCE, ARDRONE_ANIM_PHI_DANCE, + ARDRONE_ANIM_THETA_DANCE, ARDRONE_ANIM_VZ_DANCE, ARDRONE_ANIM_WAVE, ARDRONE_ANIM_PHI_THETA_MIXED, + ARDRONE_ANIM_DOUBLE_PHI_THETA_MIXED, ARDRONE_ANIM_FLIP_AHEAD, ARDRONE_ANIM_FLIP_BEHIND, ARDRONE_ANIM_FLIP_LEFT, + ARDRONE_ANIM_FLIP_RIGHT + +You can test these animations in command line using commands like `rosservice call /ardrone/setflightanimation 1 0` while drone is flying. + +Please be extra cautious about using animations, especially flip animations. + ### IMU Calibration If `do_imu_caliberation` parameter is set to true, calling `ardrone/imu_recalib` service will make the driver recalculate the biases in IMU data based on data from a short sampling period. diff --git a/src/ardrone_driver.cpp b/src/ardrone_driver.cpp index 471600c..eca6ab1 100644 --- a/src/ardrone_driver.cpp +++ b/src/ardrone_driver.cpp @@ -26,6 +26,7 @@ ARDroneDriver::ARDroneDriver() setCamChannel_service = node_handle.advertiseService("ardrone/setcamchannel",setCamChannelCallback ); setLedAnimation_service = node_handle.advertiseService("ardrone/setledanimation", setLedAnimationCallback); flatTrim_service = node_handle.advertiseService("ardrone/flattrim", flatTrimCallback); + setFlightAnimation_service = node_handle.advertiseService("ardrone/setflightanimation", setFlightAnimationCallback); /* To be honest, I am not sure why advertising a service using class members should be this complicated! diff --git a/src/ardrone_driver.h b/src/ardrone_driver.h index 2b68dfa..30dc958 100644 --- a/src/ardrone_driver.h +++ b/src/ardrone_driver.h @@ -89,6 +89,7 @@ private: ros::ServiceServer setLedAnimation_service; ros::ServiceServer imuReCalib_service; ros::ServiceServer flatTrim_service; + ros::ServiceServer setFlightAnimation_service; /* * Orange Green : 1 diff --git a/src/teleop_twist.cpp b/src/teleop_twist.cpp index fd36c2d..ba58d43 100644 --- a/src/teleop_twist.cpp +++ b/src/teleop_twist.cpp @@ -58,6 +58,19 @@ bool setLedAnimationCallback(ardrone_autonomy::LedAnim::Request& request, ardron return true; } +bool setFlightAnimationCallback(ardrone_autonomy::FlightAnim::Request &request, ardrone_autonomy::FlightAnim::Response &response) +{ + char param[20]; + const int anim_type = request.type % ARDRONE_NB_ANIM_MAYDAY; + const int anim_duration = (request.duration > 0) ? request.duration : MAYDAY_TIMEOUT[anim_type]; + snprintf(param, sizeof (param), "%d,%d", anim_type, anim_duration); + vp_os_mutex_lock(&twist_lock); + ARDRONE_TOOL_CONFIGURATION_ADDEVENT(flight_anim, param, NULL); + vp_os_mutex_unlock(&twist_lock); + response.result = true; + return true; +} + bool flatTrimCallback(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response) { vp_os_mutex_lock(&twist_lock); diff --git a/src/teleop_twist.h b/src/teleop_twist.h index 4aca5a2..fe6295c 100644 --- a/src/teleop_twist.h +++ b/src/teleop_twist.h @@ -7,6 +7,7 @@ #include #include #include +#include #define _EPS 1.0e-6 @@ -22,6 +23,7 @@ bool setCamChannelCallback(ardrone_autonomy::CamSelect::Request& request, ardron bool toggleCamCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); bool setLedAnimationCallback(ardrone_autonomy::LedAnim::Request& request, ardrone_autonomy::LedAnim::Response& response); bool flatTrimCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); +bool setFlightAnimationCallback(ardrone_autonomy::FlightAnim::Request& request, ardrone_autonomy::FlightAnim::Response& response); //All global drone configs that should be sent on init diff --git a/srv/FlightAnim.srv b/srv/FlightAnim.srv new file mode 100644 index 0000000..5e59334 --- /dev/null +++ b/srv/FlightAnim.srv @@ -0,0 +1,30 @@ +# 0 : ARDRONE_ANIM_PHI_M30_DEG +# 1 : ARDRONE_ANIM_PHI_30_DEG +# 2 : ARDRONE_ANIM_THETA_M30_DEG +# 3 : ARDRONE_ANIM_THETA_30_DEG +# 4 : ARDRONE_ANIM_THETA_20DEG_YAW_200DEG +# 5 : ARDRONE_ANIM_THETA_20DEG_YAW_M200DEG +# 6 : ARDRONE_ANIM_TURNAROUND +# 7 : ARDRONE_ANIM_TURNAROUND_GODOWN +# 8 : ARDRONE_ANIM_YAW_SHAKE +# 9 : ARDRONE_ANIM_YAW_DANCE +# 10: ARDRONE_ANIM_PHI_DANCE +# 11: ARDRONE_ANIM_THETA_DANCE +# 12: ARDRONE_ANIM_VZ_DANCE +# 13: ARDRONE_ANIM_WAVE +# 14: ARDRONE_ANIM_PHI_THETA_MIXED +# 15: ARDRONE_ANIM_DOUBLE_PHI_THETA_MIXED +# 16: ARDRONE_ANIM_FLIP_AHEAD +# 17: ARDRONE_ANIM_FLIP_BEHIND +# 18: ARDRONE_ANIM_FLIP_LEFT +# 19: ARDRONE_ANIM_FLIP_RIGHT + +uint8 type + +# In Milliseconds +# 0 For Default Duration (Recommended) +uint32 duration + +--- +bool result +