mir_bridge.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import rospy
3 
4 import copy
5 import sys
6 from collections import Iterable
7 
8 from mir_driver import rosbridge
9 from rospy_message_converter import message_converter
10 
11 from actionlib_msgs.msg import GoalID, GoalStatusArray
12 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
13 from dynamic_reconfigure.msg import Config, ConfigDescription
14 from geometry_msgs.msg import PolygonStamped, Pose, PoseArray, PoseStamped, PoseWithCovarianceStamped, Twist
15 from mir_actions.msg import *
16 from mir_msgs.msg import *
17 from move_base_msgs.msg import MoveBaseActionFeedback, MoveBaseActionGoal, MoveBaseActionResult, MoveBaseFeedback, MoveBaseResult
18 from nav_msgs.msg import GridCells, MapMetaData, OccupancyGrid, Odometry, Path
19 from rosgraph_msgs.msg import Log
20 from sensor_msgs.msg import Imu, LaserScan, PointCloud2, Range
21 from std_msgs.msg import Float64, String
22 from tf.msg import tfMessage
23 from visualization_msgs.msg import Marker, MarkerArray
24 
25 tf_prefix = ''
26 
27 class TopicConfig(object):
28  def __init__(self, topic, topic_type, latch=False, dict_filter=None):
29  self.topic = topic
30  self.topic_type = topic_type
31  self.latch = latch
32  self.dict_filter = dict_filter
33 
34 # remap mir_actions/MirMoveBaseAction => move_base_msgs/MoveBaseAction
36  # filter out slots from the dict that are not in our message definition
37  # e.g., MiRMoveBaseFeedback has the field "state", which MoveBaseFeedback doesn't
38  filtered_msg_dict = copy.deepcopy(msg_dict)
39  filtered_msg_dict['feedback'] = {key: msg_dict['feedback'][key] for key in MoveBaseFeedback.__slots__}
40  return filtered_msg_dict
41 
43  filtered_msg_dict = copy.deepcopy(msg_dict)
44  filtered_msg_dict['result'] = {key: msg_dict['result'][key] for key in MoveBaseResult.__slots__}
45  return filtered_msg_dict
46 
47 def _tf_dict_filter(msg_dict):
48  filtered_msg_dict = copy.deepcopy(msg_dict)
49  for transform in filtered_msg_dict['transforms']:
50  transform['child_frame_id'] = tf_prefix + '/' + transform['child_frame_id'].strip('/')
51  return filtered_msg_dict
52 
54  #filtered_msg_dict = copy.deepcopy(msg_dict)
55  if not isinstance(msg_dict, dict): # can happen during recursion
56  return
57  for (key, value) in msg_dict.iteritems():
58  if key == 'header':
59  try:
60  # prepend frame_id
61  frame_id = value['frame_id'].strip('/')
62  if (frame_id != 'map'):
63  # prepend tf_prefix, then remove leading '/' (e.g., when tf_prefix is empty)
64  value['frame_id'] = (tf_prefix + '/' + frame_id).strip('/')
65  else:
66  value['frame_id'] = frame_id
67 
68  except TypeError:
69  pass # value is not a dict
70  except KeyError:
71  pass # value doesn't have key 'frame_id'
72  elif isinstance(value, dict):
74  elif isinstance(value, Iterable): # an Iterable other than dict (e.g., a list)
75  for item in value:
77  return msg_dict
78 
80  #filtered_msg_dict = copy.deepcopy(msg_dict)
81  if not isinstance(msg_dict, dict): # can happen during recursion
82  return
83  for (key, value) in msg_dict.iteritems():
84  if key == 'header':
85  try:
86  # remove frame_id
87  s = value['frame_id'].strip('/')
88  if s.find(tf_prefix) == 0:
89  value['frame_id'] = (s[len(tf_prefix):]).strip('/') # strip off tf_prefix, then strip leading '/'
90  except TypeError:
91  pass # value is not a dict
92  except KeyError:
93  pass # value doesn't have key 'frame_id'
94  elif isinstance(value, dict):
96  elif isinstance(value, Iterable): # an Iterable other than dict (e.g., a list)
97  for item in value:
99  return msg_dict
100 
101 
102 
103 # topics we want to publish to ROS (and subscribe to from the MiR)
104 PUB_TOPICS = [
105  TopicConfig('LightCtrl/us_list', Range),
106  TopicConfig('MissionController/CheckArea/visualization_marker', Marker),
107  TopicConfig('SickPLC/parameter_descriptions', ConfigDescription),
108  TopicConfig('SickPLC/parameter_updates', Config),
109  TopicConfig('amcl_pose', PoseWithCovarianceStamped),
110  TopicConfig('b_raw_scan', LaserScan),
111  TopicConfig('b_scan', LaserScan),
112  TopicConfig('camera_floor/background', PointCloud2),
113  TopicConfig('camera_floor/depth/parameter_descriptions', ConfigDescription),
114  TopicConfig('camera_floor/depth/parameter_updates', Config),
115  TopicConfig('camera_floor/depth/points', PointCloud2),
116  TopicConfig('camera_floor/filter/parameter_descriptions', ConfigDescription),
117  TopicConfig('camera_floor/filter/parameter_updates', Config),
118  TopicConfig('camera_floor/floor', PointCloud2),
119  TopicConfig('camera_floor/obstacles', PointCloud2),
120  TopicConfig('camera_floor/transform/parameter_descriptions', ConfigDescription),
121  TopicConfig('camera_floor/transform/parameter_updates', Config),
122  TopicConfig('check_area/polygon', PolygonStamped),
123  TopicConfig('diagnostics', DiagnosticArray),
124  TopicConfig('diagnostics_agg', DiagnosticArray),
125  TopicConfig('diagnostics_toplevel_state', DiagnosticStatus),
126  TopicConfig('f_raw_scan', LaserScan),
127  TopicConfig('f_scan', LaserScan),
128  TopicConfig('imu_data', Imu), # not available in simulation
129  TopicConfig('laser_back/driver/parameter_descriptions', ConfigDescription),
130  TopicConfig('laser_back/driver/parameter_updates', Config),
131  TopicConfig('laser_back/transform/parameter_descriptions', ConfigDescription),
132  TopicConfig('laser_back/transform/parameter_updates', Config),
133  TopicConfig('laser_front/driver/parameter_descriptions', ConfigDescription),
134  TopicConfig('laser_front/driver/parameter_updates', Config),
135  TopicConfig('laser_front/transform/parameter_descriptions', ConfigDescription),
136  TopicConfig('laser_front/transform/parameter_updates', Config),
137  TopicConfig('map', OccupancyGrid, latch=True),
138  TopicConfig('map_metadata', MapMetaData),
139  TopicConfig('mir_amcl/parameter_descriptions', ConfigDescription),
140  TopicConfig('mir_amcl/parameter_updates', Config),
141  TopicConfig('mir_amcl/selected_points', PointCloud2),
142  TopicConfig('mir_log', Log),
143  TopicConfig('mir_serial_button', Serial),
144  TopicConfig('mir_sound', String),
145  TopicConfig('mir_status', MirStatus),
146  TopicConfig('mir_status_msg', String),
147 # TopicConfig('mirspawn/node_events', LaunchItem),
148  TopicConfig('mirwebapp/grid_map_metadata', LocalMapStat),
149  TopicConfig('mirwebapp/laser_map_metadata', LocalMapStat),
150 ## TopicConfig('move_base/feedback', MirMoveBaseActionFeedback),
151 ## TopicConfig('move_base/result', MirMoveBaseActionResult),
152  TopicConfig('move_base/feedback', MoveBaseActionFeedback, dict_filter=_move_base_feedback_dict_filter), # really mir_actions/MirMoveBaseActionFeedback
153  TopicConfig('move_base/result', MoveBaseActionResult, dict_filter=_move_base_result_dict_filter), # really mir_actions/MirMoveBaseActionResult
154  TopicConfig('move_base/status', GoalStatusArray),
155 # TopicConfig('move_base_node/MIRPlannerROS/cost_cloud', PointCloud2),
156 # TopicConfig('move_base_node/MIRPlannerROS/global_plan', Path),
157 # TopicConfig('move_base_node/MIRPlannerROS/len_to_goal', Float64),
158  TopicConfig('move_base_node/MIRPlannerROS/local_plan', Path),
159 # TopicConfig('move_base_node/MIRPlannerROS/parameter_descriptions', ConfigDescription),
160 # TopicConfig('move_base_node/MIRPlannerROS/parameter_updates', Config),
161  TopicConfig('move_base_node/SBPLLatticePlanner/plan', Path),
162 # TopicConfig('move_base_node/SBPLLatticePlanner/visualization_marker', MarkerArray),
163  TopicConfig('move_base_node/current_goal', PoseStamped),
164 # TopicConfig('move_base_node/global_costmap/forbidden_area', GridCells),
165 # TopicConfig('move_base_node/global_costmap/inflated_obstacles', GridCells),
166 # TopicConfig('move_base_node/global_costmap/obstacles', GridCells),
167 # TopicConfig('move_base_node/global_costmap/parameter_descriptions', ConfigDescription),
168 # TopicConfig('move_base_node/global_costmap/parameter_updates', Config),
169 # TopicConfig('move_base_node/global_costmap/robot_footprint', PolygonStamped),
170 # TopicConfig('move_base_node/global_costmap/unknown_space', GridCells),
171 # TopicConfig('move_base_node/global_plan', Path),
172 # TopicConfig('move_base_node/local_costmap/forbidden_area', GridCells),
173  TopicConfig('move_base_node/local_costmap/inflated_obstacles', GridCells),
174  TopicConfig('move_base_node/local_costmap/obstacles', GridCells),
175 # TopicConfig('move_base_node/local_costmap/parameter_descriptions', ConfigDescription),
176 # TopicConfig('move_base_node/local_costmap/parameter_updates', Config),
177  TopicConfig('move_base_node/local_costmap/robot_footprint', PolygonStamped),
178 # TopicConfig('move_base_node/local_costmap/unknown_space', GridCells),
179 # TopicConfig('move_base_node/mir_escape_recovery/visualization_marker', Marker),
180 # TopicConfig('move_base_node/parameter_descriptions', ConfigDescription),
181 # TopicConfig('move_base_node/parameter_updates', Config),
182  TopicConfig('odom_comb', Odometry), # odom_comb on real robot, odom on simulator
183  TopicConfig('odom_enc', Odometry),
184  TopicConfig('particlecloud', PoseArray),
185  TopicConfig('relative_move_action/feedback', RelativeMoveActionFeedback),
186  TopicConfig('relative_move_action/result', RelativeMoveActionResult),
187  TopicConfig('relative_move_action/status', GoalStatusArray),
188  TopicConfig('relative_move_node/parameter_descriptions', ConfigDescription),
189  TopicConfig('relative_move_node/parameter_updates', Config),
190  TopicConfig('relative_move_node/time_to_coll', Float64),
191  TopicConfig('relative_move_node/visualization_marker', Marker),
192  TopicConfig('robot_mode', RobotMode),
193  TopicConfig('robot_pose', Pose),
194  TopicConfig('robot_state', RobotState),
195  TopicConfig('rosout', Log),
196  TopicConfig('rosout_agg', Log),
197  TopicConfig('scan', LaserScan),
198  TopicConfig('scan_filter/visualization_marker', Marker),
199  TopicConfig('tf', tfMessage, dict_filter=_tf_dict_filter),
200  TopicConfig('transform_footprint/parameter_descriptions', ConfigDescription),
201  TopicConfig('transform_footprint/parameter_updates', Config),
202  TopicConfig('transform_imu/parameter_descriptions', ConfigDescription),
203  TopicConfig('transform_imu/parameter_updates', Config)]
204 
205 # topics we want to subscribe to from ROS (and publish to the MiR)
206 SUB_TOPICS = [TopicConfig('cmd_vel', Twist),
207  TopicConfig('initialpose', PoseWithCovarianceStamped),
208  TopicConfig('light_cmd', String),
209  TopicConfig('mir_cmd', String),
210  TopicConfig('move_base/cancel', GoalID),
211 ## TopicConfig('move_base/goal', MirMoveBaseActionGoal),
212  TopicConfig('move_base/goal', MoveBaseActionGoal), # really mir_actions/MirMoveBaseActionGoal
213  TopicConfig('move_base_simple/goal', PoseStamped),
214  TopicConfig('relative_move_action/cancel', GoalID),
215  TopicConfig('relative_move_action/goal', RelativeMoveActionGoal)]
216 
217 class PublisherWrapper(rospy.SubscribeListener):
218  def __init__(self, topic_config, robot):
219  self.topic_config = topic_config
220  self.robot = robot
221  self.connected = False
222  self.pub = rospy.Publisher(name=topic_config.topic,
223  data_class=topic_config.topic_type,
224  subscriber_listener=self,
225  latch=topic_config.latch,
226  queue_size=1)
227  rospy.loginfo("[%s] publishing topic '%s' [%s]",
228  rospy.get_name(), topic_config.topic, topic_config.topic_type._type)
229  # latched topics must be subscribed immediately
230  if topic_config.latch:
231  self.peer_subscribe(None, None, None)
232 
233 
234  def peer_subscribe(self, topic_name, topic_publish, peer_publish):
235  if (self.pub.get_num_connections() == 1 and not self.connected) or self.topic_config.latch:
236  rospy.loginfo("[%s] starting to stream messages on topic '%s'", rospy.get_name(), self.topic_config.topic)
237  self.robot.subscribe(topic=('/' + self.topic_config.topic), callback=self.callback)
238 
239  def peer_unsubscribe(self, topic_name, num_peers):
240  pass
241 ## doesn't work: once ubsubscribed, robot doesn't let us subscribe again
242 # if self.pub.get_num_connections() == 0:
243 # rospy.loginfo("[%s] stopping to stream messages on topic '%s'", rospy.get_name(), self.topic_config.topic)
244 # self.robot.unsubscribe(topic=('/' + self.topic_config.topic))
245 
246  def callback(self, msg_dict):
247  msg_dict = _prepend_tf_prefix_dict_filter(msg_dict)
248  if self.topic_config.dict_filter is not None:
249  msg_dict = self.topic_config.dict_filter(msg_dict)
250  msg = message_converter.convert_dictionary_to_ros_message(self.topic_config.topic_type._type, msg_dict)
251  self.pub.publish(msg)
252 
253 class SubscriberWrapper(object):
254  def __init__(self, topic_config, robot):
255  self.topic_config = topic_config
256  self.robot = robot
257  self.sub = rospy.Subscriber(name=topic_config.topic,
258  data_class=topic_config.topic_type,
259  callback=self.callback,
260  queue_size=1)
261  rospy.loginfo("[%s] subscribing to topic '%s' [%s]",
262  rospy.get_name(), topic_config.topic, topic_config.topic_type._type)
263 
264  def callback(self, msg):
265  msg_dict = message_converter.convert_ros_message_to_dictionary(msg)
266  msg_dict = _remove_tf_prefix_dict_filter(msg_dict)
267  if self.topic_config.dict_filter is not None:
268  msg_dict = self.topic_config.dict_filter(msg_dict)
269  self.robot.publish('/' + self.topic_config.topic, msg_dict)
270 
271 class MiR100Bridge(object):
272  def __init__(self):
273  try:
274  hostname = rospy.get_param('~hostname')
275  except KeyError:
276  rospy.logfatal('[%s] parameter "hostname" is not set!', rospy.get_name())
277  sys.exit(-1)
278  port = rospy.get_param('~port', 9090)
279 
280  global tf_prefix
281  tf_prefix = rospy.get_param('~tf_prefix', '').strip('/')
282 
283  rospy.loginfo('[%s] trying to connect to %s:%i...', rospy.get_name(), hostname, port)
284  self.robot = rosbridge.RosbridgeSetup(hostname, port)
285 
286  r = rospy.Rate(10)
287  i = 1
288  while not self.robot.is_connected():
289  if rospy.is_shutdown():
290  sys.exit(0)
291  if self.robot.is_errored():
292  rospy.logfatal('[%s] connection error to %s:%i, giving up!', rospy.get_name(), hostname, port)
293  sys.exit(-1)
294  if i % 10 == 0:
295  rospy.logwarn('[%s] still waiting for connection to %s:%i...', rospy.get_name(), hostname, port)
296  i += 1
297  r.sleep()
298 
299  rospy.loginfo('[%s] ... connected.', rospy.get_name())
300 
301  topics = self.get_topics()
302  published_topics = [topic_name for (topic_name, _, has_publishers, _) in topics if has_publishers]
303  subscribed_topics = [topic_name for (topic_name, _, _, has_subscribers) in topics if has_subscribers]
304 
305  for pub_topic in PUB_TOPICS:
306  PublisherWrapper(pub_topic, self.robot)
307  if ('/' + pub_topic.topic) not in published_topics:
308  rospy.logwarn("[%s] topic '%s' is not published by the MiR!", rospy.get_name(), pub_topic.topic)
309 
310  for sub_topic in SUB_TOPICS:
311  SubscriberWrapper(sub_topic, self.robot)
312  if ('/' + sub_topic.topic) not in subscribed_topics:
313  rospy.logwarn("[%s] topic '%s' is not yet subscribed to by the MiR!", rospy.get_name(), sub_topic.topic)
314 
315  def get_topics(self):
316  srv_response = self.robot.callService('/rosapi/topics', msg={})
317  topic_names = sorted(srv_response['topics'])
318  topics = []
319 
320  for topic_name in topic_names:
321  srv_response = self.robot.callService("/rosapi/topic_type", msg={'topic': topic_name})
322  topic_type = srv_response['type']
323 
324  srv_response = self.robot.callService("/rosapi/publishers", msg={'topic': topic_name})
325  has_publishers = True if len(srv_response['publishers']) > 0 else False
326 
327  srv_response = self.robot.callService("/rosapi/subscribers", msg={'topic': topic_name})
328  has_subscribers = True if len(srv_response['subscribers']) > 0 else False
329 
330  topics.append([topic_name, topic_type, has_publishers, has_subscribers])
331 
332  print 'Publishers:'
333  for (topic_name, topic_type, has_publishers, has_subscribers) in topics:
334  if has_publishers:
335  print ' * %s [%s]' % (topic_name, topic_type)
336 
337  print '\nSubscribers:'
338  for (topic_name, topic_type, has_publishers, has_subscribers) in topics:
339  if has_subscribers:
340  print ' * %s [%s]' % (topic_name, topic_type)
341 
342  return topics
343 
344 
345 def main():
346  rospy.init_node('mir_bridge')
347  MiR100Bridge()
348  rospy.spin()
349 
350 if __name__ == '__main__':
351  try:
352  main()
353  except rospy.ROSInterruptException:
354  pass
def peer_subscribe(self, topic_name, topic_publish, peer_publish)
Definition: mir_bridge.py:234
def _move_base_feedback_dict_filter(msg_dict)
Definition: mir_bridge.py:35
def __init__(self, topic_config, robot)
Definition: mir_bridge.py:254
def __init__(self, topic_config, robot)
Definition: mir_bridge.py:218
def __init__(self, topic, topic_type, latch=False, dict_filter=None)
Definition: mir_bridge.py:28
def peer_unsubscribe(self, topic_name, num_peers)
Definition: mir_bridge.py:239
def callback(self, msg_dict)
doesn't work: once ubsubscribed, robot doesn't let us subscribe again if self.pub.get_num_connections() == 0: rospy.loginfo("[%s] stopping to stream messages on topic '%s'", rospy.get_name(), self.topic_config.topic) self.robot.unsubscribe(topic=('/' + self.topic_config.topic))
Definition: mir_bridge.py:246
def main()
Definition: mir_bridge.py:345
def _prepend_tf_prefix_dict_filter(msg_dict)
Definition: mir_bridge.py:53
def _tf_dict_filter(msg_dict)
Definition: mir_bridge.py:47
def _move_base_result_dict_filter(msg_dict)
Definition: mir_bridge.py:42
def _remove_tf_prefix_dict_filter(msg_dict)
Definition: mir_bridge.py:79


mir_driver
Author(s): Martin G√ľnther
autogenerated on Wed May 8 2019 02:38:53