Support for flight animations added through setflightanimation

service.

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.

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!

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

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);

src/teleop_twist.h

@@ -7,6 +7,7 @@
 #include <std_srvs/Empty.h>
 #include <ardrone_autonomy/CamSelect.h>
 #include <ardrone_autonomy/LedAnim.h>
+#include <ardrone_autonomy/FlightAnim.h>
 
 #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
 

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
+