forked from rohbotics/ros2_teleop_keyboard
/
teleop_twist_keyboard.py
executable file
·137 lines (111 loc) · 2.61 KB
/
teleop_twist_keyboard.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
#!/usr/bin/env python
import rclpy
from rclpy.qos import qos_profile_default
from geometry_msgs.msg import Twist
import sys, select, termios, tty
settings = termios.tcgetattr(sys.stdin)
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),
}
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)
def main(args=None):
if args is None:
args = sys.argv
rclpy.init(args)
node = rclpy.create_node('teleop_twist_keyboard')
pub = node.create_publisher(Twist, 'cmd_vel', qos_profile_default)
speed = 0.5
turn = 1.0
x = 0
y = 0
z = 0
th = 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]
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:
x = 0
y = 0
z = 0
th = 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.0; twist.angular.y = 0.0; twist.angular.z = th*turn
pub.publish(twist)
except:
print(e)
finally:
twist = Twist()
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
pub.publish(twist)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)