diff --git a/multi_level_map_utils/nodes/level_mux b/multi_level_map_utils/nodes/level_mux index ff49dcaf..b2dc08f8 100755 --- a/multi_level_map_utils/nodes/level_mux +++ b/multi_level_map_utils/nodes/level_mux @@ -14,6 +14,7 @@ import tf class LevelMultiplexer: def __init__(self): + """ Constructor. """ rospy.init_node("map_mux") @@ -21,13 +22,15 @@ class LevelMultiplexer: self.default_current_level = rospy.get_param('~default_current_level', None) # Subcribe to the multi level map data to get information about all the maps. - self.multimap_subscriber = rospy.Subscriber("map_metadata", MultiLevelMapData, self.process_multimap) self.multimap_available = False + self.multimap_subscriber = rospy.Subscriber("map_metadata", MultiLevelMapData, self.process_multimap) multimap_topic_name = rospy.resolve_name("map_metadata") + rospy.loginfo('level_mux: multimap topic name is ' + str(multimap_topic_name)) if '/' in multimap_topic_name: self.multimap_topic_prefix = multimap_topic_name[0:multimap_topic_name.rfind('/') + 1] else: self.multimap_topic_prefix = "" + rospy.loginfo('level_mux: multimap topic prefix is ' + str(self.multimap_topic_prefix)) # We also need to publish information about the current level. self.current_level = None @@ -44,6 +47,7 @@ class LevelMultiplexer: self.change_level_service = None def process_multimap(self, msg): + """ Callback for messages received on map metadata topic. """ # Get information about all the levels. self.levels = msg.levels @@ -65,12 +69,13 @@ class LevelMultiplexer: # If the service to change the level hasn't been advertised, then advertise it now. if self.change_level_service is None: self.change_level_service = rospy.Service('~change_current_level', ChangeCurrentLevel, self.change_level) + rospy.loginfo('level_mux: change level service defined.') self.multimap_available = True # Perform some sanity checks on the current_level if len(self.levels) == 1: - # If there is only one level, then change to that level without publishing initialpose. + # If there is only one plevel, then change to that level without publishing initialpose. req = ChangeCurrentLevelRequest() req.new_level_id = self.levels[0].level_id req.publish_initial_pose = False @@ -97,7 +102,10 @@ class LevelMultiplexer: self.initialpose_publisher = None self.current_level = None - # Perform some sanity checks on the current_level + rospy.loginfo('level_mux: multimap metadata available, current level is ' + + str(self.current_level)) + + # Perform some sanity checks on the default current_level if self.default_current_level is not None and self.current_level is None: default_level_found = False for level in self.levels: @@ -113,6 +121,8 @@ class LevelMultiplexer: rospy.logwarn("Default level requested as " + self.default_current_level + ", but unavailable in multimap.") def change_level(self, req): + """ Change level ROS service request handler. """ + ###rospy.loginfo('level_mux: change level request:\n' + str(req)) success = True error_message = "" @@ -124,16 +134,17 @@ class LevelMultiplexer: # Make this the current level self.current_level = level.level_id + rospy.loginfo('level_mux: new level is ' + str(req.new_level_id)) # Initialize the mux output channels if they have not been initialized. if self.level_publisher is None: self.level_publisher = rospy.Publisher("~map", OccupancyGrid, latch=True, queue_size=1) self.level_metadata_publisher = rospy.Publisher("~map_metadata", MapMetaData, latch=True, queue_size=1) self.level_map_service = rospy.Service('~static_map', GetMap, self.process_level_service) - # Note that initialpose is not latched, as the publication is only valid when the + # Note that initialpose is not latched, as the publication is only valid when the # change_current_level service is called. Soon after, the robot may have moved, and this estimate # is incorrect. - self.initialpose_publisher = rospy.Publisher("initialpose", PoseWithCovarianceStamped, queue_size=1) + self.initialpose_publisher = rospy.Publisher("initialpose", PoseWithCovarianceStamped, queue_size=1) self.set_map = rospy.ServiceProxy("set_map", SetMap) try: clear_costmap_service = rospy.ServiceProxy('move_base/clear_costmaps', Empty) @@ -151,13 +162,14 @@ class LevelMultiplexer: # Publish latched map messages self.level_metadata_publisher.publish(self.maps[self.current_level].map.info) self.level_publisher.publish(self.maps[self.current_level].map) + rospy.loginfo('level_mux: published map for ' + str(self.current_level)) if req.publish_initial_pose: - try: + try: self.set_map(self.maps[self.current_level].map, req.initial_pose) except rospy.ServiceException: # Service does not exist, level_mux being used without move_base OR a configuration snafu - rospy.logwarn("MultiLevelMap - Multiplexer::changeLevel: " + + rospy.logwarn("MultiLevelMap - Multiplexer::changeLevel: " + "Couldn't set initial pose via service call. Trying unlatched publication.") self.initialpose_publisher.publish(req.initial_pose) self.status_publisher.publish(level) diff --git a/multi_level_map_utils/src/multi_level_map_utils/plugins.py b/multi_level_map_utils/src/multi_level_map_utils/plugins.py index 7d2583e1..32637da1 100644 --- a/multi_level_map_utils/src/multi_level_map_utils/plugins.py +++ b/multi_level_map_utils/src/multi_level_map_utils/plugins.py @@ -43,6 +43,7 @@ def __init__(self, context): self.process_multimap) self.levels = [] self.current_level = None + rospy.loginfo('subscribed to maps') # Subscribe to the current level we are on. self.status_subscriber = None @@ -51,12 +52,18 @@ def __init__(self, context): self.level_selector_proxy = rospy.ServiceProxy("level_mux/change_current_level", ChangeCurrentLevel) self.level_selector_proxy.wait_for_service() + rospy.loginfo('found "level_mux/change_current_level" service') def process_multimap(self, msg): + """ Map metadata topic callback. """ + rospy.loginfo('level selector: map metadata received.') self.levels = msg.levels - #self.update_buttons.emit() + # update level buttons in the selection window + self.update_buttons() def update_buttons(self): + """ Update buttons in Qt window. """ + rospy.loginfo('update_buttons called') self.clean() for index, level in enumerate(self.levels): self.text_label.setText("Choose Level: ") @@ -73,13 +80,22 @@ def update_buttons(self): self.process_level_status) def update_button_status(self): + """ Update button status Qt push button callback. """ + rospy.loginfo('update_button_status called') + if not self.buttons or not self.current_level: + return + rospy.loginfo('buttons: ' + str(self.buttons)) for index, level in enumerate(self.levels): + rospy.loginfo('current level: ' + str(self.current_level)) + rospy.loginfo('level[' + str(index) + ']: ' + str(level)) if self.current_level == level.level_id: self.buttons[index].setChecked(True) else: self.buttons[index].setChecked(False) def process_level_status(self, msg): + """ level_mux/current_level topic callback. """ + rospy.loginfo('current_level topic message received') level_found = False for level in self.levels: if msg.level_id == level.level_id: