keyboard_teleop.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import roslib
3 #roslib.load_manifest('teleop_twist_keyboard')
4 import rospy
5 
6 from geometry_msgs.msg import Twist
7 
8 import sys, select, termios, tty
9 
10 msg = """
11 Reading from the keyboard and Publishing to Twist!
12 ---------------------------
13 Moving around:
14  u i o
15  j k l
16  m , .
17 For Holonomic mode (strafing), hold down the shift key:
18 ---------------------------
19  U I O
20  J K L
21  M < >
22 t : up (+z)
23 b : down (-z)
24 anything else : stop
25 q/z : increase/decrease max speeds by 10%
26 w/x : increase/decrease only linear speed by 10%
27 e/c : increase/decrease only angular speed by 10%
28 CTRL-C to quit
29 """
30 
31 moveBindings = {
32  'i':(1,0,0,0),
33  'o':(1,0,0,-1),
34  'j':(0,0,0,1),
35  'l':(0,0,0,-1),
36  'u':(1,0,0,1),
37  ',':(-1,0,0,0),
38  '.':(-1,0,0,1),
39  'm':(-1,0,0,-1),
40  'O':(1,-1,0,0),
41  'I':(1,0,0,0),
42  'J':(0,1,0,0),
43  'L':(0,-1,0,0),
44  'U':(1,1,0,0),
45  '<':(-1,0,0,0),
46  '>':(-1,-1,0,0),
47  'M':(-1,1,0,0),
48  't':(0,0,1,0),
49  'b':(0,0,-1,0),
50  }
51 
52 speedBindings={
53  'q':(1.1,1.1),
54  'z':(.9,.9),
55  'w':(1.1,1),
56  'x':(.9,1),
57  'e':(1,1.1),
58  'c':(1,.9),
59  }
60 
61 def getKey():
62  tty.setraw(sys.stdin.fileno())
63  select.select([sys.stdin], [], [], 0)
64  key = sys.stdin.read(1)
65  termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
66  return key
67 
68 
69 def vels(speed,turn):
70  return "currently:\tspeed %s\tturn %s " % (speed,turn)
71 
72 if __name__=="__main__":
73  settings = termios.tcgetattr(sys.stdin)
74 
75  pub = rospy.Publisher('/cmd_vel_mux/input/teleop', Twist, queue_size = 1)
76  rospy.init_node('teleop_twist_keyboard')
77 
78  speed = rospy.get_param("~speed", 0.5)
79  turn = rospy.get_param("~turn", 1.0)
80  x = 0
81  y = 0
82  z = 0
83  th = 0
84  status = 0
85 
86  try:
87  print msg
88  print vels(speed,turn)
89  while(1):
90  key = getKey()
91  if key in moveBindings.keys():
92  x = moveBindings[key][0]
93  y = moveBindings[key][1]
94  z = moveBindings[key][2]
95  th = moveBindings[key][3]
96  elif key in speedBindings.keys():
97  speed = speed * speedBindings[key][0]
98  turn = turn * speedBindings[key][1]
99 
100  print vels(speed,turn)
101  if (status == 14):
102  print msg
103  status = (status + 1) % 15
104  else:
105  x = 0
106  y = 0
107  z = 0
108  th = 0
109  if (key == '\x03'):
110  break
111 
112  twist = Twist()
113  twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed;
114  twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn
115  pub.publish(twist)
116 
117  except:
118  print e
119 
120  finally:
121  twist = Twist()
122  twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
123  twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
124  pub.publish(twist)
125 
126  termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
127 
def vels(speed, turn)


xbot_tools
Author(s):
autogenerated on Sat Oct 10 2020 03:28:22