From 3c1e0154112729bc666a0dee3853feac2affdcc4 Mon Sep 17 00:00:00 2001
From: Louis-Guillaume DUBOIS <contact@lgdubois.fr>
Date: Sat, 25 Apr 2015 13:24:31 +0200
Subject: [PATCH] adds land takeoff reset to keyboard_azerty

---
 hand_control/src/keyboard_azerty.py | 80 +++++++++++++++++------------
 1 file changed, 47 insertions(+), 33 deletions(-)

diff --git a/hand_control/src/keyboard_azerty.py b/hand_control/src/keyboard_azerty.py
index d1ca1e5..366b09e 100755
--- a/hand_control/src/keyboard_azerty.py
+++ b/hand_control/src/keyboard_azerty.py
@@ -4,16 +4,18 @@ import roslib; roslib.load_manifest('teleop_twist_keyboard')
 import rospy
 
 from geometry_msgs.msg import Twist
+from std_msgs.msg import Empty
 
 import sys, select, termios, tty
 
 msg = """
-Reading from the keyboard  and Publishing to Twist!
----------------------------
-Moving around:
-|⇑ y|↖ u|↑ i|↗ o|
-|⇐ h|← j|  k|→ l|⇒ m|
-|⇓ n|↙ ,|↓ ;|↘ :| 
+        ---------------------
+takeoff>|  t|⇑ y|↖ u|↑ i|↗ o|  
+        |---|---|---|---|---|----
+  reset>|  g|⇐ h|← j|  k|→ l|⇒ m|
+        |---|---|---|---|---|----
+   land>|  b|⇓ n|↙ ,|↓ ;|↘ :| 
+        ---------------------
 
 a/w : increase/decrease max speeds by 10%
 z/x : increase/decrease only linear speed by 10%
@@ -63,7 +65,10 @@ def vels(speed,turn):
 
 if __name__=="__main__":
   settings = termios.tcgetattr(sys.stdin)
-  pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
+  pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+  land = rospy.Publisher('/ardrone/land', Empty, queue_size=1)
+  takeoff = rospy.Publisher('/ardrone/takeoff', Empty, queue_size=1)
+  reset = rospy.Publisher('/ardrone/reset', Empty, queue_size=1)
   rospy.init_node('keyboard_azerty')
   x = 0
   th = 0
@@ -75,33 +80,42 @@ if __name__=="__main__":
     print vels(speed,turn)
     while(1):
       key = getKey()
-      if key in moveBindings.keys():
-        x = moveBindings[key][0]
-        th = moveBindings[key][1]
-        y = moveBindings[key][2]
-        z = moveBindings[key][3]
-      elif key in speedBindings.keys():
-        speed = speed * speedBindings[key][0]
-        turn = turn * speedBindings[key][1]
-
-        print vels(speed,turn)
-        if (status == 14):
-          print msg
-        status = (status + 1) % 15
+      if (key == 't'):
+        takeoff.publish(Empty())
+        print "takeoff"
+      elif (key == 'g'):
+        reset.publish(Empty())
+        print "reset"
+      elif (key == 'b'):
+        land.publish(Empty())
+        print "land"
       else:
-        x = 0
-        th = 0
-        y = 0
-        z = 0
-        if (key == '\x03'):
-          break
-      twist = Twist()
-      twist.linear.x = x*speed
-      twist.linear.y = y*speed
-      twist.linear.z = z*speed
-      twist.angular.x = 0; twist.angular.y = 0;
-      twist.angular.z = th*turn
-      pub.publish(twist)
+        if key in moveBindings.keys():
+          x = moveBindings[key][0]
+          th = moveBindings[key][1]
+          y = moveBindings[key][2]
+          z = moveBindings[key][3]
+        elif key in speedBindings.keys():
+          speed = speed * speedBindings[key][0]
+          turn = turn * speedBindings[key][1]
+          print vels(speed,turn)
+          if (status == 14):
+            print msg
+          status = (status + 1) % 15
+        else:
+          x = 0
+          th = 0
+          y = 0
+          z = 0
+          if (key == '\x03'):
+            break
+        twist = Twist()
+        twist.linear.x = x*speed
+        twist.linear.y = y*speed
+        twist.linear.z = z*speed
+        twist.angular.x = 0; twist.angular.y = 0;
+        twist.angular.z = th*turn
+        pub.publish(twist)
   except:
     print e
   finally: