From 7a2857468dd1ad56aa2083ea3a3a405a4314ed73 Mon Sep 17 00:00:00 2001 From: Long Wang Date: Tue, 11 Dec 2018 12:15:56 -0500 Subject: [PATCH 1/4] added 6D twist version that includes both linear and angular velocity --- CMakeLists.txt | 1 + teleop_twist_keyboard_6D.py | 158 ++++++++++++++++++++++++++++++++++++ 2 files changed, 159 insertions(+) create mode 100755 teleop_twist_keyboard_6D.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 86d90cb..a17e800 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,6 +9,7 @@ catkin_package() ## in contrast to setup.py, you can choose the destination catkin_install_python(PROGRAMS teleop_twist_keyboard.py + teleop_twist_keyboard_6D.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/teleop_twist_keyboard_6D.py b/teleop_twist_keyboard_6D.py new file mode 100755 index 0000000..4e0f324 --- /dev/null +++ b/teleop_twist_keyboard_6D.py @@ -0,0 +1,158 @@ +#!/usr/bin/env python + +from __future__ import print_function + +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) + +For Holonomic mode (strafing), hold down the shift key: +--------------------------- + 8 + 4 5 6 + 2 + +7 : roll left (-x) +9 : roll right (+x) + +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 = { + # [Translation] Basic moving mode + 'i':(1,0,0,0,0,0), + 'o':(1,0,0,0,0,-1), + 'j':(0,0,0,0,0,1), + 'l':(0,0,0,0,0,-1), + 'u':(1,0,0,0,0,1), + ',':(-1,0,0,0,0,0), + '.':(-1,0,0,0,0,1), + 'm':(-1,0,0,0,0,-1), + # [Translation] Holonomic moving mode + 'O':(1,-1,0,0,0,0), + 'I':(1,0,0,0,0,0), + 'J':(0,1,0,0,0,0), + 'L':(0,-1,0,0,0,0), + 'U':(1,1,0,0,0,0), + '<':(-1,0,0,0,0,0), + '>':(-1,-1,0,0,0,0), + 'M':(-1,1,0,0,0,0), + # [Translation] height change + 't':(0,0,1,0,0,0), + 'b':(0,0,-1,0,0,0), + # [Orientation] + '8':(0,0,0,0,1,0), + '2':(0,0,0,0,-1,0), + '4':(0,0,0,0,0,1), + '6':(0,0,0,0,0,-1), + '7':(0,0,0,-1,0,0), + '9':(0,0,0,1,0,0), + } + +speedBindings={ + 'q':(1.1,1.1), + 'z':(.9,.9), + 'w':(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 + + +def vels(speed,turn): + return "currently:\tspeed %s\tturn %s " % (speed,turn) + +if __name__=="__main__": + settings = termios.tcgetattr(sys.stdin) + + pub = rospy.Publisher('CartesianVelocityMove', Twist, queue_size = 1) + rospy.init_node('teleop_twist_keyboard') + + speed = rospy.get_param("~speed", 0.005) + turn = rospy.get_param("~turn", 0.1) + x = 0 + y = 0 + z = 0 + roll = 0 + pitch = 0 + yaw = 0 + status = 0 + + try: + print(msg) + print(vels(speed,turn)) + while(1): + key = getKey() + if key in moveBindings.keys(): + x = moveBindings[key][0] + y = moveBindings[key][1] + z = moveBindings[key][2] + roll = moveBindings[key][3] + pitch = moveBindings[key][4] + yaw = moveBindings[key][5] + 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 + y = 0 + z = 0 + roll = 0 + pitch = 0 + yaw = 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 = roll*turn; twist.angular.y = pitch*turn; twist.angular.z = yaw*turn + pub.publish(twist) + + 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) From ba57e48154accd37736e6d24d0870c8d0143def8 Mon Sep 17 00:00:00 2001 From: Long Wang Date: Tue, 11 Dec 2018 12:53:22 -0500 Subject: [PATCH 2/4] modified publisher to use twistStamped --- teleop_twist_keyboard_6D.py | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/teleop_twist_keyboard_6D.py b/teleop_twist_keyboard_6D.py index 4e0f324..8e9bcf0 100755 --- a/teleop_twist_keyboard_6D.py +++ b/teleop_twist_keyboard_6D.py @@ -5,12 +5,12 @@ import roslib; roslib.load_manifest('teleop_twist_keyboard') import rospy -from geometry_msgs.msg import Twist +from geometry_msgs.msg import Twist, TwistStamped import sys, select, termios, tty msg = """ -Reading from the keyboard and Publishing to Twist! +Reading from the keyboard and Publishing to TwistStamped! --------------------------- Moving around: u i o @@ -98,7 +98,7 @@ def vels(speed,turn): if __name__=="__main__": settings = termios.tcgetattr(sys.stdin) - pub = rospy.Publisher('CartesianVelocityMove', Twist, queue_size = 1) + pub = rospy.Publisher('CartesianVelocityMove', TwistStamped, queue_size = 1) rospy.init_node('teleop_twist_keyboard') speed = rospy.get_param("~speed", 0.005) @@ -144,7 +144,9 @@ def vels(speed,turn): twist = Twist() twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed; twist.angular.x = roll*turn; twist.angular.y = pitch*turn; twist.angular.z = yaw*turn - pub.publish(twist) + twist_pub = TwistStamped() + twist_pub.twist = twist + pub.publish(twist_pub) except Exception as e: print(e) @@ -153,6 +155,8 @@ def vels(speed,turn): 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) + twist_pub = TwistStamped() + twist_pub.twist = twist + pub.publish(twist_pub) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) From 2e6fb8ade0fc1e94a6ce6e1de5b737cc444d38a2 Mon Sep 17 00:00:00 2001 From: Long Wang Date: Tue, 11 Dec 2018 13:26:57 -0500 Subject: [PATCH 3/4] modified default velocities for UR5 --- teleop_twist_keyboard_6D.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/teleop_twist_keyboard_6D.py b/teleop_twist_keyboard_6D.py index 8e9bcf0..ed465d1 100755 --- a/teleop_twist_keyboard_6D.py +++ b/teleop_twist_keyboard_6D.py @@ -101,8 +101,8 @@ def vels(speed,turn): pub = rospy.Publisher('CartesianVelocityMove', TwistStamped, queue_size = 1) rospy.init_node('teleop_twist_keyboard') - speed = rospy.get_param("~speed", 0.005) - turn = rospy.get_param("~turn", 0.1) + speed = rospy.get_param("~speed", 0.01) + turn = rospy.get_param("~turn", 0.05) x = 0 y = 0 z = 0 From a89253ac7ed9949c11b7353b8d6a6778e56f4893 Mon Sep 17 00:00:00 2001 From: Long Wang Date: Tue, 11 Dec 2018 20:58:20 -0500 Subject: [PATCH 4/4] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 1b0dd84..2d2f764 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# teleop_twist_keyboard +# teleop_twist_keyboard [deprecated, to be deleted] Generic Keyboard Teleop for ROS # Launch