Kinetic: minor multilevel_map_utils cleanup (#95)

multi_level_map_utils/nodes/level_mux

@@ -14,20 +14,23 @@ import tf
 class LevelMultiplexer:
 
     def __init__(self):
+        """ Constructor. """
 
         rospy.init_node("map_mux")
 
         self.global_frame_id = rospy.get_param('~global_frame_id', 'level_mux_map')
         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)

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: