From 9a6566cc09afb8c7f86896dfd681d96d083de847 Mon Sep 17 00:00:00 2001 From: harelb Date: Wed, 15 Apr 2026 23:45:31 -0400 Subject: [PATCH 1/6] simple way to log output --- .../src/spot_tools_ros/spot_executor_ros.py | 50 ++++++++++++++++++- 1 file changed, 48 insertions(+), 2 deletions(-) diff --git a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py index ba088eb..434304b 100755 --- a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py +++ b/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() From c832adeb170626918e7d97aae657b4aefb8b2962 Mon Sep 17 00:00:00 2001 From: harelb Date: Thu, 16 Apr 2026 00:34:54 -0400 Subject: [PATCH 2/6] more logging --- spot_tools/src/spot_skills/navigation_utils.py | 3 +++ spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/spot_tools/src/spot_skills/navigation_utils.py b/spot_tools/src/spot_skills/navigation_utils.py index d76b264..04fb08d 100644 --- a/spot_tools/src/spot_skills/navigation_utils.py +++ b/spot_tools/src/spot_skills/navigation_utils.py @@ -149,7 +149,9 @@ def follow_trajectory_continuous( # 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 + 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 +170,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) diff --git a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py index 434304b..558936c 100755 --- a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py +++ b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py @@ -117,7 +117,7 @@ def __init__(self, odom_frame: str, output_dir: str, log_to_file_level): file_handler.setLevel(logging_level) # Create a formatter and add it to the handler - formatter = logging.Formatter("%(asctime)s - %(levelname)s - %(message)s") + formatter = logging.Formatter("[%(asctime)s] - %(levelname)s - %(message)s") file_handler.setFormatter(formatter) # Add the handler to the logger From 1fd2609d5e9b3323a1197d908c76f62b51674020 Mon Sep 17 00:00:00 2001 From: harelb Date: Thu, 16 Apr 2026 00:41:51 -0400 Subject: [PATCH 3/6] Fix follow trajectory continuous logic --- .../src/spot_skills/navigation_utils.py | 20 ++++++++----------- 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/spot_tools/src/spot_skills/navigation_utils.py b/spot_tools/src/spot_skills/navigation_utils.py index 04fb08d..8007b78 100644 --- a/spot_tools/src/spot_skills/navigation_utils.py +++ b/spot_tools/src/spot_skills/navigation_utils.py @@ -122,6 +122,13 @@ def follow_trajectory_continuous( rate = 10 # TODO: reactive loop, yeild out the loop to get info while 1: + 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 + 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( @@ -137,20 +144,9 @@ def follow_trajectory_continuous( if not mlp_success: feedback.print( - "INFO", "Mid-level planner failed, following high-level path directly" + "WARNING", "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 - feedback.print("INFO", "follow_trajectory_continuous timeout") - return False tform_body_in_vision = spot.get_pose() distance_from_end = np.linalg.norm( From 63ef2ac6ddbb1b49ab15f66aaaa566f055f74717 Mon Sep 17 00:00:00 2001 From: harelb Date: Thu, 16 Apr 2026 00:55:12 -0400 Subject: [PATCH 4/6] Even more prints. --- spot_tools/src/spot_skills/navigation_utils.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/spot_tools/src/spot_skills/navigation_utils.py b/spot_tools/src/spot_skills/navigation_utils.py index 8007b78..5d18adc 100644 --- a/spot_tools/src/spot_skills/navigation_utils.py +++ b/spot_tools/src/spot_skills/navigation_utils.py @@ -122,7 +122,9 @@ def follow_trajectory_continuous( rate = 10 # TODO: reactive loop, yeild out the loop to get info while 1: - if time.time() - t0 > timeout: + 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 From ee07b4c2b469d1678d19b366ea4c706047c022e4 Mon Sep 17 00:00:00 2001 From: harelb Date: Thu, 16 Apr 2026 01:24:43 -0400 Subject: [PATCH 5/6] more prints, fix ros logger different level incompatability. --- spot_tools/src/spot_skills/navigation_utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_tools/src/spot_skills/navigation_utils.py b/spot_tools/src/spot_skills/navigation_utils.py index 5d18adc..8391956 100644 --- a/spot_tools/src/spot_skills/navigation_utils.py +++ b/spot_tools/src/spot_skills/navigation_utils.py @@ -146,7 +146,7 @@ def follow_trajectory_continuous( if not mlp_success: feedback.print( - "WARNING", "Mid-level planner failed, following high-level path directly" + "INFO", "Mid-level planner failed, following high-level path directly" ) path = shapely.LineString(waypoints_list[:, :2]) From 72d5ea6608bdc98b54ea93eaadf43743a0cb6298 Mon Sep 17 00:00:00 2001 From: harelb Date: Thu, 16 Apr 2026 01:34:57 -0400 Subject: [PATCH 6/6] temp visualization fix --- .../src/robot_executor_interface/mid_level_planner.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/robot_executor_interface/robot_executor_interface/src/robot_executor_interface/mid_level_planner.py b/robot_executor_interface/robot_executor_interface/src/robot_executor_interface/mid_level_planner.py index 218a848..6e0775e 100644 --- a/robot_executor_interface/robot_executor_interface/src/robot_executor_interface/mid_level_planner.py +++ b/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