Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
146 changes: 76 additions & 70 deletions backend/tests/fixtures/db/seed_small.sql

Large diffs are not rendered by default.

Binary file modified backend/tests/fixtures/db/seed_small_artifacts.tar.zst
Binary file not shown.
32 changes: 32 additions & 0 deletions backend/tests/unit/backends/gazebo/test_topics_schema.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,13 +3,16 @@
from __future__ import annotations

import json
import xml.etree.ElementTree as ET
from functools import cache
from pathlib import Path

import pytest
from jsonschema import Draft202012Validator

from roboprobe.scenario._documents import load_yaml_document
from roboprobe.scenario.space import ScenarioSpace
from roboprobe.scenario.task import load_task

pytestmark = pytest.mark.unit

Expand All @@ -36,3 +39,32 @@ def test_topics_yaml_matches_schema() -> None:
geometry = document["geometry"]
margin = geometry["lidar_plane_height_m"] - geometry["dropped_pallet_height_m"]
assert margin >= 0.05, "pallet top must sit at least 5 cm below the lidar plane (OQ-2)"
names = [topic["name"] for topic in document["topics"].values()]
assert len(names) == len(set(names))

task = load_task(REPO_ROOT / "tasks/measured/warehouse_delivery_measured_v1.yaml", strict=False)
pallet = REPO_ROOT / "ros_ws/src/roboprobe_ros/models/dropped_pallet/model.sdf"
model = ET.parse(pallet) # noqa: S314 - checked-in static SDF, not an external input
size = model.findtext("./model/link/collision/geometry/box/size")
assert size is not None
assert tuple(map(float, size.split())) == task.injected_obstacles[0].size
assert task.injected_obstacles[0].size[-1] == geometry["dropped_pallet_height_m"]


def test_stamped_velocity_and_depth_match_the_actual_bridge_types() -> None:
document = load_yaml_document(TOPICS_FILE.read_bytes(), source=str(TOPICS_FILE))
document["stamped_cmd_vel"] = True
assert not validator().is_valid(document)
for name in ("cmd_vel_nav", "cmd_vel"):
document["topics"][name]["type"] = "geometry_msgs/msg/TwistStamped"
validator().validate(document)
document["topics"]["depth_points_raw"]["type"] = "sensor_msgs/msg/Image"
assert not validator().is_valid(document)


def test_seeded_measured_task_versions_remain_loadable() -> None:
space_file = REPO_ROOT / "configs/scenario_spaces/warehouse_v1.yaml"
raw = load_yaml_document(space_file.read_bytes(), source=str(space_file))
archived = {**raw, "task": "tasks/measured/warehouse_delivery_measured_v1.yaml"}
historical = ScenarioSpace.from_mapping(archived, source="archived space", base_dir=REPO_ROOT)
assert historical.version == "2b4d5a96d21a"
11 changes: 8 additions & 3 deletions backend/tests/unit/cli/test_experiment_cli.py
Original file line number Diff line number Diff line change
Expand Up @@ -119,9 +119,14 @@ def test_resolve_key_marks_k2d_model_only_for_kinematic_backend() -> None:
assert _resolve_key(space, "h", "benchmark").k2d_model_version == "n/a"


def test_experiment_cli_run_then_report(
monkeypatch: pytest.MonkeyPatch, settings_env: Path, tmp_path: Path
) -> None:
@pytest.fixture
def serial_experiment_settings(monkeypatch: pytest.MonkeyPatch, settings_env: Path) -> None:
"""Limit campaign drives when the test shares one in-memory transaction."""
monkeypatch.setenv("ROBOPROBE_EXPERIMENTS__MAX_CONCURRENT_CAMPAIGNS", "1")


@pytest.mark.usefixtures("serial_experiment_settings")
def test_experiment_cli_run_then_report(monkeypatch: pytest.MonkeyPatch, tmp_path: Path) -> None:
uow = InMemoryUnitOfWork()
mod = "roboprobe.cli.experiment"
services = SimpleNamespace(
Expand Down
6 changes: 3 additions & 3 deletions backend/tests/unit/scenario/test_task.py
Original file line number Diff line number Diff line change
Expand Up @@ -147,9 +147,9 @@ def test_json_pointer_escapes_rfc_6901() -> None:


def test_find_placeholders_walks_nested_documents() -> None:
document = {"a": ["TBD_T091", {"b": "TBD_T042"}], "c": "TBD", "d": 1}
assert find_placeholders(document) == [("/a/0", "TBD_T091"), ("/a/1/b", "TBD_T042")]
assert find_placeholders({"x/y": "TBD_T091"}) == [("/x~1y", "TBD_T091")]
document = {"a": ["TBD_T001", {"b": "TBD_T042"}], "c": "TBD", "d": 1}
assert find_placeholders(document) == [("/a/0", "TBD_T001"), ("/a/1/b", "TBD_T042")]
assert find_placeholders({"x/y": "TBD_T001"}) == [("/x~1y", "TBD_T001")]


# --------------------------------------------------------------------------- nominal path
Expand Down
15 changes: 9 additions & 6 deletions docs/10-simulator-integration.md
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ flowchart LR
subgraph RP["roboprobe_ros nodes"]
SP["sensor_perturber<br/>/scan_raw to /scan"]
PD["perception_degrader<br/>depth points raw to degraded"]
CD["cmd_delay<br/>/cmd_vel_nav to /cmd_vel"]
CD["cmd_delay<br/>filtered velocity to /cmd_vel"]
GT["ground_truth_monitor<br/>pose, min distance"]
REC["episode_recorder<br/>20 Hz Parquet"]
EM["episode_manager<br/>init pose, goal, status"]
Expand All @@ -49,6 +49,7 @@ flowchart LR
PS["planner_server"]
BT["bt_navigator"]
VS["velocity_smoother"]
CM["collision_monitor"]
COST["costmaps<br/>scan + depth layers"]
end
GZ <--> BR
Expand All @@ -63,18 +64,20 @@ flowchart LR
BT --> PS
BT --> CS
CS --> VS
VS -->|"/cmd_vel_nav"| CD
VS -->|"/cmd_vel_smoothed"| CM
CM -->|"/roboprobe/cmd_vel_nav"| CD
CD -->|"/cmd_vel"| BR
GT --> REC
AMCL -->|"/amcl_pose"| REC
CD --> REC
EM --> REC
```

Exact topic names, message types (Jazzy Nav2 publishes `geometry_msgs/TwistStamped` when
`enable_stamped_cmd_vel` is true), bridge entries and the TB4 lidar/camera frames MUST be verified
recorded in `ros_ws/src/roboprobe_ros/config/topics.yaml` (validated by `schemas/ros_topics.schema.json`,
T-091 evidence in `docs/evidence/T-091`) — the code reads topic names from that file, never from literals.
Load ROS topic names, types, QoS and frame IDs from the [verified topic contract](../ros_ws/src/roboprobe_ros/config/topics.yaml).
The default Jazzy profile uses `geometry_msgs/msg/Twist`. A stamped profile must enable
`enable_stamped_cmd_vel` across Nav2's velocity pipeline and use `geometry_msgs/msg/TwistStamped`
on both sides of the delay node. The bridge converts either type to Gazebo's `gz.msgs.Twist`.
[T-091 captures and wiring](evidence/T-091/README.md)

## 4. Applying each parameter (materialisation, `GazeboMaterializer`)

Expand Down
2 changes: 1 addition & 1 deletion docs/22-roadmap-and-implementation-plan.md
Original file line number Diff line number Diff line change
Expand Up @@ -191,7 +191,7 @@ Columns: ID · title · size · depends on · implements · key docs · acceptan
| ID | Task | Size | Deps | Implements | Docs | Acceptance |
|----|------|------|------|-----------|------|-----------|
| T-090 | **Spike:** headless TB4 + Nav2 warehouse in Docker on owner's laptop; measure RTF, startup, CPU; record findings | S | T-005 | R-01 | 10 | `docs/evidence/T-090-gazebo-spike.md` |
| T-091 | Verify & record topics/frames/param names, lidar plane height, pallet height → `topics.yaml`, task file | S | T-090 | FR-SIM-003 | 10 §3, 06 §7 | no `TBD_T091` left |
| T-091 | Verify & record topics/frames/param names, lidar plane height, pallet height → `topics.yaml`, task file | S | T-090 | FR-SIM-003 | 10 §3, 06 §7 | [Live ROS and Gazebo evidence](evidence/T-091/README.md) |
| T-092 | Map + goal pose + nominal path for `warehouse_delivery_v1` | S | T-091 | FR-SIM-003 | 06 §1 | [Warehouse map alignment](evidence/T-092.md) |
| T-093 | `roboprobe_ros` nodes: sensor_perturber, perception_degrader, cmd_delay | M | T-031, T-091 | FR-SIM-003 | 10 §3–4 | node unit tests (launch_testing) |
| T-094 | ground_truth_monitor, episode_recorder, episode_manager, episode.launch.py | L | T-093 | FR-SIM-003 | 10 §5–6 | single episode produces trace |
Expand Down
2 changes: 1 addition & 1 deletion docs/26-product-requirements-document.md
Original file line number Diff line number Diff line change
Expand Up @@ -198,7 +198,7 @@ A1–A4 from SRS §2.6; the owner can dedicate several nights of laptop time to
| ID | Question | Owner | Due | Default if unresolved |
|----|----------|-------|-----|------------------------|
| OQ-1 | Exact goal pose / route in warehouse world | Agent (T-092) | M9 | pick longest single-aisle route ≥ 12 m |
| OQ-2 | TB4 lidar plane height ⇒ pallet height | Agent (T-091) | M9 | **Resolved:** 0.194 m plane, 0.144 m pallet (`topics.yaml` geometry) |
| OQ-2 | TB4 lidar plane height ⇒ pallet height | Agent (T-091) | M9 | **Resolved:** 0.188 m plane, 0.135 m pallet (`topics.yaml` geometry) |
| OQ-3 | Laptop specs ⇒ Gazebo parallelism | Owner (T-090) | M2 | 1 worker |
| OQ-4 | Is S-CEM needed for the evaluation? | Owner | M5 | skip unless time allows |
| OQ-5 | Nav2 default controller for TB4 on Jazzy (MPPI vs others) and its speed param keys | Agent (T-091) | M9 | **Resolved:** MPPI `FollowPath.vx_max` + `velocity_smoother.max_velocity[0]` |
Expand Down
60 changes: 27 additions & 33 deletions docs/evidence/T-091/README.md
Original file line number Diff line number Diff line change
@@ -1,48 +1,42 @@
# T-091 — TB4 Nav2 topic and geometry verification
# T-091 — TurtleBot 4 ROS and Gazebo interfaces

## Outcome
## Environment and repeatable capture

Recorded the Jazzy `nav2_minimal_tb4_sim` topic graph, Nav2 MPPI speed parameter paths, TF frames, and lidar/pallet geometry used in `ros_ws/src/roboprobe_ros/config/topics.yaml` and `tasks/warehouse_delivery_v1.yaml`.
The live warehouse used the [T-090 image](../T-090-gazebo-spike.md) on arm64 with ROS 2 Jazzy, Gazebo Harmonic 8.15.0, Nav2 1.3.14 and `nav2_minimal_tb4_sim` 1.0.1. Capture from the same ROS domain and Gazebo partition after Nav2 reaches ACTIVE. Mount the checkout in the container, start the warehouse as in T-090, and run the capture script in a second shell with the same ROS environment.

## Method
```bash
source /opt/ros/jazzy/setup.bash
ros2 lifecycle get /bt_navigator
bash scripts/ros_introspect.sh /tmp/t091
```

Upstream package sources (pinned to the same stack as `spikes/T-090/Dockerfile`):
The committed capture in [default](default/ros-topics.txt) records the stock endpoints, verbose QoS, six parameter dumps, the TF frame graph and the sensor declarations. Its [stock bridge config](default/stock-bridge.yaml) is from the installed Apache-2.0 Nav2 simulation package. Live timestamps and DDS participant IDs vary by capture. [Upstream Nav2 packages](https://github.com/ros-navigation/navigation2)

- `ros-navigation/navigation2` `@jazzy` — `nav2_bringup/params/nav2_params.yaml`, `tb4_simulation_launch.py`
- `ros-navigation/nav2_minimal_turtlebot_simulation` `@main` — `nav2_minimal_tb4_sim/configs/tb4_bridge.yaml`, `spawn_tb4.launch.py`, `nav2_minimal_tb4_description/urdf/**`
## Sensor geometry

Cross-checks from [`docs/evidence/T-090-gazebo-spike.md`](../T-090-gazebo-spike.md): `enable_stamped_cmd_vel=False`, depth on `/rgbd_camera/depth_image`.
The laser scan's ROS header names `rplidar_link`. The TF chain is `map → odom → base_link → shell_link → rplidar_link`. The pinned Xacro gives 0.030 m for the shell offset, 0.0642 m for the chassis offset and 0.098715 m for the sensor's offset from the shell, for a base-to-laser z offset of 0.192915 m. Live TF showed 0.193 m when rounded. The settled robot's world z was −0.00445 m, with pitch −0.004396 rad, so the scan plane was about 0.188 m above the floor at the sensor.

Reproduce a live capture inside the T-090 image:
The stock depth image and the Gazebo point cloud both use `oakd_rgb_camera_optical_frame`. The project's [dropped pallet](../../../ros_ws/src/roboprobe_ros/models/dropped_pallet/model.sdf) uses a 0.8 × 0.6 × 0.135 m collision box. It spawned in the live warehouse with its center at z = 0.0675 m, putting its top 0.053 m below the measured scan plane. The stock world includes its own warehouse assets. The injected pallet uses this repository's simpler box geometry as specified in the [task](../../../tasks/measured/warehouse_delivery_measured_v1.yaml).

```bash
docker build -t roboprobe-t090-local:jazzy spikes/T-090
docker run --rm -v "$PWD:/repo" -w /repo -e ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST \
roboprobe-t090-local:jazzy bash -lc 'source /opt/ros/jazzy/setup.bash && ./scripts/ros_introspect.sh docs/evidence/T-091/capture'
```
The regenerated demo fixture pins the measured task file so its scenario version and pallet geometry remain available after later warehouse revisions.

## ROS endpoint and bridge contract

## Geometry
The [topic contract](../../../ros_ws/src/roboprobe_ros/config/topics.yaml) uses the pinned ROS types and routes the custom inputs separately from the outputs produced by RoboProbe. The following mappings were verified with a real Jazzy bridge.

| Quantity | Value (m) | Derivation |
|----------|-----------|------------|
| Lidar plane | 0.194 | `base_link_z_offset` + `shell_z_offset` + `rplidar_z_offset` from minimal TB4 URDF + spawn `z_pose=0.01` |
| Dropped pallet height | 0.144 | EUR pallet; top ≈ 4.9 cm below lidar plane (OQ-2) |
| Gazebo topic and type | ROS topic and type | Direction |
|-----------------------|--------------------|-----------|
| `/scan`, `gz.msgs.LaserScan` | `/scan_raw`, `sensor_msgs/msg/LaserScan` | Gazebo to ROS |
| `/rgbd_camera/points`, `gz.msgs.PointCloudPacked` | `/rgbd_camera/points_raw`, `sensor_msgs/msg/PointCloud2` | Gazebo to ROS |
| `/world/warehouse/model/turtlebot4/pose`, `gz.msgs.Pose` | `/ground_truth_pose`, `geometry_msgs/msg/PoseStamped` | Gazebo to ROS |
| `/cmd_vel`, `gz.msgs.Twist` | `/cmd_vel`, `geometry_msgs/msg/Twist` | ROS to Gazebo |

## RoboProbe intercept topics
Use the model-scoped Gazebo `PosePublisher` with `publish_model_pose=true`, `publish_link_pose=false` and `use_pose_vector_msg=false` for the ground-truth row. The global pose vector also contains other entities. Preserve the stock `/clock`, `/tf`, `/odom`, joint state and IMU bridges. [Gazebo PosePublisher](https://gazebosim.org/api/sim/8/classgz_1_1sim_1_1systems_1_1PosePublisher.html)

| Upstream (verified) | RoboProbe episode (T-093+) |
|---------------------|------------------------------|
| `/scan` from `tb4_bridge.yaml` | Bridge targets `/scan_raw`; perturber publishes `/scan` |
| `/cmd_vel` from velocity smoother | Smoother → `/cmd_vel_nav`; `cmd_delay` → `/cmd_vel` |
| `/rgbd_camera/depth_image` | `depth_image_proc` → `/rgbd_camera/depth/points`; degrader → `depth_points` |
Stock Nav2 sends controller and behavior commands through `/cmd_vel_nav` to the velocity smoother, then through `/cmd_vel_smoothed` to the collision monitor. Route the collision monitor's output and docking commands to `/roboprobe/cmd_vel_nav` before the RoboProbe delay node. The delay node publishes `/cmd_vel` for the robot bridge. Sensor and point inputs use the sensor-data profile, velocity and odometry use reliable volatile delivery, and AMCL pose uses reliable transient-local delivery.

`ground_truth_pose` (`/ground_truth/pose`) is bridged from the Gazebo model pose in `episode.launch.py` (T-094d).
The stock Jazzy graph used plain `Twist`. A separate bridge probe verified that the installed bridge also accepted a `TwistStamped` message and emitted its linear and angular components as `gz.msgs.Twist`. When enabling stamped velocity, use the same message type through the entire Nav2 velocity pipeline and the bridge. [Jazzy Nav2 configuration](https://docs.nav2.org/jazzy/configuration_and_development/migration_guides/iron/Iron/)

## Artefacts
## Nav2 parameters

| File | Contents |
|------|----------|
| `tb4_bridge.yaml` | Copy of upstream bridge configuration |
| `nav2_speed_param_paths.txt` | MPPI `vx_max` and velocity-smoother keys |
| `nav2_controller_flags.txt` | `enable_stamped_cmd_vel` and `use_sim_time` |
| `tb4_lidar_offsets.txt` | URDF constants for lidar height |
The [controller dump](default/params-controller_server.yaml) identifies the MPPI plugin and `FollowPath.vx_max=0.5`. The [smoother dump](default/params-velocity_smoother.yaml) records `max_velocity=[0.5, 0.0, 2.0]`. P1 updates the controller limit and the first smoother component through the [shipped profile](../../../configs/sut_profiles/tb4_nav2_default.yaml). The pinned bringup package's alternative DWB parameter files use `FollowPath.max_vel_x` (0.26 m/s in its multirobot example). The warehouse profile uses MPPI.
46 changes: 46 additions & 0 deletions docs/evidence/T-091/bridge-live.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,46 @@
Publishers [Address, Message Type]:
tcp://172.17.0.3:38251, gz.msgs.Pose
No subscribers on topic [/world/warehouse/model/turtlebot4/pose]

# Actual bridge message headers
/scan_raw
stamp:
sec: 242
nanosec: 1000000
frame_id: rplidar_link
---
/rgbd_camera/points_raw
stamp:
sec: 243
nanosec: 102000000
frame_id: oakd_rgb_camera_optical_frame
---
/ground_truth_pose
stamp:
sec: 244
nanosec: 263000000
frame_id: warehouse
---

# Actual stamped velocity conversion
Publishers [Address, Message Type]:
tcp://172.17.0.3:43257, gz.msgs.Twist
tcp://172.17.0.3:42223, gz.msgs.Twist
No subscribers on topic [/roboprobe/introspect_cmd_vel]
publisher: beginning loop
publishing #1: geometry_msgs.msg.TwistStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id='base_link'), twist=geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.123, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=-0.25)))

header {
stamp {
}
data {
key: "frame_id"
value: "base_link"
}
}
linear {
x: 0.123
}
angular {
z: -0.25
}
25 changes: 25 additions & 0 deletions docs/evidence/T-091/default/gazebo-topics.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
/clock
/cmd_vel
/gazebo/resource_paths
/imu
/joint_states
/odom
/rgbd_camera/camera_info
/rgbd_camera/depth_image
/rgbd_camera/image
/rgbd_camera/points
/scan
/scan/points
/sensors/marker
/stats
/tf
/world/warehouse/clock
/world/warehouse/dynamic_pose/info
/world/warehouse/pose/info
/world/warehouse/scene/deletion
/world/warehouse/scene/info
/world/warehouse/state
/world/warehouse/stats
/model/turtlebot4/enable
/world/warehouse/light_config
/world/warehouse/material_color
77 changes: 77 additions & 0 deletions docs/evidence/T-091/default/params-amcl.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,77 @@
/amcl:
ros__parameters:
/bond_disable_heartbeat_timeout: true
alpha1: 0.2
alpha2: 0.2
alpha3: 0.2
alpha4: 0.2
alpha5: 0.2
always_reset_initial_pose: false
autostart_node: false
base_frame_id: base_footprint
beam_skip_distance: 0.5
beam_skip_error_threshold: 0.9
beam_skip_threshold: 0.3
bond_heartbeat_period: 0.1
do_beamskip: false
first_map_only: false
freespace_downsampling: false
global_frame_id: map
initial_pose:
x: 0.0
y: 0.0
yaw: 0.0
z: 0.0
lambda_short: 0.1
laser_likelihood_max_dist: 2.0
laser_max_range: 100.0
laser_min_range: -1.0
laser_model_type: likelihood_field
map_topic: map
max_beams: 60
max_particles: 2000
min_particles: 500
odom_frame_id: odom
pf_err: 0.05
pf_z: 0.99
qos_overrides:
/clock:
subscription:
depth: 1
durability: volatile
history: keep_last
reliability: best_effort
/tf:
publisher:
depth: 100
durability: volatile
history: keep_last
reliability: reliable
subscription:
depth: 100
durability: volatile
history: keep_last
reliability: reliable
/tf_static:
subscription:
depth: 100
history: keep_last
reliability: reliable
recovery_alpha_fast: 0.0
recovery_alpha_slow: 0.0
resample_interval: 1
robot_model_type: nav2_amcl::DifferentialMotionModel
save_pose_rate: 0.5
scan_topic: scan
set_initial_pose: false
sigma_hit: 0.2
start_type_description_service: true
tf_broadcast: true
transform_tolerance: 1.0
update_min_a: 0.2
update_min_d: 0.25
use_sim_time: true
z_hit: 0.5
z_max: 0.05
z_rand: 0.5
z_short: 0.05
Loading
Loading