key_teleop.py
Go to the documentation of this file.
1 #! /usr/bin/env python
2 # -*- coding: utf-8 -*-
3 #
4 # Copyright (c) 2013 PAL Robotics SL.
5 # Released under the BSD License.
6 #
7 # Authors:
8 # * Siegfried-A. Gevatter
9 
10 import curses
11 import math
12 
13 import rospy
14 from geometry_msgs.msg import Twist
15 
16 class Velocity(object):
17 
18  def __init__(self, min_velocity, max_velocity, num_steps):
19  assert min_velocity > 0 and max_velocity > 0 and num_steps > 0
20  self._min = min_velocity
21  self._max = max_velocity
22  self._num_steps = num_steps
23  if self._num_steps > 1:
24  self._step_incr = (max_velocity - min_velocity) / (self._num_steps - 1)
25  else:
26  # If num_steps is one, we always use the minimum velocity.
27  self._step_incr = 0
28 
29  def __call__(self, value, step):
30  """
31  Takes a value in the range [0, 1] and the step and returns the
32  velocity (usually m/s or rad/s).
33  """
34  if step == 0:
35  return 0
36 
37  assert step > 0 and step <= self._num_steps
38  max_value = self._min + self._step_incr * (step - 1)
39  return value * max_value
40 
41 class TextWindow():
42 
43  _screen = None
44  _window = None
45  _num_lines = None
46 
47  def __init__(self, stdscr, lines=10):
48  self._screen = stdscr
49  self._screen.nodelay(True)
50  curses.curs_set(0)
51 
52  self._num_lines = lines
53 
54  def read_key(self):
55  keycode = self._screen.getch()
56  return keycode if keycode != -1 else None
57 
58  def clear(self):
59  self._screen.clear()
60 
61  def write_line(self, lineno, message):
62  if lineno < 0 or lineno >= self._num_lines:
63  raise ValueError, 'lineno out of bounds'
64  height, width = self._screen.getmaxyx()
65  y = (height / self._num_lines) * lineno
66  x = 10
67  for text in message.split('\n'):
68  text = text.ljust(width)
69  self._screen.addstr(y, x, text)
70  y += 1
71 
72  def refresh(self):
73  self._screen.refresh()
74 
75  def beep(self):
76  curses.flash()
77 
78 class KeyTeleop():
79 
80  _interface = None
81 
82  _linear = None
83  _angular = None
84 
85  def __init__(self, interface):
86  self._interface = interface
87  self._pub_cmd = rospy.Publisher('key_vel', Twist)
88 
89  self._hz = rospy.get_param('~hz', 10)
90 
91  self._num_steps = rospy.get_param('~turbo/steps', 4)
92 
93  forward_min = rospy.get_param('~turbo/linear_forward_min', 0.5)
94  forward_max = rospy.get_param('~turbo/linear_forward_max', 1.0)
95  self._forward = Velocity(forward_min, forward_max, self._num_steps)
96 
97  backward_min = rospy.get_param('~turbo/linear_backward_min', 0.25)
98  backward_max = rospy.get_param('~turbo/linear_backward_max', 0.5)
99  self._backward = Velocity(backward_min, backward_max, self._num_steps)
100 
101  angular_min = rospy.get_param('~turbo/angular_min', 0.7)
102  angular_max = rospy.get_param('~turbo/angular_max', 1.2)
103  self._rotation = Velocity(angular_min, angular_max, self._num_steps)
104 
105  def run(self):
106  self._linear = 0
107  self._angular = 0
108 
109  rate = rospy.Rate(self._hz)
110  while True:
111  keycode = self._interface.read_key()
112  if keycode:
113  if self._key_pressed(keycode):
114  self._publish()
115  else:
116  self._publish()
117  rate.sleep()
118 
119  def _get_twist(self, linear, angular):
120  twist = Twist()
121  if linear >= 0:
122  twist.linear.x = self._forward(1.0, linear)
123  else:
124  twist.linear.x = self._backward(-1.0, -linear)
125  twist.angular.z = self._rotation(math.copysign(1, angular), abs(angular))
126  return twist
127 
128  def _key_pressed(self, keycode):
129  movement_bindings = {
130  curses.KEY_UP: ( 1, 0),
131  curses.KEY_DOWN: (-1, 0),
132  curses.KEY_LEFT: ( 0, 1),
133  curses.KEY_RIGHT: ( 0, -1),
134  }
135  speed_bindings = {
136  ord(' '): (0, 0),
137  }
138  if keycode in movement_bindings:
139  acc = movement_bindings[keycode]
140  ok = False
141  if acc[0]:
142  linear = self._linear + acc[0]
143  if abs(linear) <= self._num_steps:
144  self._linear = linear
145  ok = True
146  if acc[1]:
147  angular = self._angular + acc[1]
148  if abs(angular) <= self._num_steps:
149  self._angular = angular
150  ok = True
151  if not ok:
152  self._interface.beep()
153  elif keycode in speed_bindings:
154  acc = speed_bindings[keycode]
155  # Note: bounds aren't enforced here!
156  if acc[0] is not None:
157  self._linear = acc[0]
158  if acc[1] is not None:
159  self._angular = acc[1]
160 
161  elif keycode == ord('q'):
162  rospy.signal_shutdown('Bye')
163  else:
164  return False
165 
166  return True
167 
168  def _publish(self):
169  self._interface.clear()
170  self._interface.write_line(2, 'Linear: %d, Angular: %d' % (self._linear, self._angular))
171  self._interface.write_line(5, 'Use arrow keys to move, space to stop, q to exit.')
172  self._interface.refresh()
173 
174  twist = self._get_twist(self._linear, self._angular)
175  self._pub_cmd.publish(twist)
176 
177 
179  def __init__(self, interface):
180  self._interface = interface
181  self._pub_cmd = rospy.Publisher('key_vel', Twist)
182 
183  self._hz = rospy.get_param('~hz', 10)
184 
185  self._forward_rate = rospy.get_param('~forward_rate', 0.8)
186  self._backward_rate = rospy.get_param('~backward_rate', 0.5)
187  self._rotation_rate = rospy.get_param('~rotation_rate', 1.0)
188  self._last_pressed = {}
189  self._angular = 0
190  self._linear = 0
191 
192  movement_bindings = {
193  curses.KEY_UP: ( 1, 0),
194  curses.KEY_DOWN: (-1, 0),
195  curses.KEY_LEFT: ( 0, 1),
196  curses.KEY_RIGHT: ( 0, -1),
197  }
198 
199  def run(self):
200  rate = rospy.Rate(self._hz)
201  self._running = True
202  while self._running:
203  while True:
204  keycode = self._interface.read_key()
205  if keycode is None:
206  break
207  self._key_pressed(keycode)
208  self._set_velocity()
209  self._publish()
210  rate.sleep()
211 
212  def _get_twist(self, linear, angular):
213  twist = Twist()
214  twist.linear.x = linear
215  twist.angular.z = angular
216  return twist
217 
218  def _set_velocity(self):
219  now = rospy.get_time()
220  keys = []
221  for a in self._last_pressed:
222  if now - self._last_pressed[a] < 0.4:
223  keys.append(a)
224  linear = 0.0
225  angular = 0.0
226  for k in keys:
227  l, a = self.movement_bindings[k]
228  linear += l
229  angular += a
230  if linear > 0:
231  linear = linear * self._forward_rate
232  else:
233  linear = linear * self._backward_rate
234  angular = angular * self._rotation_rate
235  self._angular = angular
236  self._linear = linear
237 
238  def _key_pressed(self, keycode):
239  if keycode == ord('q'):
240  self._running = False
241  rospy.signal_shutdown('Bye')
242  elif keycode in self.movement_bindings:
243  self._last_pressed[keycode] = rospy.get_time()
244 
245  def _publish(self):
246  self._interface.clear()
247  self._interface.write_line(2, 'Linear: %f, Angular: %f' % (self._linear, self._angular))
248  self._interface.write_line(5, 'Use arrow keys to move, q to exit.')
249  self._interface.refresh()
250 
251  twist = self._get_twist(self._linear, self._angular)
252  self._pub_cmd.publish(twist)
253 
254 
255 def main(stdscr):
256  rospy.init_node('key_teleop')
257  app = SimpleKeyTeleop(TextWindow(stdscr))
258  app.run()
259 
260 if __name__ == '__main__':
261  try:
262  curses.wrapper(main)
263  except rospy.ROSInterruptException:
264  pass
def _key_pressed(self, keycode)
Definition: key_teleop.py:238
def _get_twist(self, linear, angular)
Definition: key_teleop.py:119
def __init__(self, min_velocity, max_velocity, num_steps)
Definition: key_teleop.py:18
def main(stdscr)
Definition: key_teleop.py:255
def _get_twist(self, linear, angular)
Definition: key_teleop.py:212
def __init__(self, interface)
Definition: key_teleop.py:85
def __init__(self, interface)
Definition: key_teleop.py:179
def __init__(self, stdscr, lines=10)
Definition: key_teleop.py:47
def _key_pressed(self, keycode)
Definition: key_teleop.py:128
def write_line(self, lineno, message)
Definition: key_teleop.py:61
def __call__(self, value, step)
Definition: key_teleop.py:29


key_teleop
Author(s): Siegfried-A. Gevatter Pujals
autogenerated on Mon Jun 10 2019 15:31:10