diff --git a/.gitignore b/.gitignore index a835d13..a3bf0b1 100644 --- a/.gitignore +++ b/.gitignore @@ -12,3 +12,4 @@ *.a /nbproject/private/ Makefile +/build/CMakeFiles/ \ No newline at end of file diff --git a/src/ardrone_driver.cpp b/src/ardrone_driver.cpp index d2ccb62..31c1429 100644 --- a/src/ardrone_driver.cpp +++ b/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 //////////////////////////////////////////////////////////////////////////////// @@ -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); } diff --git a/src/ardrone_driver.h b/src/ardrone_driver.h index af56fef..99e0bf8 100644 --- a/src/ardrone_driver.h +++ b/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 diff --git a/src/teleop_twist.cpp b/src/teleop_twist.cpp index 583bd51..5b9dd1e 100644 --- a/src/teleop_twist.cpp +++ b/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 diff --git a/src/teleop_twist.h b/src/teleop_twist.h index eaf24da..7cd78f0 100644 --- a/src/teleop_twist.h +++ b/src/teleop_twist.h @@ -6,6 +6,7 @@ #include #include #include +#include #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 diff --git a/srv/LedAnim.srv b/srv/LedAnim.srv new file mode 100644 index 0000000..280a998 --- /dev/null +++ b/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