Merge pull request #53 from sameerparekh/dev-unstable

Turn on and off USB stick recording

README.md

@@ -27,6 +27,7 @@
 	- [Flight Animations ](#flight-animations-new) :new:
 	- [IMU Calibration](#imu-calibration)
 	- [Flat Trim](#flat-trim)
+	- [Record to USB Stick](#record-usb)
 - [Parameters](#parameters)
 	- [AR-Drone Specific Parameters](#ar-drone-specific-parameters)
 	- [Other Parameters](#other-parameters)
@@ -277,6 +278,10 @@ If `do_imu_caliberation` parameter is set to true, calling `ardrone/imu_recalib`
 
 Calling `ardrone/flattrim` service without any parameter will send a "Flat Trim" request to AR-Drone to re-calibrate its rotation estimates assuming that it is on a flat surface. Do not call this service while Drone is flying or while the drone is not actually on a flat surface.
 
+### Record to USB Stick
+
+Calling `ardrone/setrecord` service will enable and disable recording to the USB stick. The service takes a simple `1` to enable or `0` to disable. So you can turn on recording to the USB stick with `rosservice call /ardrone/setrecord 1`
+
 ## Parameters
 
 ### AR-Drone Specific Parameters

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!

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

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

src/teleop_twist.h

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

srv/RecordEnable.srv

@@ -0,0 +1,3 @@
+bool enable
+---
+bool result