mir_driver: mir_bridge.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import rospy
00003 
00004 import copy
00005 import sys
00006 from collections import Iterable
00007 
00008 from mir_driver import rosbridge
00009 from rospy_message_converter import message_converter
00010 
00011 from actionlib_msgs.msg import GoalID, GoalStatusArray
00012 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
00013 from dynamic_reconfigure.msg import Config, ConfigDescription
00014 from geometry_msgs.msg import PolygonStamped, Pose, PoseArray, PoseStamped, PoseWithCovarianceStamped, Twist
00015 from mir_actions.msg import *
00016 from mir_msgs.msg import *
00017 from move_base_msgs.msg import MoveBaseActionFeedback, MoveBaseActionGoal, MoveBaseActionResult, MoveBaseFeedback, MoveBaseResult
00018 from nav_msgs.msg import GridCells, MapMetaData, OccupancyGrid, Odometry, Path
00019 from rosgraph_msgs.msg import Log
00020 from sensor_msgs.msg import Imu, LaserScan, PointCloud2, Range
00021 from std_msgs.msg import Float64, String
00022 from tf.msg import tfMessage
00023 from visualization_msgs.msg import Marker, MarkerArray
00024 
00025 tf_prefix = ''
00026 
00027 class TopicConfig(object):
00028     def __init__(self, topic, topic_type, latch=False, dict_filter=None):
00029         self.topic = topic
00030         self.topic_type = topic_type
00031         self.latch = latch
00032         self.dict_filter = dict_filter
00033 
00034 # remap mir_actions/MirMoveBaseAction => move_base_msgs/MoveBaseAction
00035 def _move_base_feedback_dict_filter(msg_dict):
00036     # filter out slots from the dict that are not in our message definition
00037     # e.g., MiRMoveBaseFeedback has the field "state", which MoveBaseFeedback doesn't
00038     filtered_msg_dict = copy.deepcopy(msg_dict)
00039     filtered_msg_dict['feedback'] = {key: msg_dict['feedback'][key] for key in MoveBaseFeedback.__slots__}
00040     return filtered_msg_dict
00041 
00042 def _move_base_result_dict_filter(msg_dict):
00043     filtered_msg_dict = copy.deepcopy(msg_dict)
00044     filtered_msg_dict['result'] = {key: msg_dict['result'][key] for key in MoveBaseResult.__slots__}
00045     return filtered_msg_dict
00046 
00047 def _tf_dict_filter(msg_dict):
00048     filtered_msg_dict = copy.deepcopy(msg_dict)
00049     for transform in filtered_msg_dict['transforms']:
00050         transform['child_frame_id'] = tf_prefix + '/' + transform['child_frame_id'].strip('/')
00051     return filtered_msg_dict
00052 
00053 def _prepend_tf_prefix_dict_filter(msg_dict):
00054     #filtered_msg_dict = copy.deepcopy(msg_dict)
00055     if not isinstance(msg_dict, dict):   # can happen during recursion
00056         return
00057     for (key, value) in msg_dict.iteritems():
00058         if key == 'header':
00059             try:
00060                 # prepend frame_id
00061                 frame_id = value['frame_id'].strip('/')
00062                 if (frame_id != 'map'):
00063                     # prepend tf_prefix, then remove leading '/' (e.g., when tf_prefix is empty)
00064                     value['frame_id'] = (tf_prefix + '/' + frame_id).strip('/')
00065                 else:
00066                     value['frame_id'] = frame_id
00067 
00068             except TypeError:
00069                 pass   # value is not a dict
00070             except KeyError:
00071                 pass   # value doesn't have key 'frame_id'
00072         elif isinstance(value, dict):
00073             _prepend_tf_prefix_dict_filter(value)
00074         elif isinstance(value, Iterable):    # an Iterable other than dict (e.g., a list)
00075             for item in value:
00076                 _prepend_tf_prefix_dict_filter(item)
00077     return msg_dict
00078 
00079 def _remove_tf_prefix_dict_filter(msg_dict):
00080     #filtered_msg_dict = copy.deepcopy(msg_dict)
00081     if not isinstance(msg_dict, dict):   # can happen during recursion
00082         return
00083     for (key, value) in msg_dict.iteritems():
00084         if key == 'header':
00085             try:
00086                 # remove frame_id
00087                 s = value['frame_id'].strip('/')
00088                 if s.find(tf_prefix) == 0:
00089                     value['frame_id'] = (s[len(tf_prefix):]).strip('/')  # strip off tf_prefix, then strip leading '/'
00090             except TypeError:
00091                 pass   # value is not a dict
00092             except KeyError:
00093                 pass   # value doesn't have key 'frame_id'
00094         elif isinstance(value, dict):
00095             _remove_tf_prefix_dict_filter(value)
00096         elif isinstance(value, Iterable):    # an Iterable other than dict (e.g., a list)
00097             for item in value:
00098                 _remove_tf_prefix_dict_filter(item)
00099     return msg_dict
00100 
00101 
00102 
00103 # topics we want to publish to ROS (and subscribe to from the MiR)
00104 PUB_TOPICS = [
00105               TopicConfig('LightCtrl/us_list', Range),
00106               TopicConfig('MissionController/CheckArea/visualization_marker', Marker),
00107               TopicConfig('SickPLC/parameter_descriptions', ConfigDescription),
00108               TopicConfig('SickPLC/parameter_updates', Config),
00109               TopicConfig('amcl_pose', PoseWithCovarianceStamped),
00110               TopicConfig('b_raw_scan', LaserScan),
00111               TopicConfig('b_scan', LaserScan),
00112               TopicConfig('camera_floor/background', PointCloud2),
00113               TopicConfig('camera_floor/depth/parameter_descriptions', ConfigDescription),
00114               TopicConfig('camera_floor/depth/parameter_updates', Config),
00115               TopicConfig('camera_floor/depth/points', PointCloud2),
00116               TopicConfig('camera_floor/filter/parameter_descriptions', ConfigDescription),
00117               TopicConfig('camera_floor/filter/parameter_updates', Config),
00118               TopicConfig('camera_floor/floor', PointCloud2),
00119               TopicConfig('camera_floor/obstacles', PointCloud2),
00120               TopicConfig('camera_floor/transform/parameter_descriptions', ConfigDescription),
00121               TopicConfig('camera_floor/transform/parameter_updates', Config),
00122               TopicConfig('check_area/polygon', PolygonStamped),
00123               TopicConfig('diagnostics', DiagnosticArray),
00124               TopicConfig('diagnostics_agg', DiagnosticArray),
00125               TopicConfig('diagnostics_toplevel_state', DiagnosticStatus),
00126               TopicConfig('f_raw_scan', LaserScan),
00127               TopicConfig('f_scan', LaserScan),
00128               TopicConfig('imu_data', Imu),  # not available in simulation
00129               TopicConfig('laser_back/driver/parameter_descriptions', ConfigDescription),
00130               TopicConfig('laser_back/driver/parameter_updates', Config),
00131               TopicConfig('laser_back/transform/parameter_descriptions', ConfigDescription),
00132               TopicConfig('laser_back/transform/parameter_updates', Config),
00133               TopicConfig('laser_front/driver/parameter_descriptions', ConfigDescription),
00134               TopicConfig('laser_front/driver/parameter_updates', Config),
00135               TopicConfig('laser_front/transform/parameter_descriptions', ConfigDescription),
00136               TopicConfig('laser_front/transform/parameter_updates', Config),
00137               TopicConfig('map', OccupancyGrid, latch=True),
00138               TopicConfig('map_metadata', MapMetaData),
00139               TopicConfig('mir_amcl/parameter_descriptions', ConfigDescription),
00140               TopicConfig('mir_amcl/parameter_updates', Config),
00141               TopicConfig('mir_amcl/selected_points', PointCloud2),
00142               TopicConfig('mir_log', Log),
00143               TopicConfig('mir_serial_button', Serial),
00144               TopicConfig('mir_sound', String),
00145               TopicConfig('mir_status', MirStatus),
00146               TopicConfig('mir_status_msg', String),
00147 #              TopicConfig('mirspawn/node_events', LaunchItem),
00148               TopicConfig('mirwebapp/grid_map_metadata', LocalMapStat),
00149               TopicConfig('mirwebapp/laser_map_metadata', LocalMapStat),
00150 ##              TopicConfig('move_base/feedback', MirMoveBaseActionFeedback),
00151 ##              TopicConfig('move_base/result', MirMoveBaseActionResult),
00152               TopicConfig('move_base/feedback', MoveBaseActionFeedback, dict_filter=_move_base_feedback_dict_filter),  # really mir_actions/MirMoveBaseActionFeedback
00153               TopicConfig('move_base/result',   MoveBaseActionResult,   dict_filter=_move_base_result_dict_filter),    # really mir_actions/MirMoveBaseActionResult
00154               TopicConfig('move_base/status', GoalStatusArray),
00155 #              TopicConfig('move_base_node/MIRPlannerROS/cost_cloud', PointCloud2),
00156 #              TopicConfig('move_base_node/MIRPlannerROS/global_plan', Path),
00157 #              TopicConfig('move_base_node/MIRPlannerROS/len_to_goal', Float64),
00158               TopicConfig('move_base_node/MIRPlannerROS/local_plan', Path),
00159 #              TopicConfig('move_base_node/MIRPlannerROS/parameter_descriptions', ConfigDescription),
00160 #              TopicConfig('move_base_node/MIRPlannerROS/parameter_updates', Config),
00161               TopicConfig('move_base_node/SBPLLatticePlanner/plan', Path),
00162 #              TopicConfig('move_base_node/SBPLLatticePlanner/visualization_marker', MarkerArray),
00163               TopicConfig('move_base_node/current_goal', PoseStamped),
00164 #              TopicConfig('move_base_node/global_costmap/forbidden_area', GridCells),
00165 #              TopicConfig('move_base_node/global_costmap/inflated_obstacles', GridCells),
00166 #              TopicConfig('move_base_node/global_costmap/obstacles', GridCells),
00167 #              TopicConfig('move_base_node/global_costmap/parameter_descriptions', ConfigDescription),
00168 #              TopicConfig('move_base_node/global_costmap/parameter_updates', Config),
00169 #              TopicConfig('move_base_node/global_costmap/robot_footprint', PolygonStamped),
00170 #              TopicConfig('move_base_node/global_costmap/unknown_space', GridCells),
00171 #              TopicConfig('move_base_node/global_plan', Path),
00172 #              TopicConfig('move_base_node/local_costmap/forbidden_area', GridCells),
00173               TopicConfig('move_base_node/local_costmap/inflated_obstacles', GridCells),
00174               TopicConfig('move_base_node/local_costmap/obstacles', GridCells),
00175 #              TopicConfig('move_base_node/local_costmap/parameter_descriptions', ConfigDescription),
00176 #              TopicConfig('move_base_node/local_costmap/parameter_updates', Config),
00177               TopicConfig('move_base_node/local_costmap/robot_footprint', PolygonStamped),
00178 #              TopicConfig('move_base_node/local_costmap/unknown_space', GridCells),
00179 #              TopicConfig('move_base_node/mir_escape_recovery/visualization_marker', Marker),
00180 #              TopicConfig('move_base_node/parameter_descriptions', ConfigDescription),
00181 #              TopicConfig('move_base_node/parameter_updates', Config),
00182               TopicConfig('odom_comb', Odometry),    # odom_comb on real robot, odom on simulator
00183               TopicConfig('odom_enc', Odometry),
00184               TopicConfig('particlecloud', PoseArray),
00185               TopicConfig('relative_move_action/feedback', RelativeMoveActionFeedback),
00186               TopicConfig('relative_move_action/result', RelativeMoveActionResult),
00187               TopicConfig('relative_move_action/status', GoalStatusArray),
00188               TopicConfig('relative_move_node/parameter_descriptions', ConfigDescription),
00189               TopicConfig('relative_move_node/parameter_updates', Config),
00190               TopicConfig('relative_move_node/time_to_coll', Float64),
00191               TopicConfig('relative_move_node/visualization_marker', Marker),
00192               TopicConfig('robot_mode', RobotMode),
00193               TopicConfig('robot_pose', Pose),
00194               TopicConfig('robot_state', RobotState),
00195               TopicConfig('rosout', Log),
00196               TopicConfig('rosout_agg', Log),
00197               TopicConfig('scan', LaserScan),
00198               TopicConfig('scan_filter/visualization_marker', Marker),
00199               TopicConfig('tf', tfMessage, dict_filter=_tf_dict_filter),
00200               TopicConfig('transform_footprint/parameter_descriptions', ConfigDescription),
00201               TopicConfig('transform_footprint/parameter_updates', Config),
00202               TopicConfig('transform_imu/parameter_descriptions', ConfigDescription),
00203               TopicConfig('transform_imu/parameter_updates', Config)]
00204 
00205 # topics we want to subscribe to from ROS (and publish to the MiR)
00206 SUB_TOPICS = [TopicConfig('cmd_vel', Twist),
00207               TopicConfig('initialpose', PoseWithCovarianceStamped),
00208               TopicConfig('light_cmd', String),
00209               TopicConfig('mir_cmd', String),
00210               TopicConfig('move_base/cancel', GoalID),
00211 ##              TopicConfig('move_base/goal', MirMoveBaseActionGoal),
00212               TopicConfig('move_base/goal', MoveBaseActionGoal),  # really mir_actions/MirMoveBaseActionGoal
00213               TopicConfig('move_base_simple/goal', PoseStamped),
00214               TopicConfig('relative_move_action/cancel', GoalID),
00215               TopicConfig('relative_move_action/goal', RelativeMoveActionGoal)]
00216 
00217 class PublisherWrapper(rospy.SubscribeListener):
00218     def __init__(self, topic_config, robot):
00219         self.topic_config = topic_config
00220         self.robot = robot
00221         self.connected = False
00222         self.pub = rospy.Publisher(name=topic_config.topic,
00223                                    data_class=topic_config.topic_type,
00224                                    subscriber_listener=self,
00225                                    latch=topic_config.latch,
00226                                    queue_size=1)
00227         rospy.loginfo("[%s] publishing topic '%s' [%s]",
00228                       rospy.get_name(), topic_config.topic, topic_config.topic_type._type)
00229         # latched topics must be subscribed immediately
00230         if topic_config.latch:
00231             self.peer_subscribe(None, None, None)
00232 
00233 
00234     def peer_subscribe(self, topic_name, topic_publish, peer_publish):
00235         if (self.pub.get_num_connections() == 1 and not self.connected) or self.topic_config.latch:
00236             rospy.loginfo("[%s] starting to stream messages on topic '%s'", rospy.get_name(), self.topic_config.topic)
00237             self.robot.subscribe(topic=('/' + self.topic_config.topic), callback=self.callback)
00238 
00239     def peer_unsubscribe(self, topic_name, num_peers):
00240         pass
00241 ## doesn't work: once ubsubscribed, robot doesn't let us subscribe again
00242 #         if self.pub.get_num_connections() == 0:
00243 #             rospy.loginfo("[%s] stopping to stream messages on topic '%s'", rospy.get_name(), self.topic_config.topic)
00244 #             self.robot.unsubscribe(topic=('/' + self.topic_config.topic))
00245 
00246     def callback(self, msg_dict):
00247         msg_dict = _prepend_tf_prefix_dict_filter(msg_dict)
00248         if self.topic_config.dict_filter is not None:
00249             msg_dict = self.topic_config.dict_filter(msg_dict)
00250         msg = message_converter.convert_dictionary_to_ros_message(self.topic_config.topic_type._type, msg_dict)
00251         self.pub.publish(msg)
00252 
00253 class SubscriberWrapper(object):
00254     def __init__(self, topic_config, robot):
00255         self.topic_config = topic_config
00256         self.robot = robot
00257         self.sub = rospy.Subscriber(name=topic_config.topic,
00258                                     data_class=topic_config.topic_type,
00259                                     callback=self.callback,
00260                                     queue_size=1)
00261         rospy.loginfo("[%s] subscribing to topic '%s' [%s]",
00262                       rospy.get_name(), topic_config.topic, topic_config.topic_type._type)
00263 
00264     def callback(self, msg):
00265         msg_dict = message_converter.convert_ros_message_to_dictionary(msg)
00266         msg_dict = _remove_tf_prefix_dict_filter(msg_dict)
00267         if self.topic_config.dict_filter is not None:
00268             msg_dict = self.topic_config.dict_filter(msg_dict)
00269         self.robot.publish('/' + self.topic_config.topic, msg_dict)
00270 
00271 class MiR100Bridge(object):
00272     def __init__(self):
00273         try:
00274             hostname = rospy.get_param('~hostname')
00275         except KeyError:
00276             rospy.logfatal('[%s] parameter "hostname" is not set!', rospy.get_name())
00277             sys.exit(-1)
00278         port = rospy.get_param('~port', 9090)
00279 
00280         global tf_prefix
00281         tf_prefix = rospy.get_param('~tf_prefix', '').strip('/')
00282 
00283         rospy.loginfo('[%s] trying to connect to %s:%i...', rospy.get_name(), hostname, port)
00284         self.robot = rosbridge.RosbridgeSetup(hostname, port)
00285 
00286         r = rospy.Rate(10)
00287         i = 1
00288         while not self.robot.is_connected():
00289             if rospy.is_shutdown():
00290                 sys.exit(0)
00291             if self.robot.is_errored():
00292                 rospy.logfatal('[%s] connection error to %s:%i, giving up!', rospy.get_name(), hostname, port)
00293                 sys.exit(-1)
00294             if i % 10 == 0:
00295                 rospy.logwarn('[%s] still waiting for connection to %s:%i...', rospy.get_name(), hostname, port)
00296             i += 1
00297             r.sleep()
00298 
00299         rospy.loginfo('[%s] ... connected.', rospy.get_name())
00300 
00301         topics = self.get_topics()
00302         published_topics = [topic_name for (topic_name, _, has_publishers, _) in topics if has_publishers]
00303         subscribed_topics = [topic_name for (topic_name, _, _, has_subscribers) in topics if has_subscribers]
00304 
00305         for pub_topic in PUB_TOPICS:
00306             PublisherWrapper(pub_topic, self.robot)
00307             if ('/' + pub_topic.topic) not in published_topics:
00308                 rospy.logwarn("[%s] topic '%s' is not published by the MiR!", rospy.get_name(), pub_topic.topic)
00309 
00310         for sub_topic in SUB_TOPICS:
00311             SubscriberWrapper(sub_topic, self.robot)
00312             if ('/' + sub_topic.topic) not in subscribed_topics:
00313                 rospy.logwarn("[%s] topic '%s' is not yet subscribed to by the MiR!", rospy.get_name(), sub_topic.topic)
00314 
00315     def get_topics(self):
00316         srv_response = self.robot.callService('/rosapi/topics', msg={})
00317         topic_names = sorted(srv_response['topics'])
00318         topics = []
00319 
00320         for topic_name in topic_names:
00321             srv_response = self.robot.callService("/rosapi/topic_type", msg={'topic': topic_name})
00322             topic_type = srv_response['type']
00323 
00324             srv_response = self.robot.callService("/rosapi/publishers", msg={'topic': topic_name})
00325             has_publishers = True if len(srv_response['publishers']) > 0 else False
00326 
00327             srv_response = self.robot.callService("/rosapi/subscribers", msg={'topic': topic_name})
00328             has_subscribers = True if len(srv_response['subscribers']) > 0 else False
00329 
00330             topics.append([topic_name, topic_type, has_publishers, has_subscribers])
00331 
00332         print 'Publishers:'
00333         for (topic_name, topic_type, has_publishers, has_subscribers) in topics:
00334             if has_publishers:
00335                 print ' * %s [%s]' % (topic_name, topic_type)
00336 
00337         print '\nSubscribers:'
00338         for (topic_name, topic_type, has_publishers, has_subscribers) in topics:
00339             if has_subscribers:
00340                 print ' * %s [%s]' % (topic_name, topic_type)
00341 
00342         return topics
00343 
00344 
00345 def main():
00346     rospy.init_node('mir_bridge')
00347     MiR100Bridge()
00348     rospy.spin()
00349 
00350 if __name__ == '__main__':
00351     try:
00352         main()
00353     except rospy.ROSInterruptException:
00354         pass