joy_teleop.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import importlib
3 
4 import rospy
5 import genpy.message
6 from rospy import ROSException
7 import sensor_msgs.msg
8 import actionlib
9 import rostopic
10 import rosservice
11 from threading import Thread
12 from rosservice import ROSServiceException
13 
14 class JoyTeleopException(Exception):
15  pass
16 
17 
18 class JoyTeleop:
19  """
20  Generic joystick teleoperation node.
21  Will not start without configuration, has to be stored in 'teleop' parameter.
22  See config/joy_teleop.yaml for an example.
23  """
24  def __init__(self):
25  if not rospy.has_param("teleop"):
26  rospy.logfatal("no configuration was found, taking node down")
27  raise JoyTeleopException("no config")
28 
29  self.publishers = {}
30  self.al_clients = {}
31  self.srv_clients = {}
32  self.service_types = {}
33  self.message_types = {}
34  self.command_list = {}
35  self.offline_actions = []
36  self.offline_services = []
37 
38  self.old_buttons = []
39 
40  teleop_cfg = rospy.get_param("teleop")
41 
42  for i in teleop_cfg:
43  if i in self.command_list:
44  rospy.logerr("command {} was duplicated".format(i))
45  continue
46  action_type = teleop_cfg[i]['type']
47  self.add_command(i, teleop_cfg[i])
48  if action_type == 'topic':
49  self.register_topic(i, teleop_cfg[i])
50  elif action_type == 'action':
51  self.register_action(i, teleop_cfg[i])
52  elif action_type == 'service':
53  self.register_service(i, teleop_cfg[i])
54  else:
55  rospy.logerr("unknown type '%s' for command '%s'", action_type, i)
56 
57  # Don't subscribe until everything has been initialized.
58  rospy.Subscriber('joy', sensor_msgs.msg.Joy, self.joy_callback)
59 
60  # Run a low-freq action updater
61  rospy.Timer(rospy.Duration(2.0), self.update_actions)
62 
63  def joy_callback(self, data):
64  try:
65  for c in self.command_list:
66  if self.match_command(c, data.buttons):
67  self.run_command(c, data)
68  except JoyTeleopException as e:
69  rospy.logerr("error while parsing joystick input: %s", str(e))
70  self.old_buttons = data.buttons
71 
72  def register_topic(self, name, command):
73  """Add a topic publisher for a joystick command"""
74  topic_name = command['topic_name']
75  try:
76  topic_type = self.get_message_type(command['message_type'])
77  self.publishers[topic_name] = rospy.Publisher(topic_name, topic_type, queue_size=1)
78  except JoyTeleopException as e:
79  rospy.logerr("could not register topic for command {}: {}".format(name, str(e)))
80 
81  def register_action(self, name, command):
82  """Add an action client for a joystick command"""
83  action_name = command['action_name']
84  try:
85  action_type = self.get_message_type(self.get_action_type(action_name))
86  self.al_clients[action_name] = actionlib.SimpleActionClient(action_name, action_type)
87  if action_name in self.offline_actions:
88  self.offline_actions.remove(action_name)
89  except JoyTeleopException:
90  if action_name not in self.offline_actions:
91  self.offline_actions.append(action_name)
92 
93  class AsyncServiceProxy(object):
94  def __init__(self, name, service_class, persistent=True):
95  try:
96  rospy.wait_for_service(name, timeout=2.0)
97  except ROSException:
98  raise JoyTeleopException("Service {} is not available".format(name))
99  self._service_proxy = rospy.ServiceProxy(name, service_class, persistent)
100  self._thread = Thread(target=self._service_proxy, args=[])
101 
102  def __del__(self):
103  # try to join our thread - no way I know of to interrupt a service
104  # request
105  if self._thread.is_alive():
106  self._thread.join(1.0)
107 
108  def __call__(self, request):
109  if self._thread.is_alive():
110  self._thread.join(0.01)
111  if self._thread.is_alive():
112  return False
113 
114  self._thread = Thread(target=self._service_proxy, args=[request])
115  self._thread.start()
116  return True
117 
118  def register_service(self, name, command):
119  """ Add an AsyncServiceProxy for a joystick command """
120  service_name = command['service_name']
121  try:
122  service_type = self.get_service_type(service_name)
123  self.srv_clients[service_name] = self.AsyncServiceProxy(
124  service_name,
125  service_type)
126 
127  if service_name in self.offline_services:
128  self.offline_services.remove(service_name)
129  except JoyTeleopException:
130  if service_name not in self.offline_services:
131  self.offline_services.append(service_name)
132 
133  def match_command(self, c, buttons):
134  """Find a command matching a joystick configuration"""
135  for b in self.command_list[c]['buttons']:
136  if b < 0 or len(buttons) <= b or buttons[b] != 1:
137  return False
138  return sum(buttons) >= len(self.command_list[c]['buttons'])
139 
140  def add_command(self, name, command):
141  """Add a command to the command list"""
142  if command['type'] == 'topic':
143  if 'deadman_buttons' not in command:
144  command['deadman_buttons'] = []
145  command['buttons'] = command['deadman_buttons']
146  elif command['type'] == 'action':
147  if 'action_goal' not in command:
148  command['action_goal'] = {}
149  elif command['type'] == 'service':
150  if 'service_request' not in command:
151  command['service_request'] = {}
152  self.command_list[name] = command
153 
154  def run_command(self, command, joy_state):
155  """Run a joystick command"""
156  cmd = self.command_list[command]
157  if cmd['type'] == 'topic':
158  self.run_topic(command, joy_state)
159  elif cmd['type'] == 'action':
160  if cmd['action_name'] in self.offline_actions:
161  rospy.logerr("command {} was not played because the action "
162  "server was unavailable. Trying to reconnect..."
163  .format(cmd['action_name']))
164  self.register_action(command, self.command_list[command])
165  else:
166  if joy_state.buttons != self.old_buttons:
167  self.run_action(command, joy_state)
168  elif cmd['type'] == 'service':
169  if cmd['service_name'] in self.offline_services:
170  rospy.logerr("command {} was not played because the service "
171  "server was unavailable. Trying to reconnect..."
172  .format(cmd['service_name']))
173  self.register_service(command, self.command_list[command])
174  else:
175  if joy_state.buttons != self.old_buttons:
176  self.run_service(command, joy_state)
177  else:
178  raise JoyTeleopException('command {} is neither a topic publisher nor an action or service client'
179  .format(command))
180 
181  def run_topic(self, c, joy_state):
182  cmd = self.command_list[c]
183  msg = self.get_message_type(cmd['message_type'])()
184 
185  if 'message_value' in cmd:
186  for param in cmd['message_value']:
187  self.set_member(msg, param['target'], param['value'])
188 
189  else:
190  for mapping in cmd['axis_mappings']:
191  if 'axis' in mapping:
192  if len(joy_state.axes) > mapping['axis']:
193  val = joy_state.axes[mapping['axis']] * mapping.get('scale', 1.0) + mapping.get('offset', 0.0)
194  else:
195  rospy.logerr('Joystick has only {} axes (indexed from 0), but #{} was referenced in config.'.format(len(joy_state.axes), mapping['axis']))
196  val = 0.0
197  elif 'button' in mapping:
198  if len(joy_state.buttons) > mapping['button']:
199  val = joy_state.buttons[mapping['button']] * mapping.get('scale', 1.0) + mapping.get('offset', 0.0)
200  else:
201  rospy.logerr('Joystick has only {} buttons (indexed from 0), but #{} was referenced in config.'.format(len(joy_state.buttons), mapping['button']))
202  val = 0.0
203  else:
204  rospy.logerr('No Supported axis_mappings type found in: {}'.format(mapping))
205  val = 0.0
206 
207  self.set_member(msg, mapping['target'], val)
208 
209  self.publishers[cmd['topic_name']].publish(msg)
210 
211  def run_action(self, c, joy_state):
212  cmd = self.command_list[c]
213  goal = self.get_message_type(self.get_action_type(cmd['action_name'])[:-6] + 'Goal')()
214  genpy.message.fill_message_args(goal, [cmd['action_goal']])
215  self.al_clients[cmd['action_name']].send_goal(goal)
216 
217  def run_service(self, c, joy_state):
218  cmd = self.command_list[c]
219  request = self.get_service_type(cmd['service_name'])._request_class()
220  # should work for requests, too
221  genpy.message.fill_message_args(request, [cmd['service_request']])
222  if not self.srv_clients[cmd['service_name']](request):
223  rospy.loginfo('Not sending new service request for command {} because previous request has not finished'
224  .format(c))
225 
226  def set_member(self, msg, member, value):
227  ml = member.split('.')
228  if len(ml) < 1:
229  return
230  target = msg
231  for i in ml[:-1]:
232  target = getattr(target, i)
233  setattr(target, ml[-1], value)
234 
235  def get_message_type(self, type_name):
236  if type_name not in self.message_types:
237  try:
238  package, message = type_name.split('/')
239  mod = importlib.import_module(package + '.msg')
240  self.message_types[type_name] = getattr(mod, message)
241  except ValueError:
242  raise JoyTeleopException("message type format error")
243  except ImportError:
244  raise JoyTeleopException("module {} could not be loaded".format(package))
245  except AttributeError:
246  raise JoyTeleopException("message {} could not be loaded from module {}".format(package, message))
247  return self.message_types[type_name]
248 
249  def get_action_type(self, action_name):
250  try:
251  return rostopic._get_topic_type(rospy.resolve_name(action_name) + '/goal')[0][:-4]
252  except TypeError:
253  raise JoyTeleopException("could not find action {}".format(action_name))
254 
255  def get_service_type(self, service_name):
256  if service_name not in self.service_types:
257  try:
258  self.service_types[service_name] = rosservice.get_service_class_by_name(service_name)
259  except ROSServiceException, e:
260  raise JoyTeleopException("service {} could not be loaded: {}".format(service_name, str(e)))
261  return self.service_types[service_name]
262 
263  def update_actions(self, evt=None):
264  for name, cmd in self.command_list.iteritems():
265  if cmd['type'] != 'action':
266  continue
267  if cmd['action_name'] in self.offline_actions:
268  self.register_action(name, cmd)
269 
270 
271 if __name__ == "__main__":
272  try:
273  rospy.init_node('joy_teleop')
274  jt = JoyTeleop()
275  rospy.spin()
276  except JoyTeleopException:
277  pass
278  except rospy.ROSInterruptException:
279  pass
def run_service(self, c, joy_state)
Definition: joy_teleop.py:217
def register_topic(self, name, command)
Definition: joy_teleop.py:72
def get_message_type(self, type_name)
Definition: joy_teleop.py:235
def run_topic(self, c, joy_state)
Definition: joy_teleop.py:181
def get_action_type(self, action_name)
Definition: joy_teleop.py:249
def __init__(self)
Definition: joy_teleop.py:24
def update_actions(self, evt=None)
Definition: joy_teleop.py:263
def get_service_type(self, service_name)
Definition: joy_teleop.py:255
def set_member(self, msg, member, value)
Definition: joy_teleop.py:226
def joy_callback(self, data)
Definition: joy_teleop.py:63
def run_command(self, command, joy_state)
Definition: joy_teleop.py:154
def run_action(self, c, joy_state)
Definition: joy_teleop.py:211
def add_command(self, name, command)
Definition: joy_teleop.py:140
def register_action(self, name, command)
Definition: joy_teleop.py:81
def __init__(self, name, service_class, persistent=True)
Definition: joy_teleop.py:94
def match_command(self, c, buttons)
Definition: joy_teleop.py:133
def register_service(self, name, command)
Definition: joy_teleop.py:118


joy_teleop
Author(s): Paul Mathieu
autogenerated on Mon Jun 10 2019 15:31:14