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 diff --git a/spot_tools/src/spot_skills/navigation_utils.py b/spot_tools/src/spot_skills/navigation_utils.py index d76b264..8391956 100644 --- a/spot_tools/src/spot_skills/navigation_utils.py +++ b/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) 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..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 @@ -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()