Support for AR-Drone LED animation as a service added. Not tested.

.gitignore

@@ -12,3 +12,4 @@
 *.a
 /nbproject/private/
 Makefile
+/build/CMakeFiles/

src/ardrone_driver.cpp

@@ -1,6 +1,7 @@
 #include "ardrone_driver.h"
 #include "teleop_twist.h"
 #include "video.h"
+#include "ardrone_autonomy/LedAnim.h"
 #include <signal.h>
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -21,6 +22,7 @@ ARDroneDriver::ARDroneDriver()
 	toggleCam_service = node_handle.advertiseService("ardrone/togglecam", toggleCamCallback);
 	toggleNavdataDemo_service = node_handle.advertiseService("ardrone/togglenavdatademo", toggleNavdataDemoCallback);
 	setCamChannel_service = node_handle.advertiseService("ardrone/setcamchannel",setCamChannelCallback );
+	setLedAnimation_service = node_handle.advertiseService("ardrone/setledanimation", setLedAnimationCallback);
 //	setEnemyColor_service = node_handle.advertiseService("/ardrone/setenemycolor", setEnemyColorCallback);
 //	setHullType_service = node_handle.advertiseService("/ardrone/sethulltype", setHullTypeCallback);
 }

src/ardrone_driver.h

@@ -26,7 +26,7 @@ private:
 	ros::Subscriber land_sub;
 	image_transport::ImageTransport image_transport;
 	image_transport::CameraPublisher image_pub;
-        	image_transport::CameraPublisher hori_pub;
+    image_transport::CameraPublisher hori_pub;
 	image_transport::CameraPublisher vert_pub;
 
 	ros::Publisher navdata_pub;
@@ -35,6 +35,7 @@ private:
 	ros::ServiceServer toggleCam_service;
 	ros::ServiceServer toggleNavdataDemo_service;
 	ros::ServiceServer setCamChannel_service;
+	ros::ServiceServer setLedAnimation_service;
 	
 	/*
 	 * Orange Green : 1

src/teleop_twist.cpp

@@ -1,4 +1,5 @@
 #include "teleop_twist.h"
+#include "ardrone_autonomy/LedAnim.h"
 
 inline float max(float a, float b) { return a > b ? a : b; }
 inline float min(float a, float b) { return a < b ? a : b; }
@@ -22,6 +23,11 @@ int32_t detect_indoor_hull = 0;
 int32_t detect_disable_placeholder = 0;
 int32_t detect_enable_placeholder = 1;
 
+const LED_ANIMATION_IDS ledAnimMap[14] = {
+	BLINK_GREEN_RED, BLINK_GREEN, BLINK_RED, BLINK_ORANGE,
+	SNAKE_GREEN_RED, FIRE, STANDARD, RED, GREEN, RED_SNAKE,BLANK,
+	LEFT_GREEN_RIGHT_RED, LEFT_RED_RIGHT_GREEN, BLINK_STANDARD};
+
 bool toggleNavdataDemoCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
 {
     set_navdata_demo_value ^= 1;
@@ -50,6 +56,13 @@ bool toggleCamCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Respo
     return true;
 }
 
+bool setLedAnimationCallback(ardrone_autonomy::LedAnim::Request& request, ardrone_autonomy::LedAnim::Response& response)
+{
+	LED_ANIMATION_IDS anim_id = ledAnimMap[request.type];
+	ardrone_at_set_led_animation(anim_id, request.freq, request.duration);
+	response.result = true;
+	return true;
+}
 
 /*
 // Older rostopic callback function for toggling Cam

src/teleop_twist.h

@@ -6,6 +6,7 @@
 #include <std_msgs/Empty.h>
 #include <std_srvs/Empty.h>
 #include <ardrone_autonomy/CamSelect.h>
+#include <ardrone_autonomy/LedAnim.h>
 
 #define _EPS 1.0e-6 
 
@@ -20,6 +21,7 @@ void takeoffCallback(const std_msgs::Empty &msg);
 bool setCamChannelCallback(ardrone_autonomy::CamSelect::Request& request, ardrone_autonomy::CamSelect::Response& response);
 bool toggleCamCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
 bool toggleNavdataDemoCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
+bool setLedAnimationCallback(ardrone_autonomy::LedAnim::Request& request, ardrone_autonomy::LedAnim::Response& response);
 
 //All global drone configs that should be sent on init
 

srv/LedAnim.srv

@@ -0,0 +1,24 @@
+# 0 : BLINK_GREEN_RED
+# 1 : BLINK_GREEN
+# 2 : BLINK_RED
+# 3 : BLINK_ORANGE
+# 4 : SNAKE_GREEN_RED
+# 5 : FIRE
+# 6 : STANDARD
+# 7 : RED
+# 8 : GREEN
+# 9 : RED_SNAKE
+# 10: BLANK
+# 11: LEFT_GREEN_RIGHT_RED
+# 12: LEFT_RED_RIGHT_GREEN
+# 13: BLINK_STANDARD
+uint8 type
+
+# In Hz
+float32 freq
+
+# In Seconds
+uint8 duration 
+
+---
+bool result