Support for flight animations added through setflightanimation

service.

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