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/README.md b/README.md index 54b61d6..2d26048 100644 --- a/README.md +++ b/README.md @@ -11,6 +11,154 @@ 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 + +### 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/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/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/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/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 +``` + +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..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 @@ -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,169 @@ 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): + 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 +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): + # 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 +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): + 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 +def _(action: Stop): + msg = ActionMsg() + msg.action_type = msg.STOP + return msg + + +@to_viz_msg.register +def _(action: Stop, marker_ns): + # 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 +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): + # 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/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..454699f 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) # 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) + 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..cf9c247 --- /dev/null +++ b/spot_tools/tests/test_direct_navigation.py @@ -0,0 +1,148 @@ +""" +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, + ) + + +# --- 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() 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" 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")