Add direct navigation commands (move, turn, strafe, stop, stand/sit)

.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

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

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"

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]