update myagv_teleop

Author: X-lanni

src/myagv_teleop/scripts/myagv_teleop.py

@@ -1,103 +1,257 @@
 #!/usr/bin/env python
-# -*- coding: utf-8 -*-
+
+from __future__ import print_function
+
+import threading
+
+import roslib; roslib.load_manifest('teleop_twist_keyboard')
 import rospy
+
 from geometry_msgs.msg import Twist
-import sys, select, termios, tty
+from geometry_msgs.msg import TwistStamped
+
+import sys
+from select import select
+
+if sys.platform == 'win32':
+    import msvcrt
+else:
+    import termios
+    import tty
+
+
+TwistMsg = Twist
 
 msg = """
-Control myagv!
+Reading from the keyboard  and Publishing to Twist!
 ---------------------------
 Moving around:
    u    i    o
    j    k    l
-        ,     
+   m    ,    .
+
+t : up (+z)
+b : down (-z)
+
+anything else : stop
 
-space key, k : stop
-i : foward
-, : backward
-j : turn left
-l : turn right
-u : left revolve
-o : right revolve
+q/z : increase/decrease max speeds by 10%
+w/x : increase/decrease only linear speed by 10%
+e/c : increase/decrease only angular speed by 10%
 
 CTRL-C to quit
 """
 
-def getKey():
-    tty.setraw(sys.stdin.fileno())
-    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
-    if rlist:
-        key = sys.stdin.read(1)
+moveBindings = {
+        'i':(1,0,0,0),
+        'o':(0,0,0,-1),
+        'j':(0,1,0,0),
+        'l':(0,-1,0,0),
+        'u':(0,0,0,1),
+        ',':(-1,0,0,0),
+        '.':(-1,0,0,-1),
+        'm':(-1,0,0,1),
+        'O':(0,0,0,-1),
+        'I':(1,0,0,0),
+        'J':(0,1,0,0),
+        'L':(0,-1,0,0),
+        'U':(0,0,0,1),
+        '<':(-1,0,0,0),
+        '>':(-1,0,0,-1),
+        'M':(-1,0,0,1),
+    }
+
+speedBindings={
+        'q':(1.1,1.1),
+        'z':(.9,.9),
+        'w':(1.1,1),
+        'x':(.9,1),
+        'e':(1,1.1),
+        'c':(1,.9),
+    }
+
+class PublishThread(threading.Thread):
+    def __init__(self, rate):
+        super(PublishThread, self).__init__()
+        self.publisher = rospy.Publisher('cmd_vel', TwistMsg, queue_size = 1)
+        self.x = 0.0
+        self.y = 0.0
+        self.z = 0.0
+        self.th = 0.0
+        self.speed = 0.0
+        self.turn = 0.0
+        self.condition = threading.Condition()
+        self.done = False
+
+        # Set timeout to None if rate is 0 (causes new_message to wait forever
+        # for new data to publish)
+        if rate != 0.0:
+            self.timeout = 1.0 / rate
+        else:
+            self.timeout = None
+
+        self.start()
+
+    def wait_for_subscribers(self):
+        i = 0
+        while not rospy.is_shutdown() and self.publisher.get_num_connections() == 0:
+            if i == 4:
+                print("Waiting for subscriber to connect to {}".format(self.publisher.name))
+            rospy.sleep(0.5)
+            i += 1
+            i = i % 5
+        if rospy.is_shutdown():
+            raise Exception("Got shutdown request before subscribers connected")
+
+    def update(self, x, y, z, th, speed, turn):
+        self.condition.acquire()
+        self.x = x
+        self.y = y
+        self.z = z
+        self.th = th
+        self.speed = speed
+        self.turn = turn
+        # Notify publish thread that we have a new message.
+        self.condition.notify()
+        self.condition.release()
+
+    def stop(self):
+        self.done = True
+        self.update(0, 0, 0, 0, 0, 0)
+        self.join()
+
+    def run(self):
+        twist_msg = TwistMsg()
+
+        if stamped:
+            twist = twist_msg.twist
+            twist_msg.header.stamp = rospy.Time.now()
+            twist_msg.header.frame_id = twist_frame
+        else:
+            twist = twist_msg
+        while not self.done:
+            if stamped:
+                twist_msg.header.stamp = rospy.Time.now()
+            self.condition.acquire()
+            # Wait for a new message or timeout.
+            self.condition.wait(self.timeout)
+
+            # Copy state into twist message.
+            twist.linear.x = self.x * self.speed
+            twist.linear.y = self.y * self.speed
+            twist.linear.z = self.z * self.speed
+            twist.angular.x = 0
+            twist.angular.y = 0
+            twist.angular.z = self.th * self.turn
+
+            self.condition.release()
+
+            # Publish.
+            self.publisher.publish(twist_msg)
+
+        # Publish stop message when thread exits.
+        twist.linear.x = 0
+        twist.linear.y = 0
+        twist.linear.z = 0
+        twist.angular.x = 0
+        twist.angular.y = 0
+        twist.angular.z = 0
+        self.publisher.publish(twist_msg)
+
+
+def getKey(settings, timeout):
+    if sys.platform == 'win32':
+        # getwch() returns a string on Windows
+        key = msvcrt.getwch()
     else:
-        key = ''
-    
-    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+        tty.setraw(sys.stdin.fileno())
+        # sys.stdin.read() returns a string on Linux
+        rlist, _, _ = select([sys.stdin], [], [], timeout)
+        if rlist:
+            key = sys.stdin.read(1)
+        else:
+            key = ''
+        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
     return key
 
+def saveTerminalSettings():
+    if sys.platform == 'win32':
+        return None
+    return termios.tcgetattr(sys.stdin)
+
+def restoreTerminalSettings(old_settings):
+    if sys.platform == 'win32':
+        return
+    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)
+
+def vels(speed, turn):
+    return "currently:\tspeed %s\tturn %s " % (speed,turn)
 
 if __name__=="__main__":
-    settings = termios.tcgetattr(sys.stdin)
-    
-    rospy.init_node('myagv_teleop')
-    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
+    settings = saveTerminalSettings()
+
+    rospy.init_node('teleop_twist_keyboard')
+
+    speed = rospy.get_param("~speed", 0.5)
+    turn = rospy.get_param("~turn", 0.5)
+    speed_limit = rospy.get_param("~speed_limit", 1.0)
+    turn_limit = rospy.get_param("~turn_limit", 1.0)
+    repeat = rospy.get_param("~repeat_rate", 0.0)
+    key_timeout = rospy.get_param("~key_timeout", 0.52)
+    stamped = rospy.get_param("~stamped", False)
+    twist_frame = rospy.get_param("~frame_id", '')
+    if stamped:
+        TwistMsg = TwistStamped
+
+    pub_thread = PublishThread(repeat)
 
     x = 0
     y = 0
-    theta = 0
+    z = 0
+    th = 0
+    status = 0
 
     try:
+        pub_thread.wait_for_subscribers()
+        pub_thread.update(x, y, z, th, speed, turn)
+
         print(msg)
+        print(vels(speed,turn))
         while(1):
-            key = getKey()
-
-            if key == ' ' or key == 'k' :
+            key = getKey(settings, key_timeout)
+            if key in moveBindings.keys():
+                x = moveBindings[key][0]
+                y = moveBindings[key][1]
+                z = moveBindings[key][2]
+                th = moveBindings[key][3]
+            elif key in speedBindings.keys():
+                speed = min(speed_limit, speed * speedBindings[key][0])
+                turn = min(turn_limit, turn * speedBindings[key][1])
+                if speed == speed_limit:
+                    print("Linear speed limit reached!")
+                if turn == turn_limit:
+                    print("Angular speed limit reached!")
+                print(vels(speed,turn))
+                if (status == 14):
+                    print(msg)
+                status = (status + 1) % 15
+            else:
+                # Skip updating cmd_vel if key timeout and robot already
+                # stopped.
+                if key == '' and x == 0 and y == 0 and z == 0 and th == 0:
+                    continue
                 x = 0
                 y = 0
-                theta = 0
-            
-            elif key == 'i':
-                x = 0.5           
-                y = 0
-                theta = 0
-            elif key == ',':
-                x = -0.5          
-                y = 0
-                theta = 0
-            elif key == 'j':
-                x = 0            
-                y = 0.5
-                theta = 0
-            elif key == 'l':
-                x = 0            
-                y = -0.5
-                theta = 0
-            elif key == 'u':
-                x = 0            
-                y = 0
-                theta = 0.5
-            elif key == 'o':
-                x = 0            
-                y = 0
-                theta = -0.5
-            elif key == '\x03':
-                break
-
-            twist = Twist()
-            twist.linear.x = x; 
-            twist.linear.y = y; 
-            twist.linear.z = 0
-            twist.angular.x = 0; 
-            twist.angular.y = 0; 
-            twist.angular.z = theta
-            pub.publish(twist)
-
-    except:
+                z = 0
+                th = 0
+                if (key == '\x03'):
+                    break
+
+            pub_thread.update(x, y, z, th, speed, turn)
+
+    except Exception as e:
         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)
+        pub_thread.stop()
+        restoreTerminalSettings(settings)