diff --git a/README.md b/README.md
index dce6387..a5d9e5e 100644
--- a/README.md
+++ b/README.md
@@ -6,6 +6,7 @@
### Updates
+- *March 20, 2013*: Release related to company launch/Wired article
- *October 23, 2012*: Create this seperate package
## Usage
@@ -34,6 +35,9 @@ Once everything is up and running you should see a window with the front camera
* Left Joystick: Pitch and Roll
* Right Joystick: Altitude and Yaw
+### More Buttons
+* Center PS3 Button: Toggle recording to USB stick
+
## License
Copyright (c) 2012, Falkor Systems, Inc.
diff --git a/launch/track.launch b/launch/track.launch
index a98e3f8..94df45b 100644
--- a/launch/track.launch
+++ b/launch/track.launch
@@ -27,7 +27,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-->
-
+
+
+
diff --git a/nodes/ardrone_follow.py b/nodes/ardrone_follow.py
index c640b81..1261f9b 100755
--- a/nodes/ardrone_follow.py
+++ b/nodes/ardrone_follow.py
@@ -39,13 +39,18 @@ from std_msgs.msg import Empty
from sensor_msgs.msg import Joy, Image
from ardrone_autonomy.msg import Navdata
from ardrone_autonomy.srv import LedAnim
+from ardrone_autonomy.srv import RecordEnable
+import std_srvs.srv
class ArdroneFollow:
def __init__( self ):
print "waiting for driver to startup"
rospy.wait_for_service( "ardrone/setledanimation" )
+ rospy.wait_for_service( "ardrone/setrecord" )
print "driver started"
self.led_service = rospy.ServiceProxy( "ardrone/setledanimation", LedAnim )
+ self.record_service = rospy.ServiceProxy( "ardrone/setrecord", RecordEnable )
+ self.recording = False
self.tracker_sub = rospy.Subscriber( "ardrone_tracker/found_point",
Point, self.found_point_cb )
@@ -66,12 +71,16 @@ class ArdroneFollow:
self.linearXlimit = 1.0
self.linearZlimit = 2.0
- self.xPid = pid.Pid( 0.020, 0.0, 0.0, self.angularZlimit )
- self.yPid = pid.Pid( 0.020, 0.0, 0.0, self.linearZlimit )
+ # Increasing the P term for yaw
+ self.xPid = pid.Pid( 0.080, 0.0, 0.0, self.angularZlimit )
+ self.yPid = pid.Pid( 0.050, 0.0, 0.0, self.linearZlimit )
self.zPid = pid.Pid( 0.050, 0.0, 0.0, self.linearXlimit )
- self.xPid.setPointMin = 40
- self.xPid.setPointMax = 60
+ # alpha for the ema filter on the found point
+ self.alpha = 0.5
+
+ self.xPid.setPointMin = 45
+ self.xPid.setPointMax = 55
self.yPid.setPointMin = 40
self.yPid.setPointMax = 60
@@ -103,8 +112,15 @@ class ArdroneFollow:
8: 'Landing',
9: 'Looping' }
+
+
cv2.namedWindow( 'AR.Drone Follow', cv2.cv.CV_WINDOW_NORMAL )
+ def toggle_record( self ):
+ self.recording = not self.recording
+
+ self.record_service( enable = self.recording )
+
def navdata_cb( self, data ):
self.navdata = data
@@ -133,6 +149,9 @@ class ArdroneFollow:
if data.buttons[14] == 1 and self.last_buttons[14] == 0:
self.land()
+ if data.buttons[16] == 1 and self.last_buttons[16] == 0:
+ self.toggle_record()
+
if data.buttons[13] == 1 and self.last_buttons[13] == 0:
self.reset()
@@ -192,7 +211,17 @@ class ArdroneFollow:
self.reset_pub.publish( Empty() )
def found_point_cb( self, data ):
- self.found_point = data
+ if data.z != -1.0:
+ if self.found_point.z == -1.0:
+ self.found_point = data
+ else:
+ # update the ema found_point, if we didn't just re-acquire
+ self.found_point.x = self.found_point.x * self.alpha + data.x * ( 1.0 - self.alpha )
+ self.found_point.y = self.found_point.y * self.alpha + data.y * ( 1.0 - self.alpha )
+ self.found_point.z = self.found_point.z * self.alpha + data.z * ( 1.0 - self.alpha )
+ else:
+ self.found_point = data
+
self.found_time = rospy.Time.now()
def hover( self ):
@@ -262,6 +291,9 @@ class ArdroneFollow:
if self.auto_cmd:
self.put_text( vis, 'TRACKING', ( 150, 300 ) )
+ if self.recording:
+ self.put_text( vis, 'RECORDING', ( 150, 320 ) )
+
cv2.imshow( 'AR.Drone Follow', vis )
cv2.waitKey( 1 )
@@ -308,6 +340,7 @@ def main():
rospy.spin()
except KeyboardInterrupt:
print "Keyboard interrupted"
+ af.usb_service()
if __name__ == '__main__':
main()