simple way to log output

robot_executor_interface/robot_executor_interface/src/robot_executor_interface/mid_level_planner.py

@@ -321,7 +321,10 @@ def plan_path(self, high_level_path_metric):
                 self.grid_cell_to_global_pose((pt[0], pt[1])) for pt in a_star_path_grid
             ]
             a_star_path_metric = np.array(a_star_path_metric).reshape(-1, 4)
-            a_star_path_metric = a_star_path_metric[:, :2]
+            a_star_path_metric = a_star_path_metric[:, :2].reshape(-1, 2)
+            # TODO(multy) temp fix for visualization if a star only output one point 
+            # in fact, this is probably reaching a local min we plan can't go farther. 
+            a_star_path_metric = np.vstack((self.robot_pose[:, :2], a_star_path_metric))
             a_star_path_execute = a_star_path_metric
             output.path_shapely = shapely.LineString(a_star_path_execute)
             output.path_waypoints_metric = a_star_path_metric

spot_tools/src/spot_skills/navigation_utils.py

@@ -122,6 +122,15 @@ def follow_trajectory_continuous(
     rate = 10
     # TODO: reactive loop, yeild out the loop to get info
     while 1:
+        curr_time = time.time()
+        feedback.print("INFO", f"Timeout progress {curr_time - t0} / {timeout}")
+        if curr_time - t0 > timeout:
+            # TODO: I think we need to tell Spot to stop?
+            # TODO: Also, we should probably have a finer-grained
+            # check about making progress
+            feedback.print("INFO", "follow_trajectory_continuous timeout")
+            return False
+
         # if mid_level_planner is not None:
         # update path every (couple?) loop
         mlp_success, planning_output = mid_level_planner.plan_path(
@@ -139,17 +148,8 @@ def follow_trajectory_continuous(
             feedback.print(
                 "INFO", "Mid-level planner failed, following high-level path directly"
             )
-            if time.time() - t0 > timeout:
-                return False
-
             path = shapely.LineString(waypoints_list[:, :2])
-            continue
 
-        if time.time() - t0 > timeout:
-            # TODO: I think we need to tell Spot to stop?
-            # TODO: Also, we should probably have a finer-grained
-            # check about making progress
-            return False
         tform_body_in_vision = spot.get_pose()
         distance_from_end = np.linalg.norm(
             end_pt - np.array([tform_body_in_vision[0], tform_body_in_vision[1]])
@@ -168,6 +168,7 @@ def follow_trajectory_continuous(
         current_point = shapely.Point(tform_body_in_vision[0], tform_body_in_vision[1])
         progress_distance = shapely.line_locate_point(path, current_point)
         progress_point = shapely.line_interpolate_point(path, progress_distance)
+
         # 2. get line point at lookahead
         target_distance = progress_distance + lookahead_distance
         target_point = shapely.line_interpolate_point(path, target_distance)

spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py

@@ -1,3 +1,4 @@
+import logging
 import os
 import threading
 import time
@@ -78,7 +79,7 @@ def build_markers(pts, namespaces, frames, colors):
 
 
 class RosFeedbackCollector:
-    def __init__(self, odom_frame: str, output_dir: str):
+    def __init__(self, odom_frame: str, output_dir: str, log_to_file_level):
         self.pick_confirmation_event = threading.Event()
         # self.pick_confirmation_response = False
 
@@ -90,6 +91,37 @@ def __init__(self, odom_frame: str, output_dir: str):
         self.odom_frame = odom_frame
 
         self.output_dir = output_dir
+        self.log_to_file_level = log_to_file_level
+
+        if log_to_file_level != "":
+            self.str_log_file = os.path.join(self.output_dir, "str_log.txt")
+
+            match log_to_file_level:
+                case "DEBUG":
+                    logging_level = logging.DEBUG
+                case "INFO":
+                    logging_level = logging.INFO
+                case "WARNING":
+                    logging_level = logging.WARNING
+                case "ERROR":
+                    logging_level = logging.ERROR
+                case _:
+                    raise ValueError(f"Invalid log level {log_to_file_level}")
+
+            # Create a custom logger
+            self.file_logger = logging.getLogger(__name__)
+            self.file_logger.setLevel(logging_level)
+
+            # Create a file handler
+            file_handler = logging.FileHandler(self.str_log_file)
+            file_handler.setLevel(logging_level)
+
+            # Create a formatter and add it to the handler
+            formatter = logging.Formatter("[%(asctime)s] - %(levelname)s - %(message)s")
+            file_handler.setFormatter(formatter)
+
+            # Add the handler to the logger
+            self.file_logger.addHandler(file_handler)
 
     def bounding_box_detection_feedback(
         self, detection_imgs, detection_index, centroid_x, centroid_y, semantic_class
@@ -169,16 +201,24 @@ def print(self, level, string):
         match level:
             case "DEBUG":
                 log_fn = self.logger.debug
+                file_logger_fn = self.file_logger.debug
             case "INFO":
                 log_fn = self.logger.info
+                file_logger_fn = self.file_logger.info
             case "WARNING":
                 log_fn = self.logger.warning
+                file_logger_fn = self.file_logger.warning
             case "ERROR":
                 log_fn = self.logger.error
+                file_logger_fn = self.file_logger.error
             case _:
                 raise ValueError(f"Invalid log level {level}")
         log_fn(str(string))
 
+        # TODO(multy): quick logic to log everything we print in the executor
+        if self.log_to_file_level != "":
+            file_logger_fn(str(string))
+
     def feedback_viz_2(self, y):
         pass
 
@@ -360,7 +400,13 @@ def __init__(self):
         output_dir = self.get_parameter("output_dir").value
         assert output_dir != ""
 
-        self.feedback_collector = RosFeedbackCollector(self.odom_frame, output_dir)
+        # TODO(multy): quick way to log everything feedback print to log file
+        self.declare_parameter("log_to_file", "")
+        log_to_file = self.get_parameter("log_to_file").value
+
+        self.feedback_collector = RosFeedbackCollector(
+            self.odom_frame, output_dir, log_to_file
+        )
         self.feedback_collector.register_publishers(self)
 
         self.tf_buffer = tf2_ros.Buffer()