Skip to content

spot follow combiner #52

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Aryan-Naveen wants to merge 1 commit into
base: main
Choose a base branch
from
Open

spot follow combiner #52

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
63 changes: 59 additions & 4 deletions spot_tools/src/spot_executor/spot_executor.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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 (
Expand All @@ -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)
Expand Down
1 change: 1 addition & 0 deletions spot_tools_ros/config/example_parms.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
3 changes: 3 additions & 0 deletions spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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(
Expand Down Expand Up @@ -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)

Expand Down
Loading