add teleop_twist_keyboard ros package necessary for slam homework

teleop_twist_keyboard/CHANGELOG.rst

@@ -0,0 +1,40 @@
+1.0.0 (2020-06-26)
+------------------
+* Implement repeat rate and key timeout
+* Update readme about publishing to another topic.
+* Contributors: Austin, trainman419
+
+0.6.2 (2018-10-21)
+------------------
+* Replace tabs with spaces, fixes `#15 <https://github.com/ros-teleop/teleop_twist_keyboard/issues/15>`_
+* Merge pull request `#13 <https://github.com/ros-teleop/teleop_twist_keyboard/issues/13>`_ from asukiaaa/patch-3
+  Add rosrun command to specify values
+* Add rosrun command to specify values
+* Contributors: Asuki Kono, Austin, trainman419
+
+0.6.1 (2018-05-02)
+------------------
+* Merge pull request `#11 <https://github.com/ros-teleop/teleop_twist_keyboard/issues/11>`_ from MatthijsBurgh/patch-1
+  Correct exception handling; Python3 print compatible
+* import print from future
+* Print python3 compatible
+* correct Exception handling
+* Merge pull request `#7 <https://github.com/ros-teleop/teleop_twist_keyboard/issues/7>`_ from lucasw/speed_params
+  set linear and turn speed via rosparams
+* Using tabs instead of spaces to match rest of file
+* set linear and turn speed via rosparams
+* Contributors: Austin, Lucas Walter, Matthijs van der Burgh
+
+0.6.0 (2016-03-21)
+------------------
+* Better instruction formatting
+* support holonomic base, send commmand with keyboard down
+* Fixing queue_size warning
+* Create README.md
+* Update the description string in package.xml
+* Contributors: Austin, Kei Okada, LiohAu, Mike Purvis, kk6axq, trainman419
+
+0.5.0 (2014-02-11)
+------------------
+* Initial import from brown_remotelab
+* Convert to catkin

teleop_twist_keyboard/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(teleop_twist_keyboard)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+catkin_install_python(PROGRAMS
+   teleop_twist_keyboard.py
+   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+

teleop_twist_keyboard/README.md

@@ -0,0 +1,68 @@
+# teleop_twist_keyboard
+Generic Keyboard Teleop for ROS
+
+# Launch
+Run.
+```
+rosrun teleop_twist_keyboard teleop_twist_keyboard.py
+```
+
+With custom values.
+```
+rosrun teleop_twist_keyboard teleop_twist_keyboard.py _speed:=0.9 _turn:=0.8
+```
+
+Publishing to a different topic (in this case `my_cmd_vel`).
+```
+rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=my_cmd_vel
+```
+
+# Usage
+```
+Reading from the keyboard  and Publishing to Twist!
+---------------------------
+Moving around:
+   u    i    o
+   j    k    l
+   m    ,    .
+
+For Holonomic mode (strafing), hold down the shift key:
+---------------------------
+   U    I    O
+   J    K    L
+   M    <    >
+
+t : up (+z)
+b : down (-z)
+
+anything else : stop
+
+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
+```
+
+# Repeat Rate
+
+If your mobile base requires constant updates on the cmd\_vel topic, teleop\_twist\_keyboard can be configured to repeat the last command at a fixed interval, using the `repeat_rate` private parameter.
+
+For example, to repeat the last command at 10Hz:
+
+```
+rosrun teleop_twist_keyboard teleop_twist_keyboard.py _repeat_rate:=10.0
+```
+
+It is _highly_ recommened that the repeat rate be used in conjunction with the key timeout, to prevent runaway robots.
+
+# Key Timeout
+
+Teleop\_twist\_keyboard can be configured to stop your robot if it does not receive any key presses in a configured time period, using the `key_timeout` private parameter.
+
+For example, to stop your robot if a keypress has not been received in 0.6 seconds:
+```
+rosrun teleop_twist_keyboard teleop_twist_keyboard.py _key_timeout:=0.6
+```
+
+It is recommended that you set `key_timeout` higher than the initial key repeat delay on your system (This delay is 0.5 seconds by default on Ubuntu, but can be adjusted).

teleop_twist_keyboard/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<package>
+  <name>teleop_twist_keyboard</name>
+  <version>1.0.0</version>
+  <description>Generic keyboard teleop for twist robots.</description>
+
+  <maintainer email="[email protected]">Austin Hendrix</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://wiki.ros.org/teleop_twist_keyboard</url>
+
+  <author>Graylin Trevor Jay</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>rospy</run_depend>
+</package>

teleop_twist_keyboard/teleop_twist_keyboard.py

@@ -0,0 +1,225 @@
+#!/usr/bin/env python
+
+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
+
+msg = """
+Reading from the keyboard  and Publishing to Twist!
+---------------------------
+Moving around:
+   u    i    o
+   j    k    l
+   m    ,    .
+
+For Holonomic mode (strafing), hold down the shift key:
+---------------------------
+   U    I    O
+   J    K    L
+   M    <    >
+
+t : up (+z)
+b : down (-z)
+
+anything else : stop
+
+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
+"""
+
+moveBindings = {
+        'i':(1,0,0,0),
+        'o':(1,0,0,-1),
+        'j':(0,0,0,1),
+        'l':(0,0,0,-1),
+        'u':(1,0,0,1),
+        ',':(-1,0,0,0),
+        '.':(-1,0,0,1),
+        'm':(-1,0,0,-1),
+        'O':(1,-1,0,0),
+        'I':(1,0,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),
+        'M':(-1,1,0,0),
+        't':(0,0,1,0),
+        'b':(0,0,-1,0),
+    }
+
+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', Twist, 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 = Twist()
+        while not self.done:
+            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)
+
+        # 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)
+
+
+def getKey(key_timeout):
+    tty.setraw(sys.stdin.fileno())
+    rlist, _, _ = select.select([sys.stdin], [], [], key_timeout)
+    if rlist:
+        key = sys.stdin.read(1)
+    else:
+        key = ''
+    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+    return key
+
+
+def vels(speed, turn):
+    return "currently:\tspeed %s\tturn %s " % (speed,turn)
+
+if __name__=="__main__":
+    settings = termios.tcgetattr(sys.stdin)
+
+    rospy.init_node('teleop_twist_keyboard')
+
+    speed = rospy.get_param("~speed", 0.5)
+    turn = rospy.get_param("~turn", 1.0)
+    repeat = rospy.get_param("~repeat_rate", 0.0)
+    key_timeout = rospy.get_param("~key_timeout", 0.0)
+    if key_timeout == 0.0:
+        key_timeout = None
+
+    pub_thread = PublishThread(repeat)
+
+    x = 0
+    y = 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(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 = speed * speedBindings[key][0]
+                turn = turn * speedBindings[key][1]
+
+                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
+                z = 0
+                th = 0
+                if (key == '\x03'):
+                    break
+
+            pub_thread.update(x, y, z, th, speed, turn)
+
+    except Exception as e:
+        print(e)
+
+    finally:
+        pub_thread.stop()
+
+        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)