36 from collections.abc
import Iterable
38 from mir_driver
import rosbridge
39 from rospy_message_converter
import message_converter
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
48 import move_base_msgs.msg
50 import rosgraph_msgs.msg
52 import sensor_msgs.msg
55 import visualization_msgs.msg
57 from collections
import OrderedDict
60 static_transforms = OrderedDict()
64 def __init__(self, topic, topic_type, latch=False, dict_filter=None):
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
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__
87 return filtered_msg_dict
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
98 Convert Twist to TwistStamped.
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).
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),
109 return filtered_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
121 Cache tf_static messages (simulate latching).
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.
130 global static_transforms
138 for transform
in filtered_msg_dict[
'transforms']:
139 key = transform[
'child_frame_id']
141 "[%s] tf_static: updated transform %s->%s.",
143 transform[
'header'][
'frame_id'],
144 transform[
'child_frame_id'],
146 static_transforms[key] = transform
149 filtered_msg_dict[
'transforms'] = static_transforms.values()
151 "[%s] tf_static: sent %i transforms: %s",
153 len(static_transforms),
154 str(static_transforms.keys()),
156 return filtered_msg_dict
161 if not isinstance(msg_dict, dict):
163 for key, value
in msg_dict.items():
167 frame_id = value[
'frame_id'].strip(
'/')
168 if frame_id !=
'map':
170 value[
'frame_id'] = (tf_prefix +
'/' + frame_id).strip(
'/')
172 value[
'frame_id'] = frame_id
178 elif isinstance(value, dict):
180 elif isinstance(value, Iterable):
188 if not isinstance(msg_dict, dict):
190 for key, value
in msg_dict.items():
194 s = value[
'frame_id'].strip(
'/')
195 if s.find(tf_prefix) == 0:
196 value[
'frame_id'] = (s[len(tf_prefix) :]).strip(
'/')
201 elif isinstance(value, dict):
203 elif isinstance(value, Iterable):
213 TopicConfig(
'LightCtrl/us_list', sensor_msgs.msg.Range),
216 TopicConfig(
'MC/currents', sdc21x0.msg.MotorCurrents),
218 TopicConfig(
'MissionController/CheckArea/visualization_marker', visualization_msgs.msg.Marker),
221 TopicConfig(
'SickPLC/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
222 TopicConfig(
'SickPLC/parameter_updates', dynamic_reconfigure.msg.Config),
224 TopicConfig(
'amcl_pose', geometry_msgs.msg.PoseWithCovarianceStamped),
225 TopicConfig(
'b_raw_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),
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),
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),
252 TopicConfig(
'/map', nav_msgs.msg.OccupancyGrid, latch=
True),
253 TopicConfig(
'/map_metadata', nav_msgs.msg.MapMetaData),
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),
268 TopicConfig(
'mir_status_msg', std_msgs.msg.String),
270 TopicConfig(
'mirwebapp/grid_map_metadata', mir_msgs.msg.LocalMapStat),
271 TopicConfig(
'mirwebapp/laser_map_metadata', mir_msgs.msg.LocalMapStat),
275 'move_base/feedback', move_base_msgs.msg.MoveBaseActionFeedback, dict_filter=_move_base_feedback_dict_filter
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),
283 TopicConfig(
'move_base_node/MIRPlannerROS/local_plan', nav_msgs.msg.Path),
286 TopicConfig(
'move_base_node/MIRPlannerROS/updated_global_plan', mir_msgs.msg.PlanSegments),
288 TopicConfig(
'move_base_node/SBPLLatticePlanner/plan', nav_msgs.msg.Path),
293 TopicConfig(
'move_base_node/current_goal', geometry_msgs.msg.PoseStamped),
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),
305 TopicConfig(
'move_base_node/local_costmap/robot_footprint', geometry_msgs.msg.PolygonStamped),
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),
328 TopicConfig(
'robot_state', mir_msgs.msg.RobotState),
335 TopicConfig(
'scan_filter/visualization_marker', visualization_msgs.msg.Marker),
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),
353 TopicConfig(
'cmd_vel', geometry_msgs.msg.Twist, dict_filter=_cmd_vel_dict_filter),
354 TopicConfig(
'initialpose', geometry_msgs.msg.PoseWithCovarianceStamped),
357 TopicConfig(
'move_base/cancel', actionlib_msgs.msg.GoalID),
359 TopicConfig(
'move_base/goal', move_base_msgs.msg.MoveBaseActionGoal, dict_filter=_move_base_goal_dict_filter),
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,
377 "[%s] publishing topic '%s' [%s]", rospy.get_name(), topic_config.topic, topic_config.topic_type._type
380 if topic_config.latch:
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(
'/')
388 self.
robot.subscribe(topic=absolute_topic, callback=self.
callback)
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)
412 self.
sub = rospy.Subscriber(
413 name=topic_config.topic, data_class=topic_config.topic_type, callback=self.
callback, queue_size=10
416 "[%s] subscribing to topic '%s' [%s]", rospy.get_name(), topic_config.topic, topic_config.topic_type._type
420 msg_dict = message_converter.convert_ros_message_to_dictionary(msg)
424 absolute_topic =
'/' + self.
topic_config.topic.lstrip(
'/')
425 self.
robot.publish(absolute_topic, msg_dict)
431 hostname = rospy.get_param(
'~hostname')
433 rospy.logfatal(
'[%s] parameter "hostname" is not set!', rospy.get_name())
435 port = rospy.get_param(
'~port', 9090)
438 tf_prefix = rospy.get_param(
'~tf_prefix',
'').strip(
'/')
440 rospy.loginfo(
'[%s] trying to connect to %s:%i...', rospy.get_name(), hostname, port)
441 self.
robot = rosbridge.RosbridgeSetup(hostname, port)
445 while not self.
robot.is_connected():
446 if rospy.is_shutdown():
448 if self.
robot.is_errored():
449 rospy.logfatal(
'[%s] connection error to %s:%i, giving up!', rospy.get_name(), hostname, port)
452 rospy.logwarn(
'[%s] still waiting for connection to %s:%i...', rospy.get_name(), hostname, port)
456 rospy.loginfo(
'[%s] ... connected.', rospy.get_name())
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]
462 for pub_topic
in PUB_TOPICS:
464 absolute_topic =
'/' + pub_topic.topic.lstrip(
'/')
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)
468 for sub_topic
in SUB_TOPICS:
470 absolute_topic =
'/' + sub_topic.topic.lstrip(
'/')
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)
480 srv_response = self.
robot.callService(
'/rosapi/topics', msg={})
481 topic_names = sorted(srv_response[
'topics'])
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']
488 srv_response = self.
robot.callService(
"/rosapi/publishers", msg={
'topic': topic_name})
489 has_publishers =
True if len(srv_response[
'publishers']) > 0
else False
491 srv_response = self.
robot.callService(
"/rosapi/subscribers", msg={
'topic': topic_name})
492 has_subscribers =
True if len(srv_response[
'subscribers']) > 0
else False
494 topics.append([topic_name, topic_type, has_publishers, has_subscribers])
497 for topic_name, topic_type, has_publishers, has_subscribers
in topics:
499 print((
' * %s [%s]' % (topic_name, topic_type)))
501 print(
'\nSubscribers:')
502 for topic_name, topic_type, has_publishers, has_subscribers
in topics:
504 print((
' * %s [%s]' % (topic_name, topic_type)))
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?")
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)
520 rospy.init_node(
'mir_bridge')
525 if __name__ ==
'__main__':
528 except rospy.ROSInterruptException: