changed to use ros logging instead of print (heuristicus#14)

* [spot_driver] change print() to ros logging

* [spot_driver] use ros logger instead of print()

spot_driver/scripts/graph_nav_util.py

@@ -15,17 +15,17 @@ def id_to_short_code(id):
     return None
 
 
-def pretty_print_waypoints(waypoint_id, waypoint_name, short_code_to_count, localization_id):
+def pretty_print_waypoints(waypoint_id, waypoint_name, short_code_to_count, localization_id, logger):
     short_code = id_to_short_code(waypoint_id)
     if short_code is None or short_code_to_count[short_code] != 1:
         short_code = '  '  # If the short code is not valid/unique, don't show it.
 
-    print("%s Waypoint name: %s id: %s short code: %s" %
+    logger.info("%s Waypoint name: %s id: %s short code: %s" %
             ('->' if localization_id == waypoint_id else '  ',
             waypoint_name, waypoint_id, short_code))
 
 
-def find_unique_waypoint_id(short_code, graph, name_to_id):
+def find_unique_waypoint_id(short_code, graph, name_to_id, logger):
     """Convert either a 2 letter short code or an annotation name into the associated unique id."""
     if len(short_code) != 2:
         # Not a short code, check if it is an annotation name (instead of the waypoint id).
@@ -35,7 +35,7 @@ def find_unique_waypoint_id(short_code, graph, name_to_id):
                 # Has an associated waypoint id!
                 return name_to_id[short_code]
             else:
-                print("The waypoint name %s is used for multiple different unique waypoints. Please use" + \
+                logger.error("The waypoint name %s is used for multiple different unique waypoints. Please use" + \
                         "the waypoint id." % (short_code))
                 return None
         # Also not an waypoint annotation name, so we will operate under the assumption that it is a
@@ -51,7 +51,7 @@ def find_unique_waypoint_id(short_code, graph, name_to_id):
     return ret
 
 
-def update_waypoints_and_edges(graph, localization_id):
+def update_waypoints_and_edges(graph, localization_id, logger):
     """Update and print waypoint ids and edge ids."""
     name_to_id = dict()
     edges = dict()
@@ -95,17 +95,17 @@ def update_waypoints_and_edges(graph, localization_id):
 
     # Print out the waypoints name, id, and short code in a ordered sorted by the timestamp from
     # when the waypoint was created.
-    print('%d waypoints:' % len(graph.waypoints))
+    logger.info('%d waypoints:' % len(graph.waypoints))
     for waypoint in waypoint_to_timestamp:
-        pretty_print_waypoints(waypoint[0], waypoint[2], short_code_to_count, localization_id)
+        pretty_print_waypoints(waypoint[0], waypoint[2], short_code_to_count, localization_id, logger)
 
     for edge in graph.edges:
         if edge.id.to_waypoint in edges:
             if edge.id.from_waypoint not in edges[edge.id.to_waypoint]:
                 edges[edge.id.to_waypoint].append(edge.id.from_waypoint)
         else:
             edges[edge.id.to_waypoint] = [edge.id.from_waypoint]
-        print("(Edge) from waypoint id: ", edge.id.from_waypoint, " and to waypoint id: ",
+        logger.info("(Edge) from waypoint id: ", edge.id.from_waypoint, " and to waypoint id: ",
                 edge.id.to_waypoint)
 
     return name_to_id, edges

spot_driver/scripts/ros_helpers.py

@@ -122,7 +122,7 @@ def getImageMsg(data, spot_wrapper, inverse_target_frame):
                     new_tf.transform.rotation.w = transform.parent_tform_child.rotation.w
                 tf_msg.transforms.append(new_tf)
             except Exception as e:
-                print('Error: {}'.format(e))
+                spot_wrapper.logger.error('Error: {}'.format(e))
 
     image_msg = Image()
     local_time = spot_wrapper.robotToLocalTime(data.shot.acquisition_time)
@@ -361,7 +361,7 @@ def GetTFFromState(state, spot_wrapper, inverse_target_frame):
                     new_tf.transform.rotation.w = transform.parent_tform_child.rotation.w
                 tf_msg.transforms.append(new_tf)
             except Exception as e:
-                print('Error: {}'.format(e))
+                spot_wrapper.logger.error('Error: {}'.format(e))
 
     return tf_msg
 

spot_driver/scripts/spot_ros.py

@@ -504,7 +504,7 @@ def main(self):
                     mobility_params_msg.locomotion_hint = mobility_params.locomotion_hint
                     mobility_params_msg.stair_hint = mobility_params.stair_hint
                 except Exception as e:
-                    print('Error:{}'.format(e))
+                    rospy.logerr('Error:{}'.format(e))
                     pass
                 self.mobility_params_pub.publish(mobility_params_msg)
                 rate.sleep()

spot_driver/scripts/spot_wrapper.py

@@ -281,6 +281,11 @@ def __init__(self, username, password, hostname, logger, rates = {}, callbacks =
             self._robot_id = None
             self._lease = None
 
+    @property
+    def logger(self):
+        """Return logger instance of the SpotWrapper"""
+        return self._logger
+
     @property
     def is_valid(self):
         """Return boolean indicating if the wrapper initialized successfully"""
@@ -566,9 +571,9 @@ def navigate_to(self, upload_path,
     def _get_localization_state(self, *args):
         """Get the current localization and state of the robot."""
         state = self._graph_nav_client.get_localization_state()
-        print('Got localization: \n%s' % str(state.localization))
+        self._logger.info('Got localization: \n%s' % str(state.localization))
         odom_tform_body = get_odom_tform_body(state.robot_kinematics.transforms_snapshot)
-        print('Got robot state in kinematic odometry frame: \n%s' % str(odom_tform_body))
+        self._logger.info('Got robot state in kinematic odometry frame: \n%s' % str(odom_tform_body))
 
     def _set_initial_localization_fiducial(self, *args):
         """Trigger localization when near a fiducial."""
@@ -586,10 +591,10 @@ def _set_initial_localization_waypoint(self, *args):
         # Take the first argument as the localization waypoint.
         if len(args) < 1:
             # If no waypoint id is given as input, then return without initializing.
-            print("No waypoint specified to initialize to.")
+            self._logger.error("No waypoint specified to initialize to.")
             return
         destination_waypoint = graph_nav_util.find_unique_waypoint_id(
-            args[0][0], self._current_graph, self._current_annotation_name_to_wp_id)
+            args[0][0], self._current_graph, self._current_annotation_name_to_wp_id, self._logger)
         if not destination_waypoint:
             # Failed to find the unique waypoint id.
             return
@@ -615,27 +620,27 @@ def _list_graph_waypoint_and_edge_ids(self, *args):
         # Download current graph
         graph = self._graph_nav_client.download_graph()
         if graph is None:
-            print("Empty graph.")
+            self._logger.error("Empty graph.")
             return
         self._current_graph = graph
 
         localization_id = self._graph_nav_client.get_localization_state().localization.waypoint_id
 
         # Update and print waypoints and edges
         self._current_annotation_name_to_wp_id, self._current_edges = graph_nav_util.update_waypoints_and_edges(
-            graph, localization_id)
+            graph, localization_id, self._logger)
         return self._current_annotation_name_to_wp_id, self._current_edges
 
 
     def _upload_graph_and_snapshots(self, upload_filepath):
         """Upload the graph and snapshots to the robot."""
-        print("Loading the graph from disk into local storage...")
+        self._logger.info("Loading the graph from disk into local storage...")
         with open(upload_filepath + "/graph", "rb") as graph_file:
             # Load the graph from disk.
             data = graph_file.read()
             self._current_graph = map_pb2.Graph()
             self._current_graph.ParseFromString(data)
-            print("Loaded graph has {} waypoints and {} edges".format(
+            self._logger.info("Loaded graph has {} waypoints and {} edges".format(
                 len(self._current_graph.waypoints), len(self._current_graph.edges)))
         for waypoint in self._current_graph.waypoints:
             # Load the waypoint snapshots from disk.
@@ -652,43 +657,43 @@ def _upload_graph_and_snapshots(self, upload_filepath):
                 edge_snapshot.ParseFromString(snapshot_file.read())
                 self._current_edge_snapshots[edge_snapshot.id] = edge_snapshot
         # Upload the graph to the robot.
-        print("Uploading the graph and snapshots to the robot...")
+        self._logger.info("Uploading the graph and snapshots to the robot...")
         self._graph_nav_client.upload_graph(lease=self._lease.lease_proto,
                                             graph=self._current_graph)
         # Upload the snapshots to the robot.
         for waypoint_snapshot in self._current_waypoint_snapshots.values():
             self._graph_nav_client.upload_waypoint_snapshot(waypoint_snapshot)
-            print("Uploaded {}".format(waypoint_snapshot.id))
+            self._logger.info("Uploaded {}".format(waypoint_snapshot.id))
         for edge_snapshot in self._current_edge_snapshots.values():
             self._graph_nav_client.upload_edge_snapshot(edge_snapshot)
-            print("Uploaded {}".format(edge_snapshot.id))
+            self._logger.info("Uploaded {}".format(edge_snapshot.id))
 
         # The upload is complete! Check that the robot is localized to the graph,
         # and it if is not, prompt the user to localize the robot before attempting
         # any navigation commands.
         localization_state = self._graph_nav_client.get_localization_state()
         if not localization_state.localization.waypoint_id:
             # The robot is not localized to the newly uploaded graph.
-            print("\n")
-            print("Upload complete! The robot is currently not localized to the map; please localize", \
+            self._logger.info(
+                   "Upload complete! The robot is currently not localized to the map; please localize", \
                    "the robot using commands (2) or (3) before attempting a navigation command.")
 
     def _navigate_to(self, *args):
         """Navigate to a specific waypoint."""
         # Take the first argument as the destination waypoint.
         if len(args) < 1:
             # If no waypoint id is given as input, then return without requesting navigation.
-            print("No waypoint provided as a destination for navigate to.")
+            self._logger.info("No waypoint provided as a destination for navigate to.")
             return
 
         self._lease = self._lease_wallet.get_lease()
         destination_waypoint = graph_nav_util.find_unique_waypoint_id(
-            args[0][0], self._current_graph, self._current_annotation_name_to_wp_id)
+            args[0][0], self._current_graph, self._current_annotation_name_to_wp_id, self._logger)
         if not destination_waypoint:
             # Failed to find the appropriate unique waypoint id for the navigation command.
             return
         if not self.toggle_power(should_power_on=True):
-            print("Failed to power on the robot, and cannot complete navigate to request.")
+            self._logger.info("Failed to power on the robot, and cannot complete navigate to request.")
             return
 
         # Stop the lease keepalive and create a new sublease for graph nav.
@@ -733,12 +738,12 @@ def _navigate_route(self, *args):
         """Navigate through a specific route of waypoints."""
         if len(args) < 1:
             # If no waypoint ids are given as input, then return without requesting navigation.
-            print("No waypoints provided for navigate route.")
+            self._logger.error("No waypoints provided for navigate route.")
             return
         waypoint_ids = args[0]
         for i in range(len(waypoint_ids)):
             waypoint_ids[i] = graph_nav_util.find_unique_waypoint_id(
-                waypoint_ids[i], self._current_graph, self._current_annotation_name_to_wp_id)
+                waypoint_ids[i], self._current_graph, self._current_annotation_name_to_wp_id, self._logger)
             if not waypoint_ids[i]:
                 # Failed to find the unique waypoint id.
                 return
@@ -755,16 +760,16 @@ def _navigate_route(self, *args):
                 edge_ids_list.append(edge_id)
             else:
                 all_edges_found = False
-                print("Failed to find an edge between waypoints: ", start_wp, " and ", end_wp)
-                print(
+                self._logger.error("Failed to find an edge between waypoints: ", start_wp, " and ", end_wp)
+                self._logger.error(
                     "List the graph's waypoints and edges to ensure pairs of waypoints has an edge."
                 )
                 break
 
         self._lease = self._lease_wallet.get_lease()
         if all_edges_found:
             if not self.toggle_power(should_power_on=True):
-                print("Failed to power on the robot, and cannot complete navigate route request.")
+                self._logger.error("Failed to power on the robot, and cannot complete navigate route request.")
                 return
 
             # Stop the lease keepalive and create a new sublease for graph nav.
@@ -839,13 +844,13 @@ def _check_success(self, command_id=-1):
             # Successfully completed the navigation commands!
             return True
         elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST:
-            print("Robot got lost when navigating the route, the robot will now sit down.")
+            self._logger.error("Robot got lost when navigating the route, the robot will now sit down.")
             return True
         elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK:
-            print("Robot got stuck when navigating the route, the robot will now sit down.")
+            self._logger.error("Robot got stuck when navigating the route, the robot will now sit down.")
             return True
         elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED:
-            print("Robot is impaired.")
+            self._logger.error("Robot is impaired.")
             return True
         else:
             # Navigation command is not complete yet.