From 24df2ddcdf2da804b8c9bf6f059a434807a98671 Mon Sep 17 00:00:00 2001 From: harelb Date: Wed, 25 Mar 2026 00:57:26 -0400 Subject: [PATCH 1/4] Add direct navigation commands, pause/resume, and tests Add 5 new action types (MoveRelative, TurnRelative, Strafe, Stop, StandSit) that bypass the PDDL planning pipeline for direct robot control via chat. Add pause/resume mechanism on ~/pause topic for non-LLM emergency stopping. Include unit tests and on-robot verification script. --- README.md | 104 ++++++ .../action_descriptions.py | 25 ++ .../action_descriptions_ros.py | 82 ++++ .../robot_executor_msgs/msg/ActionMsg.msg | 11 + .../test_direct_navigation_on_robot.py | 262 +++++++++++++ spot_tools/src/spot_executor/spot_executor.py | 78 ++++ spot_tools/tests/__init__.py | 0 spot_tools/tests/test_direct_navigation.py | 350 ++++++++++++++++++ .../src/spot_tools_ros/spot_executor_ros.py | 46 +++ 9 files changed, 958 insertions(+) create mode 100644 spot_tools/examples/test_direct_navigation_on_robot.py create mode 100644 spot_tools/tests/__init__.py create mode 100644 spot_tools/tests/test_direct_navigation.py diff --git a/README.md b/README.md index 54b61d6..b6e5b8a 100644 --- a/README.md +++ b/README.md @@ -11,6 +11,110 @@ pre-commit run --all-files ``` +# Direct Navigation Commands + +In addition to high-level PDDL-based planning, the spot executor supports +direct navigation commands for teleoperation-style control through the chat +interface. These commands bypass the PDDL planning pipeline and are sent +directly to the executor. + +## Available Commands + +| Command | Action Type | Parameters | Description | +|---------|-------------|------------|-------------| +| Move | `MOVE_RELATIVE` | `distance_m` (float) | Move forward (positive) or backward (negative) by a distance in meters | +| Turn | `TURN_RELATIVE` | `angle_deg` (float) | Turn left/CCW (positive) or right/CW (negative) by an angle in degrees | +| Strafe | `STRAFE` | `distance_m` (float) | Move left (positive) or right (negative) sideways by a distance in meters | +| Stop | `STOP` | none | Halt all motion and cancel any in-progress action sequence | +| Stand/Sit | `STAND_SIT` | `action` ("stand" or "sit") | Stand up or sit down | + +## Architecture + +These commands are defined as action dataclasses in +`robot_executor_interface/action_descriptions.py` alongside the existing +`Follow`, `Gaze`, `Pick`, and `Place` actions. They are dispatched by the +`SpotExecutor` in `spot_executor.py`, which uses `navigate_to_relative_pose()` +from `navigation_utils.py` to convert body-frame relative commands into +vision-frame absolute trajectories. + +The corresponding LLM tools are defined in `heracles_agents` at +`src/heracles_agents/tools/navigation_tools.py`. When the LLM calls a +navigation tool, it publishes an `ActionSequenceMsg` via `ros2 topic pub` +directly to the executor's action sequence topic. + +``` +Chat input -> LLM tool call (e.g., move_relative) -> ros2 topic pub ActionSequenceMsg -> SpotExecutor +``` + +## Testing Direct Commands + +You can manually publish direct commands for testing without the chat interface: + +```bash +# Move forward 2 meters +ros2 topic pub /hamilton/spot_executor_node/action_sequence_subscriber \ + robot_executor_msgs/msg/ActionSequenceMsg \ + "{plan_id: 'test', robot_name: 'spot', actions: [{action_type: 'MOVE_RELATIVE', scalar_value: 2.0}]}" -1 + +# Turn right 90 degrees +ros2 topic pub /hamilton/spot_executor_node/action_sequence_subscriber \ + robot_executor_msgs/msg/ActionSequenceMsg \ + "{plan_id: 'test', robot_name: 'spot', actions: [{action_type: 'TURN_RELATIVE', scalar_value: -90.0}]}" -1 + +# Stop +ros2 topic pub /hamilton/spot_executor_node/action_sequence_subscriber \ + robot_executor_msgs/msg/ActionSequenceMsg \ + "{plan_id: 'test', robot_name: 'spot', actions: [{action_type: 'STOP'}]}" -1 + +# Sit down +ros2 topic pub /hamilton/spot_executor_node/action_sequence_subscriber \ + robot_executor_msgs/msg/ActionSequenceMsg \ + "{plan_id: 'test', robot_name: 'spot', actions: [{action_type: 'STAND_SIT', stand_sit_action: 'sit'}]}" -1 +``` + +After modifying `ActionMsg.msg`, rebuild with: +```bash +colcon build --packages-select robot_executor_msgs +``` + +## Pause / Resume + +The executor supports a lightweight pause/resume mechanism via a ROS topic. +When paused, the robot cancels any in-progress action sequence, holds its +current pose (stays standing), and rejects new action sequences until resumed. + +```bash +# Pause — robot stops and holds position +ros2 topic pub /hamilton/spot_executor_node/pause std_msgs/msg/Bool "{data: true}" -1 + +# Resume — robot accepts commands again +ros2 topic pub /hamilton/spot_executor_node/pause std_msgs/msg/Bool "{data: false}" -1 +``` + +This can be bound to a joystick button, keyboard shortcut, or RViz panel +button for one-press pause without going through the chat/LLM stack. The robot +stays powered on and standing — no lease transfer, no E-Stop, no recovery +sequence needed. + +## Stopping Behavior + +There are three levels of stopping: + +- **Pause** (`~/pause` topic): Cancels the action sequence and holds position. + The robot stays standing with motors on. Resume at any time by publishing + `false`. No LLM or chat interface needed. Best for: operator wants to + temporarily halt the robot. + +- **Software stop** (`STOP` command via chat): Same effect as pause, but + triggered through the chat interface / LLM tool call. Adds LLM latency. + Best for: "I changed my mind" during a chat session. + +- **Hardware E-Stop** (tablet): Cuts motor power immediately — the robot will + collapse. This is always available and should be used for safety-critical + situations. The robot requires a full recovery sequence (clear faults, power + on, stand) before it can move again. + + # Examples You can find an example of the ROS-free spot executor in diff --git a/robot_executor_interface/robot_executor_interface/src/robot_executor_interface/action_descriptions.py b/robot_executor_interface/robot_executor_interface/src/robot_executor_interface/action_descriptions.py index 8376aca..558d45d 100644 --- a/robot_executor_interface/robot_executor_interface/src/robot_executor_interface/action_descriptions.py +++ b/robot_executor_interface/robot_executor_interface/src/robot_executor_interface/action_descriptions.py @@ -41,3 +41,28 @@ class Place: robot_point: np.ndarray object_point: np.ndarray object_id: str + + +@dataclass +class MoveRelative: + distance_m: float # positive=forward, negative=backward + + +@dataclass +class TurnRelative: + angle_deg: float # positive=left(CCW), negative=right(CW) + + +@dataclass +class Strafe: + distance_m: float # positive=left, negative=right + + +@dataclass +class Stop: + pass + + +@dataclass +class StandSit: + action: str # "stand" or "sit" diff --git a/robot_executor_interface/robot_executor_interface_ros/src/robot_executor_interface_ros/action_descriptions_ros.py b/robot_executor_interface/robot_executor_interface_ros/src/robot_executor_interface_ros/action_descriptions_ros.py index 306b316..61e2e76 100644 --- a/robot_executor_interface/robot_executor_interface_ros/src/robot_executor_interface_ros/action_descriptions_ros.py +++ b/robot_executor_interface/robot_executor_interface_ros/src/robot_executor_interface_ros/action_descriptions_ros.py @@ -11,8 +11,13 @@ ActionSequence, Follow, Gaze, + MoveRelative, Pick, Place, + StandSit, + Stop, + Strafe, + TurnRelative, ) from spot_tools_ros.utils import path_to_waypoints, waypoints_to_path @@ -64,6 +69,16 @@ def from_msg(msg): actions.append(place_from_msg(a)) case a.GAZE: actions.append(gaze_from_msg(a)) + case a.MOVE_RELATIVE: + actions.append(MoveRelative(distance_m=a.scalar_value)) + case a.TURN_RELATIVE: + actions.append(TurnRelative(angle_deg=a.scalar_value)) + case a.STRAFE: + actions.append(Strafe(distance_m=a.scalar_value)) + case a.STOP: + actions.append(Stop()) + case a.STAND_SIT: + actions.append(StandSit(action=a.stand_sit_action)) case _: raise Exception(f"Received invalid action type {a.action_type}") return ActionSequence( @@ -339,3 +354,70 @@ def _(action: Place, marker_ns): m.points = [pt1, pt2] return [m] + + +# --- Direct navigation commands --- + + +@to_msg.register +def _(action: MoveRelative): + msg = ActionMsg() + msg.action_type = msg.MOVE_RELATIVE + msg.scalar_value = action.distance_m + return msg + + +@to_viz_msg.register +def _(action: MoveRelative, marker_ns): + return [] + + +@to_msg.register +def _(action: TurnRelative): + msg = ActionMsg() + msg.action_type = msg.TURN_RELATIVE + msg.scalar_value = action.angle_deg + return msg + + +@to_viz_msg.register +def _(action: TurnRelative, marker_ns): + return [] + + +@to_msg.register +def _(action: Strafe): + msg = ActionMsg() + msg.action_type = msg.STRAFE + msg.scalar_value = action.distance_m + return msg + + +@to_viz_msg.register +def _(action: Strafe, marker_ns): + return [] + + +@to_msg.register +def _(action: Stop): + msg = ActionMsg() + msg.action_type = msg.STOP + return msg + + +@to_viz_msg.register +def _(action: Stop, marker_ns): + return [] + + +@to_msg.register +def _(action: StandSit): + msg = ActionMsg() + msg.action_type = msg.STAND_SIT + msg.stand_sit_action = action.action + return msg + + +@to_viz_msg.register +def _(action: StandSit, marker_ns): + return [] diff --git a/robot_executor_interface/robot_executor_msgs/msg/ActionMsg.msg b/robot_executor_interface/robot_executor_msgs/msg/ActionMsg.msg index 9d494ac..624f5fe 100644 --- a/robot_executor_interface/robot_executor_msgs/msg/ActionMsg.msg +++ b/robot_executor_interface/robot_executor_msgs/msg/ActionMsg.msg @@ -6,6 +6,11 @@ string FOLLOW="FOLLOW" string GAZE="GAZE" string PICK="PICK" string PLACE="PLACE" +string MOVE_RELATIVE="MOVE_RELATIVE" +string TURN_RELATIVE="TURN_RELATIVE" +string STRAFE="STRAFE" +string STOP="STOP" +string STAND_SIT="STAND_SIT" string action_type @@ -29,3 +34,9 @@ string object_id # Definition for PLACE string place_frame # Should drop the object for now. + +# Definition for MOVE_RELATIVE, TURN_RELATIVE, STRAFE +float64 scalar_value + +# Definition for STAND_SIT +string stand_sit_action diff --git a/spot_tools/examples/test_direct_navigation_on_robot.py b/spot_tools/examples/test_direct_navigation_on_robot.py new file mode 100644 index 0000000..bb44f60 --- /dev/null +++ b/spot_tools/examples/test_direct_navigation_on_robot.py @@ -0,0 +1,262 @@ +""" +On-robot verification script for direct navigation commands. + +This script runs a series of tests on the real Spot robot to verify +the direct navigation commands work correctly. Each test requires +manual confirmation before proceeding. + +Usage: + python -i test_direct_navigation_on_robot.py --ip --username --password + +Prerequisites: + - Robot is powered on and standing + - You have line of sight to the robot + - BD tablet E-Stop is accessible + - Clear area (~5m radius) around the robot + +Test sequence: + 1. Small move forward (0.5m) - verify direction and distance + 2. Small move backward (0.5m) - verify return to start + 3. Turn left 90 degrees - verify rotation direction + 4. Turn right 90 degrees - verify return to original heading + 5. Strafe left (0.5m) - verify lateral movement + 6. Strafe right (0.5m) - verify return + 7. Sit down - verify robot sits + 8. Stand up - verify robot stands + 9. Stop during move - verify preemption + 10. Pause/resume (if running via ROS) +""" + +import argparse +import sys +import time + +import numpy as np + +from spot_executor.spot import Spot +from spot_executor.spot_executor import SpotExecutor +from robot_executor_interface.action_descriptions import ( + ActionSequence, + MoveRelative, + StandSit, + Stop, + Strafe, + TurnRelative, +) + + +class TestFeedback: + """Minimal feedback for on-robot tests.""" + + def __init__(self): + self.break_out_of_waiting_loop = False + + def print(self, level, s): + print(f"[{level}] {s}") + + def follow_path_feedback(self, path): + pass + + def path_following_progress_feedback(self, progress_point, target_point): + pass + + def path_follow_MLP_feedback(self, path_wp, target_point_metric): + pass + + def gaze_feedback(self, current_pose, gaze_point): + pass + + def set_robot_holding_state(self, is_holding, object_id): + pass + + def log_lease_takeover(self, event): + pass + + +def get_pose_str(spot): + pose = spot.get_pose() + return f"x={pose[0]:.2f}, y={pose[1]:.2f}, yaw={np.rad2deg(pose[2]):.1f}deg" + + +def confirm(prompt): + response = input(f"\n{prompt} [y/n/q]: ").strip().lower() + if response == "q": + print("Quitting tests.") + sys.exit(0) + return response == "y" + + +def run_action(executor, feedback, name, actions): + print(f"\n{'='*60}") + print(f"TEST: {name}") + print(f"{'='*60}") + seq = ActionSequence("test", "spot", actions) + executor.process_action_sequence(seq, feedback) + + +def tf_identity(parent, child): + return np.array([0, 0, 0.0]), np.array([0.0, 0, 0, 1]) + + +def main(): + parser = argparse.ArgumentParser("Direct Navigation On-Robot Tests") + parser.add_argument("--ip", required=True, help="Spot IP address") + parser.add_argument("--username", required=True) + parser.add_argument("--password", required=True) + args = parser.parse_args() + + print("Connecting to Spot...") + spot = Spot( + username=args.username, + password=args.password, + ip=args.ip, + take_lease=True, + set_estop=False, + ) + + from robot_executor_interface.mid_level_planner import IdentityPlanner + + feedback = TestFeedback() + planner = IdentityPlanner(feedback) + executor = SpotExecutor( + spot, + detector=None, + transform_lookup=tf_identity, + planner=planner, + ) + + print("Connected. Ensure robot is standing.") + spot.stand() + time.sleep(2) + + results = [] + + # --- Test 1: Move forward --- + if confirm("Test 1: Move FORWARD 0.5m. Robot should take ~1 step forward. Ready?"): + start = spot.get_pose() + run_action(executor, feedback, "Move Forward 0.5m", [MoveRelative(distance_m=0.5)]) + end = spot.get_pose() + dist = np.linalg.norm(end[:2] - start[:2]) + print(f" Measured displacement: {dist:.2f}m (expected ~0.5m)") + passed = confirm("Did the robot move forward approximately 0.5m?") + results.append(("Move Forward", passed)) + + # --- Test 2: Move backward --- + if confirm("Test 2: Move BACKWARD 0.5m. Robot should return close to start. Ready?"): + start = spot.get_pose() + run_action(executor, feedback, "Move Backward 0.5m", [MoveRelative(distance_m=-0.5)]) + end = spot.get_pose() + dist = np.linalg.norm(end[:2] - start[:2]) + print(f" Measured displacement: {dist:.2f}m (expected ~0.5m)") + passed = confirm("Did the robot move backward approximately 0.5m?") + results.append(("Move Backward", passed)) + + # --- Test 3: Turn left --- + if confirm("Test 3: Turn LEFT 90 degrees (counter-clockwise from above). Ready?"): + start_yaw = np.rad2deg(spot.get_pose()[2]) + run_action(executor, feedback, "Turn Left 90deg", [TurnRelative(angle_deg=90.0)]) + end_yaw = np.rad2deg(spot.get_pose()[2]) + delta = end_yaw - start_yaw + # Normalize to [-180, 180] + delta = (delta + 180) % 360 - 180 + print(f" Measured rotation: {delta:.1f}deg (expected ~+90deg)") + passed = confirm("Did the robot turn left (CCW) approximately 90 degrees?") + results.append(("Turn Left", passed)) + + # --- Test 4: Turn right --- + if confirm("Test 4: Turn RIGHT 90 degrees (should return to original heading). Ready?"): + start_yaw = np.rad2deg(spot.get_pose()[2]) + run_action(executor, feedback, "Turn Right 90deg", [TurnRelative(angle_deg=-90.0)]) + end_yaw = np.rad2deg(spot.get_pose()[2]) + delta = end_yaw - start_yaw + delta = (delta + 180) % 360 - 180 + print(f" Measured rotation: {delta:.1f}deg (expected ~-90deg)") + passed = confirm("Did the robot turn right (CW) approximately 90 degrees?") + results.append(("Turn Right", passed)) + + # --- Test 5: Strafe left --- + if confirm("Test 5: Strafe LEFT 0.5m. Robot should step sideways to its left. Ready?"): + start = spot.get_pose() + run_action(executor, feedback, "Strafe Left 0.5m", [Strafe(distance_m=0.5)]) + end = spot.get_pose() + dist = np.linalg.norm(end[:2] - start[:2]) + print(f" Measured displacement: {dist:.2f}m (expected ~0.5m)") + passed = confirm("Did the robot strafe left approximately 0.5m?") + results.append(("Strafe Left", passed)) + + # --- Test 6: Strafe right --- + if confirm("Test 6: Strafe RIGHT 0.5m. Robot should return to original position. Ready?"): + start = spot.get_pose() + run_action(executor, feedback, "Strafe Right 0.5m", [Strafe(distance_m=-0.5)]) + end = spot.get_pose() + dist = np.linalg.norm(end[:2] - start[:2]) + print(f" Measured displacement: {dist:.2f}m (expected ~0.5m)") + passed = confirm("Did the robot strafe right approximately 0.5m?") + results.append(("Strafe Right", passed)) + + # --- Test 7: Sit --- + if confirm("Test 7: SIT. Robot should sit down gracefully. Ready?"): + run_action(executor, feedback, "Sit", [StandSit(action="sit")]) + passed = confirm("Did the robot sit down?") + results.append(("Sit", passed)) + + # --- Test 8: Stand --- + if confirm("Test 8: STAND. Robot should stand back up. Ready?"): + run_action(executor, feedback, "Stand", [StandSit(action="stand")]) + passed = confirm("Did the robot stand up?") + results.append(("Stand", passed)) + + # --- Test 9: Stop preempts sequence --- + if confirm( + "Test 9: STOP preemption. Will send [Move 3m, Turn 90deg]. " + "The Stop is the first action so the Turn should NOT execute. Ready?" + ): + run_action( + executor, + feedback, + "Stop then Turn (Turn should be skipped)", + [Stop(), TurnRelative(angle_deg=90.0)], + ) + passed = confirm("Did the robot stop without turning?") + results.append(("Stop Preemption", passed)) + + # --- Test 10: Multi-step sequence --- + if confirm( + "Test 10: Multi-step sequence. Robot will: " + "move forward 1m, turn right 90deg, move forward 1m. " + "Should trace an L-shape. Ready?" + ): + run_action( + executor, + feedback, + "L-shaped path", + [ + MoveRelative(distance_m=1.0), + TurnRelative(angle_deg=-90.0), + MoveRelative(distance_m=1.0), + ], + ) + passed = confirm("Did the robot trace an L-shaped path?") + results.append(("L-shape Sequence", passed)) + + # --- Summary --- + print(f"\n{'='*60}") + print("TEST RESULTS") + print(f"{'='*60}") + for name, passed in results: + status = "PASS" if passed else "FAIL" + print(f" [{status}] {name}") + + total = len(results) + passed_count = sum(1 for _, p in results if p) + print(f"\n{passed_count}/{total} tests passed") + + if passed_count < total: + print("\nFailed tests may indicate:") + print(" - Sign convention mismatch (check body frame conventions)") + print(" - Sleep timing too short (robot didn't finish moving)") + print(" - Odometry drift (normal for longer sequences)") + + +if __name__ == "__main__": + main() diff --git a/spot_tools/src/spot_executor/spot_executor.py b/spot_tools/src/spot_executor/spot_executor.py index e952249..f03b337 100644 --- a/spot_tools/src/spot_executor/spot_executor.py +++ b/spot_tools/src/spot_executor/spot_executor.py @@ -9,15 +9,25 @@ from robot_executor_interface.action_descriptions import ( Follow, Gaze, + MoveRelative, Pick, Place, + StandSit, + Stop, + Strafe, + TurnRelative, ) from scipy.spatial.transform import Rotation from spot_skills.arm_utils import gaze_at_vision_pose from spot_skills.grasp_utils import object_grasp, object_place, stow_arm +from bosdyn.client import math_helpers from spot_skills.navigation_utils import ( + MAX_LINEAR_VEL, + MAX_ROTATION_VEL, follow_trajectory_continuous, + navigate_to_absolute_pose, + navigate_to_relative_pose, turn_to_point, ) @@ -232,6 +242,21 @@ def process_action_sequence(self, sequence, feedback): elif type(command) is Place: success = self.execute_place(command, feedback) + elif type(command) is MoveRelative: + success = self.execute_move_relative(command, feedback) + + elif type(command) is TurnRelative: + success = self.execute_turn_relative(command, feedback) + + elif type(command) is Strafe: + success = self.execute_strafe(command, feedback) + + elif type(command) is Stop: + success = self.execute_stop(command, feedback) + + elif type(command) is StandSit: + success = self.execute_stand_sit(command, feedback) + else: raise Exception( f"SpotExecutor received unknown command type {type(command)}" @@ -361,3 +386,56 @@ def execute_follow(self, command, feedback): feedback=feedback, ) return ret + + def execute_move_relative(self, command, feedback): + feedback.print("INFO", f"Moving {command.distance_m}m forward") + body_tform_goal = math_helpers.SE2Pose( + x=command.distance_m, y=0, angle=0 + ) + navigate_to_relative_pose(self.spot_interface, body_tform_goal) + time.sleep(abs(command.distance_m) / MAX_LINEAR_VEL + 2) + feedback.print("INFO", "Finished `move_relative` command") + return True + + def execute_turn_relative(self, command, feedback): + feedback.print("INFO", f"Turning {command.angle_deg} degrees") + angle_rad = np.deg2rad(command.angle_deg) + body_tform_goal = math_helpers.SE2Pose(x=0, y=0, angle=angle_rad) + navigate_to_relative_pose(self.spot_interface, body_tform_goal) + time.sleep(abs(angle_rad) / MAX_ROTATION_VEL + 2) + feedback.print("INFO", "Finished `turn_relative` command") + return True + + def execute_strafe(self, command, feedback): + feedback.print("INFO", f"Strafing {command.distance_m}m") + body_tform_goal = math_helpers.SE2Pose( + x=0, y=command.distance_m, angle=0 + ) + navigate_to_relative_pose(self.spot_interface, body_tform_goal) + time.sleep(abs(command.distance_m) / MAX_LINEAR_VEL + 2) + feedback.print("INFO", "Finished `strafe` command") + return True + + def execute_stop(self, command, feedback): + feedback.print("INFO", "Stopping robot") + self.keep_going = False + feedback.break_out_of_waiting_loop = True + current_pose = self.spot_interface.get_pose() + waypoint = math_helpers.SE2Pose( + x=current_pose[0], y=current_pose[1], angle=current_pose[2] + ) + navigate_to_absolute_pose(self.spot_interface, waypoint) + time.sleep(1.0) + feedback.print("INFO", "Robot stopped") + return True + + def execute_stand_sit(self, command, feedback): + if command.action not in ("stand", "sit"): + raise ValueError(f"Unknown StandSit action: {command.action!r}") + feedback.print("INFO", f"Executing {command.action}") + if command.action == "stand": + self.spot_interface.stand() + else: + self.spot_interface.sit() + feedback.print("INFO", f"Finished `{command.action}` command") + return True diff --git a/spot_tools/tests/__init__.py b/spot_tools/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/spot_tools/tests/test_direct_navigation.py b/spot_tools/tests/test_direct_navigation.py new file mode 100644 index 0000000..013d201 --- /dev/null +++ b/spot_tools/tests/test_direct_navigation.py @@ -0,0 +1,350 @@ +""" +Unit tests for direct navigation commands (MoveRelative, TurnRelative, Strafe, Stop, StandSit). + +These tests use FakeSpot and mock navigate_to_relative_pose (which requires get_state() +that FakeSpot doesn't implement) to verify executor dispatch and logic. + +Run with: python -m pytest spot_tools/tests/test_direct_navigation.py -v +""" + +from dataclasses import dataclass +from unittest.mock import MagicMock, patch + +import numpy as np +import pytest +from robot_executor_interface.action_descriptions import ( + ActionSequence, + MoveRelative, + StandSit, + Stop, + Strafe, + TurnRelative, +) + +from spot_executor.fake_spot import FakeSpot +from spot_executor.spot_executor import SpotExecutor + + +# --- Test Fixtures --- + + +class MinimalFeedback: + """Minimal feedback collector for tests (no matplotlib dependency).""" + + def __init__(self): + self.break_out_of_waiting_loop = False + self.logs = [] + + def print(self, level, s): + self.logs.append((level, str(s))) + + def follow_path_feedback(self, path): + pass + + def path_following_progress_feedback(self, progress_point, target_point): + pass + + def path_follow_MLP_feedback(self, path_wp, target_point_metric): + pass + + def gaze_feedback(self, current_pose, gaze_point): + pass + + def set_robot_holding_state(self, is_holding, object_id): + pass + + def log_lease_takeover(self, event): + pass + + +def tf_identity(parent, child): + return np.array([0, 0, 0.0]), np.array([0.0, 0, 0, 1]) + + +@pytest.fixture +def fake_spot(): + return FakeSpot(init_pose=np.array([0.0, 0.0, 0.0, 0.0])) + + +@pytest.fixture +def feedback(): + return MinimalFeedback() + + +@pytest.fixture +def executor(fake_spot): + from robot_executor_interface.mid_level_planner import IdentityPlanner + + planner = IdentityPlanner(MinimalFeedback()) + return SpotExecutor( + fake_spot, + detector=None, + transform_lookup=tf_identity, + planner=planner, + ) + + +# --- Action Dataclass Tests --- + + +class TestActionDataclasses: + def test_move_relative_creation(self): + cmd = MoveRelative(distance_m=2.5) + assert cmd.distance_m == 2.5 + + def test_move_relative_negative(self): + cmd = MoveRelative(distance_m=-1.0) + assert cmd.distance_m == -1.0 + + def test_turn_relative_creation(self): + cmd = TurnRelative(angle_deg=90.0) + assert cmd.angle_deg == 90.0 + + def test_turn_relative_negative(self): + cmd = TurnRelative(angle_deg=-45.0) + assert cmd.angle_deg == -45.0 + + def test_strafe_creation(self): + cmd = Strafe(distance_m=1.5) + assert cmd.distance_m == 1.5 + + def test_stop_creation(self): + cmd = Stop() + assert isinstance(cmd, Stop) + + def test_stand_sit_stand(self): + cmd = StandSit(action="stand") + assert cmd.action == "stand" + + def test_stand_sit_sit(self): + cmd = StandSit(action="sit") + assert cmd.action == "sit" + + +# --- Executor Dispatch Tests --- + + +class TestExecutorDispatch: + """Test that SpotExecutor correctly dispatches to the right execute_* method.""" + + @patch("spot_executor.spot_executor.navigate_to_relative_pose") + @patch("spot_executor.spot_executor.time.sleep") + def test_move_relative_dispatches(self, mock_sleep, mock_nav, executor, feedback): + seq = ActionSequence("test", "spot", [MoveRelative(distance_m=2.0)]) + executor.process_action_sequence(seq, feedback) + mock_nav.assert_called_once() + args = mock_nav.call_args + assert args[0][1].x == 2.0 + assert args[0][1].y == 0.0 + assert args[0][1].angle == 0.0 + + @patch("spot_executor.spot_executor.navigate_to_relative_pose") + @patch("spot_executor.spot_executor.time.sleep") + def test_move_relative_backward(self, mock_sleep, mock_nav, executor, feedback): + seq = ActionSequence("test", "spot", [MoveRelative(distance_m=-3.0)]) + executor.process_action_sequence(seq, feedback) + args = mock_nav.call_args + assert args[0][1].x == -3.0 + + @patch("spot_executor.spot_executor.navigate_to_relative_pose") + @patch("spot_executor.spot_executor.time.sleep") + def test_turn_relative_dispatches(self, mock_sleep, mock_nav, executor, feedback): + seq = ActionSequence("test", "spot", [TurnRelative(angle_deg=90.0)]) + executor.process_action_sequence(seq, feedback) + mock_nav.assert_called_once() + args = mock_nav.call_args + assert args[0][1].x == 0.0 + assert args[0][1].y == 0.0 + assert abs(args[0][1].angle - np.deg2rad(90.0)) < 1e-6 + + @patch("spot_executor.spot_executor.navigate_to_relative_pose") + @patch("spot_executor.spot_executor.time.sleep") + def test_turn_relative_negative(self, mock_sleep, mock_nav, executor, feedback): + seq = ActionSequence("test", "spot", [TurnRelative(angle_deg=-45.0)]) + executor.process_action_sequence(seq, feedback) + args = mock_nav.call_args + assert abs(args[0][1].angle - np.deg2rad(-45.0)) < 1e-6 + + @patch("spot_executor.spot_executor.navigate_to_relative_pose") + @patch("spot_executor.spot_executor.time.sleep") + def test_strafe_dispatches(self, mock_sleep, mock_nav, executor, feedback): + seq = ActionSequence("test", "spot", [Strafe(distance_m=1.5)]) + executor.process_action_sequence(seq, feedback) + mock_nav.assert_called_once() + args = mock_nav.call_args + assert args[0][1].x == 0.0 + assert args[0][1].y == 1.5 + assert args[0][1].angle == 0.0 + + @patch("spot_executor.spot_executor.navigate_to_absolute_pose") + @patch("spot_executor.spot_executor.time.sleep") + def test_stop_dispatches(self, mock_sleep, mock_nav, executor, fake_spot, feedback): + fake_spot.set_pose(np.array([5.0, 3.0, 0.0, 1.2])) + seq = ActionSequence("test", "spot", [Stop()]) + executor.process_action_sequence(seq, feedback) + mock_nav.assert_called_once() + # Verify it commands hold at current pose + args = mock_nav.call_args + waypoint = args[0][1] + assert abs(waypoint.x - 5.0) < 1e-6 + assert abs(waypoint.y - 3.0) < 1e-6 + assert abs(waypoint.angle - 1.2) < 1e-6 + + def test_stand_sit_stand(self, executor, fake_spot, feedback): + fake_spot.stand = MagicMock() + seq = ActionSequence("test", "spot", [StandSit(action="stand")]) + executor.process_action_sequence(seq, feedback) + fake_spot.stand.assert_called_once() + + def test_stand_sit_sit(self, executor, fake_spot, feedback): + fake_spot.sit = MagicMock() + seq = ActionSequence("test", "spot", [StandSit(action="sit")]) + executor.process_action_sequence(seq, feedback) + fake_spot.sit.assert_called_once() + + def test_stand_sit_invalid_raises(self, executor, feedback): + seq = ActionSequence("test", "spot", [StandSit(action="jump")]) + with pytest.raises(ValueError, match="Unknown StandSit action"): + executor.process_action_sequence(seq, feedback) + + +# --- Stop Preemption Tests --- + + +class TestStopPreemption: + @patch("spot_executor.spot_executor.navigate_to_relative_pose") + @patch("spot_executor.spot_executor.navigate_to_absolute_pose") + @patch("spot_executor.spot_executor.time.sleep") + def test_stop_cancels_remaining_actions( + self, mock_sleep, mock_abs_nav, mock_rel_nav, executor, fake_spot, feedback + ): + """Stop should prevent subsequent actions from executing.""" + fake_spot.set_pose(np.array([0.0, 0.0, 0.0, 0.0])) + seq = ActionSequence( + "test", + "spot", + [Stop(), MoveRelative(distance_m=5.0)], + ) + executor.process_action_sequence(seq, feedback) + # navigate_to_absolute_pose called for Stop's hold-position + mock_abs_nav.assert_called_once() + # navigate_to_relative_pose should NOT be called (MoveRelative was skipped) + mock_rel_nav.assert_not_called() + + @patch("spot_executor.spot_executor.navigate_to_absolute_pose") + @patch("spot_executor.spot_executor.time.sleep") + def test_stop_sets_break_flag(self, mock_sleep, mock_nav, executor, feedback): + seq = ActionSequence("test", "spot", [Stop()]) + executor.process_action_sequence(seq, feedback) + assert feedback.break_out_of_waiting_loop is True + + +# --- Sequence Tests --- + + +class TestActionSequences: + @patch("spot_executor.spot_executor.navigate_to_relative_pose") + @patch("spot_executor.spot_executor.time.sleep") + def test_multiple_moves_in_sequence(self, mock_sleep, mock_nav, executor, feedback): + seq = ActionSequence( + "test", + "spot", + [ + MoveRelative(distance_m=1.0), + TurnRelative(angle_deg=90.0), + MoveRelative(distance_m=2.0), + ], + ) + executor.process_action_sequence(seq, feedback) + assert mock_nav.call_count == 3 + + @patch("spot_executor.spot_executor.navigate_to_relative_pose") + @patch("spot_executor.spot_executor.time.sleep") + def test_move_then_sit(self, mock_sleep, mock_nav, executor, fake_spot, feedback): + fake_spot.sit = MagicMock() + seq = ActionSequence( + "test", + "spot", + [MoveRelative(distance_m=1.0), StandSit(action="sit")], + ) + executor.process_action_sequence(seq, feedback) + mock_nav.assert_called_once() + fake_spot.sit.assert_called_once() + + +# --- ROS Message Serialization Tests --- + + +class TestMessageSerialization: + def test_move_relative_roundtrip(self): + from robot_executor_interface.action_descriptions import ActionSequence + from robot_executor_interface_ros.action_descriptions_ros import from_msg, to_msg + + original = ActionSequence( + "test", "spot", [MoveRelative(distance_m=2.5)] + ) + msg = to_msg(original) + result = from_msg(msg) + assert len(result.actions) == 1 + assert isinstance(result.actions[0], MoveRelative) + assert result.actions[0].distance_m == 2.5 + + def test_turn_relative_roundtrip(self): + from robot_executor_interface_ros.action_descriptions_ros import from_msg, to_msg + + original = ActionSequence( + "test", "spot", [TurnRelative(angle_deg=-45.0)] + ) + msg = to_msg(original) + result = from_msg(msg) + assert isinstance(result.actions[0], TurnRelative) + assert result.actions[0].angle_deg == -45.0 + + def test_strafe_roundtrip(self): + from robot_executor_interface_ros.action_descriptions_ros import from_msg, to_msg + + original = ActionSequence("test", "spot", [Strafe(distance_m=1.0)]) + msg = to_msg(original) + result = from_msg(msg) + assert isinstance(result.actions[0], Strafe) + assert result.actions[0].distance_m == 1.0 + + def test_stop_roundtrip(self): + from robot_executor_interface_ros.action_descriptions_ros import from_msg, to_msg + + original = ActionSequence("test", "spot", [Stop()]) + msg = to_msg(original) + result = from_msg(msg) + assert isinstance(result.actions[0], Stop) + + def test_stand_sit_roundtrip(self): + from robot_executor_interface_ros.action_descriptions_ros import from_msg, to_msg + + original = ActionSequence( + "test", "spot", [StandSit(action="sit")] + ) + msg = to_msg(original) + result = from_msg(msg) + assert isinstance(result.actions[0], StandSit) + assert result.actions[0].action == "sit" + + def test_mixed_sequence_roundtrip(self): + from robot_executor_interface_ros.action_descriptions_ros import from_msg, to_msg + + original = ActionSequence( + "test", + "spot", + [ + MoveRelative(distance_m=1.0), + TurnRelative(angle_deg=90.0), + Strafe(distance_m=-0.5), + StandSit(action="stand"), + ], + ) + msg = to_msg(original) + result = from_msg(msg) + assert len(result.actions) == 4 + assert isinstance(result.actions[0], MoveRelative) + assert isinstance(result.actions[1], TurnRelative) + assert isinstance(result.actions[2], Strafe) + assert isinstance(result.actions[3], StandSit) 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 8b7dc0b..8628335 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 @@ -27,11 +27,13 @@ from std_msgs.msg import Bool, String from visualization_msgs.msg import Marker, MarkerArray +from bosdyn.client import math_helpers from robot_executor_interface.mid_level_planner import ( IdentityPlanner, MidLevelPlanner, OccupancyMap, ) +from spot_skills.navigation_utils import navigate_to_absolute_pose from spot_tools_ros.fake_spot_ros import FakeSpotRos from spot_tools_ros.occupancy_grid_ros_updater import OccupancyGridROSUpdater from spot_tools_ros.utils import get_tf_pose, waypoints_to_path @@ -477,6 +479,8 @@ def tf_lookup_fn(parent, child): ) self.spot_executor.initialize_lease_manager(self.feedback_collector) + self.paused = False + self.action_sequence_sub = self.create_subscription( ActionSequenceMsg, "~/action_sequence_subscriber", @@ -484,6 +488,13 @@ def tf_lookup_fn(parent, child): 10, ) + self.pause_sub = self.create_subscription( + Bool, + "~/pause", + self.pause_callback, + 10, + ) + self.heartbeat_pub = self.create_publisher(NodeInfoMsg, "~/node_status", 1) heartbeat_timer_group = MutuallyExclusiveCallbackGroup() timer_period_s = 0.1 @@ -491,6 +502,34 @@ def tf_lookup_fn(parent, child): timer_period_s, self.hb_callback, callback_group=heartbeat_timer_group ) + def pause_callback(self, msg): + if msg.data: + self.get_logger().info("PAUSE received — stopping robot and holding position") + self.paused = True + self.status_str = "Paused" + + # Signal the action sequence to stop (non-blocking) + self.spot_executor.keep_going = False + self.feedback_collector.break_out_of_waiting_loop = True + + # Command robot to hold current pose (stay standing) + try: + current_pose = self.spot_interface.get_pose() + waypoint = math_helpers.SE2Pose( + x=current_pose[0], + y=current_pose[1], + angle=current_pose[2], + ) + navigate_to_absolute_pose(self.spot_interface, waypoint) + except Exception as e: + self.get_logger().error(f"Failed to hold position on pause: {e}") + + else: + self.get_logger().info("RESUME received — accepting commands") + self.paused = False + self.feedback_collector.break_out_of_waiting_loop = False + self.status_str = "Idle" + def hb_callback(self): msg = NodeInfoMsg() msg.nickname = "spot_executor" @@ -500,6 +539,13 @@ def hb_callback(self): self.heartbeat_pub.publish(msg) def process_action_sequence(self, msg): + if self.paused: + self.get_logger().warn( + "Received action sequence while paused — ignoring. " + "Publish False to ~/pause to resume." + ) + return + def process_sequence(): self.status_str = "Processing action sequence" self.get_logger().info("Starting action sequence") From b2404e5aeaeed62597daa3fee222b143fc09b96f Mon Sep 17 00:00:00 2001 From: harelb Date: Thu, 26 Mar 2026 11:49:40 -0400 Subject: [PATCH 2/4] Fix turn direction: negate angle to match BD SE2Pose CW-positive convention --- spot_tools/src/spot_executor/spot_executor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_tools/src/spot_executor/spot_executor.py b/spot_tools/src/spot_executor/spot_executor.py index f03b337..454699f 100644 --- a/spot_tools/src/spot_executor/spot_executor.py +++ b/spot_tools/src/spot_executor/spot_executor.py @@ -399,7 +399,7 @@ def execute_move_relative(self, command, feedback): def execute_turn_relative(self, command, feedback): feedback.print("INFO", f"Turning {command.angle_deg} degrees") - angle_rad = np.deg2rad(command.angle_deg) + angle_rad = -np.deg2rad(command.angle_deg) # BD SE2Pose: positive=CW=right, so negate for positive=CCW=left body_tform_goal = math_helpers.SE2Pose(x=0, y=0, angle=angle_rad) navigate_to_relative_pose(self.spot_interface, body_tform_goal) time.sleep(abs(angle_rad) / MAX_ROTATION_VEL + 2) From 4b52c93704f74537a1819592e3a84fb340f20909 Mon Sep 17 00:00:00 2001 From: harelb Date: Thu, 26 Mar 2026 12:03:05 -0400 Subject: [PATCH 3/4] Add nav_test.sh script and update README with testing instructions --- README.md | 54 +++++++++++++++++++++++++++--- spot_tools_ros/scripts/nav_test.sh | 48 ++++++++++++++++++++++++++ 2 files changed, 97 insertions(+), 5 deletions(-) create mode 100755 spot_tools_ros/scripts/nav_test.sh diff --git a/README.md b/README.md index b6e5b8a..2d26048 100644 --- a/README.md +++ b/README.md @@ -48,26 +48,70 @@ Chat input -> LLM tool call (e.g., move_relative) -> ros2 topic pub ActionSequen ## Testing Direct Commands -You can manually publish direct commands for testing without the chat interface: +### nav_test.sh (recommended) + +A helper script at `spot_tools_ros/scripts/nav_test.sh` provides one-liner +commands for interactive testing without the chat interface. Source it in any +terminal on the robot: + +```bash +source ~/dcist_ws/src/awesome_dcist_t4/spot_tools/spot_tools_ros/scripts/nav_test.sh +``` + +This auto-sources ROS, sets Zenoh env vars, and loads the workspace. Available +commands (optional argument shown in brackets): + +```bash +forward [m] # move forward (default 1.0m) +backward [m] # move backward +turn_left [deg] # turn CCW (default 90deg) +turn_right [deg] # turn CW +strafe_left [m] # move left (default 0.5m) +strafe_right [m] # move right +stand +sit +stop # cancel sequence and hold position +pause # hold position, reject new commands +resume # accept commands again +``` + +Example session: +```bash +forward 0.5 +turn_left 90 +strafe_right 0.3 +sit +stand +stop +``` + +> **Topic note:** The executor's `~/action_sequence_subscriber` topic is +> remapped in the hamilton launch config to +> `/hamilton/omniplanner_node/compiled_plan_out`. Update `EXEC_TOPIC` in the +> script if deploying on a different robot or launch configuration. + +### Raw ros2 topic pub + +You can also publish commands directly: ```bash # Move forward 2 meters -ros2 topic pub /hamilton/spot_executor_node/action_sequence_subscriber \ +ros2 topic pub /hamilton/omniplanner_node/compiled_plan_out \ robot_executor_msgs/msg/ActionSequenceMsg \ "{plan_id: 'test', robot_name: 'spot', actions: [{action_type: 'MOVE_RELATIVE', scalar_value: 2.0}]}" -1 # Turn right 90 degrees -ros2 topic pub /hamilton/spot_executor_node/action_sequence_subscriber \ +ros2 topic pub /hamilton/omniplanner_node/compiled_plan_out \ robot_executor_msgs/msg/ActionSequenceMsg \ "{plan_id: 'test', robot_name: 'spot', actions: [{action_type: 'TURN_RELATIVE', scalar_value: -90.0}]}" -1 # Stop -ros2 topic pub /hamilton/spot_executor_node/action_sequence_subscriber \ +ros2 topic pub /hamilton/omniplanner_node/compiled_plan_out \ robot_executor_msgs/msg/ActionSequenceMsg \ "{plan_id: 'test', robot_name: 'spot', actions: [{action_type: 'STOP'}]}" -1 # Sit down -ros2 topic pub /hamilton/spot_executor_node/action_sequence_subscriber \ +ros2 topic pub /hamilton/omniplanner_node/compiled_plan_out \ robot_executor_msgs/msg/ActionSequenceMsg \ "{plan_id: 'test', robot_name: 'spot', actions: [{action_type: 'STAND_SIT', stand_sit_action: 'sit'}]}" -1 ``` diff --git a/spot_tools_ros/scripts/nav_test.sh b/spot_tools_ros/scripts/nav_test.sh new file mode 100755 index 0000000..a9bb38b --- /dev/null +++ b/spot_tools_ros/scripts/nav_test.sh @@ -0,0 +1,48 @@ +#!/usr/bin/env bash +# Quick navigation test commands for Spot direct control. +# Usage: source nav_test.sh +# +# Then call: forward 0.5 | backward 1.0 | turn_left 90 | turn_right 45 +# strafe_left 0.5 | strafe_right 0.5 | stand | sit | stop +# pause | resume +# +# The EXEC_TOPIC must match the remapped action_sequence_subscriber topic for +# your deployment. In the hamilton launch config this is remapped to: +# omniplanner_node/compiled_plan_out +# Adjust EXEC_TOPIC below if deploying on a different robot or launch config. + +# Source ROS and workspace if not already set up +if [ -z "$RMW_IMPLEMENTATION" ]; then + source /opt/ros/jazzy/setup.zsh 2>/dev/null || source /opt/ros/jazzy/setup.bash 2>/dev/null + export RMW_IMPLEMENTATION=rmw_zenoh_cpp + export ZENOH_ROUTER_CONFIG_URI=/home/swarm/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 +fi + +ADT4_SETUP=/home/swarm/dcist_ws/install/setup.zsh +[ -f "$ADT4_SETUP" ] && source "$ADT4_SETUP" + +EXEC_TOPIC="/hamilton/omniplanner_node/compiled_plan_out" +PAUSE_TOPIC="/hamilton/spot_executor_node/pause" +MSG_TYPE="robot_executor_msgs/msg/ActionSequenceMsg" + +_pub() { ros2 topic pub "$EXEC_TOPIC" "$MSG_TYPE" "$1" -1; } + +forward() { _pub "{plan_id: 'test', robot_name: 'spot', actions: [{action_type: 'MOVE_RELATIVE', scalar_value: ${1:-1.0}}]}"; } +backward() { _pub "{plan_id: 'test', robot_name: 'spot', actions: [{action_type: 'MOVE_RELATIVE', scalar_value: -${1:-1.0}}]}"; } +turn_left() { _pub "{plan_id: 'test', robot_name: 'spot', actions: [{action_type: 'TURN_RELATIVE', scalar_value: ${1:-90.0}}]}"; } +turn_right() { _pub "{plan_id: 'test', robot_name: 'spot', actions: [{action_type: 'TURN_RELATIVE', scalar_value: -${1:-90.0}}]}"; } +strafe_left() { _pub "{plan_id: 'test', robot_name: 'spot', actions: [{action_type: 'STRAFE', scalar_value: ${1:-0.5}}]}"; } +strafe_right() { _pub "{plan_id: 'test', robot_name: 'spot', actions: [{action_type: 'STRAFE', scalar_value: -${1:-0.5}}]}"; } +stand() { _pub "{plan_id: 'test', robot_name: 'spot', actions: [{action_type: 'STAND_SIT', stand_sit_action: 'stand'}]}"; } +sit() { _pub "{plan_id: 'test', robot_name: 'spot', actions: [{action_type: 'STAND_SIT', stand_sit_action: 'sit'}]}"; } +stop() { _pub "{plan_id: 'test', robot_name: 'spot', actions: [{action_type: 'STOP'}]}"; } +pause() { ros2 topic pub "$PAUSE_TOPIC" std_msgs/msg/Bool "{data: true}" -1; } +resume() { ros2 topic pub "$PAUSE_TOPIC" std_msgs/msg/Bool "{data: false}" -1; } + +echo "Nav test commands loaded:" +echo " forward [m] backward [m]" +echo " turn_left [deg] turn_right [deg]" +echo " strafe_left [m] strafe_right [m]" +echo " stand sit" +echo " stop pause resume" +echo "Defaults: distance=1.0m, angle=90deg, strafe=0.5m" From aafdc03760a46c4dd59318cb86155a5eb62b8ced Mon Sep 17 00:00:00 2001 From: harelb Date: Fri, 27 Mar 2026 12:14:26 -0400 Subject: [PATCH 4/4] Address PR review: add visualization markers, refactor tests, enable CI - Add @to_viz_msg functions for MoveRelative (cyan arrow), TurnRelative (orange cylinder), Strafe (yellow arrow), Stop (red cube), StandSit (green/blue sphere) - Remove TestActionDataclasses (tests Python, not our code) - Remove TestExecutorDispatch (mock-based, breaks abstraction) - Move TestMessageSerialization to robot_executor_interface_ros/tests - Enable pytest in CI with proper dependencies --- .github/workflows/ci-action.yml | 7 +- .../action_descriptions_ros.py | 109 +++++++++- .../tests/test_action_descriptions_ros.py | 98 +++++++++ spot_tools/tests/test_direct_navigation.py | 202 ------------------ 4 files changed, 206 insertions(+), 210 deletions(-) create mode 100644 robot_executor_interface/robot_executor_interface_ros/tests/test_action_descriptions_ros.py diff --git a/.github/workflows/ci-action.yml b/.github/workflows/ci-action.yml index 9944c33..647967b 100644 --- a/.github/workflows/ci-action.yml +++ b/.github/workflows/ci-action.yml @@ -28,8 +28,9 @@ jobs: run: pip install pre-commit && cd ${{ github.workspace }}/spot_tools_repo && pre-commit run --all-files - name: Install Spot Tools run: | + pip install ${{ github.workspace }}/spot_tools_repo/robot_executor_interface + pip install ${{ github.workspace }}/spot_tools_repo/robot_executor_interface/robot_executor_interface_ros pip install ${{ github.workspace }}/spot_tools_repo/spot_tools pip install ${{ github.workspace }}/spot_tools_repo/spot_tools_ros - #- name: Run test script - # run: cd ${{ github.workspace }}/ouroboros_repo && pytest --ignore=third_party - #- run: echo "🍏 This job's status is ${{ job.status }}." + - name: Run tests + run: cd ${{ github.workspace }}/spot_tools_repo && pytest spot_tools/tests spot_tools_ros/tests robot_executor_interface/robot_executor_interface_ros/tests -v diff --git a/robot_executor_interface/robot_executor_interface_ros/src/robot_executor_interface_ros/action_descriptions_ros.py b/robot_executor_interface/robot_executor_interface_ros/src/robot_executor_interface_ros/action_descriptions_ros.py index 61e2e76..47a3b61 100644 --- a/robot_executor_interface/robot_executor_interface_ros/src/robot_executor_interface_ros/action_descriptions_ros.py +++ b/robot_executor_interface/robot_executor_interface_ros/src/robot_executor_interface_ros/action_descriptions_ros.py @@ -369,7 +369,30 @@ def _(action: MoveRelative): @to_viz_msg.register def _(action: MoveRelative, marker_ns): - return [] + m = Marker() + m.header.frame_id = "body" + m.header.stamp = gtm() + m.ns = marker_ns + m.id = 0 + m.type = m.ARROW + m.action = m.ADD + m.pose.orientation.w = 1.0 + m.scale.x = 0.1 # shaft diameter + m.scale.y = 0.15 # head diameter + m.color.a = 1.0 + m.color.r = 0.0 + m.color.g = 1.0 + m.color.b = 1.0 + pt1 = Point() + pt1.x = 0.0 + pt1.y = 0.0 + pt1.z = 0.0 + pt2 = Point() + pt2.x = action.distance_m + pt2.y = 0.0 + pt2.z = 0.0 + m.points = [pt1, pt2] + return [m] @to_msg.register @@ -382,7 +405,23 @@ def _(action: TurnRelative): @to_viz_msg.register def _(action: TurnRelative, marker_ns): - return [] + # Create a cone to visualize rotation + m = Marker() + m.header.frame_id = "body" + m.header.stamp = gtm() + m.ns = marker_ns + m.id = 0 + m.type = m.CYLINDER + m.action = m.ADD + m.pose.orientation.w = 1.0 + m.scale.x = 0.5 # radius + m.scale.y = 0.5 + m.scale.z = 0.1 # height + m.color.a = 0.3 + m.color.r = 1.0 + m.color.g = 0.5 + m.color.b = 0.0 + return [m] @to_msg.register @@ -395,7 +434,30 @@ def _(action: Strafe): @to_viz_msg.register def _(action: Strafe, marker_ns): - return [] + m = Marker() + m.header.frame_id = "body" + m.header.stamp = gtm() + m.ns = marker_ns + m.id = 0 + m.type = m.ARROW + m.action = m.ADD + m.pose.orientation.w = 1.0 + m.scale.x = 0.1 # shaft diameter + m.scale.y = 0.15 # head diameter + m.color.a = 1.0 + m.color.r = 1.0 + m.color.g = 1.0 + m.color.b = 0.0 + pt1 = Point() + pt1.x = 0.0 + pt1.y = 0.0 + pt1.z = 0.0 + pt2 = Point() + pt2.x = 0.0 + pt2.y = action.distance_m + pt2.z = 0.0 + m.points = [pt1, pt2] + return [m] @to_msg.register @@ -407,7 +469,23 @@ def _(action: Stop): @to_viz_msg.register def _(action: Stop, marker_ns): - return [] + # Red X marker to indicate stop + m = Marker() + m.header.frame_id = "body" + m.header.stamp = gtm() + m.ns = marker_ns + m.id = 0 + m.type = m.CUBE + m.action = m.ADD + m.pose.orientation.w = 1.0 + m.scale.x = 0.3 + m.scale.y = 0.3 + m.scale.z = 0.3 + m.color.a = 0.8 + m.color.r = 1.0 + m.color.g = 0.0 + m.color.b = 0.0 + return [m] @to_msg.register @@ -420,4 +498,25 @@ def _(action: StandSit): @to_viz_msg.register def _(action: StandSit, marker_ns): - return [] + # Green sphere for stand, blue sphere for sit + m = Marker() + m.header.frame_id = "body" + m.header.stamp = gtm() + m.ns = marker_ns + m.id = 0 + m.type = m.SPHERE + m.action = m.ADD + m.pose.orientation.w = 1.0 + m.scale.x = 0.2 + m.scale.y = 0.2 + m.scale.z = 0.2 + m.color.a = 0.8 + if action.action == "stand": + m.color.r = 0.0 + m.color.g = 1.0 + m.color.b = 0.0 + else: # sit + m.color.r = 0.0 + m.color.g = 0.0 + m.color.b = 1.0 + return [m] diff --git a/robot_executor_interface/robot_executor_interface_ros/tests/test_action_descriptions_ros.py b/robot_executor_interface/robot_executor_interface_ros/tests/test_action_descriptions_ros.py new file mode 100644 index 0000000..4e252c2 --- /dev/null +++ b/robot_executor_interface/robot_executor_interface_ros/tests/test_action_descriptions_ros.py @@ -0,0 +1,98 @@ +""" +Tests for ROS message serialization of direct navigation actions. + +Tests conversion between action dataclasses and ROS ActionMsg messages, +verifying roundtrip serialization/deserialization. +""" + +import pytest +from robot_executor_interface.action_descriptions import ( + ActionSequence, + MoveRelative, + StandSit, + Stop, + Strafe, + TurnRelative, +) +from robot_executor_interface_ros.action_descriptions_ros import from_msg, to_msg + + +class TestDirectNavigationSerialization: + """Test ROS message serialization for direct navigation commands.""" + + def test_move_relative_roundtrip(self): + original = ActionSequence( + "test", "spot", [MoveRelative(distance_m=2.5)] + ) + msg = to_msg(original) + result = from_msg(msg) + assert len(result.actions) == 1 + assert isinstance(result.actions[0], MoveRelative) + assert result.actions[0].distance_m == 2.5 + + def test_move_relative_negative(self): + original = ActionSequence( + "test", "spot", [MoveRelative(distance_m=-3.0)] + ) + msg = to_msg(original) + result = from_msg(msg) + assert result.actions[0].distance_m == -3.0 + + def test_turn_relative_roundtrip(self): + original = ActionSequence( + "test", "spot", [TurnRelative(angle_deg=-45.0)] + ) + msg = to_msg(original) + result = from_msg(msg) + assert isinstance(result.actions[0], TurnRelative) + assert result.actions[0].angle_deg == -45.0 + + def test_strafe_roundtrip(self): + original = ActionSequence("test", "spot", [Strafe(distance_m=1.0)]) + msg = to_msg(original) + result = from_msg(msg) + assert isinstance(result.actions[0], Strafe) + assert result.actions[0].distance_m == 1.0 + + def test_stop_roundtrip(self): + original = ActionSequence("test", "spot", [Stop()]) + msg = to_msg(original) + result = from_msg(msg) + assert isinstance(result.actions[0], Stop) + + def test_stand_sit_stand(self): + original = ActionSequence( + "test", "spot", [StandSit(action="stand")] + ) + msg = to_msg(original) + result = from_msg(msg) + assert isinstance(result.actions[0], StandSit) + assert result.actions[0].action == "stand" + + def test_stand_sit_sit(self): + original = ActionSequence( + "test", "spot", [StandSit(action="sit")] + ) + msg = to_msg(original) + result = from_msg(msg) + assert isinstance(result.actions[0], StandSit) + assert result.actions[0].action == "sit" + + def test_mixed_sequence_roundtrip(self): + original = ActionSequence( + "test", + "spot", + [ + MoveRelative(distance_m=1.0), + TurnRelative(angle_deg=90.0), + Strafe(distance_m=-0.5), + StandSit(action="stand"), + ], + ) + msg = to_msg(original) + result = from_msg(msg) + assert len(result.actions) == 4 + assert isinstance(result.actions[0], MoveRelative) + assert isinstance(result.actions[1], TurnRelative) + assert isinstance(result.actions[2], Strafe) + assert isinstance(result.actions[3], StandSit) diff --git a/spot_tools/tests/test_direct_navigation.py b/spot_tools/tests/test_direct_navigation.py index 013d201..cf9c247 100644 --- a/spot_tools/tests/test_direct_navigation.py +++ b/spot_tools/tests/test_direct_navigation.py @@ -84,130 +84,6 @@ def executor(fake_spot): ) -# --- Action Dataclass Tests --- - - -class TestActionDataclasses: - def test_move_relative_creation(self): - cmd = MoveRelative(distance_m=2.5) - assert cmd.distance_m == 2.5 - - def test_move_relative_negative(self): - cmd = MoveRelative(distance_m=-1.0) - assert cmd.distance_m == -1.0 - - def test_turn_relative_creation(self): - cmd = TurnRelative(angle_deg=90.0) - assert cmd.angle_deg == 90.0 - - def test_turn_relative_negative(self): - cmd = TurnRelative(angle_deg=-45.0) - assert cmd.angle_deg == -45.0 - - def test_strafe_creation(self): - cmd = Strafe(distance_m=1.5) - assert cmd.distance_m == 1.5 - - def test_stop_creation(self): - cmd = Stop() - assert isinstance(cmd, Stop) - - def test_stand_sit_stand(self): - cmd = StandSit(action="stand") - assert cmd.action == "stand" - - def test_stand_sit_sit(self): - cmd = StandSit(action="sit") - assert cmd.action == "sit" - - -# --- Executor Dispatch Tests --- - - -class TestExecutorDispatch: - """Test that SpotExecutor correctly dispatches to the right execute_* method.""" - - @patch("spot_executor.spot_executor.navigate_to_relative_pose") - @patch("spot_executor.spot_executor.time.sleep") - def test_move_relative_dispatches(self, mock_sleep, mock_nav, executor, feedback): - seq = ActionSequence("test", "spot", [MoveRelative(distance_m=2.0)]) - executor.process_action_sequence(seq, feedback) - mock_nav.assert_called_once() - args = mock_nav.call_args - assert args[0][1].x == 2.0 - assert args[0][1].y == 0.0 - assert args[0][1].angle == 0.0 - - @patch("spot_executor.spot_executor.navigate_to_relative_pose") - @patch("spot_executor.spot_executor.time.sleep") - def test_move_relative_backward(self, mock_sleep, mock_nav, executor, feedback): - seq = ActionSequence("test", "spot", [MoveRelative(distance_m=-3.0)]) - executor.process_action_sequence(seq, feedback) - args = mock_nav.call_args - assert args[0][1].x == -3.0 - - @patch("spot_executor.spot_executor.navigate_to_relative_pose") - @patch("spot_executor.spot_executor.time.sleep") - def test_turn_relative_dispatches(self, mock_sleep, mock_nav, executor, feedback): - seq = ActionSequence("test", "spot", [TurnRelative(angle_deg=90.0)]) - executor.process_action_sequence(seq, feedback) - mock_nav.assert_called_once() - args = mock_nav.call_args - assert args[0][1].x == 0.0 - assert args[0][1].y == 0.0 - assert abs(args[0][1].angle - np.deg2rad(90.0)) < 1e-6 - - @patch("spot_executor.spot_executor.navigate_to_relative_pose") - @patch("spot_executor.spot_executor.time.sleep") - def test_turn_relative_negative(self, mock_sleep, mock_nav, executor, feedback): - seq = ActionSequence("test", "spot", [TurnRelative(angle_deg=-45.0)]) - executor.process_action_sequence(seq, feedback) - args = mock_nav.call_args - assert abs(args[0][1].angle - np.deg2rad(-45.0)) < 1e-6 - - @patch("spot_executor.spot_executor.navigate_to_relative_pose") - @patch("spot_executor.spot_executor.time.sleep") - def test_strafe_dispatches(self, mock_sleep, mock_nav, executor, feedback): - seq = ActionSequence("test", "spot", [Strafe(distance_m=1.5)]) - executor.process_action_sequence(seq, feedback) - mock_nav.assert_called_once() - args = mock_nav.call_args - assert args[0][1].x == 0.0 - assert args[0][1].y == 1.5 - assert args[0][1].angle == 0.0 - - @patch("spot_executor.spot_executor.navigate_to_absolute_pose") - @patch("spot_executor.spot_executor.time.sleep") - def test_stop_dispatches(self, mock_sleep, mock_nav, executor, fake_spot, feedback): - fake_spot.set_pose(np.array([5.0, 3.0, 0.0, 1.2])) - seq = ActionSequence("test", "spot", [Stop()]) - executor.process_action_sequence(seq, feedback) - mock_nav.assert_called_once() - # Verify it commands hold at current pose - args = mock_nav.call_args - waypoint = args[0][1] - assert abs(waypoint.x - 5.0) < 1e-6 - assert abs(waypoint.y - 3.0) < 1e-6 - assert abs(waypoint.angle - 1.2) < 1e-6 - - def test_stand_sit_stand(self, executor, fake_spot, feedback): - fake_spot.stand = MagicMock() - seq = ActionSequence("test", "spot", [StandSit(action="stand")]) - executor.process_action_sequence(seq, feedback) - fake_spot.stand.assert_called_once() - - def test_stand_sit_sit(self, executor, fake_spot, feedback): - fake_spot.sit = MagicMock() - seq = ActionSequence("test", "spot", [StandSit(action="sit")]) - executor.process_action_sequence(seq, feedback) - fake_spot.sit.assert_called_once() - - def test_stand_sit_invalid_raises(self, executor, feedback): - seq = ActionSequence("test", "spot", [StandSit(action="jump")]) - with pytest.raises(ValueError, match="Unknown StandSit action"): - executor.process_action_sequence(seq, feedback) - - # --- Stop Preemption Tests --- @@ -270,81 +146,3 @@ def test_move_then_sit(self, mock_sleep, mock_nav, executor, fake_spot, feedback executor.process_action_sequence(seq, feedback) mock_nav.assert_called_once() fake_spot.sit.assert_called_once() - - -# --- ROS Message Serialization Tests --- - - -class TestMessageSerialization: - def test_move_relative_roundtrip(self): - from robot_executor_interface.action_descriptions import ActionSequence - from robot_executor_interface_ros.action_descriptions_ros import from_msg, to_msg - - original = ActionSequence( - "test", "spot", [MoveRelative(distance_m=2.5)] - ) - msg = to_msg(original) - result = from_msg(msg) - assert len(result.actions) == 1 - assert isinstance(result.actions[0], MoveRelative) - assert result.actions[0].distance_m == 2.5 - - def test_turn_relative_roundtrip(self): - from robot_executor_interface_ros.action_descriptions_ros import from_msg, to_msg - - original = ActionSequence( - "test", "spot", [TurnRelative(angle_deg=-45.0)] - ) - msg = to_msg(original) - result = from_msg(msg) - assert isinstance(result.actions[0], TurnRelative) - assert result.actions[0].angle_deg == -45.0 - - def test_strafe_roundtrip(self): - from robot_executor_interface_ros.action_descriptions_ros import from_msg, to_msg - - original = ActionSequence("test", "spot", [Strafe(distance_m=1.0)]) - msg = to_msg(original) - result = from_msg(msg) - assert isinstance(result.actions[0], Strafe) - assert result.actions[0].distance_m == 1.0 - - def test_stop_roundtrip(self): - from robot_executor_interface_ros.action_descriptions_ros import from_msg, to_msg - - original = ActionSequence("test", "spot", [Stop()]) - msg = to_msg(original) - result = from_msg(msg) - assert isinstance(result.actions[0], Stop) - - def test_stand_sit_roundtrip(self): - from robot_executor_interface_ros.action_descriptions_ros import from_msg, to_msg - - original = ActionSequence( - "test", "spot", [StandSit(action="sit")] - ) - msg = to_msg(original) - result = from_msg(msg) - assert isinstance(result.actions[0], StandSit) - assert result.actions[0].action == "sit" - - def test_mixed_sequence_roundtrip(self): - from robot_executor_interface_ros.action_descriptions_ros import from_msg, to_msg - - original = ActionSequence( - "test", - "spot", - [ - MoveRelative(distance_m=1.0), - TurnRelative(angle_deg=90.0), - Strafe(distance_m=-0.5), - StandSit(action="stand"), - ], - ) - msg = to_msg(original) - result = from_msg(msg) - assert len(result.actions) == 4 - assert isinstance(result.actions[0], MoveRelative) - assert isinstance(result.actions[1], TurnRelative) - assert isinstance(result.actions[2], Strafe) - assert isinstance(result.actions[3], StandSit)