From 68870d193ada44f45508e9dcdba0a532ef5adc02 Mon Sep 17 00:00:00 2001
From: Louis-Guillaume DUBOIS <contact@lgdubois.fr>
Date: Fri, 24 Apr 2015 14:54:01 +0200
Subject: [PATCH] makes a good keyboard controler for our drone

---
 .../src/teleop_twist_keyboard_azerty.py       | 113 ++++++++++++++++++
 1 file changed, 113 insertions(+)
 create mode 100755 hand_control/src/teleop_twist_keyboard_azerty.py

diff --git a/hand_control/src/teleop_twist_keyboard_azerty.py b/hand_control/src/teleop_twist_keyboard_azerty.py
new file mode 100755
index 0000000..0276173
--- /dev/null
+++ b/hand_control/src/teleop_twist_keyboard_azerty.py
@@ -0,0 +1,113 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+import roslib; roslib.load_manifest('teleop_twist_keyboard')
+import rospy
+
+from geometry_msgs.msg import Twist
+
+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|↙ ,|↓ ;|↘ :| 
+
+a/w : increase/decrease max speeds by 10%
+z/x : increase/decrease only linear speed by 10%
+e/c : increase/decrease only angular speed by 10%
+anything else : stop
+
+CTRL-C to quit
+"""
+
+moveBindings = {
+    #   x th y z
+    'i':(1,0,0,0),
+    'o':(1,-1,0,0),
+    'j':(0,1,0,0),
+    'l':(0,-1,0,0),
+    'u':(1,1,0,0),
+    ';':(-1,0,0,0),
+    ':':(-1,1,0,0),
+    ',':(-1,-1,0,0),
+    'h':(0,0,-1,0),
+    'm':(0,0,1,0),
+    'y':(0,0,0,1),
+    'n':(0,0,0,-1),
+         }
+
+speedBindings={
+    'a':(1.1,1.1),
+    'w':(.9,.9),
+    'z':(1.1,1),
+    'x':(.9,1),
+    'e':(1,1.1),
+    'c':(1,.9),
+        }
+
+def getKey():
+  tty.setraw(sys.stdin.fileno())
+  select.select([sys.stdin], [], [], 0)
+  key = sys.stdin.read(1)
+  termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+  return key
+
+speed = .5
+turn = 1
+
+def vels(speed,turn):
+  return "currently:\tspeed %s\tturn %s " % (speed,turn)
+
+if __name__=="__main__":
+  settings = termios.tcgetattr(sys.stdin)
+  pub = rospy.Publisher('cmd_vel', Twist)
+  rospy.init_node('teleop_twist_keyboard_azerty')
+  x = 0
+  th = 0
+  y = 0
+  z = 0
+  status = 0
+  try:
+    print msg
+    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
+      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:
+    twist = Twist()
+    twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
+    twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
+    pub.publish(twist)
+    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+