mir_bridge.py
Go to the documentation of this file.
1 #!/usr/bin/env python3
2 # Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors
3 #
4 # Redistribution and use in source and binary forms, with or without
5 # modification, are permitted provided that the following conditions are met:
6 #
7 # * Redistributions of source code must retain the above copyright
8 # notice, this list of conditions and the following disclaimer.
9 #
10 # * Redistributions in binary form must reproduce the above copyright
11 # notice, this list of conditions and the following disclaimer in the
12 # documentation and/or other materials provided with the distribution.
13 #
14 # * Neither the name of the copyright holder nor the names of its
15 # contributors may be used to endorse or promote products derived from
16 # this software without specific prior written permission.
17 #
18 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
22 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 # POSSIBILITY OF SUCH DAMAGE.
29 #
30 # Author: Martin Günther
31 
32 import rospy
33 
34 import copy
35 import sys
36 from collections.abc import Iterable
37 
38 from mir_driver import rosbridge
39 from rospy_message_converter import message_converter
40 
41 from actionlib import SimpleActionClient
42 import actionlib_msgs.msg
43 import diagnostic_msgs.msg
44 import dynamic_reconfigure.msg
45 import geometry_msgs.msg
46 import mir_actions.msg
47 import mir_msgs.msg
48 import move_base_msgs.msg
49 import nav_msgs.msg
50 import rosgraph_msgs.msg
51 import sdc21x0.msg
52 import sensor_msgs.msg
53 import std_msgs.msg
54 import tf2_msgs.msg
55 import visualization_msgs.msg
56 
57 from collections import OrderedDict
58 
59 tf_prefix = ''
60 static_transforms = OrderedDict()
61 
62 
63 class TopicConfig(object):
64  def __init__(self, topic, topic_type, latch=False, dict_filter=None):
65  self.topic = topic
66  self.topic_type = topic_type
67  self.latch = latch
68  self.dict_filter = dict_filter
69 
70 
71 # remap mir_actions/MirMoveBaseAction => move_base_msgs/MoveBaseAction
73  filtered_msg_dict = copy.deepcopy(msg_dict)
74  filtered_msg_dict['goal']['move_task'] = mir_actions.msg.MirMoveBaseGoal.GLOBAL_MOVE
75  filtered_msg_dict['goal']['goal_dist_threshold'] = 0.25
76  filtered_msg_dict['goal']['clear_costmaps'] = True
77  return filtered_msg_dict
78 
79 
81  # filter out slots from the dict that are not in our message definition
82  # e.g., MiRMoveBaseFeedback has the field "state", which MoveBaseFeedback doesn't
83  filtered_msg_dict = copy.deepcopy(msg_dict)
84  filtered_msg_dict['feedback'] = {
85  key: msg_dict['feedback'][key] for key in move_base_msgs.msg.MoveBaseFeedback.__slots__
86  }
87  return filtered_msg_dict
88 
89 
91  filtered_msg_dict = copy.deepcopy(msg_dict)
92  filtered_msg_dict['result'] = {key: msg_dict['result'][key] for key in move_base_msgs.msg.MoveBaseResult.__slots__}
93  return filtered_msg_dict
94 
95 
96 def _cmd_vel_dict_filter(msg_dict):
97  """
98  Convert Twist to TwistStamped.
99 
100  Convert a geometry_msgs/Twist message dict (as sent from the ROS side) to
101  a geometry_msgs/TwistStamped message dict (as expected by the MiR on
102  software version >=2.7).
103  """
104  header = std_msgs.msg.Header(frame_id='', stamp=rospy.Time.now())
105  filtered_msg_dict = {
106  'header': message_converter.convert_ros_message_to_dictionary(header),
107  'twist': copy.deepcopy(msg_dict),
108  }
109  return filtered_msg_dict
110 
111 
112 def _tf_dict_filter(msg_dict):
113  filtered_msg_dict = copy.deepcopy(msg_dict)
114  for transform in filtered_msg_dict['transforms']:
115  transform['child_frame_id'] = tf_prefix + '/' + transform['child_frame_id'].strip('/')
116  return filtered_msg_dict
117 
118 
120  """
121  Cache tf_static messages (simulate latching).
122 
123  The tf_static topic needs special handling. Publishers on tf_static are *latched*, which means that the ROS master
124  caches the last message that was sent by each publisher on that topic, and will forward it to new subscribers.
125  However, since the mir_driver node appears to the ROS master as a single node with a single publisher on tf_static,
126  and there are multiple actual publishers hiding behind it on the MiR side, only one of those messages will be
127  cached. Therefore, we need to implement the caching ourselves and make sure that we always publish the full set of
128  transforms as a single message.
129  """
130  global static_transforms
131 
132  # prepend tf_prefix
133  filtered_msg_dict = _tf_dict_filter(msg_dict)
134 
135  # The following code was copied + modified from https://github.com/tradr-project/static_transform_mux .
136 
137  # Process the incoming transforms, merge them with our cache.
138  for transform in filtered_msg_dict['transforms']:
139  key = transform['child_frame_id']
140  rospy.loginfo(
141  "[%s] tf_static: updated transform %s->%s.",
142  rospy.get_name(),
143  transform['header']['frame_id'],
144  transform['child_frame_id'],
145  )
146  static_transforms[key] = transform
147 
148  # Return the cached messages.
149  filtered_msg_dict['transforms'] = static_transforms.values()
150  rospy.loginfo(
151  "[%s] tf_static: sent %i transforms: %s",
152  rospy.get_name(),
153  len(static_transforms),
154  str(static_transforms.keys()),
155  )
156  return filtered_msg_dict
157 
158 
160  # filtered_msg_dict = copy.deepcopy(msg_dict)
161  if not isinstance(msg_dict, dict): # can happen during recursion
162  return
163  for key, value in msg_dict.items():
164  if key == 'header':
165  try:
166  # prepend frame_id
167  frame_id = value['frame_id'].strip('/')
168  if frame_id != 'map':
169  # prepend tf_prefix, then remove leading '/' (e.g., when tf_prefix is empty)
170  value['frame_id'] = (tf_prefix + '/' + frame_id).strip('/')
171  else:
172  value['frame_id'] = frame_id
173 
174  except TypeError:
175  pass # value is not a dict
176  except KeyError:
177  pass # value doesn't have key 'frame_id'
178  elif isinstance(value, dict):
180  elif isinstance(value, Iterable): # an Iterable other than dict (e.g., a list)
181  for item in value:
183  return msg_dict
184 
185 
187  # filtered_msg_dict = copy.deepcopy(msg_dict)
188  if not isinstance(msg_dict, dict): # can happen during recursion
189  return
190  for key, value in msg_dict.items():
191  if key == 'header':
192  try:
193  # remove frame_id
194  s = value['frame_id'].strip('/')
195  if s.find(tf_prefix) == 0:
196  value['frame_id'] = (s[len(tf_prefix) :]).strip('/') # strip off tf_prefix, then strip leading '/'
197  except TypeError:
198  pass # value is not a dict
199  except KeyError:
200  pass # value doesn't have key 'frame_id'
201  elif isinstance(value, dict):
203  elif isinstance(value, Iterable): # an Iterable other than dict (e.g., a list)
204  for item in value:
206  return msg_dict
207 
208 
209 # topics we want to publish to ROS (and subscribe to from the MiR)
210 PUB_TOPICS = [
211  # TopicConfig('LightCtrl/bms_data', mir_msgs.msg.BMSData),
212  # TopicConfig('LightCtrl/charging_state', mir_msgs.msg.ChargingState),
213  TopicConfig('LightCtrl/us_list', sensor_msgs.msg.Range),
214  # TopicConfig('MC/battery_currents', mir_msgs.msg.BatteryCurrents),
215  # TopicConfig('MC/battery_voltage', std_msgs.msg.Float64),
216  TopicConfig('MC/currents', sdc21x0.msg.MotorCurrents),
217  # TopicConfig('MC/encoders', sdc21x0.msg.StampedEncoders),
218  TopicConfig('MissionController/CheckArea/visualization_marker', visualization_msgs.msg.Marker),
219  # TopicConfig('MissionController/goal_position_guid', std_msgs.msg.String),
220  # TopicConfig('MissionController/prompt_user', mir_msgs.msg.UserPrompt),
221  TopicConfig('SickPLC/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
222  TopicConfig('SickPLC/parameter_updates', dynamic_reconfigure.msg.Config),
223  # TopicConfig('active_mapping_guid', std_msgs.msg.String),
224  TopicConfig('amcl_pose', geometry_msgs.msg.PoseWithCovarianceStamped),
225  TopicConfig('b_raw_scan', sensor_msgs.msg.LaserScan),
226  TopicConfig('b_scan', sensor_msgs.msg.LaserScan),
227  TopicConfig('camera_floor/background', sensor_msgs.msg.PointCloud2),
228  TopicConfig('camera_floor/depth/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
229  TopicConfig('camera_floor/depth/parameter_updates', dynamic_reconfigure.msg.Config),
230  TopicConfig('camera_floor/depth/points', sensor_msgs.msg.PointCloud2),
231  TopicConfig('camera_floor/filter/visualization_marker', visualization_msgs.msg.Marker),
232  TopicConfig('camera_floor/floor', sensor_msgs.msg.PointCloud2),
233  TopicConfig('camera_floor/obstacles', sensor_msgs.msg.PointCloud2),
234  TopicConfig('check_area/polygon', geometry_msgs.msg.PolygonStamped),
235  # TopicConfig('check_pose_area/polygon', geometry_msgs.msg.PolygonStamped),
236  # TopicConfig('data_events/area_events', mir_data_msgs.msg.AreaEventEvent),
237  # TopicConfig('data_events/maps', mir_data_msgs.msg.MapEvent),
238  # TopicConfig('data_events/positions', mir_data_msgs.msg.PositionEvent),
239  # TopicConfig('data_events/registers', mir_data_msgs.msg.PLCRegisterEvent),
240  # TopicConfig('data_events/sounds', mir_data_msgs.msg.SoundEvent),
241  TopicConfig('diagnostics', diagnostic_msgs.msg.DiagnosticArray),
242  TopicConfig('diagnostics_agg', diagnostic_msgs.msg.DiagnosticArray),
243  TopicConfig('diagnostics_toplevel_state', diagnostic_msgs.msg.DiagnosticStatus),
244  TopicConfig('f_raw_scan', sensor_msgs.msg.LaserScan),
245  TopicConfig('f_scan', sensor_msgs.msg.LaserScan),
246  TopicConfig('imu_data', sensor_msgs.msg.Imu),
247  TopicConfig('laser_back/driver/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
248  TopicConfig('laser_back/driver/parameter_updates', dynamic_reconfigure.msg.Config),
249  TopicConfig('laser_front/driver/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
250  TopicConfig('laser_front/driver/parameter_updates', dynamic_reconfigure.msg.Config),
251  # TopicConfig('localization_score', std_msgs.msg.Float64),
252  TopicConfig('/map', nav_msgs.msg.OccupancyGrid, latch=True),
253  TopicConfig('/map_metadata', nav_msgs.msg.MapMetaData),
254  # TopicConfig('marker_tracking_node/feedback', mir_marker_tracking.msg.MarkerTrackingActionFeedback),
255  # TopicConfig(
256  # 'marker_tracking_node/laser_line_extract/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription
257  # ),
258  # TopicConfig('marker_tracking_node/laser_line_extract/parameter_updates', dynamic_reconfigure.msg.Config),
259  # TopicConfig('marker_tracking_node/laser_line_extract/visualization_marker', visualization_msgs.msg.Marker),
260  # TopicConfig('marker_tracking_node/result', mir_marker_tracking.msg.MarkerTrackingActionResult),
261  # TopicConfig('marker_tracking_node/status', actionlib_msgs.msg.GoalStatusArray),
262  # TopicConfig('mirEventTrigger/events', mir_msgs.msg.Events),
263  TopicConfig('mir_amcl/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
264  TopicConfig('mir_amcl/parameter_updates', dynamic_reconfigure.msg.Config),
265  TopicConfig('mir_amcl/selected_points', sensor_msgs.msg.PointCloud2),
266  TopicConfig('mir_log', rosgraph_msgs.msg.Log),
267  # TopicConfig('mir_sound/sound_event', mir_msgs.msg.SoundEvent),
268  TopicConfig('mir_status_msg', std_msgs.msg.String),
269  # TopicConfig('mirspawn/node_events', mirSpawn.msg.LaunchItem),
270  TopicConfig('mirwebapp/grid_map_metadata', mir_msgs.msg.LocalMapStat),
271  TopicConfig('mirwebapp/laser_map_metadata', mir_msgs.msg.LocalMapStat),
272  # TopicConfig('mirwebapp/web_path', mir_msgs.msg.WebPath),
273  # really mir_actions/MirMoveBaseActionFeedback:
274  TopicConfig(
275  'move_base/feedback', move_base_msgs.msg.MoveBaseActionFeedback, dict_filter=_move_base_feedback_dict_filter
276  ),
277  # really mir_actions/MirMoveBaseActionResult:
278  TopicConfig('move_base/result', move_base_msgs.msg.MoveBaseActionResult, dict_filter=_move_base_result_dict_filter),
279  TopicConfig('move_base/status', actionlib_msgs.msg.GoalStatusArray),
280  # TopicConfig('move_base_node/MIRPlannerROS/cost_cloud', sensor_msgs.msg.PointCloud2),
281  # TopicConfig('move_base_node/MIRPlannerROS/global_plan', nav_msgs.msg.Path),
282  # TopicConfig('move_base_node/MIRPlannerROS/len_to_goal', std_msgs.msg.Float64),
283  TopicConfig('move_base_node/MIRPlannerROS/local_plan', nav_msgs.msg.Path),
284  # TopicConfig('move_base_node/MIRPlannerROS/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
285  # TopicConfig('move_base_node/MIRPlannerROS/parameter_updates', dynamic_reconfigure.msg.Config),
286  TopicConfig('move_base_node/MIRPlannerROS/updated_global_plan', mir_msgs.msg.PlanSegments),
287  # TopicConfig('move_base_node/MIRPlannerROS/visualization_marker', visualization_msgs.msg.MarkerArray),
288  TopicConfig('move_base_node/SBPLLatticePlanner/plan', nav_msgs.msg.Path),
289  # TopicConfig(
290  # 'move_base_node/SBPLLatticePlanner/sbpl_lattice_planner_stats', sbpl_lattice_planner.msg.SBPLLatticePlannerStats
291  # ),
292  # TopicConfig('move_base_node/SBPLLatticePlanner/visualization_marker', visualization_msgs.msg.MarkerArray),
293  TopicConfig('move_base_node/current_goal', geometry_msgs.msg.PoseStamped),
294  # TopicConfig('move_base_node/global_costmap/inflated_obstacles', nav_msgs.msg.GridCells),
295  # TopicConfig('move_base_node/global_costmap/obstacles', nav_msgs.msg.GridCells),
296  # TopicConfig('move_base_node/global_costmap/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
297  # TopicConfig('move_base_node/global_costmap/parameter_updates', dynamic_reconfigure.msg.Config),
298  # TopicConfig('move_base_node/global_costmap/robot_footprint', geometry_msgs.msg.PolygonStamped),
299  # TopicConfig('move_base_node/global_costmap/unknown_space', nav_msgs.msg.GridCells),
300  # TopicConfig('move_base_node/global_plan', nav_msgs.msg.Path),
301  TopicConfig('move_base_node/local_costmap/inflated_obstacles', nav_msgs.msg.GridCells),
302  TopicConfig('move_base_node/local_costmap/obstacles', nav_msgs.msg.GridCells),
303  # TopicConfig('move_base_node/local_costmap/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
304  # TopicConfig('move_base_node/local_costmap/parameter_updates', dynamic_reconfigure.msg.Config),
305  TopicConfig('move_base_node/local_costmap/robot_footprint', geometry_msgs.msg.PolygonStamped),
306  # TopicConfig('move_base_node/local_costmap/safety_zone', geometry_msgs.msg.PolygonStamped),
307  # TopicConfig('move_base_node/local_costmap/unknown_space', nav_msgs.msg.GridCells),
308  # TopicConfig('move_base_node/mir_escape_recovery/visualization_marker', visualization_msgs.msg.Marker),
309  # TopicConfig('move_base_node/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
310  # TopicConfig('move_base_node/parameter_updates', dynamic_reconfigure.msg.Config),
311  TopicConfig('move_base_node/time_to_coll', std_msgs.msg.Float64),
312  TopicConfig('move_base_node/traffic_costmap/inflated_obstacles', nav_msgs.msg.GridCells),
313  TopicConfig('move_base_node/traffic_costmap/obstacles', nav_msgs.msg.GridCells),
314  TopicConfig('move_base_node/traffic_costmap/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
315  TopicConfig('move_base_node/traffic_costmap/parameter_updates', dynamic_reconfigure.msg.Config),
316  TopicConfig('move_base_node/traffic_costmap/robot_footprint', geometry_msgs.msg.PolygonStamped),
317  TopicConfig('move_base_node/traffic_costmap/unknown_space', nav_msgs.msg.GridCells),
318  TopicConfig('move_base_node/visualization_marker', visualization_msgs.msg.Marker),
319  TopicConfig('move_base_simple/visualization_marker', visualization_msgs.msg.Marker),
320  TopicConfig('odom', nav_msgs.msg.Odometry),
321  TopicConfig('odom_enc', nav_msgs.msg.Odometry),
322  # TopicConfig('one_way_map', nav_msgs.msg.OccupancyGrid),
323  # TopicConfig('param_update', std_msgs.msg.String),
324  # TopicConfig('particlevizmarker', visualization_msgs.msg.MarkerArray),
325  # TopicConfig('resource_tracker/needed_resources', mir_msgs.msg.ResourcesState),
326  TopicConfig('robot_mode', mir_msgs.msg.RobotMode),
327  TopicConfig('robot_pose', geometry_msgs.msg.Pose),
328  TopicConfig('robot_state', mir_msgs.msg.RobotState),
329  # TopicConfig('robot_status', mir_msgs.msg.RobotStatus),
330  TopicConfig('/rosout', rosgraph_msgs.msg.Log),
331  TopicConfig('/rosout_agg', rosgraph_msgs.msg.Log),
332  TopicConfig('scan', sensor_msgs.msg.LaserScan),
333  # TopicConfig('scan_filter/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
334  # TopicConfig('scan_filter/parameter_updates', dynamic_reconfigure.msg.Config),
335  TopicConfig('scan_filter/visualization_marker', visualization_msgs.msg.Marker),
336  # TopicConfig('session_importer_node/info', mirSessionImporter.msg.SessionImportInfo),
337  # TopicConfig('set_mc_PID', std_msgs.msg.Float64MultiArray),
338  TopicConfig('/tf', tf2_msgs.msg.TFMessage, dict_filter=_tf_dict_filter),
339  TopicConfig('/tf_static', tf2_msgs.msg.TFMessage, dict_filter=_tf_static_dict_filter, latch=True),
340  # TopicConfig('traffic_map', nav_msgs.msg.OccupancyGrid),
341  # TopicConfig('wifi_diagnostics', diagnostic_msgs.msg.DiagnosticArray),
342  # TopicConfig('wifi_diagnostics/cur_ap', mir_wifi_msgs.msg.APInfo),
343  # TopicConfig('wifi_diagnostics/roam_events', mir_wifi_msgs.msg.WifiRoamEvent),
344  # TopicConfig('wifi_diagnostics/wifi_ap_interface_stats', mir_wifi_msgs.msg.WifiInterfaceStats),
345  # TopicConfig('wifi_diagnostics/wifi_ap_rssi', mir_wifi_msgs.msg.APRssiStats),
346  # TopicConfig('wifi_diagnostics/wifi_ap_time_stats', mir_wifi_msgs.msg.APTimeStats),
347  # TopicConfig('wifi_watchdog/ping', mir_wifi_msgs.msg.APPingStats),
348 ]
349 
350 # topics we want to subscribe to from ROS (and publish to the MiR)
351 SUB_TOPICS = [
352  # really geometry_msgs.msg.TwistStamped:
353  TopicConfig('cmd_vel', geometry_msgs.msg.Twist, dict_filter=_cmd_vel_dict_filter),
354  TopicConfig('initialpose', geometry_msgs.msg.PoseWithCovarianceStamped),
355  TopicConfig('light_cmd', std_msgs.msg.String),
356  TopicConfig('mir_cmd', std_msgs.msg.String),
357  TopicConfig('move_base/cancel', actionlib_msgs.msg.GoalID),
358  # really mir_actions/MirMoveBaseActionGoal:
359  TopicConfig('move_base/goal', move_base_msgs.msg.MoveBaseActionGoal, dict_filter=_move_base_goal_dict_filter),
360 ]
361 
362 
363 class PublisherWrapper(rospy.SubscribeListener):
364  def __init__(self, topic_config, robot):
365  self.topic_config = topic_config
366  self.robot = robot
367  self.connected = False
368  # Use topic_config.topic directly here. If it does not have a leading slash, it will use the private namespace.
369  self.pub = rospy.Publisher(
370  name=topic_config.topic,
371  data_class=topic_config.topic_type,
372  subscriber_listener=self,
373  latch=topic_config.latch,
374  queue_size=10,
375  )
376  rospy.loginfo(
377  "[%s] publishing topic '%s' [%s]", rospy.get_name(), topic_config.topic, topic_config.topic_type._type
378  )
379  # latched topics must be subscribed immediately
380  if topic_config.latch:
381  self.peer_subscribe("", None, None)
382 
383  def peer_subscribe(self, topic_name, topic_publish, peer_publish):
384  if not self.connected:
385  self.connected = True
386  rospy.loginfo("[%s] starting to stream messages on topic '%s'", rospy.get_name(), self.topic_config.topic)
387  absolute_topic = '/' + self.topic_config.topic.lstrip('/') # ensure exactly 1 leading slash for MiR comm
388  self.robot.subscribe(topic=absolute_topic, callback=self.callback)
389 
390  def peer_unsubscribe(self, topic_name, num_peers):
391  pass
392  # doesn't work: once ubsubscribed, robot doesn't let us subscribe again
393  # if self.connected and self.pub.get_num_connections() == 0 and not self.topic_config.latch:
394  # self.connected = False
395  # rospy.loginfo("[%s] stopping to stream messages on topic '%s'", rospy.get_name(), self.topic_config.topic)
396  # absolute_topic = '/' + self.topic_config.topic.lstrip('/') # ensure exactly 1 leading slash for MiR comm
397  # self.robot.unsubscribe(topic=absolute_topic)
398 
399  def callback(self, msg_dict):
400  msg_dict = _prepend_tf_prefix_dict_filter(msg_dict)
401  if self.topic_config.dict_filter is not None:
402  msg_dict = self.topic_config.dict_filter(msg_dict)
403  msg = message_converter.convert_dictionary_to_ros_message(self.topic_config.topic_type._type, msg_dict)
404  self.pub.publish(msg)
405 
406 
407 class SubscriberWrapper(object):
408  def __init__(self, topic_config, robot):
409  self.topic_config = topic_config
410  self.robot = robot
411  # Use topic_config.topic directly here. If it does not have a leading slash, it will use the private namespace.
412  self.sub = rospy.Subscriber(
413  name=topic_config.topic, data_class=topic_config.topic_type, callback=self.callback, queue_size=10
414  )
415  rospy.loginfo(
416  "[%s] subscribing to topic '%s' [%s]", rospy.get_name(), topic_config.topic, topic_config.topic_type._type
417  )
418 
419  def callback(self, msg):
420  msg_dict = message_converter.convert_ros_message_to_dictionary(msg)
421  msg_dict = _remove_tf_prefix_dict_filter(msg_dict)
422  if self.topic_config.dict_filter is not None:
423  msg_dict = self.topic_config.dict_filter(msg_dict)
424  absolute_topic = '/' + self.topic_config.topic.lstrip('/') # ensure exactly 1 leading slash for MiR comm
425  self.robot.publish(absolute_topic, msg_dict)
426 
427 
428 class MiRBridge(object):
429  def __init__(self):
430  try:
431  hostname = rospy.get_param('~hostname')
432  except KeyError:
433  rospy.logfatal('[%s] parameter "hostname" is not set!', rospy.get_name())
434  sys.exit(-1)
435  port = rospy.get_param('~port', 9090)
436 
437  global tf_prefix
438  tf_prefix = rospy.get_param('~tf_prefix', '').strip('/')
439 
440  rospy.loginfo('[%s] trying to connect to %s:%i...', rospy.get_name(), hostname, port)
441  self.robot = rosbridge.RosbridgeSetup(hostname, port)
442 
443  r = rospy.Rate(10)
444  i = 1
445  while not self.robot.is_connected():
446  if rospy.is_shutdown():
447  sys.exit(0)
448  if self.robot.is_errored():
449  rospy.logfatal('[%s] connection error to %s:%i, giving up!', rospy.get_name(), hostname, port)
450  sys.exit(-1)
451  if i % 10 == 0:
452  rospy.logwarn('[%s] still waiting for connection to %s:%i...', rospy.get_name(), hostname, port)
453  i += 1
454  r.sleep()
455 
456  rospy.loginfo('[%s] ... connected.', rospy.get_name())
457 
458  topics = self.get_topics()
459  published_topics = [topic_name for (topic_name, _, has_publishers, _) in topics if has_publishers]
460  subscribed_topics = [topic_name for (topic_name, _, _, has_subscribers) in topics if has_subscribers]
461 
462  for pub_topic in PUB_TOPICS:
463  PublisherWrapper(pub_topic, self.robot)
464  absolute_topic = '/' + pub_topic.topic.lstrip('/') # ensure exactly 1 leading slash for MiR comm
465  if absolute_topic not in published_topics:
466  rospy.logwarn("[%s] topic '%s' is not published by the MiR!", rospy.get_name(), pub_topic.topic)
467 
468  for sub_topic in SUB_TOPICS:
469  SubscriberWrapper(sub_topic, self.robot)
470  absolute_topic = '/' + sub_topic.topic.lstrip('/') # ensure exactly 1 leading slash for MiR comm
471  if absolute_topic not in subscribed_topics:
472  rospy.logwarn("[%s] topic '%s' is not yet subscribed to by the MiR!", rospy.get_name(), sub_topic.topic)
473 
474  # At least with software version 2.8 there were issues when forwarding a simple goal to the robot
475  # This workaround converts it into an action. Check https://github.com/DFKI-NI/mir_robot/issues/60 for details.
476  self._move_base_client = SimpleActionClient('move_base', move_base_msgs.msg.MoveBaseAction)
477  rospy.Subscriber("move_base_simple/goal", geometry_msgs.msg.PoseStamped, self._move_base_simple_goal_callback)
478 
479  def get_topics(self):
480  srv_response = self.robot.callService('/rosapi/topics', msg={})
481  topic_names = sorted(srv_response['topics'])
482  topics = []
483 
484  for topic_name in topic_names:
485  srv_response = self.robot.callService("/rosapi/topic_type", msg={'topic': topic_name})
486  topic_type = srv_response['type']
487 
488  srv_response = self.robot.callService("/rosapi/publishers", msg={'topic': topic_name})
489  has_publishers = True if len(srv_response['publishers']) > 0 else False
490 
491  srv_response = self.robot.callService("/rosapi/subscribers", msg={'topic': topic_name})
492  has_subscribers = True if len(srv_response['subscribers']) > 0 else False
493 
494  topics.append([topic_name, topic_type, has_publishers, has_subscribers])
495 
496  print('Publishers:')
497  for topic_name, topic_type, has_publishers, has_subscribers in topics:
498  if has_publishers:
499  print((' * %s [%s]' % (topic_name, topic_type)))
500 
501  print('\nSubscribers:')
502  for topic_name, topic_type, has_publishers, has_subscribers in topics:
503  if has_subscribers:
504  print((' * %s [%s]' % (topic_name, topic_type)))
505 
506  return topics
507 
509  if not self._move_base_client.wait_for_server(rospy.Duration(2)):
510  rospy.logwarn("Could not connect to 'move_base' server after two seconds. Dropping goal.")
511  rospy.logwarn("Did you activate 'planner' in the MIR web interface?")
512  return
513  goal = move_base_msgs.msg.MoveBaseGoal()
514  goal.target_pose.header = copy.deepcopy(msg.header)
515  goal.target_pose.pose = copy.deepcopy(msg.pose)
516  self._move_base_client.send_goal(goal)
517 
518 
519 def main():
520  rospy.init_node('mir_bridge')
521  MiRBridge()
522  rospy.spin()
523 
524 
525 if __name__ == '__main__':
526  try:
527  main()
528  except rospy.ROSInterruptException:
529  pass
mir_bridge.main
def main()
Definition: mir_bridge.py:519
mir_bridge.PublisherWrapper.peer_unsubscribe
def peer_unsubscribe(self, topic_name, num_peers)
Definition: mir_bridge.py:390
mir_bridge._move_base_goal_dict_filter
def _move_base_goal_dict_filter(msg_dict)
Definition: mir_bridge.py:72
mir_bridge._tf_static_dict_filter
def _tf_static_dict_filter(msg_dict)
Definition: mir_bridge.py:119
mir_bridge.PublisherWrapper.robot
robot
Definition: mir_bridge.py:366
mir_bridge._cmd_vel_dict_filter
def _cmd_vel_dict_filter(msg_dict)
Definition: mir_bridge.py:96
mir_bridge.TopicConfig.dict_filter
dict_filter
Definition: mir_bridge.py:68
mir_bridge.TopicConfig.topic
topic
Definition: mir_bridge.py:65
mir_bridge.PublisherWrapper.peer_subscribe
def peer_subscribe(self, topic_name, topic_publish, peer_publish)
Definition: mir_bridge.py:383
mir_bridge.SubscriberWrapper.callback
def callback(self, msg)
Definition: mir_bridge.py:419
mir_bridge.TopicConfig.topic_type
topic_type
Definition: mir_bridge.py:66
mir_bridge._move_base_result_dict_filter
def _move_base_result_dict_filter(msg_dict)
Definition: mir_bridge.py:90
mir_bridge.SubscriberWrapper.__init__
def __init__(self, topic_config, robot)
Definition: mir_bridge.py:408
mir_bridge._remove_tf_prefix_dict_filter
def _remove_tf_prefix_dict_filter(msg_dict)
Definition: mir_bridge.py:186
actionlib::SimpleActionClient
mir_bridge.PublisherWrapper.callback
def callback(self, msg_dict)
Definition: mir_bridge.py:399
mir_bridge.MiRBridge.get_topics
def get_topics(self)
Definition: mir_bridge.py:479
mir_bridge.PublisherWrapper.topic_config
topic_config
Definition: mir_bridge.py:365
mir_bridge.MiRBridge.robot
robot
Definition: mir_bridge.py:441
mir_bridge.TopicConfig.__init__
def __init__(self, topic, topic_type, latch=False, dict_filter=None)
Definition: mir_bridge.py:64
mir_bridge.SubscriberWrapper.sub
sub
Definition: mir_bridge.py:412
mir_bridge.SubscriberWrapper.topic_config
topic_config
Definition: mir_bridge.py:409
mir_bridge.SubscriberWrapper.robot
robot
Definition: mir_bridge.py:410
mir_bridge._move_base_feedback_dict_filter
def _move_base_feedback_dict_filter(msg_dict)
Definition: mir_bridge.py:80
mir_bridge.MiRBridge._move_base_client
_move_base_client
Definition: mir_bridge.py:476
mir_bridge.TopicConfig
Definition: mir_bridge.py:63
mir_bridge._tf_dict_filter
def _tf_dict_filter(msg_dict)
Definition: mir_bridge.py:112
mir_bridge.PublisherWrapper.connected
connected
Definition: mir_bridge.py:367
mir_bridge.MiRBridge.__init__
def __init__(self)
Definition: mir_bridge.py:429
mir_bridge._prepend_tf_prefix_dict_filter
def _prepend_tf_prefix_dict_filter(msg_dict)
Definition: mir_bridge.py:159
mir_bridge.MiRBridge._move_base_simple_goal_callback
def _move_base_simple_goal_callback(self, msg)
Definition: mir_bridge.py:508
mir_bridge.TopicConfig.latch
latch
Definition: mir_bridge.py:67
mir_bridge.PublisherWrapper.pub
pub
Definition: mir_bridge.py:369
mir_bridge.PublisherWrapper.__init__
def __init__(self, topic_config, robot)
Definition: mir_bridge.py:364
mir_bridge.SubscriberWrapper
Definition: mir_bridge.py:407
mir_bridge.PublisherWrapper
Definition: mir_bridge.py:363
mir_bridge.MiRBridge
Definition: mir_bridge.py:428


mir_driver
Author(s): Martin Günther
autogenerated on Wed Nov 13 2024 03:34:54