diff --git a/AGENTS.md b/AGENTS.md index 72354305fa..5813a32aa8 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -186,6 +186,28 @@ hatch run format # ruff check --fix, ruff format has to be pinned structurally. Pinned by tests/mesh/test_zenoh_transport_bypasses_backend_routing.py. +18. **A new bridge is un-gated until its surfaces are named, and refused until + it forwards the context** - `use_ros`'s operator-approval gate is keyed on + the graph name of the surface a command targets, so it knows nothing about + a transport added later: a bridge whose topics and services match no + `_command_gate.COMMAND_BLOCKLIST` entry sends LLM-initiated motion with no + prompt, no allowlist check and no audit row, and its tests stay green + because a bridge suite patches the module's `use_ros` symbol at the + boundary the gate lives behind. The two halves are one change and neither + ships alone: naming the surfaces without threading `tool_context` through + every `use_ros` call and declaring the command tools `@tool(context=True)` + converts the silent bypass into a fail-closed refusal of the whole command + surface, `stop` included, and threading the context without naming the + surfaces changes nothing at all. Spell blocklist entries bare - matching is + on the final path segment, so one `/manual_drive` entry covers every + namespaced instance - and state the posture where an operator sizes a + pre-approval (`docs/ros2-integration.md`, `docs/security.md`, the example), + because `STRANDS_ROS2_COMMAND_ALLOW` is what makes a headless run work and + it cannot be discovered from a refusal that has not happened yet. Pinned by + tests/mesh/test_ackermann_command_gate.py, whose inventory of bridges owing + a gate suite is derived from the tree, so the next transport is graded on + arrival rather than at the review that happens to look. + ## PR Workflow 1. Create the feature branch **on your fork**. Branch creation in the base diff --git a/changelog.d/1035-ackermann-ros-robot.md b/changelog.d/1035-ackermann-ros-robot.md new file mode 100644 index 0000000000..eca5bcc962 --- /dev/null +++ b/changelog.d/1035-ackermann-ros-robot.md @@ -0,0 +1,55 @@ +### Added: `AckermannRosRobot` - Ackermann ROS 2 cars (AWS DeepRacer) as strands robots + +`RosBridgedRobot` covers differential-drive bases (Twist on `cmd_vel` + +odometry); Ackermann cars expose neither. The new bridge keeps the same +`drive(linear, angular, duration)` agent contract and converts through a +bicycle model to normalized servo commands (`ServoCtrlMsg` on the DeepRacer), +runs a declarative `init_services` handshake once before the first command +(the DeepRacer's manual-mode two-step, preconfigured in `from_deepracer()`), +clamps speed, rejects over-long holds loudly, and always trails timed +commands with a zero servo message so a timed drive cannot leave the car +driving (a bare single-shot command latches until stop, matching raw servo +semantics). Conditional `get_scan` tool; no `get_pose` (the stock platform has +no odometry). + +`drive` and the constructor limits validate through the same +`finite_number_error`, `positive_finite_number_error` and +`positive_whole_number_error` domains the three differential-drive bridges +call, so the fourth transport refuses an unusable velocity, hold or count with +byte-identical text rather than a hand-rolled copy of the contract. + +`__repr__` renders a half-built instance rather than raising. The constructor +validates `node_name` before assigning it, so a refused construction left an +instance whose `repr` raised `AttributeError: 'AckermannRosRobot' object has no +attribute 'node_name'` - sending a reader after the attribute instead of the +`ValueError: invalid node_name` they already caused. It now delegates to the +single owner of that wording, `strands_robots.utils.partial_construction_repr`, +as the three differential-drive bridges do, and the class is triaged into the +survey in `tests/test_repr_survives_partial_construction.py` (both the +half-built sweep and the documented-refusal cases). + +A timed `drive()` whose trailing halt fails now reports that failure instead of +the main publish's success. `use_ros` reports a transport failure as an error +dict rather than raising, and the halt's verdict was discarded - so a node dying +mid-hold left the call returning `success` for a car still holding the commanded +throttle, on a tool whose description promises that a command with a duration +stops itself. The result now names the published command, the failed halt and +its cause, so the agent's next action is `stop`. + +Its command surfaces now reach the `use_ros` operator-approval gate. Two halves +were missing. The gate only fires on a blocklisted surface, and the stock +DeepRacer wiring - `/webserver_pkg/manual_drive` plus the `/ctrl_pkg` mode +services that arm the vehicle - matched no entry, so every servo publish and +handshake call went out with no prompt, no allowlist check and no audit row while +the functionally identical `/motor_enable` was blocked; `/manual_drive`, +`/vehicle_state` and `/enable_state` are now default blocklist entries, spelled +bare so the base-name rule covers the namespaced spellings. And the bridge +forwarded no context, so a fix to the first half alone would have made every +command - `stop` included - a fail-closed refusal: `drive`, `stop` and `enable` +now take a `tool_context` and thread it into every `use_ros` call, and the +`drive_`/`stop_` tools are declared `@tool(context=True)`, so an +agent prompts the operator exactly as the differential-drive bridge does and a +headless run pre-approves the three surfaces via `STRANDS_ROS2_COMMAND_ALLOW`. +`docs/ros2-integration.md`, `docs/security.md` and +`examples/ros2/deepracer_agent.py` state that posture where an operator sizes a +pre-approval. diff --git a/docs/ros2-integration.md b/docs/ros2-integration.md index 01c93ebd83..c2988f217f 100644 --- a/docs/ros2-integration.md +++ b/docs/ros2-integration.md @@ -38,6 +38,7 @@ have and what you want to do: | **`use_rtps`** tool | participant / **act as a robot** | pure `cyclonedds` (pip) | **no** | Join a graph as a DDS peer and publish topics a real stack consumes; works on macOS/CI/Jetson, all distros | | **`use_rosbridge`** tool + **`RosbridgeRobot`** | ROS1 / remote robots over a rosbridge WebSocket | pure-pip `roslibpy` | **no** | Drive ROS1 robots (e.g. the NASA Curiosity Gazebo sim) or any remote rosbridge robot from a machine with no ROS install - see [rosbridge integration](rosbridge-integration.md) | | **`RosBridgedRobot`** | a ROS 2 robot as a strands `Robot` | `use_ros` | yes | `drive()`/`get_pose()` a `cmd_vel`/odom base with the same `Agent(tools=[robot])` UX as sim/hardware | +| **`AckermannRosRobot`** | an Ackermann ROS 2 car as a strands `Robot` | `use_ros` | yes | `drive()`/`get_scan()` a steering-geometry car (AWS DeepRacer servo stack) with bicycle-model conversion and an automatic enable handshake | | **`SimEngine(ros2_bridge=True)`** | the **simulation as a ROS node** | `rclpy` | yes | Publish a running MuJoCo sim's `joint_states` + camera `image_raw` so rviz/nav2/agents can subscribe | | **`Robot(ros2_bridge=True)`** | a **real robot as a ROS node** (full duplex) | `rclpy` | yes | Publish a physical arm's live `joint_states` + camera `image_raw` so rviz/nav2/agents subscribe to the hardware, **and** subscribe to `joint_command` to drive the arm - symmetric to the sim bridge, plus an inbound command path the sim does not need | @@ -179,15 +180,18 @@ unenforceable. These surfaces are blocked by default: | Surface | Usually reached by | |---------|--------------------| -| `/cmd_vel`, `/cmd_vel_unstamped` | `publish` | +| `/cmd_vel`, `/cmd_vel_unstamped`, `/manual_drive` | `publish` | | `/joint_command`, `/joint_trajectory`, `/joint_trajectory_controller/joint_trajectory` | `publish` | | `/emergency_stop`, `/e_stop` | `service_call`, sometimes `publish` | | `/motor_enable`, `/enable_motor`, `/disable_motor` | `service_call` | +| `/vehicle_state`, `/enable_state` | `service_call` | | `/navigate_to_pose`, `/follow_path` | `action_send_goal` | Matching is on the final path segment, so a namespaced form -(`/my_robot/cmd_vel`, `/fleet/robot1/emergency_stop`) is caught while a lookalike -(`/cmd_vel_evil`, `/joint_trajectory_status`) is not. The name is compared in the +(`/my_robot/cmd_vel`, `/fleet/robot1/emergency_stop`, and the DeepRacer's +`/webserver_pkg/manual_drive`, `/ctrl_pkg/vehicle_state`, +`/ctrl_pkg/enable_state`) is caught while a lookalike (`/cmd_vel_evil`, +`/joint_trajectory_status`) is not. The name is compared in the form rclpy resolves it to, so the unrooted `cmd_vel` and the trailing-separator `/cmd_vel/` are the same surface as `/cmd_vel`. Case is deliberately **not** folded: ROS 2 graph names are case-sensitive, so `/CMD_VEL` is a genuinely @@ -238,6 +242,65 @@ surface, so telemetry stays available to the agent. The gate also runs *after* t action's required arguments are validated, so an operator is never asked to approve a call that could not have run. +## Ackermann robots (AWS DeepRacer) + +Differential-drive bases take `geometry_msgs/msg/Twist`; Ackermann cars do +not. The AWS DeepRacer's stock stack subscribes to normalized servo pairs +(`deepracer_interfaces_pkg/msg/ServoCtrlMsg`, `angle`/`throttle` in [-1, 1]) +and acts on them only after a two-step manual-mode service handshake +(`/ctrl_pkg/vehicle_state` with `state=1`, then `/ctrl_pkg/enable_state` with +`is_active=true`). `AckermannRosRobot` absorbs both differences: + + from strands_robots.mesh import AckermannRosRobot + + car = AckermannRosRobot.from_deepracer(node_name="deepracer") + car.drive(linear=0.5, angular=1.0, duration=2.0) + car.get_scan() + +`drive()` keeps the same `(linear, angular)` contract as `RosBridgedRobot` - +a bicycle model (`atan(wheelbase * angular / linear)`, clamped to the steering +limit) converts to servo values internally. The handshake declared in +`init_services` runs once, automatically, before the first command; a failed +handshake aborts the drive. Timed and multi-message commands are always +followed by a zero servo message - even when the publish fails - so a timed +drive cannot leave the car with a live throttle, and a halt that itself fails is +reported: the call returns an error naming the throttle that may still be live +instead of the drive's success, so the agent's next action is `stop()`. A bare +single-shot `drive()` +(no `duration`) latches like any raw servo command until `stop()`. Commands +are clamped to `max_speed`; holds longer +than `max_duration` are rejected loudly rather than silently truncated. The +`linear`/`angular`/`duration`/`count` values themselves are checked against the +same shared domains the differential-drive bridges use, so an unusable value is +refused with identical text on every transport. The +stock platform publishes no odometry, so there is deliberately no +`get_pose`. + +Like `RosBridgedRobot`, the bridge inherits the [command +gate](#safety-critical-command-surfaces-need-operator-approval): the servo topic +(`/manual_drive`) and both mode services (`/vehicle_state`, `/enable_state`) are +blocklisted surfaces, so every command this bridge sends is gated. + +| Method / tool | Reaches | Gated | +|---------------|---------|-------| +| `drive()` / `drive_` | `publish` to the servo topic (plus the handshake on the first call) | yes | +| `stop()` / `stop_` | `publish` to the servo topic | yes - the gate is keyed on the surface, not the payload | +| `enable()` | `service_call` to both mode services | yes | +| `get_scan()` / `get_scan_` | `echo` | never gated | + +The `drive_` and `stop_` agent tools forward the operator context, so +an agent driving the car prompts rather than failing closed. A programmatic +`car.drive(...)` / `car.stop()` has no operator to prompt, so pre-approve the +three surfaces for a headless run (bare names cover the namespaced DeepRacer +spellings): + +```bash +export STRANDS_ROS2_COMMAND_ALLOW=/manual_drive,/vehicle_state,/enable_state +``` + +See `examples/ros2/deepracer_agent.py`. + + ## Sim bridge: publish a simulation on a ROS 2 domain The simulator can advertise its own live state on ROS 2. Construct any diff --git a/docs/security.md b/docs/security.md index c338da74d1..7f2cdc4232 100644 --- a/docs/security.md +++ b/docs/security.md @@ -28,7 +28,7 @@ Defense-in-depth controls the SDK already provides, and which you should rely on - **Tool scoping.** The single most effective mitigation is to give the agent only the tools a task actually needs. An agent that never receives the `robot_mesh`, `serial_tool`, or `Robot(mode="real")` tools cannot be coerced into a fleet broadcast, a raw serial write, or a physical actuation no matter what the injected text says. - **Out-of-band human approval for physical actuation** (see [Operator approval](#operator-approval-for-fleet-wide-actions)) - the approval is delivered outside the LLM's tool-argument flow, so an injected prompt that tries to set an "approved" flag in the command body cannot bypass the gate. -- **Operator approval for ROS 2 command surfaces.** Every tool that reaches a ROS 2 graph - `use_ros` over in-process rclpy, `use_rtps` over raw RTPS and `use_rosbridge` over a WebSocket - gates each verb it offers that can carry a command to a robot - a topic `publish`, a `service_call` and an `action_send_goal` - against a blocklist of safety-critical surfaces (`/cmd_vel`, the joint command/trajectory topics, e-stop, motor enable/disable, `/navigate_to_pose`, `/follow_path`). Reading the same surfaces stays ungated. See [safety-critical command surfaces](ros2-integration.md#safety-critical-command-surfaces-need-operator-approval). +- **Operator approval for ROS 2 command surfaces.** Every tool that reaches a ROS 2 graph - `use_ros` over in-process rclpy, `use_rtps` over raw RTPS and `use_rosbridge` over a WebSocket - gates each verb it offers that can carry a command to a robot - a topic `publish`, a `service_call` and an `action_send_goal` - against a blocklist of safety-critical surfaces (`/cmd_vel`, the joint command/trajectory topics, e-stop, motor enable/disable, the Ackermann servo topic `/manual_drive` and the mode services that arm such a vehicle, `/navigate_to_pose`, `/follow_path`). Reading the same surfaces stays ungated. See [safety-critical command surfaces](ros2-integration.md#safety-critical-command-surfaces-need-operator-approval). - **Payload validation** of every mesh command, so an injected instruction still cannot smuggle an out-of-bounds duration, an attacker-controlled inference host, or an arbitrary model path. ## Robot mesh authentication @@ -168,7 +168,7 @@ Reference: `strands_robots.tools.gr00t_inference`. - **DDS Security gate (pure-RTPS transport).** When `ros2_transport="rtps"` and commands are enabled, the bridge **refuses to start** unless given a `dds_security_config` (identity CA, participant certificate + private key, signed governance + permissions) or the explicit `STRANDS_ROS2_BRIDGE_I_KNOW_THIS_IS_INSECURE=1` opt-out. The credentials are wired into the cyclonedds participant QoS so the whole graph is authenticated and access-controlled. The rclpy transport gets its DDS Security from the ROS 2 RMW keystore/env (`ROS_SECURITY_*` / `sros2`) instead. See the [RTPS integration guide](rtps-integration.md#securing-the-inbound-command-surface). - **Joint position bounds.** Pass `joint_limits={".pos": (min, max)}` to reject any inbound command whose any joint falls outside its declared range - the whole command is dropped, never partially applied, so one out-of-range value cannot drive part of the arm. Keys are matched against the joint names the command carries - the same `.pos` names the bridge publishes in `joint_states`, so a controller can echo them straight back - and a key that names no commanded joint constrains nothing. Every bound must be a finite number; a non-finite one declares a range that admits nothing, and the bridge refuses it at construction rather than dropping every command for that joint mid-run. -- **Agent-side command gate (`use_ros`).** The bridge protections above harden the *inbound* surface a robot exposes. The three graph tools - `use_ros`, `use_rtps` and `use_rosbridge` - are the other direction, an agent publishing onto someone else's graph, and each gates the safety-critical surfaces behind an operator interrupt, keyed on the surface name so a topic publish, a service call and an action goal are all covered. One shared blocklist serves all three, so a surface refused on one transport cannot be sent on another. `RosBridgedRobot` forwards the operator context into it, so a bridged robot driven by an agent prompts too, while a programmatic `robot.drive(...)`/`stop()` needs the surface pre-approved. Pre-approve surfaces with `STRANDS_ROS2_COMMAND_ALLOW` - matched by base name as well as exactly, so a `/cmd_vel` entry lifts the gate on every namespaced `cmd_vel` and not only the robot being driven; name the namespace to scope it to one - or bypass with `BYPASS_TOOL_CONSENT=true`; with neither set and no interrupt reachable it fails closed. See [safety-critical command surfaces](ros2-integration.md#safety-critical-command-surfaces-need-operator-approval). +- **Agent-side command gate (`use_ros`).** The bridge protections above harden the *inbound* surface a robot exposes. The three graph tools - `use_ros`, `use_rtps` and `use_rosbridge` - are the other direction, an agent publishing onto someone else's graph, and each gates the safety-critical surfaces behind an operator interrupt, keyed on the surface name so a topic publish, a service call and an action goal are all covered. One shared blocklist serves all three, so a surface refused on one transport cannot be sent on another. `RosBridgedRobot` and `AckermannRosRobot` both forward the operator context into it, so a bridged robot driven by an agent prompts too, while a programmatic `robot.drive(...)`/`stop()` needs the surface pre-approved. Pre-approve surfaces with `STRANDS_ROS2_COMMAND_ALLOW` - matched by base name as well as exactly, so a `/cmd_vel` entry lifts the gate on every namespaced `cmd_vel` and not only the robot being driven; name the namespace to scope it to one - or bypass with `BYPASS_TOOL_CONSENT=true`; with neither set and no interrupt reachable it fails closed. See [safety-critical command surfaces](ros2-integration.md#safety-critical-command-surfaces-need-operator-approval). - **Telemetry-only is ungated.** `ros2_commands=False` is publish-only (no inbound surface) and needs no security config. That posture is only as good as how the flag is read, so `ros2_bridge` / `ros2_commands` (and `enable_commands` on either bridge class directly) are **checked** against the shared boolean domain rather than read by truthiness: a non-boolean is refused at construction, so a deployment config that spells the flag `"false"` cannot select the surface it asks to close. ## Credentials and secrets diff --git a/examples/ros2/deepracer_agent.py b/examples/ros2/deepracer_agent.py new file mode 100644 index 0000000000..2711da3179 --- /dev/null +++ b/examples/ros2/deepracer_agent.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 +"""Drive an AWS DeepRacer with natural language - Ackermann bridge demo. + +Goal: Show AckermannRosRobot presenting a steering-geometry ROS 2 car as +strands agent tools. The bridge converts (linear, angular) commands through a +bicycle model to the DeepRacer's normalized servo interface, runs the car's +manual-mode service handshake automatically before the first command, and +always trails sustained commands with a zero so the car cannot be left +driving. + +Dependencies: + pip install strands-robots strands-agents + A sourced ROS 2 environment (rclpy) on the same network/domain as the car, + with the DeepRacer core stack running (ctrl_pkg, webserver_pkg, rplidar). + +SAFETY: put the car on blocks or a clear track before running. The servo topic +and both mode services are gated command surfaces, so this agent prompts for +operator approval before each command it sends. For an unattended run, +pre-approve exactly those three surfaces instead (bare names cover the +namespaced DeepRacer spellings): + + export STRANDS_ROS2_COMMAND_ALLOW=/manual_drive,/vehicle_state,/enable_state + +Expected output: the agent drives a short pattern and summarizes the lidar. +Runtime: ~20 seconds (depends on LLM latency). +""" + +from strands import Agent + +from strands_robots.mesh import AckermannRosRobot + +# Stock DeepRacer wiring: servo topic, lidar, and the two-step manual-mode +# handshake are preconfigured; override any keyword for a modified car. +car = AckermannRosRobot.from_deepracer(node_name="deepracer") + +agent = Agent(tools=car.tools) + +result = agent( + "Drive forward at 0.3 m/s for two seconds, then arc gently left for two " + "seconds, stop, and read the lidar to tell me the nearest obstacle." +) + +print(f"Agent completed: {result}") diff --git a/strands_robots/mesh/__init__.py b/strands_robots/mesh/__init__.py index cf65ccb239..3e498eda25 100644 --- a/strands_robots/mesh/__init__.py +++ b/strands_robots/mesh/__init__.py @@ -28,6 +28,7 @@ defined; this package re-exports the public surface only. """ +from strands_robots.mesh.ackermann_robot import AckermannRosRobot from strands_robots.mesh.audit import log_safety_event from strands_robots.mesh.core import Mesh, get_local_robots, init_mesh from strands_robots.mesh.input import InputPublisher, InputReceiver @@ -48,6 +49,7 @@ __all__ = [ # Core types + "AckermannRosRobot", "Mesh", "InputPublisher", "InputReceiver", diff --git a/strands_robots/mesh/ackermann_robot.py b/strands_robots/mesh/ackermann_robot.py new file mode 100644 index 0000000000..dbc9a805f6 --- /dev/null +++ b/strands_robots/mesh/ackermann_robot.py @@ -0,0 +1,452 @@ +"""Ackermann ROS 2 mesh bridge - steering-geometry cars as strands robots. + +An :class:`AckermannRosRobot` wraps an Ackermann-steering ROS 2 car (reference +platform: AWS DeepRacer) so an agent can drive it with the same +``Agent(tools=robot.tools)`` pattern as every other strands robot. Ackermann +platforms differ from the differential-drive bases served by +:class:`~strands_robots.mesh.ros_bridge.RosBridgedRobot` in three ways this +class absorbs: + +1. Commands are normalized servo pairs (``angle``/``throttle`` in [-1, 1]), + not ``geometry_msgs/msg/Twist``. :meth:`drive` keeps the fleet-wide + ``(linear, angular)`` contract and converts internally with a bicycle + model, so the agent never learns a second vocabulary. +2. The vehicle must be switched into a commandable state first (the DeepRacer + needs a two-step manual-mode service handshake). ``init_services`` declares + that handshake; :meth:`drive` runs it once, automatically, before the first + command and refuses to drive if it fails. +3. There is no odometry topic on the stock platform, so there is deliberately + no ``get_pose`` here. + +All ROS 2 I/O forwards through :func:`strands_robots.tools.use_ros.use_ros`, +so the bridge owns no rclpy state and is safe to construct without ROS 2. + +Commanding the car goes through ``use_ros``'s operator-approval gate, because a +servo topic that moves a vehicle and the mode services that arm it are +safety-critical surfaces. The bridge forwards an operator context to it: the +``drive_`` / ``stop_`` agent tools are declared ``@tool(context=True)`` +and hand the context to :func:`use_ros`, so an agent driving this car prompts the +operator exactly as a direct ``use_ros`` call does. The read path +(:meth:`get_scan`) is never gated. + +A **programmatic** call carries no operator context, so it is refused unless the +surfaces are pre-approved with ``STRANDS_ROS2_COMMAND_ALLOW`` (or the gate is +bypassed with ``BYPASS_TOOL_CONSENT=true``). That includes :meth:`stop` and the +trailing halt of a timed :meth:`drive`: the gate is keyed on the *surface*, not +on the payload, so a zero servo message is gated like a full-throttle one. + +Typical usage:: + + import os + + from strands import Agent + from strands_robots.mesh import AckermannRosRobot + + car = AckermannRosRobot.from_deepracer(node_name="deepracer") + + # Direct, programmatic control - nobody to prompt, so the command surfaces + # this car uses have to be pre-approved (bare names cover the namespaced + # DeepRacer spellings): + os.environ["STRANDS_ROS2_COMMAND_ALLOW"] = "/manual_drive,/vehicle_state,/enable_state" + car.drive(linear=0.5, duration=2.0) # forward half a metre per second + print(car.get_scan()) # reads are never gated + + # Or hand the bridge to an agent as first-class tools - the command tools + # carry the operator context, so the gate prompts instead of refusing: + agent = Agent(tools=car.tools) + agent("drive forward slowly for two seconds, then read the lidar") +""" + +from __future__ import annotations + +import math +from typing import Any + +from strands import tool +from strands.types.tools import AgentTool, ToolContext + +from strands_robots.mesh.ros_bridge import _check_topic +from strands_robots.tools.use_ros import use_ros +from strands_robots.utils import ( + finite_number_error, + partial_construction_repr, + positive_finite_number_error, + positive_whole_number_error, +) + +_SERVO_TYPE = "deepracer_interfaces_pkg/msg/ServoCtrlMsg" + +# Below this absolute linear velocity a command is treated as rest and maps to +# zero output: a stock Ackermann platform cannot yaw at rest, and the bicycle +# model's atan2 would otherwise amplify noise near v = 0 into hard steering. +_REST_VELOCITY_EPS = 1e-3 + + +def _twist_to_servo( + linear: float, + angular: float, + *, + wheelbase_m: float, + max_speed: float, + max_steering_rad: float, +) -> tuple[float, float]: + """Convert a body-frame velocity command to a normalized servo pair. + + Bicycle model: the equivalent front-wheel steering angle for yaw rate + ``angular`` at speed ``linear`` is ``atan(wheelbase_m * angular / linear)``, + clamped to the platform's steering limit and normalized by it. Throttle is + ``linear`` normalized by ``max_speed``. Both outputs are in [-1, 1]. + + Implemented as ``atan2`` on ``abs(linear)`` with the sign restored for + reverse motion: the naive ``atan2(wheelbase_m * angular, linear)`` lands in + the second quadrant when ``linear`` is negative, commanding near-full + opposite steering while backing up instead of the mirrored angle. + """ + v = float(linear) + if abs(v) < _REST_VELOCITY_EPS: + return (0.0, 0.0) + delta = math.atan2(wheelbase_m * float(angular), abs(v)) + if v < 0: + delta = -delta + delta = max(-max_steering_rad, min(max_steering_rad, delta)) + throttle = max(-1.0, min(1.0, v / max_speed)) + return (delta / max_steering_rad, throttle) + + +class AckermannRosRobot: + """An Ackermann-steering ROS 2 car exposed as a strands-controllable robot. + + The bridge owns no ROS 2 state; every method forwards to :func:`use_ros`. + Constructing it never needs a ROS 2 environment - errors surface as + structured results when a method actually runs. + + Args: + node_name: Identifier used to name this robot's agent tools + (``drive_`` etc.); it does not need to match a ROS 2 + node name. + servo_topic: Topic the vehicle's servo stack subscribes to (DeepRacer: + ``/webserver_pkg/manual_drive``). + scan_topic: Optional laser-scan topic. Read by :meth:`get_scan`. + servo_type: Interface type of ``servo_topic``. Defaults to the + DeepRacer ``ServoCtrlMsg`` (normalized ``angle``/``throttle``). + scan_type: Interface type of ``scan_topic``. Optional - resolved from + the live graph when omitted. + wheelbase_m: Front-to-rear axle distance for the bicycle model. + max_speed: Linear speed (m/s) mapped to full throttle; commands are + clamped to this magnitude. + max_steering_rad: Steering angle mapped to full servo deflection. + max_duration: Longest single :meth:`drive` hold accepted; longer + requests are rejected loudly rather than silently truncated. + publish_rate: Command publish rate (Hz) for held :meth:`drive` calls. + init_services: Ordered service calls (``{"service", "type", "fields"}`` + dicts) that put the vehicle into a commandable state. Run once, + automatically, before the first :meth:`drive`. The DeepRacer + manual-mode handshake in :meth:`from_deepracer` is the reference + use. + + There is deliberately no ``get_pose``: the stock platform publishes no + odometry. + """ + + def __init__( + self, + node_name: str, + servo_topic: str, + scan_topic: str | None = None, + *, + servo_type: str = _SERVO_TYPE, + scan_type: str | None = None, + wheelbase_m: float = 0.164, + max_speed: float = 1.5, + max_steering_rad: float = 0.5236, + max_duration: float = 10.0, + publish_rate: float = 20.0, + init_services: list[dict[str, Any]] | None = None, + ) -> None: + self.node_name = _check_topic("node_name", node_name) + self.servo_topic = _check_topic("servo_topic", servo_topic) + self.scan_topic = _check_topic("scan_topic", scan_topic) if scan_topic else None + self.servo_type = servo_type + self.scan_type = scan_type + for label, value in ( + ("wheelbase_m", wheelbase_m), + ("max_speed", max_speed), + ("max_steering_rad", max_steering_rad), + ("max_duration", max_duration), + ("publish_rate", publish_rate), + ): + if limit_err := positive_finite_number_error(value, label, type(self).__name__): + raise ValueError(limit_err) + self.wheelbase_m = float(wheelbase_m) + self.max_speed = float(max_speed) + self.max_steering_rad = float(max_steering_rad) + self.max_duration = float(max_duration) + self.publish_rate = float(publish_rate) + self.init_services = list(init_services or []) + for item in self.init_services: + _check_topic("init_services service", item.get("service", "")) + if not item.get("type"): + raise ValueError( + f"init_services entry for {item.get('service')!r} is missing its 'type' " + "(expected an interface type like pkg/srv/Name)" + ) + self._enabled = False + + @classmethod + def from_deepracer(cls, node_name: str, **overrides: Any) -> AckermannRosRobot: + """Construct a bridge wired for the stock AWS DeepRacer software stack. + + Servo commands go to the webserver package's manual-drive topic, the + RPLIDAR scan topic is preconfigured, and ``init_services`` carries the + two-step manual-mode handshake (``vehicle_state`` state=1, then + ``enable_state`` is_active=True) the car requires before it acts on + servo messages. Any keyword can be overridden for a modified car. + """ + wiring: dict[str, Any] = { + "servo_topic": "/webserver_pkg/manual_drive", + "scan_topic": "/rplidar_ros/scan", + "init_services": [ + { + "service": "/ctrl_pkg/vehicle_state", + "type": "deepracer_interfaces_pkg/srv/ActiveStateSrv", + "fields": {"state": 1}, + }, + { + "service": "/ctrl_pkg/enable_state", + "type": "deepracer_interfaces_pkg/srv/EnableStateSrv", + "fields": {"is_active": True}, + }, + ], + } + wiring.update(overrides) + servo_topic = wiring.pop("servo_topic") + scan_topic = wiring.pop("scan_topic") + return cls(node_name, servo_topic, scan_topic, **wiring) + + @staticmethod + def _error(text: str) -> dict[str, Any]: + return {"status": "error", "content": [{"text": text}]} + + def enable(self, tool_context: ToolContext | None = None) -> dict[str, Any]: + """Run the ``init_services`` handshake once; idempotent on success. + + Stops at the first failing call and returns its structured error + without latching, so a later attempt retries from the start. + + ``tool_context`` is the operator context forwarded to ``use_ros``: the + services that arm a vehicle are gated command surfaces, so a handshake + that forwards nothing is refused rather than prompted (see :meth:`drive`). + """ + if self._enabled: + return { + "status": "success", + "content": [{"text": f"{self.node_name}: already enabled"}], + } + for item in self.init_services: + result = use_ros( + action="service_call", + service=item["service"], + type=item["type"], + fields=item.get("fields", {}), + tool_context=tool_context, + ) + if result.get("status") != "success": + return result + self._enabled = True + return { + "status": "success", + "content": [{"text": f"{self.node_name}: enabled ({len(self.init_services)} init call(s))"}], + } + + def drive( + self, + linear: float = 0.0, + angular: float = 0.0, + duration: float | None = None, + count: int = 1, + tool_context: ToolContext | None = None, + ) -> dict[str, Any]: + """Publish a velocity command, converted through the bicycle model. + + Same contract as ``RosBridgedRobot.drive``: ``linear`` (m/s) and + ``angular`` (rad/s), an optional ``duration`` hold (publishes + ``round(duration * publish_rate)`` messages, takes precedence over + ``count``). Validation runs before any side effect, in order: inputs + must be finite, ``duration`` (when given) must be a positive finite + number within ``max_duration``, and only then does the vehicle's + ``init_services`` handshake run (automatically, before the first + command; a failed handshake aborts the drive). Every timed or + multi-message non-zero command is followed by a single zero servo + message - even if the main publish failed - so it can never leave the + car with a live throttle latched, and when that halt is the call that + fails the result says so instead of reporting the drive's success. A + bare single-shot command (no ``duration``, ``count=1``) latches like a + raw servo command until :meth:`stop`. + + ``tool_context`` is the operator context forwarded to ``use_ros``, whose + command gate prompts for approval on a safety-critical surface such as + this vehicle's servo topic and mode services. The ``drive_`` + agent tool passes the one the framework injects; a programmatic call has + none, and the gate then refuses unless the surface is pre-approved via + ``STRANDS_ROS2_COMMAND_ALLOW`` / ``BYPASS_TOOL_CONSENT``. + """ + # A servo command is the one call on this bridge that physically moves + # the car, so every knob it carries is checked before anything reaches + # the wire, through the same shared domains the sibling bridges use. + cmd_err = ( + finite_number_error(linear, "linear", "drive") + or finite_number_error(angular, "angular", "drive") + or ( + positive_finite_number_error(duration, "duration", "drive") + if duration is not None + # ``duration`` supersedes ``count``, so ``count`` is only the + # effective horizon when no duration was given. + else positive_whole_number_error(count, "count", "drive") + ) + ) + if cmd_err: + return self._error(cmd_err) + # Ordered after the finiteness guard on purpose: every comparison + # against ``nan`` is false, so a ceiling test cannot stand in for one. + if duration is not None and duration > self.max_duration: + return self._error( + f"drive: duration {duration}s exceeds max_duration {self.max_duration}s " + "- issue shorter commands instead of one long hold" + ) + if not self._enabled and self.init_services: + enabled = self.enable(tool_context=tool_context) + if enabled.get("status") != "success": + return enabled + v = max(-self.max_speed, min(self.max_speed, float(linear))) + angle, throttle = _twist_to_servo( + v, + angular, + wheelbase_m=self.wheelbase_m, + max_speed=self.max_speed, + max_steering_rad=self.max_steering_rad, + ) + n = max(1, round(duration * self.publish_rate)) if duration is not None else count + # The trailing halt runs from ``finally`` so it goes out even if the + # main publish raised, but its verdict is kept rather than dropped: + # ``use_ros`` reports a transport failure as an error dict rather than + # raising, so returning the main publish's success after a failed halt + # would report a car still holding the commanded throttle as a drive + # that stopped itself - and an agent reading ``success`` never issues + # ``stop``. + halt: dict[str, Any] | None = None + try: + result = self._publish_servo(angle, throttle, count=n, tool_context=tool_context) + finally: + if (duration is not None or n > 1) and (angle or throttle): + halt = self._publish_servo(0.0, 0.0, count=1, tool_context=tool_context) + if halt is not None and halt.get("status") != "success" and result.get("status") == "success": + cause = " ".join( + block.get("text", "") for block in halt.get("content", []) if isinstance(block, dict) + ).strip() + return self._error( + f"drive: the command was published to {self.servo_topic}, but the trailing stop " + f"failed - the car may still be holding the commanded throttle. Halt it with stop. " + f"Halt failure: {cause or 'no detail reported'}" + ) + return result + + def _publish_servo( + self, + angle: float, + throttle: float, + count: int, + tool_context: ToolContext | None = None, + ) -> dict[str, Any]: + return use_ros( + action="publish", + topic=self.servo_topic, + type=self.servo_type, + fields={"angle": float(angle), "throttle": float(throttle)}, + count=count, + rate=self.publish_rate, + tool_context=tool_context, + ) + + def stop(self, tool_context: ToolContext | None = None) -> dict[str, Any]: + """Publish a single zero servo command; it does not require :meth:`enable`. + + The halt reaches the servo topic through the same publish as + :meth:`drive`, so it passes the same operator gate - which is why + ``tool_context`` is forwarded here too (see :meth:`drive`). + """ + return self._publish_servo(0.0, 0.0, count=1, tool_context=tool_context) + + def get_scan(self, timeout: float = 5.0) -> dict[str, Any]: + """Read one sample from the laser-scan topic (error when unconfigured).""" + if not self.scan_topic: + return self._error("get_scan: no scan_topic configured for this robot") + return use_ros( + action="echo", + topic=self.scan_topic, + type=self.scan_type, + count=1, + timeout=timeout, + ) + + @property + def tools(self) -> list[AgentTool]: + """This robot's capabilities as named strands agent tools. + + Tools are bound to this instance and suffixed with ``node_name`` so + multiple robots coexist in one ``Agent(tools=[...])`` call. The drive + tool's description states the Ackermann kinematic limit (minimum + turning radius) so the agent can plan paths the platform can follow. + + Every tool that carries a command (``drive``, ``stop``) is declared + ``@tool(context=True)`` and forwards the injected context into + ``use_ros``, so its operator-approval gate prompts rather than failing + closed. ``get_scan`` takes no context because ``use_ros`` never gates + ``echo``. + """ + suffix = self.node_name.strip("/").replace("/", "_") + min_radius = self.wheelbase_m / math.tan(self.max_steering_rad) + + @tool( + name=f"drive_{suffix}", + description=( + f"Drive the {self.node_name} Ackermann robot (linear m/s, angular rad/s, " + f"optional duration s). Steering geometry: cannot turn in place; " + f"minimum turning radius ~{min_radius:.2f} m. A command with duration " + f"stops automatically afterwards; without duration the last command " + f"latches until stop." + ), + context=True, + ) + def drive( + linear: float = 0.0, + angular: float = 0.0, + duration: float | None = None, + tool_context: ToolContext | None = None, + ) -> dict[str, Any]: + return self.drive(linear=linear, angular=angular, duration=duration, tool_context=tool_context) + + @tool( + name=f"stop_{suffix}", + description=f"Immediately stop the {self.node_name} robot.", + context=True, + ) + def stop(tool_context: ToolContext | None = None) -> dict[str, Any]: + return self.stop(tool_context=tool_context) + + @tool(name=f"get_scan_{suffix}", description=f"Read one laser scan from the {self.node_name} robot.") + def get_scan() -> dict[str, Any]: + return self.get_scan() + + agent_tools: list[AgentTool] = [drive, stop] + if self.scan_topic: + agent_tools.append(get_scan) + return agent_tools + + def __repr__(self) -> str: + try: + return ( + f"AckermannRosRobot(node_name={self.node_name!r}, " + f"servo_topic={self.servo_topic!r}, scan_topic={self.scan_topic!r})" + ) + except AttributeError: + return partial_construction_repr(self) diff --git a/strands_robots/tools/_command_gate.py b/strands_robots/tools/_command_gate.py index 03d86563d0..826ba6ac1e 100644 --- a/strands_robots/tools/_command_gate.py +++ b/strands_robots/tools/_command_gate.py @@ -44,13 +44,18 @@ # set is consulted from every verb that can carry a command rather than from # publish alone - /navigate_to_pose and /follow_path are ROS 2 actions, and the # e-stop / motor-enable surfaces are usually services, so a publish-only gate -# leaves its most dangerous entries unenforceable. An operator can pre-approve -# individual surfaces via STRANDS_ROS2_COMMAND_ALLOW (comma-separated) or bypass -# the gate entirely with BYPASS_TOOL_CONSENT=true. +# leaves its most dangerous entries unenforceable. /manual_drive covers the +# DeepRacer's /webserver_pkg/manual_drive, and /vehicle_state and /enable_state +# cover the /ctrl_pkg/... services that arm it; the arming pair belongs here for +# the same reason /motor_enable does - it is what makes the vehicle act on a +# command at all. An operator can pre-approve individual surfaces via +# STRANDS_ROS2_COMMAND_ALLOW (comma-separated) or bypass the gate entirely with +# BYPASS_TOOL_CONSENT=true. COMMAND_BLOCKLIST = frozenset( { "/cmd_vel", "/cmd_vel_unstamped", + "/manual_drive", "/joint_command", "/joint_trajectory", "/joint_trajectory_controller/joint_trajectory", @@ -59,6 +64,8 @@ "/motor_enable", "/enable_motor", "/disable_motor", + "/vehicle_state", + "/enable_state", "/navigate_to_pose", "/follow_path", } diff --git a/tests/mesh/test_ackermann_command_gate.py b/tests/mesh/test_ackermann_command_gate.py new file mode 100644 index 0000000000..6fc190b2bc --- /dev/null +++ b/tests/mesh/test_ackermann_command_gate.py @@ -0,0 +1,343 @@ +"""The Ackermann bridge's commands must reach the ``use_ros`` operator gate. + +Every :class:`AckermannRosRobot` command forwards to ``use_ros``, whose command +gate refuses a safety-critical surface when no operator context is reachable. +The surfaces this bridge drives are the DeepRacer's servo topic and the two mode +services that arm the vehicle, and all three are blocklisted - so a bridge that +forwards no context turns its whole command surface, ``stop`` included, into a +per-call refusal, while a bridge whose surfaces are *not* on the blocklist sends +throttle commands with no prompt, no allowlist check and no audit row. +``tests/mesh/test_ackermann_robot.py`` can see neither: it patches the +``use_ros`` symbol at the boundary the gate lives behind. + +These tests keep the real ``use_ros`` and substitute the rclpy transport +instead (the same boundary ``tests/tools/test_use_ros.py`` doubles), so the gate +and the bridge wiring under test both run unmodified while no message can reach +a real DDS graph. They are the Ackermann half of +``tests/mesh/test_ros_bridge_command_gate.py``; +:class:`TestEveryCommandingMeshBridgeHasAGateSuite` derives the set of bridges +owing such a suite from the tree, so a fifth one is graded on arrival rather +than shipping ungated the way this one did. +""" + +from __future__ import annotations + +import ast +from pathlib import Path +from typing import Any +from unittest.mock import MagicMock + +import pytest + +import strands_robots.tools._command_gate as gate_mod +import strands_robots.tools.use_ros as ros_mod +from strands_robots.mesh import AckermannRosRobot + +_MESH_DIR = Path(ros_mod.__file__).parent.parent / "mesh" +_BRIDGE_SOURCE = _MESH_DIR / "ackermann_robot.py" +_TESTS_DIR = Path(__file__).parent + +_COMMAND_METHODS = frozenset({"drive", "stop", "enable", "_publish_servo"}) +_COMMAND_ACTIONS = frozenset({"publish", "service_call", "action_send_goal"}) + +#: Every mesh bridge that sends a command through ``use_ros``, and the suite that +#: grades its gate wiring. Discovered modules are compared against these keys, so +#: a new commanding bridge fails until it is triaged into a gate suite of its own. +_GATE_SUITES: dict[str, str] = { + "ros_bridge.py": "test_ros_bridge_command_gate.py", + "ackermann_robot.py": "test_ackermann_command_gate.py", +} + + +def _texts(result: dict[str, Any]) -> str: + return " ".join(block.get("text", "") for block in result["content"]) + + +def _car() -> AckermannRosRobot: + """A stock DeepRacer bridge: every surface it commands is blocklisted. + + ``/webserver_pkg/manual_drive`` matches ``/manual_drive`` on the + final-segment rule, and the handshake's ``/ctrl_pkg/vehicle_state`` and + ``/ctrl_pkg/enable_state`` match ``/vehicle_state`` and ``/enable_state`` the + same way - which is what makes the shipped wiring the right instance to test + the gate with. + + Only ``scan_type`` departs from the stock wiring: an omitted type is resolved + from the live graph, which is the one path in this class that needs a real + rclpy, and it is a read the gate never sees. + """ + return AckermannRosRobot.from_deepracer(node_name="deepracer", scan_type="sensor_msgs/msg/LaserScan") + + +def _tool(robot: AckermannRosRobot, name: str) -> Any: + return next(t for t in robot.tools if t.tool_name == name) + + +@pytest.fixture(scope="module") +def bridge_ast() -> ast.Module: + """Parse the bridge source once for the structural guards below.""" + return ast.parse(_BRIDGE_SOURCE.read_text(encoding="utf-8")) + + +class TestBridgeCommandsReachTheGate: + """The bridge's agent tools must prompt the operator, not refuse outright.""" + + published: list[tuple[Any, ...]] + services: list[tuple[Any, ...]] + echoed: list[tuple[Any, ...]] + + @pytest.fixture(autouse=True) + def _hermetic(self, monkeypatch: pytest.MonkeyPatch) -> None: + # Both env vars short-circuit the gate, so an ambient BYPASS_TOOL_CONSENT + # (common in agent/automation shells) would make these assertions pass + # without the gate ever running. Cases that need them opt in explicitly. + monkeypatch.delenv("BYPASS_TOOL_CONSENT", raising=False) + monkeypatch.delenv(gate_mod.COMMAND_ALLOW_ENV, raising=False) + monkeypatch.setattr(ros_mod._backend, "available", lambda: True) + self.published, self.services, self.echoed = [], [], [] + + def _record(sink: list[tuple[Any, ...]], outcome: Any) -> Any: + def _fake(*args: Any) -> Any: + sink.append(args) + return outcome + + return _fake + + monkeypatch.setattr(ros_mod, "_publish", _record(self.published, None)) + monkeypatch.setattr(ros_mod, "_service_call", _record(self.services, {"success": True})) + monkeypatch.setattr(ros_mod, "_echo", _record(self.echoed, [{"ranges": [1.0, 2.0]}])) + + def test_drive_tool_prompts_for_every_surface_and_commands_on_approval(self) -> None: + """The handshake services and the servo publish are each gated.""" + ctx = MagicMock() + ctx.interrupt.return_value = "y" + result = _tool(_car(), "drive_deepracer")(linear=0.5, tool_context=ctx) + assert ctx.interrupt.called, "the drive tool never reached the operator gate" + prompted = [call[1]["reason"]["target"] for call in ctx.interrupt.call_args_list] + assert prompted == [ + "/ctrl_pkg/vehicle_state", + "/ctrl_pkg/enable_state", + "/webserver_pkg/manual_drive", + ] + assert result["status"] == "success" + assert len(self.services) == 2 + assert len(self.published) == 1 + + def test_drive_tool_declined_at_the_servo_topic_publishes_nothing(self) -> None: + ctx = MagicMock() + ctx.interrupt.side_effect = ["y", "y", "n"] + result = _tool(_car(), "drive_deepracer")(linear=0.5, tool_context=ctx) + assert result["status"] == "error" + assert "declined" in _texts(result) + assert self.published == [] + + def test_drive_tool_declined_at_the_handshake_never_reaches_the_servo_topic(self) -> None: + """A refused arming service aborts the drive, as a failed one does.""" + ctx = MagicMock() + ctx.interrupt.return_value = "n" + result = _tool(_car(), "drive_deepracer")(linear=0.5, tool_context=ctx) + assert result["status"] == "error" + assert ctx.interrupt.call_count == 1 # stops at the first refusal + assert self.services == [] + assert self.published == [] + + def test_stop_tool_halts_the_car_once_the_operator_approves(self) -> None: + """The halt is gated like any other servo publish, and must be reachable. + + A bridge that cannot forward an operator context makes ``stop`` an + unconditional refusal - removing the one control that makes an + ungated-looking latching command safe to issue at all. + """ + ctx = MagicMock() + ctx.interrupt.return_value = "y" + result = _tool(_car(), "stop_deepracer")(tool_context=ctx) + assert ctx.interrupt.called, "the stop tool never reached the operator gate" + assert result["status"] == "success" + topic, _type, fields = self.published[0][:3] + assert topic == "/webserver_pkg/manual_drive" + assert fields == {"angle": 0.0, "throttle": 0.0} + + def test_read_tool_needs_no_context_because_reads_are_never_gated(self) -> None: + result = _tool(_car(), "get_scan_deepracer")() + assert result["status"] == "success" + assert len(self.echoed) == 1 + + def test_programmatic_command_without_a_context_names_the_headless_variables(self) -> None: + """The documented decision: no operator context means the gate refuses.""" + result = _car().drive(linear=0.5) + assert result["status"] == "error" + assert gate_mod.COMMAND_ALLOW_ENV in _texts(result) + assert "BYPASS_TOOL_CONSENT" in _texts(result) + assert self.services == [] + assert self.published == [] + + @pytest.mark.parametrize( + "allow", + [ + "/manual_drive,/vehicle_state,/enable_state", + "/webserver_pkg/manual_drive,/ctrl_pkg/vehicle_state,/ctrl_pkg/enable_state", + ], + ) + def test_programmatic_drive_and_stop_run_under_the_headless_allowlist( + self, monkeypatch: pytest.MonkeyPatch, allow: str + ) -> None: + """The pre-approval the docs and the example hand an operator works. + + Both spellings are pinned because the bare form is what they publish: it + is the namespaced surfaces the bridge actually sends to that have to be + covered by it. + """ + monkeypatch.setenv(gate_mod.COMMAND_ALLOW_ENV, allow) + car = _car() + assert car.drive(linear=0.5)["status"] == "success" + assert car.stop()["status"] == "success" + assert len(self.published) == 2 + + def test_a_timed_drive_gates_the_command_and_its_trailing_halt(self) -> None: + """The halt publish is a command too, so it carries the same context.""" + ctx = MagicMock() + ctx.interrupt.return_value = "y" + result = _tool(_car(), "drive_deepracer")(linear=0.5, duration=0.1, tool_context=ctx) + assert result["status"] == "success" + assert len(self.published) == 2 # the hold, then the trailing zero + assert self.published[1][2] == {"angle": 0.0, "throttle": 0.0} + + +class TestCommandToolsDeclareTheOperatorContext: + """Structural guards, so a command tool added later cannot ship contextless. + + The behavioural tests above cover the two command tools that exist today. A + third one added without ``context=True`` would fail closed on every call with + no test noticing, so the wiring itself is pinned here. + """ + + @staticmethod + def _decorated_tools(tree: ast.Module) -> list[ast.FunctionDef]: + return [ + node + for node in ast.walk(tree) + if isinstance(node, ast.FunctionDef) + and any( + isinstance(dec, ast.Call) and getattr(dec.func, "id", None) == "tool" for dec in node.decorator_list + ) + ] + + @staticmethod + def _forwards_a_command_method(func: ast.FunctionDef) -> bool: + return any( + isinstance(node, ast.Call) and isinstance(node.func, ast.Attribute) and node.func.attr in _COMMAND_METHODS + for node in ast.walk(func) + ) + + def test_every_command_tool_is_context_enabled_and_forwards_it(self, bridge_ast: ast.Module) -> None: + command_tools = [f for f in self._decorated_tools(bridge_ast) if self._forwards_a_command_method(f)] + assert {f.name for f in command_tools} == {"drive", "stop"} + for func in command_tools: + decorator = next(d for d in func.decorator_list if isinstance(d, ast.Call)) + context_kwarg = next((kw for kw in decorator.keywords if kw.arg == "context"), None) + assert context_kwarg is not None and getattr(context_kwarg.value, "value", None) is True, ( + f"bridge command tool {func.name!r} is not declared @tool(context=True), " + "so it can never reach the use_ros operator gate" + ) + params = [a.arg for a in func.args.args] + [a.arg for a in func.args.kwonlyargs] + assert "tool_context" in params, f"{func.name!r} does not receive the injected operator context" + forwarded = [ + kw.arg + for node in ast.walk(func) + if isinstance(node, ast.Call) + for kw in node.keywords + if kw.arg == "tool_context" + ] + assert forwarded, f"{func.name!r} receives the operator context but does not forward it" + + def test_read_only_tools_stay_contextless(self, bridge_ast: ast.Module) -> None: + """``echo`` is never gated, so a read tool must not ask for an operator.""" + read_tools = [f for f in self._decorated_tools(bridge_ast) if not self._forwards_a_command_method(f)] + assert {f.name for f in read_tools} == {"get_scan"} + for func in read_tools: + params = [a.arg for a in func.args.args] + assert "tool_context" not in params, f"read-only tool {func.name!r} should not require an operator context" + + def test_every_bridge_command_call_forwards_the_context_to_use_ros(self, bridge_ast: ast.Module) -> None: + """A bridge method that carries a command must not drop the context. + + This is the failure this suite exists for: the gate lives inside + ``use_ros``, so a call site that omits ``tool_context`` silently becomes + a fail-closed refusal for its whole method. + """ + checked = 0 + for node in ast.walk(bridge_ast): + if not (isinstance(node, ast.Call) and getattr(node.func, "id", None) == "use_ros"): + continue + action = next((kw.value for kw in node.keywords if kw.arg == "action"), None) + if not (isinstance(action, ast.Constant) and action.value in _COMMAND_ACTIONS): + continue + checked += 1 + assert any(kw.arg == "tool_context" for kw in node.keywords), ( + f"use_ros(action={action.value!r}) at ackermann_robot.py:{node.lineno} does not forward tool_context" + ) + assert checked == 2, f"expected the servo publish and the handshake service_call, found {checked}" + + def test_command_tools_do_not_expose_the_context_in_their_input_schema(self) -> None: + """The operator context is injected, never a parameter the model fills.""" + for name in ("drive_deepracer", "stop_deepracer"): + spec = _tool(_car(), name).tool_spec + assert "tool_context" not in spec["inputSchema"]["json"].get("properties", {}) + + +class TestTheCommandedSurfacesAreBlocklisted: + """The gate only fires on a blocklisted surface, so coverage is half the fix. + + A bridge can forward a context perfectly and still be ungated if none of the + surfaces it drives is on the blocklist - which is how this one shipped: the + stock DeepRacer wiring matched no entry, so every publish and handshake call + proceeded with no prompt and no audit row. + """ + + def test_every_surface_the_shipped_wiring_commands_is_gated(self) -> None: + car = _car() + commanded = [car.servo_topic] + [item["service"] for item in car.init_services] + assert commanded, "the DeepRacer wiring commands nothing - the premise is gone" + for name in commanded: + assert gate_mod.command_block_message("publish", name) is not None, ( + f"{name} is a DeepRacer motion/arming surface but no blocklist entry covers it" + ) + + def test_the_bare_entries_are_what_covers_the_namespaced_spellings(self) -> None: + """The entries are spelled bare, so the base-name rule is load-bearing.""" + for entry in ("/manual_drive", "/vehicle_state", "/enable_state"): + assert entry in gate_mod.COMMAND_BLOCKLIST + + +class TestEveryCommandingMeshBridgeHasAGateSuite: + """A mesh bridge that commands through ``use_ros`` owes a gate suite. + + Derived from the tree rather than from a list, because the defect this file + fixes was a *new* bridge shipping with neither blocklist coverage nor context + threading while the existing bridge's suite stayed green. A fifth bridge + fails here until it is triaged, which is the only moment the question gets + asked cheaply. + """ + + @staticmethod + def _modules_sending_commands() -> set[str]: + found: set[str] = set() + for path in sorted(_MESH_DIR.glob("*.py")): + tree = ast.parse(path.read_text(encoding="utf-8")) + for node in ast.walk(tree): + if not (isinstance(node, ast.Call) and getattr(node.func, "id", None) == "use_ros"): + continue + action = next((kw.value for kw in node.keywords if kw.arg == "action"), None) + if isinstance(action, ast.Constant) and action.value in _COMMAND_ACTIONS: + found.add(path.name) + return found + + def test_the_inventory_names_every_bridge_that_sends_a_command(self) -> None: + assert self._modules_sending_commands() == set(_GATE_SUITES), ( + "a mesh module sends commands through use_ros without being triaged into " + "_GATE_SUITES; add it with the suite that grades its gate wiring" + ) + + @pytest.mark.parametrize("module, suite", sorted(_GATE_SUITES.items())) + def test_each_bridge_gate_suite_exists(self, module: str, suite: str) -> None: + assert (_TESTS_DIR / suite).is_file(), f"{module} has no gate suite at tests/mesh/{suite}" diff --git a/tests/mesh/test_ackermann_robot.py b/tests/mesh/test_ackermann_robot.py new file mode 100644 index 0000000000..a11f72110c --- /dev/null +++ b/tests/mesh/test_ackermann_robot.py @@ -0,0 +1,435 @@ +"""Behavior tests for the AckermannRosRobot mesh bridge. + +The bridge presents an Ackermann-steering ROS 2 car (reference platform: AWS +DeepRacer) as a strands robot. Like ``test_ros_bridge.py``, every test runs +rclpy-free: the module's ``use_ros`` reference is monkeypatched with a +recorder, so the forwarding contract, the bicycle-model conversion, the +enable handshake, and the safety behaviors are all exercised with no ROS 2 +installed. +""" + +from __future__ import annotations + +import math +from typing import Any + +import pytest + +from strands_robots.mesh import ackermann_robot as ack_mod +from strands_robots.mesh.ackermann_robot import _twist_to_servo + +# Bicycle model --------------------------------------------------------------- + +_LIMITS = {"wheelbase_m": 0.164, "max_speed": 1.5, "max_steering_rad": 0.5236} + + +def test_model_straight_full_speed() -> None: + angle, throttle = _twist_to_servo(1.5, 0.0, **_LIMITS) + assert angle == 0.0 + assert throttle == 1.0 + + +def test_model_rest_is_zero() -> None: + # Below the rest epsilon the command maps to zeros: an Ackermann platform + # cannot yaw at rest, and atan2 would amplify noise near v = 0. + assert _twist_to_servo(0.0, 2.0, **_LIMITS) == (0.0, 0.0) + assert _twist_to_servo(5e-4, 2.0, **_LIMITS) == (0.0, 0.0) + + +def test_model_left_turn_matches_hand_computation() -> None: + angle, throttle = _twist_to_servo(1.0, 2.0, **_LIMITS) + expected_delta = math.atan2(0.164 * 2.0, 1.0) # ~0.317 rad, inside the clamp + assert angle == pytest.approx(expected_delta / 0.5236) + assert throttle == pytest.approx(1.0 / 1.5) + assert angle > 0 # positive angular -> positive (left) steering + + +def test_model_right_turn_is_negative() -> None: + angle, _ = _twist_to_servo(1.0, -2.0, **_LIMITS) + assert angle == pytest.approx(-math.atan2(0.164 * 2.0, 1.0) / 0.5236) + + +def test_model_steering_clamps_to_limit() -> None: + # Huge angular rate: delta saturates at max_steering_rad -> angle_norm +/-1. + angle, _ = _twist_to_servo(0.1, 50.0, **_LIMITS) + assert angle == pytest.approx(1.0) + angle, _ = _twist_to_servo(0.1, -50.0, **_LIMITS) + assert angle == pytest.approx(-1.0) + + +def test_model_throttle_clamps_beyond_max_speed() -> None: + _, throttle = _twist_to_servo(4.0, 0.0, **_LIMITS) + assert throttle == 1.0 + _, throttle = _twist_to_servo(-4.0, 0.0, **_LIMITS) + assert throttle == -1.0 + + +def test_model_reverse_steering_geometry() -> None: + # Reversing with positive angular: atan2 handles the quadrant (delta flips + # sign relative to forward motion), matching real Ackermann behavior. + fwd_angle, _ = _twist_to_servo(1.0, 2.0, **_LIMITS) + rev_angle, rev_throttle = _twist_to_servo(-1.0, 2.0, **_LIMITS) + assert rev_throttle == pytest.approx(-1.0 / 1.5) + assert rev_angle == pytest.approx(-fwd_angle) + + +# Recorder -------------------------------------------------------------------- + + +class _Recorder: + """Stand-in for use_ros: records calls, returns scripted results.""" + + def __init__(self) -> None: + self.calls: list[dict[str, Any]] = [] + self.responses: list[dict[str, Any]] = [] + + def __call__(self, **kwargs: Any) -> dict[str, Any]: + self.calls.append(kwargs) + if self.responses: + return self.responses.pop(0) + return {"status": "success", "content": [{"text": "ok"}]} + + +@pytest.fixture +def rec(monkeypatch: pytest.MonkeyPatch) -> _Recorder: + recorder = _Recorder() + monkeypatch.setattr(ack_mod, "use_ros", recorder) + return recorder + + +def _car(**overrides: Any) -> ack_mod.AckermannRosRobot: + kwargs: dict[str, Any] = {"node_name": "car", "servo_topic": "/servo"} + kwargs.update(overrides) + return ack_mod.AckermannRosRobot(**kwargs) + + +_HANDSHAKE = [ + { + "service": "/ctrl_pkg/vehicle_state", + "type": "deepracer_interfaces_pkg/srv/ActiveStateSrv", + "fields": {"state": 1}, + }, + { + "service": "/ctrl_pkg/enable_state", + "type": "deepracer_interfaces_pkg/srv/EnableStateSrv", + "fields": {"is_active": True}, + }, +] + + +# Construction ---------------------------------------------------------------- + + +def test_invalid_names_rejected_at_construction() -> None: + with pytest.raises(ValueError, match="invalid servo_topic"): + _car(servo_topic="/servo topic") + with pytest.raises(ValueError, match="invalid node_name"): + _car(node_name="car; rm") + with pytest.raises(ValueError, match="invalid init_services service"): + _car(init_services=[{"service": "/bad svc", "type": "a/srv/B", "fields": {}}]) + + +def test_init_service_missing_type_rejected_at_construction() -> None: + # enable() indexes item["type"]; a missing type must fail loudly at + # construction, not as a KeyError escaping enable() later. + with pytest.raises(ValueError, match="missing its 'type'"): + _car(init_services=[{"service": "/ctrl_pkg/vehicle_state", "fields": {"state": 1}}]) + + +@pytest.mark.parametrize("field", ["wheelbase_m", "max_speed", "max_steering_rad", "max_duration", "publish_rate"]) +def test_nonpositive_numerics_rejected(field: str) -> None: + with pytest.raises(ValueError, match=field): + _car(**{field: 0.0}) + + +def test_from_deepracer_wiring_pinned() -> None: + car = ack_mod.AckermannRosRobot.from_deepracer(node_name="dr") + assert car.servo_topic == "/webserver_pkg/manual_drive" + assert car.servo_type == "deepracer_interfaces_pkg/msg/ServoCtrlMsg" + assert car.scan_topic == "/rplidar_ros/scan" + assert car.wheelbase_m == 0.164 + assert car.max_speed == 1.5 + assert car.max_steering_rad == 0.5236 + assert car.publish_rate == 20.0 + assert car.init_services == _HANDSHAKE + + +def test_from_deepracer_accepts_overrides() -> None: + car = ack_mod.AckermannRosRobot.from_deepracer(node_name="dr", max_speed=0.8, scan_topic=None) + assert car.max_speed == 0.8 + assert car.scan_topic is None + assert car.servo_topic == "/webserver_pkg/manual_drive" # untouched default + + +# enable ---------------------------------------------------------------------- + + +def test_enable_calls_init_services_in_order(rec: _Recorder) -> None: + car = _car(init_services=_HANDSHAKE) + result = car.enable() + assert result["status"] == "success" + assert [c["action"] for c in rec.calls] == ["service_call", "service_call"] + assert rec.calls[0]["service"] == "/ctrl_pkg/vehicle_state" + assert rec.calls[0]["type"] == "deepracer_interfaces_pkg/srv/ActiveStateSrv" + assert rec.calls[0]["fields"] == {"state": 1} + assert rec.calls[1]["service"] == "/ctrl_pkg/enable_state" + assert rec.calls[1]["fields"] == {"is_active": True} + + +def test_enable_is_idempotent(rec: _Recorder) -> None: + car = _car(init_services=_HANDSHAKE) + car.enable() + car.enable() + assert len(rec.calls) == 2 # handshake ran once, not twice + + +def test_enable_failure_surfaces_and_does_not_latch(rec: _Recorder) -> None: + car = _car(init_services=_HANDSHAKE) + failure = {"status": "error", "content": [{"text": "use_ros: service not available"}]} + rec.responses = [failure] + result = car.enable() + assert result is failure + assert len(rec.calls) == 1 # stopped at the first failing call + # A later retry runs the handshake again from the start. + result = car.enable() + assert result["status"] == "success" + assert len(rec.calls) == 3 + + +def test_enable_without_init_services_is_success(rec: _Recorder) -> None: + result = _car().enable() + assert result["status"] == "success" + assert rec.calls == [] + + +# stop / get_scan --------------------------------------------------------------- + + +def test_stop_publishes_zero_and_needs_no_enable(rec: _Recorder) -> None: + car = _car(init_services=_HANDSHAKE) + result = car.stop() + assert result["status"] == "success" + assert len(rec.calls) == 1 # no handshake - stopping must never be gated + call = rec.calls[0] + assert call["action"] == "publish" + assert call["topic"] == "/servo" + assert call["fields"] == {"angle": 0.0, "throttle": 0.0} + assert call["count"] == 1 + + +def test_get_scan_without_topic_is_error(rec: _Recorder) -> None: + result = _car().get_scan() + assert result["status"] == "error" + assert "no scan_topic" in result["content"][0]["text"] + assert rec.calls == [] + + +def test_get_scan_forwards_echo(rec: _Recorder) -> None: + car = _car(scan_topic="/scan") + car.get_scan(timeout=3.0) + call = rec.calls[0] + assert call["action"] == "echo" + assert call["topic"] == "/scan" + assert call["timeout"] == 3.0 + assert call["count"] == 1 + + +# drive ------------------------------------------------------------------------- + + +def test_drive_converts_and_publishes(rec: _Recorder) -> None: + car = _car() + result = car.drive(linear=1.5, angular=0.0) + assert result["status"] == "success" + call = rec.calls[0] + assert call["action"] == "publish" + assert call["topic"] == "/servo" + assert call["type"] == "deepracer_interfaces_pkg/msg/ServoCtrlMsg" + assert call["fields"] == {"angle": 0.0, "throttle": 1.0} + assert call["count"] == 1 + assert call["rate"] == 20.0 + assert len(rec.calls) == 1 # single-shot command: no trailing zero needed + + +def test_drive_runs_enable_first_and_only_once(rec: _Recorder) -> None: + car = _car(init_services=_HANDSHAKE) + car.drive(linear=0.5) + car.drive(linear=0.5) + actions = [c["action"] for c in rec.calls] + assert actions == ["service_call", "service_call", "publish", "publish"] + + +def test_drive_aborts_when_enable_fails(rec: _Recorder) -> None: + car = _car(init_services=_HANDSHAKE) + failure = {"status": "error", "content": [{"text": "use_ros: service not available"}]} + rec.responses = [failure] + result = car.drive(linear=0.5) + assert result is failure + assert [c["action"] for c in rec.calls] == ["service_call"] # nothing published + + +def test_drive_rejects_overlong_duration(rec: _Recorder) -> None: + car = _car(max_duration=2.0) + result = car.drive(linear=0.5, duration=3.0) + assert result["status"] == "error" + assert "max_duration" in result["content"][0]["text"] + assert rec.calls == [] # rejected loudly, nothing published + + +def test_drive_clamps_linear_to_max_speed(rec: _Recorder) -> None: + car = _car() + car.drive(linear=99.0) + assert rec.calls[0]["fields"]["throttle"] == 1.0 + + +def test_drive_duration_publishes_n_then_trailing_zero(rec: _Recorder) -> None: + car = _car() + car.drive(linear=1.0, duration=1.0) # 20 Hz -> 20 messages + assert len(rec.calls) == 2 + main, trailing = rec.calls + assert main["count"] == 20 + assert main["fields"]["throttle"] == pytest.approx(1.0 / 1.5) + assert trailing["fields"] == {"angle": 0.0, "throttle": 0.0} + assert trailing["count"] == 1 + + +def test_drive_trailing_zero_sent_even_when_publish_errors(rec: _Recorder) -> None: + car = _car() + failure = {"status": "error", "content": [{"text": "use_ros: publish failed"}]} + rec.responses = [failure] + result = car.drive(linear=1.0, duration=1.0) + assert result is failure # the main publish outcome is what the caller sees + assert len(rec.calls) == 2 # ...but the trailing zero still went out + assert rec.calls[1]["fields"] == {"angle": 0.0, "throttle": 0.0} + + +def test_drive_reports_a_failed_trailing_halt_rather_than_the_publish_success(rec: _Recorder) -> None: + """The other direction of the case above: the halt is what fails. + + ``use_ros`` reports a transport failure as an error dict rather than raising, + so a discarded halt verdict would present a car still holding the commanded + throttle as a drive that stopped itself - and the tool description promises + exactly that ("a command with duration stops automatically afterwards"), so + an agent reading ``success`` never issues ``stop``. + """ + rec.responses = [ + {"status": "success", "content": [{"text": "published 20 message(s)"}]}, + {"status": "error", "content": [{"text": "use_ros: publish failed"}]}, + ] + result = _car().drive(linear=1.0, duration=1.0) + assert result["status"] == "error" + text = result["content"][0]["text"] + assert "stop" in text # names the action the agent has to take next + assert "use_ros: publish failed" in text # the halt's own cause survives + assert len(rec.calls) == 2 + + +def test_drive_failing_on_both_publishes_reports_the_drive_failure(rec: _Recorder) -> None: + """A failed drive is the cause; a halt that also failed does not replace it.""" + drive_failure = {"status": "error", "content": [{"text": "use_ros: publish failed"}]} + rec.responses = [drive_failure, {"status": "error", "content": [{"text": "halt failed too"}]}] + result = _car().drive(linear=1.0, duration=1.0) + assert result is drive_failure + assert len(rec.calls) == 2 + + +def test_drive_single_shot_success_is_not_reinterpreted(rec: _Recorder) -> None: + """Control: no halt is sent for a latching command, so none can be judged. + + A scripted failure sits next in the queue; a verdict read from a publish + this call never makes would turn a successful single-shot into an error. + """ + rec.responses = [ + {"status": "success", "content": [{"text": "published 1 message(s)"}]}, + {"status": "error", "content": [{"text": "use_ros: publish failed"}]}, + ] + result = _car().drive(linear=1.0) + assert result["status"] == "success" + assert len(rec.calls) == 1 + + +def test_drive_sustained_zero_command_has_no_trailing_zero(rec: _Recorder) -> None: + car = _car() + car.drive(linear=0.0, duration=1.0) # already zero: trailing zero is redundant + assert len(rec.calls) == 1 + + +def test_drive_rejects_nonfinite_inputs(rec: _Recorder) -> None: + # NaN passes a min/max clamp silently - it must be rejected loudly, not + # published as full throttle / full steering. + cases: list[dict[str, Any]] = [ + {"linear": float("nan")}, + {"angular": float("inf")}, + {"linear": 0.5, "duration": float("nan")}, + ] + for kwargs in cases: + result = _car().drive(**kwargs) + assert result["status"] == "error" + assert rec.calls == [] + + +def test_constructor_rejects_nonfinite_numerics() -> None: + with pytest.raises(ValueError, match="wheelbase_m"): + _car(wheelbase_m=float("nan")) + + +def test_drive_rejects_nonpositive_duration(rec: _Recorder) -> None: + for bad in (0.0, -2.0): + result = _car().drive(linear=0.5, duration=bad) + assert result["status"] == "error" + assert "duration" in result["content"][0]["text"] + assert rec.calls == [] + + +def test_drive_short_duration_still_gets_trailing_zero(rec: _Recorder) -> None: + # duration=0.01 rounds to a single message - but it is still a TIMED + # command, so the trailing zero must go out (the latch hole). + _car().drive(linear=1.0, duration=0.01) + assert len(rec.calls) == 2 + assert rec.calls[0]["count"] == 1 + assert rec.calls[1]["fields"] == {"angle": 0.0, "throttle": 0.0} + + +def test_drive_overlong_duration_rejected_before_enable(rec: _Recorder) -> None: + # Validation precedes side effects: an invalid request must not switch the + # car into manual mode. + car = _car(init_services=_HANDSHAKE, max_duration=2.0) + result = car.drive(linear=0.5, duration=99.0) + assert result["status"] == "error" + assert rec.calls == [] + + +# tools ------------------------------------------------------------------------- + + +def test_tools_naming_and_conditional_scan() -> None: + with_scan = {t.tool_name for t in _car(scan_topic="/scan").tools} + without_scan = {t.tool_name for t in _car().tools} + assert with_scan == {"drive_car", "stop_car", "get_scan_car"} + assert without_scan == {"drive_car", "stop_car"} + + +def test_drive_tool_description_states_turning_radius() -> None: + drive_tool = next(t for t in _car().tools if t.tool_name == "drive_car") + # wheelbase 0.164 / tan(0.5236) ~= 0.28 m - the agent must know it cannot + # turn in place. + spec_desc = drive_tool.tool_spec["description"] + assert "cannot turn in place" in spec_desc + assert "0.28" in spec_desc + + +def test_tools_forward_to_instance(rec: _Recorder) -> None: + car = _car(scan_topic="/scan") + tools: dict[str, Any] = {t.tool_name: t for t in car.tools} + tools["drive_car"](linear=1.5) + assert rec.calls[-1]["fields"]["throttle"] == 1.0 + tools["stop_car"]() + assert rec.calls[-1]["fields"] == {"angle": 0.0, "throttle": 0.0} + tools["get_scan_car"]() + assert rec.calls[-1]["action"] == "echo" + + +def test_exported_from_mesh_package() -> None: + from strands_robots.mesh import AckermannRosRobot as exported + + assert exported is ack_mod.AckermannRosRobot diff --git a/tests/mesh/test_bridge_stop_tool_parity.py b/tests/mesh/test_bridge_stop_tool_parity.py index 7eaa35ef0f..901a8757e3 100644 --- a/tests/mesh/test_bridge_stop_tool_parity.py +++ b/tests/mesh/test_bridge_stop_tool_parity.py @@ -181,7 +181,17 @@ def test_the_rosbridge_stop_and_scan_tools_forward_to_the_instance(monkeypatch: # Structural: a future transport cannot ship a drive without a halt ------------ #: Bridge classes that own a mobile base's drive/stop pair today. -EXPECTED_DRIVE_STOP_CLASSES = {"RosBridgedRobot", "RosbridgeRobot", "RtpsRobot"} +#: +#: ``AckermannRosRobot`` is here but deliberately not in :data:`BRIDGES`: the +#: cross-transport parity cases above assert a ``geometry_msgs/msg/Twist`` of +#: zeros on ``/cmd_vel``, and an Ackermann car halts with a zero +#: ``ServoCtrlMsg`` (``angle``/``throttle``) on its servo topic instead. The +#: rule those cases exist for - a transport that can start motion exposes the +#: halt in the same tool set - is enforced for it by +#: :func:`test_every_bridge_owning_a_drive_stop_pair_builds_a_stop_tool`, which +#: reads the survey rather than a wire format, and its zero-servo halt is +#: pinned in ``tests/mesh/test_ackermann_robot.py``. +EXPECTED_DRIVE_STOP_CLASSES = {"AckermannRosRobot", "RosBridgedRobot", "RosbridgeRobot", "RtpsRobot"} def _mesh_dir() -> pathlib.Path: diff --git a/tests/mesh/test_drive_command_numeric_domains.py b/tests/mesh/test_drive_command_numeric_domains.py index 7e5f25e84d..fb3a1121c0 100644 --- a/tests/mesh/test_drive_command_numeric_domains.py +++ b/tests/mesh/test_drive_command_numeric_domains.py @@ -1,13 +1,15 @@ """The numeric domains a mobile-base bridge's drive command must share. -Three bridges expose the same fleet-standard ``drive(linear, angular, duration, -count)`` contract over three transports. A velocity command is the one call on +Four bridges expose the same fleet-standard ``drive(linear, angular, duration, +count)`` contract over four transports. A velocity command is the one call on any of them that physically moves the robot, so each knob it carries is checked before anything reaches the wire - and the accepted domain has to be the *same* -on all three, because an agent that learns the contract from one bridge drives -the others with it. +on all of them, because an agent that learns the contract from one bridge drives +the others with it. The wire message differs (a ``Twist`` for the three +differential-drive bases, a normalized servo pair for the Ackermann car); the +domain of every value the caller supplies does not. -Every check here therefore compares the three against each other rather than +Every check here therefore compares the bridges against each other rather than against a hardcoded expectation, and asserts the *reason* a value was refused rather than only that it was: a parity assertion on ``status == "error"`` alone passes when two bridges refuse one value for two unrelated causes. @@ -27,6 +29,7 @@ import pytest +import strands_robots.mesh.ackermann_robot as ackermann_mod import strands_robots.mesh.ros_bridge as ros_bridge_mod import strands_robots.mesh.rosbridge_robot as rosbridge_mod import strands_robots.mesh.rtps_robot as rtps_mod @@ -48,21 +51,47 @@ def __call__(self, **kwargs: Any) -> dict[str, Any]: return {"status": "success", "content": [{"text": "ok"}]} -#: (label, module, forwarded transport symbol, robot factory). Every bridge is -#: built with the same publish rate so a duration hold derives the same count. -_TRANSPORTS: list[tuple[str, Any, str, Callable[[], Any]]] = [ +#: The wire fields a bridge publishes for the usable probe command below +#: (``linear=0.5``, ``angular=-0.25``). A differential-drive base sends the +#: velocities straight through as a ``Twist``. +_TWIST_FIELDS: dict[str, Any] = {"linear": {"x": 0.5}, "angular": {"z": -0.25}} + +#: The Ackermann car sends a normalized servo pair instead: the documented +#: bicycle model ``atan(wheelbase * angular / linear)`` normalized by the +#: steering limit, and the speed normalized by ``max_speed``. Re-derived here +#: from the platform defaults rather than by calling the converter under test. +_SERVO_FIELDS: dict[str, Any] = { + "angle": pytest.approx(-0.15625849618727), + "throttle": pytest.approx(0.3333333333333333), +} + +#: (label, module, forwarded transport symbol, keyword-taking constructor, +#: published wire fields). The constructor takes the limit keywords so a +#: construction-time check cannot silently target the wrong bridge, and every +#: bridge is built with the same publish rate so a duration hold derives the +#: same message count. +_TRANSPORTS: list[tuple[str, Any, str, Callable[..., Any], dict[str, Any]]] = [ ( "ros_bridge", ros_bridge_mod, "use_ros", - lambda: ros_bridge_mod.RosBridgedRobot("rover", "/cmd_vel", "/odom", publish_rate=10.0), + lambda **kw: ros_bridge_mod.RosBridgedRobot("rover", "/cmd_vel", "/odom", **kw), + _TWIST_FIELDS, ), - ("rtps", rtps_mod, "use_rtps", lambda: rtps_mod.RtpsRobot("rover", "/cmd_vel", publish_rate=10.0)), + ("rtps", rtps_mod, "use_rtps", lambda **kw: rtps_mod.RtpsRobot("rover", "/cmd_vel", **kw), _TWIST_FIELDS), ( "rosbridge", rosbridge_mod, "use_rosbridge", - lambda: rosbridge_mod.RosbridgeRobot("rover", "/cmd_vel", "/odom", publish_rate=10.0), + lambda **kw: rosbridge_mod.RosbridgeRobot("rover", "/cmd_vel", "/odom", **kw), + _TWIST_FIELDS, + ), + ( + "ackermann", + ackermann_mod, + "use_ros", + lambda **kw: ackermann_mod.AckermannRosRobot("car", "/servo", **kw), + _SERVO_FIELDS, ), ] @@ -79,12 +108,12 @@ def __call__(self, **kwargs: Any) -> dict[str, Any]: UNUSABLE_COUNTS: list[Any] = [0, -5, 2.7, float("nan"), float("inf"), "3", True, None, [3]] -def _drive(monkeypatch: pytest.MonkeyPatch, transport: tuple[str, Any, str, Callable[[], Any]], **kwargs: Any) -> Any: +def _drive(monkeypatch: pytest.MonkeyPatch, transport: tuple[Any, ...], **kwargs: Any) -> Any: """Drive one bridge with the transport recorded, returning (result, recorder).""" - _label, module, symbol, factory = transport + _label, module, symbol, ctor, _fields = transport rec = _Recorder() monkeypatch.setattr(module, symbol, rec) - return factory().drive(**kwargs), rec + return ctor(publish_rate=10.0).drive(**kwargs), rec def _reason(result: dict[str, Any]) -> str: @@ -151,20 +180,20 @@ def test_every_bridge_refuses_an_unusable_count_for_the_same_reason( def test_every_bridge_refuses_an_unusable_publish_rate_at_construction(value: Any) -> None: """A publish rate that cannot be honored is refused before a robot exists. - ``publish_rate`` is the one constructor limit all three bridges share; it + ``publish_rate`` is the one constructor limit every bridge shares; it converts a hold into a message count, so an unusable value silently reshapes every later timed command. + + The bridge is constructed through the tuple's own constructor rather than a + dispatch on its label: a chain with a fallback branch quietly re-checks an + already-covered bridge when a new transport is added, which is exactly when + the check matters. """ - for label, _module, _symbol, _factory in _TRANSPORTS: + for label, _module, _symbol, ctor, _fields in _TRANSPORTS: expected = positive_finite_number_error(value, "publish_rate", "") assert expected is not None, "probe value must be outside the domain" with pytest.raises(ValueError, match="publish_rate must be > 0"): - if label == "rtps": - rtps_mod.RtpsRobot("rover", "/cmd_vel", publish_rate=value) - elif label == "rosbridge": - rosbridge_mod.RosbridgeRobot("rover", "/cmd_vel", "/odom", publish_rate=value) - else: - ros_bridge_mod.RosBridgedRobot("rover", "/cmd_vel", "/odom", publish_rate=value) + ctor(publish_rate=value) def test_every_bridge_still_publishes_a_usable_command(monkeypatch: pytest.MonkeyPatch) -> None: @@ -177,7 +206,7 @@ def test_every_bridge_still_publishes_a_usable_command(monkeypatch: pytest.Monke assert result["status"] == "success", f"{transport[0]} refused a usable command" assert rec.calls, f"{transport[0]} published nothing" assert rec.calls[0]["count"] == 3 - assert rec.calls[0]["fields"] == {"linear": {"x": 0.5}, "angular": {"z": -0.25}} + assert rec.calls[0]["fields"] == transport[4], f"{transport[0]} published other than its wire contract" held, rec_held = _drive(monkeypatch, transport, linear=0.5, duration=1.5) assert held["status"] == "success" @@ -303,14 +332,14 @@ def _drive_owning_modules() -> list[tuple[Path, ast.Module, ast.ClassDef]]: return owners -def test_the_drive_owning_bridges_are_the_three_known_ones() -> None: +def test_the_drive_owning_bridges_are_the_known_ones() -> None: """Non-vacuity: the scan finds every bridge, so the guards below apply. - A fourth transport is picked up automatically - which is the point: it is + A fifth transport is picked up automatically - which is the point: it is checked the moment it lands rather than after it has drifted. """ names = {cls.name for _path, _tree, cls in _drive_owning_modules()} - assert {"RosBridgedRobot", "RtpsRobot", "RosbridgeRobot"} <= names, names + assert {"RosBridgedRobot", "RtpsRobot", "RosbridgeRobot", "AckermannRosRobot"} <= names, names def test_every_drive_owning_bridge_calls_the_shared_numeric_domains() -> None: diff --git a/tests/test_repr_survives_partial_construction.py b/tests/test_repr_survives_partial_construction.py index fb8df280e8..e8d97f86de 100644 --- a/tests/test_repr_survives_partial_construction.py +++ b/tests/test_repr_survives_partial_construction.py @@ -49,6 +49,7 @@ import strands_robots from strands_robots.dataset_recorder import DatasetRecorder from strands_robots.hardware_rtps_bridge import HardwareRtpsBridge +from strands_robots.mesh.ackermann_robot import AckermannRosRobot from strands_robots.mesh.core import Mesh from strands_robots.mesh.input import InputPublisher, InputReceiver from strands_robots.mesh.ros_bridge import RosBridgedRobot @@ -71,6 +72,7 @@ { "dataset_recorder::DatasetRecorder", "hardware_rtps_bridge::HardwareRtpsBridge", + "mesh/ackermann_robot::AckermannRosRobot", "mesh/core::Mesh", "mesh/input::InputPublisher", "mesh/input::InputReceiver", @@ -218,6 +220,12 @@ class _Dataset: "node_name", ), (RtpsRobot, ValueError, lambda: RtpsRobot(node_name="bad name!", cmd_vel_topic="/cmd_vel"), "node_name"), + ( + AckermannRosRobot, + ValueError, + lambda: AckermannRosRobot(node_name="bad name!", servo_topic="/ctrl_pkg/servo_msg"), + "node_name", + ), (HardwareRtpsBridge, ValueError, lambda: HardwareRtpsBridge(None, domain_id=99999), "_robot_name"), ( InputPublisher,