diff --git a/src/ardrone_driver.cpp b/src/ardrone_driver.cpp index 77587d9..5fa5220 100644 --- a/src/ardrone_driver.cpp +++ b/src/ardrone_driver.cpp @@ -27,6 +27,7 @@ ARDroneDriver::ARDroneDriver() setLedAnimation_service = node_handle.advertiseService("ardrone/setledanimation", setLedAnimationCallback); flatTrim_service = node_handle.advertiseService("ardrone/flattrim", flatTrimCallback); setFlightAnimation_service = node_handle.advertiseService("ardrone/setflightanimation", setFlightAnimationCallback); + setRecord_service = node_handle.advertiseService("ardrone/setrecord", setRecordCallback ); /* 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 30dc958..7bb1b74 100644 --- a/src/ardrone_driver.h +++ b/src/ardrone_driver.h @@ -90,6 +90,7 @@ private: ros::ServiceServer imuReCalib_service; ros::ServiceServer flatTrim_service; ros::ServiceServer setFlightAnimation_service; + ros::ServiceServer setRecord_service; /* * Orange Green : 1 diff --git a/src/teleop_twist.cpp b/src/teleop_twist.cpp index ba58d43..b306acf 100644 --- a/src/teleop_twist.cpp +++ b/src/teleop_twist.cpp @@ -1,5 +1,6 @@ #include "teleop_twist.h" #include "ardrone_autonomy/LedAnim.h" +#include "utils/ardrone_date.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; } @@ -48,6 +49,34 @@ bool toggleCamCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Respo return true; } +// ros service callback to turn on and off camera recording +bool setRecordCallback(ardrone_autonomy::RecordEnable::Request &request, ardrone_autonomy::RecordEnable::Response& response) +{ + char record_command[ARDRONE_DATE_MAXSIZE + 64]; + int32_t new_codec; + + if( request.enable == true ) { + char date[ARDRONE_DATE_MAXSIZE]; + time_t t = time(NULL); + // For some reason the linker can't find this, so we'll just do it manually, cutting and pasting + // ardrone_time2date(t, ARDRONE_FILE_DATE_FORMAT, date); + strftime(date, ARDRONE_DATE_MAXSIZE, ARDRONE_FILE_DATE_FORMAT, localtime(&t)); + snprintf(record_command, sizeof(record_command), "%d,%s", USERBOX_CMD_START, date); + new_codec = MP4_360P_H264_720P_CODEC; + } else { + snprintf(record_command, sizeof(record_command), "%d", USERBOX_CMD_STOP ); + new_codec = H264_360P_CODEC; + } + + vp_os_mutex_lock(&twist_lock); + ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_codec, &new_codec, NULL ); + ARDRONE_TOOL_CONFIGURATION_ADDEVENT (userbox_cmd, record_command, NULL ); + vp_os_mutex_unlock(&twist_lock); + + response.result = true; + return true; +} + bool setLedAnimationCallback(ardrone_autonomy::LedAnim::Request& request, ardrone_autonomy::LedAnim::Response& response) { LED_ANIMATION_IDS anim_id = ledAnimMap[request.type % 14]; // Don't trick me diff --git a/src/teleop_twist.h b/src/teleop_twist.h index fe6295c..406ca28 100644 --- a/src/teleop_twist.h +++ b/src/teleop_twist.h @@ -8,6 +8,7 @@ #include #include #include +#include #define _EPS 1.0e-6 @@ -24,6 +25,7 @@ bool toggleCamCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Respo 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); +bool setRecordCallback(ardrone_autonomy::RecordEnable::Request &request, ardrone_autonomy::RecordEnable::Response& response); //All global drone configs that should be sent on init diff --git a/srv/RecordEnable.srv b/srv/RecordEnable.srv new file mode 100644 index 0000000..6a01a57 --- /dev/null +++ b/srv/RecordEnable.srv @@ -0,0 +1,3 @@ +bool enable +--- +bool result \ No newline at end of file