diff --git a/.agents/skills/run-system-tests/SKILL.md b/.agents/skills/run-system-tests/SKILL.md index d44b5ba3d..868d8c695 100644 --- a/.agents/skills/run-system-tests/SKILL.md +++ b/.agents/skills/run-system-tests/SKILL.md @@ -1,6 +1,6 @@ --- name: run-system-tests -description: Run, interpret, and extend AirStack's pytest system test suite (build_packages, build_docker, liveliness, takeoff_hover_land), trigger runs via /pytest PR comments, and read metrics.json regression reports. Use for invoking tests, debugging failures from results.xml/metrics.json, or adding a new system test. +description: Run, interpret, and extend AirStack's pytest system test suite (build_packages, build_docker, liveliness, sensors, takeoff_hover_land), trigger runs via /pytest PR comments, and read metrics.json regression reports. Use for invoking tests, debugging failures from results.xml/metrics.json, or adding a new system test. license: Apache-2.0 metadata: author: AirLab CMU @@ -22,32 +22,53 @@ This skill is about the **test harness itself** — pytest marks, fixtures, the ## Test Suite Overview -The suite lives at `tests/` (repo root) and is fully pytest-based. Configuration is in `tests/pytest.ini` and shared infrastructure in `tests/conftest.py`. There are exactly four marks today: +The suite lives at `tests/` (repo root) and is fully pytest-based. Configuration is in `tests/pytest.ini` and shared infrastructure in `tests/conftest.py`. Marks include `build_docker`, `build_packages`, `liveliness`, `sensors`, and `takeoff_hover_land`: | File | Mark | What it tests | Hardware required | |------|------|---------------|-------------------| | `tests/test_build_docker.py` | `build_docker` | `airstack image-build` for `robot-desktop`, `gcs`, `isaac-sim`, `ms-airsim`; records image size to `metrics.json` | Docker daemon | | `tests/test_build_packages.py` | `build_packages` | `colcon build` (`bws`) inside the robot, GCS, and ms-airsim ROS workspaces — brought up with `AUTOLAUNCH=false` | Docker daemon | -| `tests/test_liveliness.py` | `liveliness` | Full stack up, container Running state, tmux pane survival, sentinel ROS 2 nodes (mavros, robot_state_publisher, trajectory_control_node), sim topic Hz, compute usage, sustained `test_stable` polling window, sim realtime factor | Docker daemon, NVIDIA GPU + `nvidia-container-toolkit`, sim license / Omniverse creds | +| `tests/test_liveliness.py` | `liveliness` | Stack bring-up: containers Running, `/clock` readiness, tmux panes, sentinel ROS 2 nodes, compute, infra-only `test_stable` | Docker daemon, NVIDIA GPU + `nvidia-container-toolkit`, sim license / Omniverse creds | +| `tests/test_sensors.py` | `sensors` | Topic Hz (Isaac: batched on sim + robot; LiDAR `echo-once` + cloud sanity), RTF, `test_sensor_streams_stable` | Docker daemon, NVIDIA GPU + `nvidia-container-toolkit`, sim license / Omniverse creds | | `tests/test_takeoff_hover_land.py` | `takeoff_hover_land` | 4-phase flight chain per `(sim, num_robots, iteration, velocity)`: `test_px4_ready` → `test_takeoff` → `test_hover` → `test_landing`. Records altitude error, overshoot, hover stability, landing accuracy, odometry drift | Docker daemon, NVIDIA GPU, sim license | -The four marks are declared in `tests/pytest.ini`. **Do not invent new marks ad-hoc** — register any new mark there or pytest will warn about unknown marks. +The marks are declared in `tests/pytest.ini`. **Do not invent new marks ad-hoc** — register any new mark there or pytest will warn about unknown marks. ### Test ordering (set by `pytest_collection_modifyitems`) `conftest.py` enforces a deterministic global order so cheap-and-fast-failing tests surface first: ``` -test_build_docker → test_build_packages → test_liveliness → test_takeoff_hover_land +test_build_docker → test_build_packages → test_liveliness → test_sensors → test_takeoff_hover_land ``` Within `test_takeoff_hover_land`, items are re-sorted to `(airstack_env, velocity, phase)` so each `(sim, robots, iter)` env brings the stack up once and the drone goes ground → air → ground per velocity before pytest moves to the next velocity. +### Isaac Sim (`sensors`): why Hz is batched and LiDAR uses `echo --once` + +[`tests/sensor_probes.py`](../../../tests/sensor_probes.py) implements the `sensors` +mark. Pegasus / OmniGraph ROS bridges and the sim→robot path can stop reporting +rates if too many `ros2 topic hz` processes run concurrently. + +- **Sim-side (Isaac):** three `parallel_sample_hz` passes — `/clock`, then both + `image_rect` topics, then both `depth_ground_truth` topics. +- **Robot-side (Isaac):** two passes — both stereo images, then both depths. + **ms-airsim** keeps a single four-topic parallel batch on the robot container. +- **Filtered LiDAR** (`PointCloud2`): uses `ros2 topic echo --once` per robot + (see `parallel_echo_once_robot_topics` in `conftest.py`), not `topic hz`. +- **Multi-drone Pegasus script:** pytest sets `ENABLE_LIDAR=true` in + `conftest.py` `SIM_CONFIG["isaacsim"]["extra_env"]` so LiDAR matches the + single-drone example (which always enables RTX LiDAR). + +User-facing write-up: [`tests/README.md`](../../../tests/README.md) (section +*Isaac Sim and the sensors mark*). + ### `build_packages` is auto-prepended in CI The `system-tests.yml` workflow's `Parse pytest args` step automatically prepends `build_packages` to the marks expression whenever the user specifies any marks (and `build_packages` isn't already in the expression). For example: - `/pytest -m liveliness` → effectively runs `-m "build_packages or liveliness"` +- `/pytest -m sensors` → effectively runs `-m "build_packages or sensors"` - `/pytest -m takeoff_hover_land` → effectively runs `-m "build_packages or takeoff_hover_land"` - `/pytest` (no marks) → pytest defaults (everything) - `/pytest -m build_docker` → unchanged (the build_docker tests rebuild from scratch anyway) @@ -74,6 +95,14 @@ airstack test -m liveliness \ --stable-duration 60 \ -v +# Sensors (sim + robot Hz, LiDAR, RTF) — Isaac example; runs after liveliness in collection order +airstack test -m sensors \ + --sim isaacsim \ + --num-robots 1 \ + --stress-iterations 1 \ + --stable-duration 60 \ + -v + # Takeoff/hover/land — sweep three velocities airstack test -m takeoff_hover_land \ --sim msairsim \ @@ -105,8 +134,8 @@ The `airstack_env` fixture is parametrized over `(sim, num_robots, iteration)` t | `--sim` | `msairsim,isaacsim` | `airstack_env` | One env-tuple per sim | | `--num-robots` | `1,3` | `airstack_env` | Cross-product with sim | | `--stress-iterations` | `1` | `airstack_env` | Up/down cycles per `(sim, num_robots)` | -| `--stable-duration` | `120` | `test_liveliness::test_stable` | Total seconds polled | -| `--stable-interval` | `10` | `test_liveliness::test_stable` | Seconds between polls | +| `--stable-duration` | `120` | `test_liveliness::test_stable` and `test_sensors::test_sensor_streams_stable` | Total seconds polled | +| `--stable-interval` | `10` | `test_liveliness::test_stable` and `test_sensors::test_sensor_streams_stable` | Seconds between polls | | `--gui` | off (headless) | `airstack_env` | Sets `QT_QPA_PLATFORM=offscreen` when off | | `--takeoff-velocities` | `0.5` (current default) | `test_takeoff_hover_land` | One full 4-phase chain per velocity | @@ -115,7 +144,7 @@ Total parametrize cardinality for sim tests = `len(sims) × len(num_robots) × s ### Prerequisites - Docker daemon running, your user in the `docker` group -- For `liveliness` / `takeoff_hover_land`: NVIDIA driver + `nvidia-container-toolkit` +- For `liveliness` / `sensors` / `takeoff_hover_land`: NVIDIA driver + `nvidia-container-toolkit` - For `isaacsim`: `simulation/isaac-sim/docker/omni_pass.env` populated with Omniverse credentials (CI generates a `guest`/`guest` version automatically) - `airstack setup` already run so `airstack` is on `PATH` - All required compose images present locally — `airstack_env` calls `missing_images()` and fails fast otherwise. Build them first via `airstack test -m build_docker` or `airstack image-build `. @@ -170,7 +199,8 @@ tests/results/2025-04-21_14-30-00/ ├── metrics.json # Custom metrics keyed by test_node_id → metric_key └── logs/ ├── test_build_docker.TestDockerBuilds.test_build_robot_desktop.log - ├── test_liveliness.TestLiveliness.test_stable[msairsim-rob#1-iter0].log + ├── test_sensors.TestSensors.test_sensor_streams_stable[msairsim-rob#1-iter0].log + ├── test_liveliness.TestLiveliness.test_stable[msairsim-rob#1-iter0].log ├── airstack_env.test_liveliness.TestLiveliness.test_robot_containers_running[...].log └── ... ``` @@ -213,7 +243,7 @@ python tests/parse_metrics.py \ The report has three sections per test module: - **Metrics** — flat scalar metrics (test, key, current, baseline, change%) -- **Sim publishing rates** — pivoted Hz aggregates per topic (`mean`, `start_mean`, `end_mean`, `min`, `max`) +- **Sim publishing rates** — pivoted Hz aggregates per topic (`mean`, `start_mean`, `end_mean`, `min`, `max`) from the `sensors` mark (sim + robot streams) - **Compute usage** — pivoted CPU/mem/GPU per container Regressions exceeding `--threshold` (default 20%) are flagged `:red_circle:`; improvements beyond threshold get `:green_circle:`. CI fails the job on any regression. @@ -230,7 +260,7 @@ If your test... - Builds a Docker image → reuse `build_docker` - Builds a colcon workspace → reuse `build_packages` -- Verifies the running stack → reuse `liveliness` +- Verifies the running stack → `liveliness` (infra); sensor topic rates / LiDAR / RTF → `sensors` - Drives the autonomy stack to fly → reuse `takeoff_hover_land` - Doesn't fit any of these → **register a new mark in `tests/pytest.ini`** before using it. Update the table in `tests/README.md` and the AGENTS.md "System Test Suite" table at the same time. @@ -296,11 +326,11 @@ If multiple tests need the same setup, add a fixture in `conftest.py` (not in yo ## Common Pitfalls - **Forgetting `build_packages`**. If you run `-m liveliness` locally on a fresh checkout, the workspace inside the container is empty and sentinel nodes won't appear. Either run `-m "build_packages or liveliness"` or rely on the CI auto-prepend. -- **Mixing marks unintentionally**. `-m "liveliness or takeoff_hover_land"` brings the stack up multiple times (once per parametrize tuple per mark). Combine deliberately, not by reflex. -- **Running on insufficient hardware**. `liveliness` and `takeoff_hover_land` require an NVIDIA GPU plus nvidia-container-toolkit; without them the sim container won't get GPU access and topic Hz checks will time out. If you only have a CPU, scope to `-m "build_docker or build_packages"`. +- **Mixing marks unintentionally**. `-m "liveliness or takeoff_hover_land"` brings the stack up once per selected mark's test classes (per parametrization). `-m "liveliness or sensors"` runs **both** classes for each tuple — **two** full ``airstack up`` / ``down`` cycles per `(sim, robots, iter)` because ``airstack_env`` is class-scoped. Combine deliberately, not by reflex. +- **Running on insufficient hardware**. `liveliness`, `sensors`, and `takeoff_hover_land` require an NVIDIA GPU plus nvidia-container-toolkit; without them the sim container won't get GPU access and topic Hz checks will time out. If you only have a CPU, scope to `-m "build_docker or build_packages"`. - **Expecting interactive sim feedback**. `airstack_env` runs headless by default (`MS_AIRSIM_HEADLESS=true`, `ISAAC_SIM_HEADLESS=true`, `QT_QPA_PLATFORM=offscreen`). Don't add stdin prompts, GUI dialogs, or `input()` calls to test code — they will hang in CI. For local visual debugging only, pass `--gui`. - **Not capturing metrics in a new test**. If a test fails silently (no metric recorded) the regression report has nothing to compare. Always record at least one scalar via `MetricsRecorder` so the test shows up in `metrics.json`. -- **Letting parametrize cardinality explode**. Defaults `--sim msairsim,isaacsim --num-robots 1,3` with `--stress-iterations 3` is 12 stack up/downs per liveliness test — expensive. Override locally to a single tuple while iterating. +- **Letting parametrize cardinality explode**. Defaults `--sim msairsim,isaacsim --num-robots 1,3` with `--stress-iterations 3` multiply stack bring-ups for each selected mark (`liveliness`, `sensors`, `takeoff_hover_land`, …) — expensive. Override locally to a single tuple while iterating. - **Hardcoded container names**. Always use `find_container`, `get_robot_containers`, or `wait_for_container` — replica suffixes (`-1`, `-2`, `-3`) and compose project prefixes change. - **Asserting on stdout instead of using `read_log_tail`**. The conftest tees subprocess output to per-test log files; assertions should reference those logs (`f"airstack up failed:\n{read_log_tail()}"`) so failures attach the relevant context to the JUnit XML. - **Trying to SSH into a CI runner mid-job**. Workers are ephemeral OpenStack VMs destroyed within ~30s of job completion. Re-running the job creates a fresh VM. For genuine debugging on the runner, see `.github/orchestrator/README.md` (also exposed at `tests/ci-cd-orchestrator.md`) — but in 99% of cases, reproduce locally with `airstack test`. @@ -318,6 +348,10 @@ airstack test -m "build_docker or build_packages" -v airstack test -m liveliness --sim msairsim --num-robots 1 \ --stress-iterations 1 --stable-duration 60 -v +# Single-config sensors (Isaac topic Hz + LiDAR; see tests/README § Isaac) +airstack test -m sensors --sim isaacsim --num-robots 1 \ + --stress-iterations 1 --stable-duration 60 -v + # Full takeoff/hover/land sweep with three velocities airstack test -m takeoff_hover_land --sim msairsim --num-robots 1 \ --stress-iterations 1 --takeoff-velocities 0.5,1,2 -v @@ -342,6 +376,7 @@ python tests/parse_metrics.py \ ``` /pytest /pytest -m liveliness --sim msairsim --num-robots 1 +/pytest -m sensors --sim isaacsim --num-robots 1 /pytest -m takeoff_hover_land --takeoff-velocities 0.5,1 /pytest -m "build_docker or build_packages" ``` @@ -354,8 +389,9 @@ python tests/parse_metrics.py \ |---------------|---------------------| | Smoke-test image + workspace builds | `-m "build_docker or build_packages"` | | Verify the stack comes up clean | `-m liveliness` | +| Verify sim + robot sensor streams (Hz, LiDAR, RTF) | `-m sensors` | | Verify autonomy can fly the drone | `-m takeoff_hover_land` | -| Full PR validation (CI default for manual dispatch) | `-m "liveliness or takeoff_hover_land"` (CI auto-prepends `build_packages`) | +| Full PR validation (CI default for manual dispatch) | `-m "liveliness or takeoff_hover_land"` (CI auto-prepends `build_packages`). Add `or sensors` when you need topic-rate regression signal. | | Run literally everything | omit `-m` | ### Files to know diff --git a/.agents/skills/use-airstack-cli/SKILL.md b/.agents/skills/use-airstack-cli/SKILL.md index 3739e5db0..7fe7d9374 100644 --- a/.agents/skills/use-airstack-cli/SKILL.md +++ b/.agents/skills/use-airstack-cli/SKILL.md @@ -268,11 +268,16 @@ The system test suite (pytest, runs against the full Docker stack) is invoked th ```bash airstack test -m "build_docker or build_packages" -v airstack test -m liveliness --sim msairsim --num-robots 1 -v +airstack test -m sensors --sim isaacsim --num-robots 1 -v # topic Hz + LiDAR (after liveliness if both selected) airstack test -m takeoff_hover_land --sim msairsim --takeoff-velocities 0.5,1,2 -v ``` -For full details on the test fixtures, marks, and metrics reporting, see the -`test-in-simulation` skill and `tests/README.md`. +For full details on **pytest system tests** (fixtures, marks, `liveliness` vs +`sensors`, Isaac Hz batching, metrics, `/pytest` CI), see the +[`run-system-tests`](../run-system-tests/SKILL.md) skill and +[`tests/README.md`](../../../tests/README.md). For **authoring and running +scenarios inside Isaac Sim or AirSim** (missions, RViz checks, scene tweaks), see +the [`test-in-simulation`](../test-in-simulation/SKILL.md) skill. ### Lint and format @@ -377,7 +382,8 @@ docker logs -f airstack-robot-desktop-1 2>&1 | grep -iE "error|fail" # ---- Other ---- airstack docs # Build + serve MkDocs -airstack test -m liveliness -v # Run system tests +airstack test -m liveliness -v # Stack infra tests +airstack test -m sensors -v # Sensor topic + LiDAR tests (Isaac batching — see tests/README) airstack lint # Lint airstack format # Format ``` diff --git a/.agents/skills/write-isaac-sim-scene/SKILL.md b/.agents/skills/write-isaac-sim-scene/SKILL.md index 47c93ff4c..d8df17c68 100644 --- a/.agents/skills/write-isaac-sim-scene/SKILL.md +++ b/.agents/skills/write-isaac-sim-scene/SKILL.md @@ -102,7 +102,7 @@ from pegasus.simulator.params import SIMULATION_ENVIRONMENTS, ROBOTS from pegasus.simulator.logic.interface.pegasus_interface import PegasusInterface from pegasus.simulator.ogn.api.spawn_multirotor import spawn_px4_multirotor_node from pegasus.simulator.ogn.api.spawn_zed_camera import add_zed_stereo_camera_subgraph -from pegasus.simulator.ogn.api.spawn_ouster_lidar import add_ouster_lidar_subgraph +from pegasus.simulator.ogn.api.spawn_rtx_lidar import add_rtx_lidar_subgraph from pegasus.simulator.logic.vehicles.multirotor import Multirotor, MultirotorConfig from pegasus.simulator.logic.state import State from pegasus.simulator.logic.backends.px4_mavlink_backend import ( @@ -339,9 +339,10 @@ class YourSceneApp: # Add sensors if sensors.get("camera", False): self._add_camera_sensor(vehicle) - - if sensors.get("lidar", False): - self._add_lidar_sensor(vehicle) + + # RTX LiDAR uses OmniGraph: spawn_px4_multirotor_node() returns graph_handle, + # then call self._add_lidar_sensor(vehicle, graph_handle). See + # example_one_px4_pegasus_launch_script.py for the full pattern. # Initialize vehicle in world self.world.scene.add(vehicle) @@ -362,16 +363,16 @@ class YourSceneApp: } ) - def _add_lidar_sensor(self, vehicle): - """Add LiDAR sensor to vehicle.""" - add_ouster_lidar_subgraph( - lidar_prim_path=vehicle.prim_path + "/OusterLidar", - parent_prim_path=vehicle.prim_path, - config={ - "graph_evaluator": "execution", - "position": (0.0, 0.0, -0.15), # Relative to vehicle - "orientation": (0.0, 0.0, 0.0, 1.0), - } + def _add_lidar_sensor(self, vehicle, graph_handle): + """Add RTX LiDAR (OmniGraph subgraph) to vehicle.""" + add_rtx_lidar_subgraph( + parent_graph_handle=graph_handle, + drone_prim=vehicle.prim_path, + robot_name="robot_1", + lidar_config="ouster_os1", + lidar_offset=[0.0, 0.0, 0.025], + lidar_rotation_offset=[0.0, 0.0, 0.0], + min_range=0.75, ) def run(self): diff --git a/.env b/.env index bd9e20f3e..63c6f9101 100644 --- a/.env +++ b/.env @@ -12,7 +12,7 @@ PROJECT_NAME="airstack" # If you've run ./airstack.sh setup, then this will auto-generate from the git commit hash every time a change is made # to a Dockerfile or docker-compose.yaml file. Otherwise this can also be set explicitly to make a release version. # auto-generated from git commit hash -VERSION="0.18.0-alpha.8" +VERSION="0.18.0-alpha.9" # Choose "dev" or "prebuilt". "dev" is for mounted code that must be built live. "prebuilt" is for built ros_ws baked into the image DOCKER_IMAGE_BUILD_MODE="dev" # Where to push and pull images from. Can replace with your docker hub username if using docker hub. diff --git a/AGENTS.md b/AGENTS.md index 952f0ab68..3d6bd7370 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -202,13 +202,14 @@ docker exec airstack-robot-desktop-1 bash -c "ros2 topic echo --onc ### System Test Suite (`tests/`) -Pytest-based system tests live at the repo root in [`tests/`](tests/). They bring up the full Docker stack (sim + robot + GCS) and verify container health, ROS 2 node presence, sensor publishing rates, compute usage, and end-to-end flight behavior. +Pytest-based system tests live at the repo root in [`tests/`](tests/). They bring up the full Docker stack (sim + robot + GCS) and verify container health, ROS 2 node presence, compute usage, sensor topic streams (``sensors`` mark), and end-to-end flight behavior. | File | Mark | What it tests | Hardware | |------|------|---------------|----------| | [`tests/test_build_docker.py`](tests/test_build_docker.py) | `build_docker` | Docker image builds (robot-desktop, gcs, isaac-sim, ms-airsim) | Docker | | [`tests/test_build_packages.py`](tests/test_build_packages.py) | `build_packages` | `colcon build` inside each container | Docker | -| [`tests/test_liveliness.py`](tests/test_liveliness.py) | `liveliness` | Full-stack health: containers, tmux, ROS 2 nodes, topic Hz, compute, sustained stability | Docker, GPU, sim license | +| [`tests/test_liveliness.py`](tests/test_liveliness.py) | `liveliness` | Stack bring-up: containers, ``/clock`` readiness, tmux, sentinel ROS 2 nodes, compute, infra-only stability poll | Docker, GPU, sim license | +| [`tests/test_sensors.py`](tests/test_sensors.py) | `sensors` | Topic Hz (Isaac: batched sim + robot ``ros2 topic hz``; filtered LiDAR ``echo-once`` + validation script), RTF, sensor stability time-series | Docker, GPU, sim license | | [`tests/test_takeoff_hover_land.py`](tests/test_takeoff_hover_land.py) | `takeoff_hover_land` | 4-phase flight chain (PX4 ready → takeoff → hover → land) per (sim, num_robots, iter, velocity) | Docker, GPU, sim license | Shared fixtures, the `airstack_env` parametrized fixture, and `MetricsRecorder` live in [`tests/conftest.py`](tests/conftest.py). Each run produces a timestamped directory under `tests/results//` with `results.xml`, `metrics.json`, and per-test logs. [`tests/parse_metrics.py`](tests/parse_metrics.py) generates a markdown report (single-run or diff-vs-baseline; exits 1 on regression). @@ -218,10 +219,14 @@ Shared fixtures, the `airstack_env` parametrized fixture, and `MetricsRecorder` ```bash airstack test -m "build_docker or build_packages" -v airstack test -m liveliness --sim msairsim --num-robots 1 --stress-iterations 1 -v +airstack test -m sensors --sim isaacsim --num-robots 1 --stress-iterations 1 -v airstack test -m takeoff_hover_land --sim msairsim --takeoff-velocities 0.5,1,2 -v ``` -Full reference: [`tests/README.md`](tests/README.md). +Full reference: [`tests/README.md`](tests/README.md) — including **liveliness vs +sensors** (infra vs topic streams), **class-scoped `airstack_env`** (two bring-ups +when you select both marks with `and`), and **Isaac Sim** batching of +`ros2 topic hz` plus LiDAR `echo-once` / `ENABLE_LIDAR` for pytest. ### Autonomous Debugging Approach When a module doesn't work: diff --git a/common/ros_packages/desktop_bringup/rviz/robot.rviz b/common/ros_packages/desktop_bringup/rviz/robot.rviz index b6c494c98..15b6cecd9 100644 --- a/common/ros_packages/desktop_bringup/rviz/robot.rviz +++ b/common/ros_packages/desktop_bringup/rviz/robot.rviz @@ -74,7 +74,7 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: false - OS1_REV6_128_10hz___512_resolution: + lidar_mount: Value: false base_link: Value: true @@ -92,7 +92,7 @@ Visualization Manager: Value: true imu: Value: false - lidar: + ouster: Value: false look_ahead_point: Value: true @@ -130,8 +130,8 @@ Visualization Manager: map: base_link: base_link_body_body_link: - OS1_REV6_128_10hz___512_resolution: - lidar: + lidar_mount: + ouster: {} base_link_ZED_X: camera_left: @@ -182,7 +182,7 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - OS1_REV6_128_10hz___512_resolution: + lidar_mount: Alpha: 1 Show Axes: false Show Trail: false @@ -213,7 +213,7 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - lidar: + ouster: Alpha: 1 Show Axes: false Show Trail: false @@ -304,7 +304,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: sensors/lidar/point_cloud + Value: sensors/ouster/point_cloud Use Fixed Frame: true Use rainbow: true Value: false diff --git a/common/ros_packages/robot_descriptions/iris/urdf/iris_with_sensors.pegasus.robot.urdf b/common/ros_packages/robot_descriptions/iris/urdf/iris_with_sensors.pegasus.robot.urdf index ebea35166..fed2decde 100644 --- a/common/ros_packages/robot_descriptions/iris/urdf/iris_with_sensors.pegasus.robot.urdf +++ b/common/ros_packages/robot_descriptions/iris/urdf/iris_with_sensors.pegasus.robot.urdf @@ -5,10 +5,10 @@ - + - + @@ -39,10 +39,10 @@ - - - - + + + + @@ -59,7 +59,7 @@ - + @@ -105,7 +105,7 @@ - + diff --git a/docs/development/intermediate/testing/index.md b/docs/development/intermediate/testing/index.md index 94cfd7d77..9279f96c4 100644 --- a/docs/development/intermediate/testing/index.md +++ b/docs/development/intermediate/testing/index.md @@ -1 +1,23 @@ -# Testing \ No newline at end of file +# Testing + +AirStack uses several test layers: ROS 2 package tests (`colcon test`), and **system tests** under [`tests/`](../../../../tests/) at the repo root (pytest, full Docker stack). + +## System tests (`tests/`) + +The canonical reference is **[`tests/README.md`](../../../../tests/README.md)** (also included in the MkDocs site). In short: + +| Mark | Module | Role | +|------|--------|------| +| `liveliness` | `test_liveliness.py` | Containers, `/clock` readiness, tmux, sentinel ROS 2 nodes, compute, infra-only stability poll | +| `sensors` | `test_sensors.py` | Sim + robot stereo/depth Hz, filtered LiDAR (`echo --once` + validation script on Isaac), sim RTF, sensor stability time-series | +| `takeoff_hover_land` | `test_takeoff_hover_land.py` | Four-phase flight chain per configuration | + +Collection order is defined in `tests/conftest.py` (`liveliness` before `sensors` before `takeoff_hover_land`). Each mark’s test **class** uses **class-scoped** `airstack_env`, so combining marks with **`and`** runs multiple full stack bring-ups per `(sim, num_robots, iteration)` — see *Bring-up scope* in `tests/README.md`. + +**Isaac Sim:** the `sensors` implementation batches `ros2 topic hz` on sim and robot paths and avoids `hz` on filtered `PointCloud2`; pytest enables `ENABLE_LIDAR` for the multi-drone Pegasus script. Details: **`tests/README.md`** → *Isaac Sim and the sensors mark*. + +## Other testing docs + +- [Testing frameworks](testing_frameworks.md) — `colcon test`, rostest patterns +- [Integration testing](integration_testing.md) +- [CI/CD](ci_cd.md) — pipeline overview diff --git a/docs/robot/autonomy/sensors/index.md b/docs/robot/autonomy/sensors/index.md index eb1958fba..a5142333a 100644 --- a/docs/robot/autonomy/sensors/index.md +++ b/docs/robot/autonomy/sensors/index.md @@ -1,8 +1,69 @@ -We'll fill this with different things like the ZED-X package, LiDAR, etc +# Sensor Packages +The **sensors** layer holds ROS 2 nodes that sit next to hardware or simulation bridges: light preprocessing, remapping, and calibration helpers so **perception** and downstream layers see stable topics. + +## Overview + +The sensors layer is responsible for: + +- **Bridged sensor topics**: Normalizing names and QoS for data coming from Isaac Sim, MAVROS, or onboard drivers +- **Preprocessing**: Near-range LiDAR cleanup and similar filters that should run on the robot graph, not only in simulation +- **Supporting documentation**: Patterns for optional tools such as the gimbal extension in simulation ## Launch -Launch files are under `src/robot/autonomy/sensors/sensors_bringup/launch`. -The main launch command is `ros2 launch sensors_bringup sensors.launch.xml`. +Launch files are located under `robot/ros_ws/src/sensors/sensors_bringup/launch/`. + +The main launch command is: + +```bash +ros2 launch sensors_bringup sensors.launch.xml +``` + +The bringup group uses the `sensors` namespace under each robot; see that package for which nodes are started. + +## Key Topics + +### Outputs + +- `/{robot_name}/sensors/ouster/point_cloud` — Filtered `sensor_msgs/msg/PointCloud2` (xyz) after `lidar_point_cloud_filter`, when using the default Ouster-style names in config + +### Inputs + +- `/{robot_name}/sensors/ouster/point_cloud_raw` — Raw cloud from the simulator or driver (typical input to the LiDAR filter) +- Other hardware- or bridge-specific topics as wired in `sensors_bringup` + +Topic strings are parameterized with `$(env ROBOT_NAME)` in YAML; override `input_topic` / `output_topic` in the filter config if your stack uses different names. + +## Modules + +- [**LiDAR point cloud filter**](#lidar-point-cloud-filter) (`lidar_point_cloud_filter`) — near-range sphere filter for `PointCloud2` +- [**Gimbal stabilizer**](gimbal.md) — gimbal extension usage in simulation + +## LiDAR point cloud filter (`lidar_point_cloud_filter`){#lidar-point-cloud-filter} + +**Package:** `robot/ros_ws/src/sensors/lidar_point_cloud_filter` + +**Role:** Subscribe to a **raw** `sensor_msgs/msg/PointCloud2`, drop points whose distance from the cloud origin is below `near_range_m` (typical self-hit / multipath noise near the sensor), and republish a **clean xyz float32** cloud for mapping, exploration, RViz, and VDB. + +**Why it exists:** Isaac Sim’s RTX OmniLidar path exposes a **`min_range` / `nearRangeM`** hook in Pegasus (`spawn_rtx_lidar.py`), but applying near range **inside the simulator is unreliable** across Kit builds (attribute missing or ineffective). That behavior is documented as a **known limitation** in [Pegasus / Isaac Sim setup](../../../simulation/isaac_sim/pegasus_scene_setup.md#rtx-lidar-near-range). **AirStack’s supported approach** is to run this **robot-side** filter so the stack always sees a consistent filtered topic. + +**Defaults (configurable):** + +- Parameters and defaults: `robot/ros_ws/src/sensors/lidar_point_cloud_filter/config/lidar_point_cloud_filter.yaml` +- Typical topics: `/{robot_name}/sensors/ouster/point_cloud_raw` → `/{robot_name}/sensors/ouster/point_cloud` (override `input_topic` / `output_topic` if your bridge uses different names, for example under `sensors/lidar/...`) +- QoS: `qos_reliable` defaults to **true** to match common Isaac bridges and RViz; see the package README + +**Further detail:** `robot/ros_ws/src/sensors/lidar_point_cloud_filter/README.md` + +## Configuration + +- **Bringup:** `robot/ros_ws/src/sensors/sensors_bringup/config/` and launch XML under `sensors_bringup/launch/` +- **LiDAR filter:** `robot/ros_ws/src/sensors/lidar_point_cloud_filter/config/lidar_point_cloud_filter.yaml` (`near_range_m`, topics, QoS) + +## See Also +- [System Architecture](../system_architecture.md) — overall autonomy stack architecture +- [Perception](../perception/index.md) — downstream consumers of filtered sensor data +- [Integration Checklist](../integration_checklist.md) — adding new sensor-layer packages +- [Pegasus / Isaac Sim — RTX LiDAR and near range](../../../simulation/isaac_sim/pegasus_scene_setup.md#rtx-lidar-near-range) — simulation-side `min_range` limitation and why the filter runs on the robot diff --git a/docs/simulation/isaac_sim/docker.md b/docs/simulation/isaac_sim/docker.md index 92b33c41b..74edf49f8 100644 --- a/docs/simulation/isaac_sim/docker.md +++ b/docs/simulation/isaac_sim/docker.md @@ -87,7 +87,7 @@ command: > tmux new -d -s isaac; if [ $$AUTOLAUNCH = 'true' ]; then if [ \"${ISAAC_SIM_USE_STANDALONE}\" = 'true' ]; then - tmux send-keys -t isaac 'run_isaac_python /isaac-sim/AirStack/simulation/isaac-sim/launch_scripts/${ISAAC_SIM_SCRIPT_NAME}' ENTER + tmux send-keys -t isaac 'PYTHONPATH="$$ISAAC_SIM_PYTHONPATH" /isaac-sim/python.sh /isaac-sim/AirStack/simulation/isaac-sim/launch_scripts/${ISAAC_SIM_SCRIPT_NAME} --ext-folder ~/.local/share/ov/data/documents/Kit/shared/exts' ENTER else tmux send-keys -t isaac 'ros2 launch isaacsim run_isaacsim.launch.py install_path:=/isaac-sim gui:=\"${ISAAC_SIM_GUI}\" play_sim_on_start:=\"${PLAY_SIM_ON_START}\"' ENTER fi @@ -394,6 +394,11 @@ docker compose -f simulation/isaac-sim/docker/docker-compose.yaml build --no-cac - Inspect DDS: `fastdds.xml` configuration - Test connection: `ros2 topic list` in Isaac Sim container +**`rclpy` / `_rclpy_pybind11` warnings when starting Kit with `python.sh`:** + +- Jazzy’s `setup.bash` puts **Python 3.12** ROS packages on `PYTHONPATH`. Isaac’s `python.sh` uses **Kit Python (~3.10)**. Importing system `rclpy` from the wrong interpreter causes ABI errors in the log (topics from Omnigraph may still work). +- Standalone launch uses `PYTHONPATH="$ISAAC_SIM_PYTHONPATH"` in the **tmux** command (`$$ISAAC_SIM_PYTHONPATH` in `docker-compose.yaml` so Compose does not treat it as a host variable). See container `.bashrc` and `docker-compose.yaml`: it drops `lib/python3.12/site-packages` and appends the bridge’s internal `rclpy` path. + **Performance issues:** - Reduce scene complexity diff --git a/docs/simulation/isaac_sim/index.md b/docs/simulation/isaac_sim/index.md index 39f27258c..5f11f3ef5 100644 --- a/docs/simulation/isaac_sim/index.md +++ b/docs/simulation/isaac_sim/index.md @@ -19,7 +19,7 @@ Isaac Sim provides a wide range of high-fidelity, GPU-accelerated virtual sensor - Stereo and fisheye cameras -- LiDARs and Radars +- LiDARs and Radars (see [Pegasus scene setup](pegasus_scene_setup.md#rtx-lidar-near-range) for RTX OmniLidar in AirStack and near-range filtering on the robot stack) - IMUs, GPS, and odometry sensors diff --git a/docs/simulation/isaac_sim/pegasus_scene_setup.md b/docs/simulation/isaac_sim/pegasus_scene_setup.md index e794f9bf4..0c8917fac 100644 --- a/docs/simulation/isaac_sim/pegasus_scene_setup.md +++ b/docs/simulation/isaac_sim/pegasus_scene_setup.md @@ -110,6 +110,15 @@ Scripts must live in `simulation/isaac-sim/launch_scripts/`. Set `ISAAC_SIM_SCRI | `scene_prep` not found / `ModuleNotFoundError` | `utils/` not on `sys.path` in Isaac Sim's Python | Use `sys.path.insert` to add the `utils/` directory before importing `scene_prep` | | Drone spawns at wrong height in cm-scale scene | Spawn coordinates not converted to stage space | Multiply metric `init_pos` values by `scene_scale` from `get_stage_meters_per_unit` | +## RTX OmniLidar and near range (`min_range`) — known limitation {#rtx-lidar-near-range} + +AirStack’s Pegasus fork (Isaac Sim **5.1+**) wires **RTX OmniLidar** through OmniGraph helpers such as `add_rtx_lidar_subgraph` in `pegasus.simulator.ogn.api.spawn_rtx_lidar` (used from `simulation/isaac-sim/launch_scripts/example_one_px4_pegasus_launch_script.py`, `example_multi_px4_pegasus_launch_script.py`, etc.). Recent work in this repo switched those scripts from the legacy Ouster graph path to this **RTX** API and reconciled ROS topic names (e.g. raw cloud on `…/sensors/ouster/point_cloud_raw`, filtered consumer topic `…/sensors/ouster/point_cloud`). + +### `min_range` → `nearRangeM` in simulation + +The spawn code maps the Python argument **`min_range`** to the OmniLidar prim attribute **`omni:sensor:Core:nearRangeM`** when it exists, and logs a warning when it does not. The module docstring in `spawn_rtx_lidar.py` states the reality: some Kit builds only express echo spacing in the **vendor lidar JSON profile**, not as a writable **Core** prim attribute, so **setting near range in Isaac does not consistently remove short-range returns**. + +**Known bug / policy:** Do **not** rely on Isaac-only `min_range` / `nearRangeM` as your primary near-field cleanup. Use the **robot-side** package **`lidar_point_cloud_filter`** (see [Sensors — LiDAR filter](../../../robot/autonomy/sensors/index.md#lidar-point-cloud-filter)), which applies a configurable **`near_range_m`** sphere filter in the ROS graph and publishes a stable cloud for VDB, exploration, and RViz. ## Known bugs and workarounds for Scripted Scene Generation diff --git a/robot/ros_ws/src/global/global_bringup/config/vdb_params.yaml b/robot/ros_ws/src/global/global_bringup/config/vdb_params.yaml index 412755898..c7e5bdbd0 100644 --- a/robot/ros_ws/src/global/global_bringup/config/vdb_params.yaml +++ b/robot/ros_ws/src/global/global_bringup/config/vdb_params.yaml @@ -22,15 +22,17 @@ apply_raw_sensor_data: true - # sources: [lidar] - # lidar: - # topic: sensors/lidar/point_cloud - # sensor_origin_frame: lidar - - sources: [stereo_image_proc_point_cloud] - stereo_image_proc_point_cloud: - topic: perception/stereo_image_proc/point_cloud - sensor_origin_frame: camera_left + sources: [lidar] + lidar: + topic: sensors/ouster/point_cloud + sensor_origin_frame: ouster + # In cloud header frame: drop points closer than this (m) to sensor origin. 0 = off. + min_sensor_range: 0.0 + + # sources: [stereo_image_proc_point_cloud] + # stereo_image_proc_point_cloud: + # topic: perception/stereo_image_proc/point_cloud + # sensor_origin_frame: camera_left # Remote mapping publish_updates: true diff --git a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/README.md b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/README.md new file mode 100644 index 000000000..2822c059f --- /dev/null +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/README.md @@ -0,0 +1,47 @@ +# lidar_point_cloud_filter + +Subscribes to a raw `sensor_msgs/PointCloud2`, removes points whose distance from the origin is less than `near_range_m` (distance in the cloud frame, same idea as the exploration planner’s lidar pre-process), and publishes a filtered cloud as **xyz float32** only. + +## Topic flow (sim → robot) + +In **Isaac Sim / Pegasus** example launch scripts (e.g. `example_one_px4_pegasus_launch_script.py`, `example_multi_px4_pegasus_launch_script.py` with `ENABLE_LIDAR`), the RTX LiDAR OmniGraph publishes **`point_cloud_raw`** under the vehicle namespace (e.g. `/robot_N/sensors/ouster/point_cloud_raw`). That stream crosses the sim→robot bridge and is what this node **subscribes** to. + +On the **robot** stack: + +| Topic | Role | +|-------|------| +| `.../sensors/ouster/point_cloud_raw` | **Input** — dense cloud from the sim (or hardware driver); may include near-field self-hits / clutter. | +| `.../sensors/ouster/point_cloud` | **Output** — same logical topic name consumers expect for “the” LiDAR cloud, but **filtered** (near-range sphere crop, xyz-only). | + +Downstream nodes should use **`point_cloud`** for planning / visualization unless they explicitly need the raw stream. + +## Parameters + +| Parameter | Meaning | +|-----------|--------| +| `near_range_m` | Points with Euclidean range strictly less than this (meters) are dropped. `<= 0` disables filtering. | +| `input_topic` | Subscription topic; absolute path recommended (see defaults). | +| `output_topic` | Publication topic. | +| `qos_depth` | History depth (keep last). | +| `qos_reliable` | If true, **RELIABLE** (matches Isaac Replicator and typical RViz). If false, **BEST_EFFORT** for drivers that publish that way. | + +Defaults are in `config/lidar_point_cloud_filter.yaml`. `$(env ROBOT_NAME)` is expanded when launch loads the file with `allow_substs="true"`. Python fallbacks use the `ROBOT_NAME` environment variable the same way. + +## Launch + +```bash +ros2 launch lidar_point_cloud_filter lidar_point_cloud_filter.launch.xml +``` + +Included from `sensors_bringup` under the robot and `sensors` namespaces. Defaults use **`sensors/ouster/point_cloud_raw` → `sensors/ouster/point_cloud`** to match Pegasus / Isaac and `vdb_params`. For RTX-only topic names, override `input_topic` and `output_topic` (for example under `sensors/lidar/...`). + +## System tests (`sensors` mark) + +Sensor checks (sim + robot topic rates, LiDAR validation) live in repo-root **`tests/test_sensors.py`** (`pytest -m sensors`), which runs **after** **`tests/test_liveliness.py`** in the default collection order. For **Isaac Sim** (`--sim isaacsim`), that suite: + +- Proves the **filtered** topic is alive (`ros2 topic echo --once` on `.../point_cloud` — large clouds are not probed with `ros2 topic hz`). +- Runs `scripts/validate_lidar_filter_clouds.py` inside each robot container: checks the **filtered** cloud against `near_range_m`, optionally compares behavior when **`point_cloud_raw`** has near-field returns. + +**Microsoft AirSim** does not guarantee `sensors/ouster` topics on that profile; those steps are skipped there. + +See [`tests/README.md`](../../../../../tests/README.md) (Bring-up scope) for how `airstack_env` applies when you combine `liveliness` and `sensors` marks. diff --git a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/config/lidar_point_cloud_filter.yaml b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/config/lidar_point_cloud_filter.yaml new file mode 100644 index 000000000..2b3a4fd75 --- /dev/null +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/config/lidar_point_cloud_filter.yaml @@ -0,0 +1,13 @@ +# Default parameters for lidar_point_cloud_filter (override in launch or a layered params file). +# +# Absolute topics avoid resolving under .../lidar_point_cloud_filter/ when the node is namespaced. +# $(env ROBOT_NAME) is expanded when the launch file loads this file with allow_substs="true". +/**: + ros__parameters: + near_range_m: 0.75 + # Match Pegasus / Isaac ouster namespace (see global_bringup vdb_params sensors/ouster/...). + input_topic: "/$(env ROBOT_NAME)/sensors/ouster/point_cloud_raw" + output_topic: "/$(env ROBOT_NAME)/sensors/ouster/point_cloud" + qos_depth: 10 + # Isaac / Replicator point_cloud_raw uses RELIABLE; RViz often subscribes RELIABLE too. + qos_reliable: true diff --git a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/launch/lidar_point_cloud_filter.launch.xml b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/launch/lidar_point_cloud_filter.launch.xml new file mode 100644 index 000000000..9f92302e2 --- /dev/null +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/launch/lidar_point_cloud_filter.launch.xml @@ -0,0 +1,9 @@ + + + + + diff --git a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/lidar_point_cloud_filter/__init__.py b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/lidar_point_cloud_filter/__init__.py new file mode 100644 index 000000000..1cf9767f8 --- /dev/null +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/lidar_point_cloud_filter/__init__.py @@ -0,0 +1 @@ +"""Lidar point cloud filter ROS 2 package.""" diff --git a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/lidar_point_cloud_filter/lidar_point_cloud_filter_node.py b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/lidar_point_cloud_filter/lidar_point_cloud_filter_node.py new file mode 100644 index 000000000..1f769f8b4 --- /dev/null +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/lidar_point_cloud_filter/lidar_point_cloud_filter_node.py @@ -0,0 +1,162 @@ +# Copyright 2026 AirLab CMU +# SPDX-License-Identifier: Apache-2.0 + +"""ROS 2 node: near-range sphere filter for sensor_msgs/PointCloud2.""" + +import os + +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy +from sensor_msgs.msg import PointCloud2, PointField +from sensor_msgs_py import point_cloud2 + +try: + from numpy.lib.recfunctions import structured_to_unstructured +except ImportError: # pragma: no cover + from sensor_msgs_py.numpy_compat import structured_to_unstructured + + +def _read_pointcloud_xyz_nx3(msg: PointCloud2) -> np.ndarray: + """Return ``(N, 3)`` float32 xyz rows; ``N == 0`` if empty. + + ``read_points`` may return a structured ``ndarray`` (buffer-backed) or a + Python iterator depending on ``sensor_msgs_py`` / distro. Prefer + ``read_points_numpy`` when present and compatible; otherwise normalize to a + dense ``(N, 3)`` array before filtering. + """ + read_numpy = getattr(point_cloud2, 'read_points_numpy', None) + if read_numpy is not None: + try: + arr = read_numpy(msg, field_names=('x', 'y', 'z'), skip_nans=True) + except (AssertionError, TypeError, ValueError): + arr = None + if arr is not None: + arr = np.asarray(arr, dtype=np.float32, order='C') + if arr.size == 0: + return np.zeros((0, 3), dtype=np.float32) + if arr.ndim != 2 or arr.shape[1] != 3: + arr = arr.reshape(-1, 3) + return arr + + pts = point_cloud2.read_points( + msg, field_names=('x', 'y', 'z'), skip_nans=True + ) + if isinstance(pts, np.ndarray): + if pts.size == 0: + return np.zeros((0, 3), dtype=np.float32) + return structured_to_unstructured(pts).astype(np.float32, copy=False) + + rows = list(pts) + if not rows: + return np.zeros((0, 3), dtype=np.float32) + return np.asarray(rows, dtype=np.float32) + + +def _point_cloud_qos(depth: int, reliable: bool) -> QoSProfile: + """QoS aligned with common bridges (e.g. Isaac Replicator: RELIABLE) and RViz.""" + return QoSProfile( + depth=max(1, depth), + reliability=( + QoSReliabilityPolicy.RELIABLE + if reliable + else QoSReliabilityPolicy.BEST_EFFORT + ), + history=QoSHistoryPolicy.KEEP_LAST, + ) + + +class LidarPointCloudFilterNode(Node): + """Drop near-range points (sensor-frame radius) and republish as xyz float32.""" + + def __init__(self) -> None: + super().__init__('lidar_point_cloud_filter') + + _robot = os.environ.get('ROBOT_NAME', 'robot') + self.declare_parameter('near_range_m', 0.75) + self.declare_parameter( + 'input_topic', + f'/{_robot}/sensors/ouster/point_cloud_raw', + ) + self.declare_parameter( + 'output_topic', + f'/{_robot}/sensors/ouster/point_cloud', + ) + self.declare_parameter('qos_depth', 10) + # Match RELIABLE publishers (e.g. Isaac Sim ROS2 bridge); set false for best-effort lidar. + self.declare_parameter('qos_reliable', True) + + self._near_range_m = float(self.get_parameter('near_range_m').value) + self._input_topic = str(self.get_parameter('input_topic').value) + self._output_topic = str(self.get_parameter('output_topic').value) + qos_depth = int(self.get_parameter('qos_depth').value) + qos_reliable = bool(self.get_parameter('qos_reliable').value) + self._qos = _point_cloud_qos(qos_depth, qos_reliable) + + self._fields = [ + PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), + ] + + self._pub = self.create_publisher( + PointCloud2, self._output_topic, self._qos + ) + self._sub = self.create_subscription( + PointCloud2, self._input_topic, self._cloud_callback, self._qos + ) + + self._warned_missing_xyz = False + + self.get_logger().info( + f'Filtering lidar: in={self._input_topic} out={self._output_topic} ' + f'near_range_m={self._near_range_m} qos_reliable={qos_reliable}' + ) + + def _cloud_callback(self, msg: PointCloud2) -> None: + if not self._has_xyz_fields(msg): + if not self._warned_missing_xyz: + self.get_logger().error( + 'PointCloud2 missing x, y, or z fields; not publishing.' + ) + self._warned_missing_xyz = True + return + + arr = _read_pointcloud_xyz_nx3(msg) + if arr.shape[0] == 0: + self._pub.publish(point_cloud2.create_cloud(msg.header, self._fields, [])) + return + + finite = np.isfinite(arr).all(axis=1) + r2 = np.sum(arr * arr, axis=1) + if self._near_range_m <= 0.0: + near_ok = np.ones(arr.shape[0], dtype=bool) + else: + near_ok = r2 >= (self._near_range_m ** 2) + valid = finite & near_ok + + if not np.any(valid): + self._pub.publish(point_cloud2.create_cloud(msg.header, self._fields, [])) + return + + out = arr[valid] + if out.dtype != np.float32: + out = out.astype(np.float32, copy=False) + self._pub.publish(point_cloud2.create_cloud(msg.header, self._fields, out)) + + @staticmethod + def _has_xyz_fields(msg: PointCloud2) -> bool: + names = {f.name for f in msg.fields} + return {'x', 'y', 'z'}.issubset(names) + + +def main(args=None) -> None: + """Run the lidar point cloud filter node.""" + rclpy.init(args=args) + node = LidarPointCloudFilterNode() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() diff --git a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/package.xml b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/package.xml new file mode 100644 index 000000000..5f3786cd5 --- /dev/null +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/package.xml @@ -0,0 +1,23 @@ + + + + lidar_point_cloud_filter + 0.1.0 + Near-range lidar noise filter: subscribes raw PointCloud2, drops points inside a sensor-frame sphere, publishes xyz float32 cloud. + AirLab CMU + Apache-2.0 + + rclpy + sensor_msgs + sensor_msgs_py + python3-numpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/resource/lidar_point_cloud_filter b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/resource/lidar_point_cloud_filter new file mode 100644 index 000000000..893ee5c69 --- /dev/null +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/resource/lidar_point_cloud_filter @@ -0,0 +1 @@ +lidar_point_cloud_filter diff --git a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py new file mode 100644 index 000000000..7f50d45f6 --- /dev/null +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py @@ -0,0 +1,190 @@ +#!/usr/bin/env python3 +# Copyright 2026 AirLab CMU +# SPDX-License-Identifier: Apache-2.0 +"""One-shot ROS 2 check for liveliness: filtered LiDAR cloud vs raw (Isaac / Pegasus). + +Run inside the robot container with workspace sourced and ROS_DOMAIN_ID set:: + + python3 /root/AirStack/robot/ros_ws/src/sensors/lidar_point_cloud_filter/scripts/validate_lidar_filter_clouds.py --robot-num 1 + +Checks (after receiving a non-empty filtered cloud): + * Filtered ``.../point_cloud``: all coordinates finite; **minimum range** must be at + least ``near_range_m`` from the running filter node (minus tolerance). The + tolerance is ``max(0.05 m, 5% of near_range_m)`` — the ``0.05`` is **slack + around the configured near range**, not a standalone “no points within 5 cm” + rule. At least one return beyond 2 m so long-range points are not stripped. + * Raw ``.../point_cloud_raw`` (optional): if present and contains near-field + returns below ``near_range_m``, the filtered cloud must still respect the + near-range floor (filter removes self-hits / clutter; NaNs skipped when reading). + +Exit 0 on success, 1 on validation failure, 2 on ROS/runtime errors. +""" + +from __future__ import annotations + +import argparse +import os +import re +import subprocess +import sys +import time + +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSReliabilityPolicy +from sensor_msgs.msg import PointCloud2 +from sensor_msgs_py import point_cloud2 + + +def _read_near_range_m(robot_num: int) -> float: + """Query ``near_range_m`` from the running filter node; default 0.75.""" + rid = os.environ.get('ROS_DOMAIN_ID', '1') + inner = ( + 'set -e; source /opt/ros/jazzy/setup.bash; ' + 'source /root/AirStack/robot/ros_ws/install/setup.bash; ' + f'export ROS_DOMAIN_ID={rid}; ' + f'ros2 param get /robot_{robot_num}/sensors/lidar_point_cloud_filter near_range_m' + ) + try: + proc = subprocess.run( + ['bash', '-c', inner], + capture_output=True, + text=True, + timeout=20, + ) + except (subprocess.TimeoutExpired, OSError): + return 0.75 + if proc.returncode != 0: + return 0.75 + m = re.search(r'[\d.]+', proc.stdout) + try: + return float(m.group(0)) if m else 0.75 + except ValueError: + return 0.75 + + +def _ranges_xyz(msg: PointCloud2) -> np.ndarray | None: + names = {f.name for f in msg.fields} + if not {'x', 'y', 'z'}.issubset(names): + return None + pts = list( + point_cloud2.read_points(msg, field_names=('x', 'y', 'z'), skip_nans=True) + ) + if not pts: + return np.array([], dtype=np.float64) + arr = np.array([(float(p[0]), float(p[1]), float(p[2])) for p in pts], dtype=np.float64) + if not np.isfinite(arr).all(): + return None + return np.linalg.norm(arr, axis=1) + + +def _wait_for_cloud( + node: Node, topic: str, timeout_s: float, reliable: bool +) -> PointCloud2 | None: + qos = QoSProfile( + depth=10, + reliability=( + QoSReliabilityPolicy.RELIABLE + if reliable + else QoSReliabilityPolicy.BEST_EFFORT + ), + ) + holder: list[PointCloud2] = [] + + def _cb(msg: PointCloud2) -> None: + if not holder: + holder.append(msg) + + sub = node.create_subscription(PointCloud2, topic, _cb, qos) + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline and not holder: + rclpy.spin_once(node, timeout_sec=0.5) + sub.destroy() + return holder[0] if holder else None + + +def main() -> int: + ap = argparse.ArgumentParser(description=__doc__) + ap.add_argument('--robot-num', type=int, required=True) + ap.add_argument('--timeout', type=float, default=60.0) + ap.add_argument( + '--qos-best-effort', + action='store_true', + help='Subscribe with BEST_EFFORT (default: RELIABLE to match filter config)', + ) + args = ap.parse_args() + n = args.robot_num + filt_topic = f'/robot_{n}/sensors/ouster/point_cloud' + raw_topic = f'/robot_{n}/sensors/ouster/point_cloud_raw' + reliable = not args.qos_best_effort + + near_range_m = _read_near_range_m(n) + tol = max(0.05, near_range_m * 0.05) + + node = None + try: + rclpy.init() + node = Node('validate_lidar_filter_clouds') + fmsg = _wait_for_cloud(node, filt_topic, args.timeout, reliable) + if fmsg is None: + print(f'ERROR: no message on {filt_topic} within {args.timeout}s', file=sys.stderr) + return 1 + + fr = _ranges_xyz(fmsg) + if fr is None: + print('ERROR: filtered cloud has non-finite coordinates or missing xyz', file=sys.stderr) + return 1 + if fr.size == 0: + print('ERROR: filtered cloud is empty', file=sys.stderr) + return 1 + + mn_f = float(fr.min()) + if mn_f < near_range_m - tol: + print( + f'ERROR: filtered min range {mn_f:.4f}m < near_range_m ({near_range_m}) - tol {tol:.4f}', + file=sys.stderr, + ) + return 1 + + if float(fr.max()) < 2.0: + print( + f'ERROR: expected long-range returns; filtered max range {float(fr.max()):.4f}m', + file=sys.stderr, + ) + return 1 + + rmsg = _wait_for_cloud(node, raw_topic, min(30.0, args.timeout), reliable) + if rmsg is not None: + rr = _ranges_xyz(rmsg) + if rr is not None and rr.size > 0: + mn_r = float(rr.min()) + if mn_r < near_range_m - tol: + print( + f'INFO: raw min range {mn_r:.4f}m (< near_range_m); ' + f'filtered min {mn_f:.4f}m (must stay >= near_range_m)', + ) + if mn_r < near_range_m - tol and mn_f < near_range_m - tol: + print( + 'ERROR: raw has near-field clutter but filtered min still below near_range_m', + file=sys.stderr, + ) + return 1 + + print( + f'OK robot_{n}: near_range_m={near_range_m:.3f} filtered ' + f'points={fr.size} min_r={mn_f:.3f}m max_r={float(fr.max()):.3f}m', + ) + return 0 + except Exception as e: + print(f'ERROR: {e}', file=sys.stderr) + return 2 + finally: + if node is not None: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/setup.cfg b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/setup.cfg new file mode 100644 index 000000000..ba67e395d --- /dev/null +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/lidar_point_cloud_filter +[install] +install_scripts=$base/lib/lidar_point_cloud_filter diff --git a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/setup.py b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/setup.py new file mode 100644 index 000000000..3935a7fe3 --- /dev/null +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/setup.py @@ -0,0 +1,27 @@ +from setuptools import setup + +package_name = 'lidar_point_cloud_filter' + +setup( + name=package_name, + version='0.1.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', ['launch/lidar_point_cloud_filter.launch.xml']), + ('share/' + package_name + '/config', ['config/lidar_point_cloud_filter.yaml']), + ], + install_requires=['setuptools', 'numpy'], + zip_safe=True, + maintainer='AirLab CMU', + maintainer_email='ajong@andrew.cmu.edu', + description='Near-range sphere filter for lidar PointCloud2', + license='Apache-2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'lidar_point_cloud_filter_node = lidar_point_cloud_filter.lidar_point_cloud_filter_node:main', + ], + }, +) diff --git a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_copyright.py b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_copyright.py new file mode 100644 index 000000000..cc8ff03f7 --- /dev/null +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_flake8.py b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_flake8.py new file mode 100644 index 000000000..27ee1078f --- /dev/null +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_pep257.py b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_pep257.py new file mode 100644 index 000000000..36effa783 --- /dev/null +++ b/robot/ros_ws/src/sensors/lidar_point_cloud_filter/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.pep257 +@pytest.mark.linter +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/robot/ros_ws/src/sensors/sensors_bringup/launch/sensors.launch.xml b/robot/ros_ws/src/sensors/sensors_bringup/launch/sensors.launch.xml index 8529d1c1f..95acdc865 100644 --- a/robot/ros_ws/src/sensors/sensors_bringup/launch/sensors.launch.xml +++ b/robot/ros_ws/src/sensors/sensors_bringup/launch/sensors.launch.xml @@ -2,7 +2,7 @@ - +