diff --git a/spot_tools/src/spot_executor/spot_executor.py b/spot_tools/src/spot_executor/spot_executor.py index b8a1e98..32537a1 100644 --- a/spot_tools/src/spot_executor/spot_executor.py +++ b/spot_tools/src/spot_executor/spot_executor.py @@ -132,6 +132,7 @@ def __init__( goal_tolerance=2.8, feedback=None, use_fake_path_planner=False, + combine_follow_commands=False, ): self.debug = False self.spot_interface = spot_interface @@ -143,12 +144,56 @@ def __init__( self.processing_action_sequence = False self.mid_level_planner = planner self.use_fake_path_planner = use_fake_path_planner + self.combine_follow_commands = combine_follow_commands self.lease_manager = None def initialize_lease_manager(self, feedback): self.lease_manager = LeaseManager(self.spot_interface, feedback) + def _merge_consecutive_follows(self, actions, feedback=None): + """Merge consecutive Follow commands that share the same frame into one. + + Consecutive Follow commands with different frames are kept as separate + merged groups — a frame boundary acts as a natural split point. + """ + merged = [] + i = 0 + while i < len(actions): + if type(actions[i]) is not Follow: + merged.append(actions[i]) + i += 1 + continue + + # Collect a run of consecutive Follow commands with the same frame + group = [actions[i]] + j = i + 1 + while ( + j < len(actions) + and type(actions[j]) is Follow + and actions[j].frame == actions[i].frame + ): + group.append(actions[j]) + j += 1 + + if len(group) == 1: + merged.append(group[0]) + else: + combined_path = np.concatenate([f.path2d for f in group], axis=0) + combined = Follow(frame=group[0].frame, path2d=combined_path) + if feedback is not None: + feedback.print( + "INFO", + f"Merged {len(group)} consecutive Follow commands " + f"(frame='{group[0].frame}') into one with " + f"{len(combined_path)} waypoints.", + ) + merged.append(combined) + + i = j + + return merged + def terminate_sequence(self, feedback): # Tell the actions sequence to break self.keep_going = False @@ -174,12 +219,22 @@ def process_action_sequence(self, sequence, feedback): for command in sequence.actions: feedback.print("INFO", command) + if self.combine_follow_commands: + actions = self._merge_consecutive_follows(sequence.actions, feedback) + feedback.print( + "INFO", + f"combine_follow_commands enabled: reduced {len(sequence.actions)} " + f"actions to {len(actions)} after merging consecutive Follow commands.", + ) + else: + actions = sequence.actions + self.spot_interface.robot.time_sync.wait_for_sync() self.spot_interface.take_lease() ix = 0 inner_loop_attempts = 0 - while ix < len(sequence.actions): + while ix < len(actions): # If the lease manager is actively taking back the lease and getting the # robot to stand back up, we don't want to send it any commands. It will break. if ( @@ -201,14 +256,14 @@ def process_action_sequence(self, sequence, feedback): time.sleep(0.5) continue - command = sequence.actions[ix] + command = actions[ix] if not self.keep_going: feedback.print("INFO", "Action sequence was pre-empted.") break pick_next = False - if ix < len(sequence.actions) - 1: - pick_next = type(sequence.actions[ix + 1]) is Pick + if ix < len(actions) - 1: + pick_next = type(actions[ix + 1]) is Pick feedback.print("INFO", "\n") feedback.print("INFO", "Spot executor executing command: ") feedback.print("INFO", command) diff --git a/spot_tools_ros/config/example_parms.yaml b/spot_tools_ros/config/example_parms.yaml index 58b69be..928cb60 100644 --- a/spot_tools_ros/config/example_parms.yaml +++ b/spot_tools_ros/config/example_parms.yaml @@ -21,6 +21,7 @@ lookahead_distance: 50 # grid cells occupancy_inflation_radius: 0.2 # meters use_fake_path_plan: false + combine_follow_commands: false # set to true to merge consecutive Follow commands into one # cost map: penalizes paths near obstacles (adds proximity cost to A* g_cost) use_cost_map: false cost_map_safe_distance: 1.0 # meters; cells within this distance incur extra cost 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..82413ee 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 @@ -375,6 +375,7 @@ def __init__(self): self.declare_parameter("lookahead_distance", 50) self.declare_parameter("occupancy_inflation_radius", 0.5) self.declare_parameter("use_fake_path_plan", False) + self.declare_parameter("combine_follow_commands", False) self.declare_parameter("use_cost_map", False) self.declare_parameter("cost_map_safe_distance", 0.5) self.declare_parameter("cost_map_nearest_obstacle_cost", 5.0) @@ -386,6 +387,7 @@ def __init__(self): ).value assert occupancy_inflation_radius > 0 use_fake_path_plan = self.get_parameter("use_fake_path_plan").value + combine_follow_commands = self.get_parameter("combine_follow_commands").value use_cost_map = self.get_parameter("use_cost_map").value cost_map_safe_distance = self.get_parameter("cost_map_safe_distance").value cost_map_nearest_obstacle_cost = self.get_parameter( @@ -507,6 +509,7 @@ def tf_lookup_fn(parent, child): goal_tolerance, self.feedback_collector, use_fake_path_plan, + combine_follow_commands, ) self.spot_executor.initialize_lease_manager(self.feedback_collector)