From ab96092f046807e9c3c9829f1f2758a50134db67 Mon Sep 17 00:00:00 2001 From: ll7 Date: Fri, 20 Feb 2026 08:03:13 +0100 Subject: [PATCH 01/43] feat(devcontainer): add CUDA devcontainer and bootstrap script --- .devcontainer/devcontainer.json | 39 +++++++++++++++++++++++++++++++++ scripts/dev-up.sh | 17 ++++++++++++++ 2 files changed, 56 insertions(+) create mode 100644 .devcontainer/devcontainer.json create mode 100755 scripts/dev-up.sh diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 00000000..5984339d --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,39 @@ +{ + "name": "PAF Agent Dev (CUDA)", + "dockerComposeFile": [ + "../build/docker-compose.dev.cuda.yml" + ], + "service": "agent-dev", + "runServices": [ + "agent-dev" + ], + "workspaceFolder": "/workspace", + "initializeCommand": "bash ${localWorkspaceFolder}/scripts/update-dotenv.sh", + "shutdownAction": "stopCompose", + "overrideCommand": false, + "postStartCommand": "bash -lc 'source /internal_workspace/dev.bashrc >/dev/null 2>&1 && ros2 --version'", + "customizations": { + "vscode": { + "extensions": [ + "davidanson.vscode-markdownlint", + "github.vscode-pull-request-github", + "vscode-icons-team.vscode-icons", + "yzhang.markdown-all-in-one", + "njpwerner.autodocstring", + "ms-azuretools.vscode-docker", + "bierner.markdown-mermaid", + "richardkotze.git-mob", + "ms-vscode-remote.remote-containers", + "valentjn.vscode-ltex", + "augustocdias.tasks-shell-input", + "ktnrg45.vscode-cython", + "ranch-hand-robotics.rde-pack", + "timonwong.shellcheck", + "editorconfig.editorconfig", + "phil294.git-log--graph", + "ms-vscode.cmake-tools", + "charliermarsh.ruff" + ] + } + } +} diff --git a/scripts/dev-up.sh b/scripts/dev-up.sh new file mode 100755 index 00000000..67fc145d --- /dev/null +++ b/scripts/dev-up.sh @@ -0,0 +1,17 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &>/dev/null && pwd) +REPO_ROOT=$(dirname "$SCRIPT_DIR") + +"${SCRIPT_DIR}/update-dotenv.sh" + +if [[ -x "${SCRIPT_DIR}/check-nvidia.sh" ]]; then + "${SCRIPT_DIR}/check-nvidia.sh" +fi + +cd "${REPO_ROOT}" +docker compose -f build/docker-compose.dev.cuda.yml up -d agent-dev + +echo "agent-dev is running." +echo "Open in container via VS Code: 'Dev Containers: Reopen in Container'." From 324b7e81ffdcbba15d0d40a07929e821e22b3fb2 Mon Sep 17 00:00:00 2001 From: ll7 Date: Fri, 20 Feb 2026 08:03:16 +0100 Subject: [PATCH 02/43] chore(tooling): add scoped Ruff tasks and pre-commit hooks --- .pre-commit-config.yaml | 13 +++++++++++++ .vscode/tasks.json | 35 +++++++++++++++++++++++++++++++++++ 2 files changed, 48 insertions(+) create mode 100644 .pre-commit-config.yaml diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 00000000..5011911b --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,13 @@ +repos: + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.14.8 + hooks: + - id: ruff + args: ["--fix"] + - id: ruff-format + + - repo: https://github.com/igorshubovych/markdownlint-cli + rev: v0.45.0 + hooks: + - id: markdownlint + args: ["--config", ".markdownlint.yaml"] diff --git a/.vscode/tasks.json b/.vscode/tasks.json index be7705e5..ada01f9c 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -57,6 +57,41 @@ "problemMatcher": [], "detail": "Manually trigger the ruff formatter to format the python files.", }, + { + "label": "Start dev container stack (CUDA)", + "type": "shell", + "command": "bash ${workspaceFolder}/scripts/dev-up.sh", + "problemMatcher": [], + "detail": "Updates build/.env, checks NVIDIA driver, and starts agent-dev via Docker Compose." + }, + { + "label": "Lint current Python file with ruff", + "type": "shell", + "command": "ruff check ${file}", + "problemMatcher": [], + "detail": "Runs ruff check for the currently active file." + }, + { + "label": "Format current Python file with ruff", + "type": "shell", + "command": "ruff format ${file}", + "problemMatcher": [], + "detail": "Runs ruff format for the currently active file." + }, + { + "label": "Lint active package with ruff", + "type": "shell", + "command": "bash -lc 'if [[ \"${relativeFile}\" == code/* ]]; then pkg=${relativeFile#code/}; pkg=${pkg%%/*}; ruff check /workspace/code/${pkg}; else echo \"Open a file inside code//... first.\"; exit 1; fi'", + "problemMatcher": [], + "detail": "Runs ruff check on the package derived from the currently active file path." + }, + { + "label": "Check active package formatting with ruff", + "type": "shell", + "command": "bash -lc 'if [[ \"${relativeFile}\" == code/* ]]; then pkg=${relativeFile#code/}; pkg=${pkg%%/*}; ruff format /workspace/code/${pkg} --check; else echo \"Open a file inside code//... first.\"; exit 1; fi'", + "problemMatcher": [], + "detail": "Checks ruff formatting for the package derived from the currently active file path." + }, ], "inputs": [ { From 894fd28c9d97e9d9cfd69e32b057a417847a4dc7 Mon Sep 17 00:00:00 2001 From: ll7 Date: Fri, 20 Feb 2026 08:03:23 +0100 Subject: [PATCH 03/43] docs(dx): add contributor quickstart and agent guidance --- README.md | 1 + agents.md | 109 ++++++++++++++++++++++ doc/development/README.md | 1 + doc/development/first_steps.md | 10 +- doc/development/quickstart_contributor.md | 89 ++++++++++++++++++ 5 files changed, 209 insertions(+), 1 deletion(-) create mode 100644 agents.md create mode 100644 doc/development/quickstart_contributor.md diff --git a/README.md b/README.md index 2f2838d2..25676a53 100644 --- a/README.md +++ b/README.md @@ -39,6 +39,7 @@ More detailed instructions about the setup can be found [here](./doc/general/ins To get an overview of the current architecture of the agent you can look at the general documentation [here](./doc/general/architecture.md). The individual components are explained in the README files of their subfolders. If you contribute to this project please read the guidelines first. They can be found [here](./doc/development/README.md). +For a fast contributor setup, use the [10-minute quickstart](./doc/development/quickstart_contributor.md). ## Research diff --git a/agents.md b/agents.md new file mode 100644 index 00000000..dc3a6806 --- /dev/null +++ b/agents.md @@ -0,0 +1,109 @@ +# AGENTS.md + +Guidance for AI coding agents working in this repository. + +## 1) Mission and scope + +- Keep changes **small, focused, and reversible**. +- Prefer fixing root causes over surface patches. +- Respect existing architecture and naming conventions. +- Do not modify unrelated files. + +## 2) Repository map (quick orientation) + +- `code/`: main ROS2 Python workspace packages (for example `acting/`, `control/`, `localization/`, `mapping/`, `perception/`, `planning/`). +- `code-ros1/`: legacy ROS1 code kept for reference/migration context. +- `build/`: Docker Compose stacks, pins, and container definitions. +- `doc/`: architecture, development process, and subsystem docs. +- `scripts/`: host helper scripts (for example NVIDIA check and env generation). + +## 3) Source of truth for conventions + +Before making substantial edits, check: + +1. `README.md` +2. `doc/development/README.md` +3. `doc/development/linting.md` +4. `doc/development/git_workflow.md` +5. `ruff.toml` + +If guidance conflicts, prefer repository config files and actively used CI/lint settings. + +## 4) Environment assumptions + +- Primary development is Linux + Docker, typically with NVIDIA GPU support. +- Python target is **3.12** (`ruff.toml`). +- Use containerized workflows when project docs expect them. + +## 5) Code change rules + +- Preserve public interfaces unless the task explicitly requires API changes. +- Match existing style and file structure in each package. +- Avoid adding new dependencies unless required; if added, update the relevant requirements file in `code/`. +- Do not add license headers or broad refactors unless requested. +- Keep comments/docstrings meaningful; avoid noisy inline commentary. + +## 6) Linting and formatting + +Python linting/formatting is done with **Ruff**. + +- Repo config: `ruff.toml` +- Pinned version: `build/pins/ruff.env` + +Run one of: + +- VS Code tasks (preferred in this workspace): + - `Lint python code with ruff` + - `Lint python code with ruff and apply safe fixes` + - `Check python code formatting with ruff` + - `Format python code with ruff` +- Or Compose-based linting from docs: + +```bash +docker compose -f build/docker-compose.linter.yaml up +``` + +## 7) Validation strategy + +After changes: + +1. Run targeted lint/format checks for touched Python files. +2. Run nearest relevant tests (if present) before broad test runs. +3. If Docker/ROS behavior changed, validate with the closest existing launch/build flow. + +Do not attempt to fix unrelated failing tests/lints outside the requested scope. + +## 8) ROS and package specifics + +- `code/` contains ROS2-era packages; `code-ros1/` is legacy reference. +- Respect package boundaries (`package.xml`, `setup.py`, `setup.cfg` per package). +- Keep message/service interface changes isolated and coordinated with dependents. + +## 9) Documentation expectations + +- Update docs when behavior, setup, commands, or interfaces change. +- For developer-facing changes, prefer updating docs under `doc/development/` or package README files. +- Keep Markdown concise, structured, and actionable. + +## 10) Git and PR hygiene + +- Use feature-branch style consistent with `doc/development/git_workflow.md`. +- Keep PRs focused; include a brief validation summary. +- Avoid forceful history rewrites unless explicitly requested. + +## 11) Safety and secrets + +- Never commit credentials, tokens, or machine-specific secrets. +- Treat `.env`-style files as environment-specific unless clearly intended for version control. +- Avoid destructive commands (deleting volumes/data, hard resets) unless explicitly requested. + +## 12) Agent behavior checklist + +Before finishing, ensure: + +- [ ] Request scope is fully addressed. +- [ ] Changes are minimal and localized. +- [ ] Lint/format checks were run (or clearly state why not). +- [ ] Relevant docs were updated when needed. +- [ ] Any remaining risks or manual follow-ups are clearly noted. + diff --git a/doc/development/README.md b/doc/development/README.md index f522f8fd..769b2f94 100644 --- a/doc/development/README.md +++ b/doc/development/README.md @@ -9,6 +9,7 @@ ## First steps If this is your first time working with the project you can follow the first steps in [/doc/development/first_steps.md](/doc/development/first_steps.md). +For a compact setup path see also [/doc/development/quickstart_contributor.md](/doc/development/quickstart_contributor.md). ## Development Guidelines diff --git a/doc/development/first_steps.md b/doc/development/first_steps.md index 694a4a64..7af5d940 100644 --- a/doc/development/first_steps.md +++ b/doc/development/first_steps.md @@ -16,7 +16,15 @@ After you cloned the repository open the corresponding folder with VS Code. The ## Start the development container -In order to make changes to the code or add new functionality you can edit the files directly in VS Code now. However, for the default development environment head to the `/build` folder and execute the `docker-compose.dev.yaml` file via right-click and selecting `Compose Up` in the menu: +In order to make changes to the code or add new functionality you can edit the files directly in VS Code now. However, for the default development environment run: + +```bash +bash scripts/dev-up.sh +``` + +After that use `Dev Containers: Reopen in Container` in VS Code. This uses `.devcontainer/devcontainer.json` and opens `/workspace` in the `agent-dev` service. + +Manual alternative: head to the `/build` folder and execute the `docker-compose.dev.yaml` file via right-click and selecting `Compose Up` in the menu: ![devcontainer.png](/doc/assets/development/devcontainer.png) diff --git a/doc/development/quickstart_contributor.md b/doc/development/quickstart_contributor.md new file mode 100644 index 00000000..5d99770e --- /dev/null +++ b/doc/development/quickstart_contributor.md @@ -0,0 +1,89 @@ +# Contributor Quickstart (10 minutes) + +Use this page for the fastest path from clone to productive development. + +- [1) Open and prepare the workspace](#1-open-and-prepare-the-workspace) +- [2) Start the development environment](#2-start-the-development-environment) +- [3) Open in Dev Container](#3-open-in-dev-container) +- [4) Verify local toolchain](#4-verify-local-toolchain) +- [5) Enable pre-commit checks](#5-enable-pre-commit-checks) +- [6) Typical daily workflow](#6-typical-daily-workflow) +- [7) Common problems](#7-common-problems) + +## 1) Open and prepare the workspace + +1. Clone and open the repository root in VS Code. +2. Install the recommended extensions from `.vscode/extensions.json`. + +## 2) Start the development environment + +Run: + +```bash +bash scripts/dev-up.sh +``` + +This command: + +- updates `build/.env` with your host user IDs, +- checks NVIDIA driver compatibility, +- starts `agent-dev` with `build/docker-compose.dev.cuda.yml`. + +## 3) Open in Dev Container + +1. Open command palette (`Ctrl+Shift+P`). +2. Run `Dev Containers: Reopen in Container`. +3. Select the `PAF Agent Dev (CUDA)` configuration if prompted. + +The devcontainer uses the existing `agent-dev` service and opens `/workspace`. + +## 4) Verify local toolchain + +Inside the container terminal: + +```bash +ros2 --version +ruff --version +``` + +Optionally run parity checks used in CI/doc workflows: + +```bash +ruff check /workspace/code/ +ruff format /workspace/code/ --check +docker compose -f build/docker-compose.linter.yaml up +``` + +## 5) Enable pre-commit checks + +Inside the container: + +```bash +pip install --user pre-commit +pre-commit install +``` + +Run all hooks once: + +```bash +pre-commit run --all-files +``` + +## 6) Typical daily workflow + +1. Pull latest `main`. +2. Create a feature branch (`-`). +3. Implement a small focused change. +4. Run scoped tasks from VS Code: + - `Lint current Python file with ruff` + - `Format current Python file with ruff` + - `Lint active package with ruff` +5. Commit only related files and open a focused PR. + +## 7) Common problems + +- If Ruff version mismatch appears, run `bash scripts/update-dotenv.sh` again. +- If container GPU access fails, re-run `bash scripts/check-nvidia.sh` and review `doc/general/installation.md`. +- If rosdep/pip setup failed during image build, inspect logs inside container: + - `/internal_workspace/rosdep_install.log` + - `/internal_workspace/pip_install.log` From d8271df267934f88b17b1599e8e75cafaca4c94f Mon Sep 17 00:00:00 2001 From: ll7 Date: Fri, 20 Feb 2026 16:09:38 +0000 Subject: [PATCH 04/43] devcontainer: avoid persistent install errors and gate in-container VS Code --- .devcontainer/devcontainer.json | 3 +++ build/docker/agent-ros2/Dockerfile | 4 ++-- build/docker/agent-ros2/scripts/entrypoint-dev.sh | 5 ++++- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 5984339d..541f852c 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -8,6 +8,9 @@ "agent-dev" ], "workspaceFolder": "/workspace", + "containerEnv": { + "PAF_LAUNCH_CONTAINER_VSCODE": "0" + }, "initializeCommand": "bash ${localWorkspaceFolder}/scripts/update-dotenv.sh", "shutdownAction": "stopCompose", "overrideCommand": false, diff --git a/build/docker/agent-ros2/Dockerfile b/build/docker/agent-ros2/Dockerfile index d0fbbad8..f14c4451 100644 --- a/build/docker/agent-ros2/Dockerfile +++ b/build/docker/agent-ros2/Dockerfile @@ -383,7 +383,7 @@ RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ sudo apt-get update && \ rosdep update --rosdistro ${ROS_DISTRO} && \ ((rosdep install --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} -y -r |& tee ${INTERNAL_WORKSPACE_DIR}/rosdep_install.log; exit \${PIPESTATUS[0]}) \ - || echo \"echo -e \\\"\\\\e[31mERROR: rosdep install failed, check ${INTERNAL_WORKSPACE_DIR}/rosdep_install.log in the container\\\\e[0m\\\"\" >> ${INTERNAL_WORKSPACE_DIR}/dev.bashrc) \ + || echo -e \"\\e[33mWARNING: rosdep install failed, check ${INTERNAL_WORKSPACE_DIR}/rosdep_install.log in the container\\e[0m\") \ " # Install python requirements.txt dependencies (but allow it to fail, because this is a dev container) @@ -392,7 +392,7 @@ RUN --mount=type=cache,target=${PIP_CACHE_DIR},sharing=locked,uid="${PAF_UID}",g bash -c " \ source ${INTERNAL_WORKSPACE_DIR}/dev.bashrc && \ ((${INTERNAL_WORKSPACE_DIR}/scripts/install-python-requirements.sh |& tee ${INTERNAL_WORKSPACE_DIR}/pip_install.log; exit \${PIPESTATUS[0]}) \ - || echo \"echo -e \\\"\\\\e[31mERROR: pip install failed, check ${INTERNAL_WORKSPACE_DIR}/pip_install.log in the container\\\\e[0m\\\"\" >> ${INTERNAL_WORKSPACE_DIR}/dev.bashrc) \ + || echo -e \"\\e[33mWARNING: pip install failed, check ${INTERNAL_WORKSPACE_DIR}/pip_install.log in the container\\e[0m\") \ " RUN echo source ${INTERNAL_WORKSPACE_DIR}/dev.bashrc >> ~/.bashrc diff --git a/build/docker/agent-ros2/scripts/entrypoint-dev.sh b/build/docker/agent-ros2/scripts/entrypoint-dev.sh index 1d72304f..88b7dfc3 100755 --- a/build/docker/agent-ros2/scripts/entrypoint-dev.sh +++ b/build/docker/agent-ros2/scripts/entrypoint-dev.sh @@ -34,7 +34,10 @@ ros_gui_params=(--ros-args --param use_sim_time:=true) ros2 run rqt_console rqt_console "${ros_gui_params[@]}" & ros2 run rqt_gui rqt_gui "${ros_gui_params[@]}" & ros2 run rviz2 rviz2 -d /workspace/rviz2.rviz "${ros_gui_params[@]}" & -code /workspace & + +if [ "${PAF_LAUNCH_CONTAINER_VSCODE:-1}" = "1" ]; then + code /workspace & +fi "$@" & wait "$!" From fdb8e22bde3dfd016d81e85e4c1b6bf2783bac2a Mon Sep 17 00:00:00 2001 From: ll7 Date: Fri, 20 Feb 2026 16:09:41 +0000 Subject: [PATCH 05/43] quality: add pytest marker baseline and strict Ruff profile --- .vscode/tasks.json | 35 +++++++++++++++++++ .../test/test_mapping_common/test_entity.py | 4 +++ .../test/test_mapping_common/test_shape.py | 4 +++ .../tests/test_ego_motion_compensation.py | 2 ++ code/requirements_infrastructure.txt | 2 ++ code/test/conftest.py | 35 +++++++++++++++++++ pytest.ini | 12 +++++++ ruff-strict.toml | 13 +++++++ 8 files changed, 107 insertions(+) create mode 100644 code/test/conftest.py create mode 100644 pytest.ini create mode 100644 ruff-strict.toml diff --git a/.vscode/tasks.json b/.vscode/tasks.json index ada01f9c..40039625 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -43,6 +43,13 @@ "problemMatcher": [], "detail": "Manually trigger the ruff linter to check the python files and apply the safe fixes that it encounters during linting.", }, + { + "label": "Lint python code with ruff (strict profile)", + "type": "shell", + "command": "ruff check /workspace/code/ --config /workspace/ruff-strict.toml", + "problemMatcher": [], + "detail": "Runs stricter lint rules including docstring checks for incremental quality hardening." + }, { "label": "Check python code formatting with ruff", "type": "shell", @@ -92,6 +99,34 @@ "problemMatcher": [], "detail": "Checks ruff formatting for the package derived from the currently active file path." }, + { + "label": "Run pytest (unit)", + "type": "shell", + "command": "pytest -m unit", + "problemMatcher": [], + "detail": "Runs fast unit tests marked with @pytest.mark.unit." + }, + { + "label": "Run pytest (integration)", + "type": "shell", + "command": "pytest -m integration", + "problemMatcher": [], + "detail": "Runs integration tests marked with @pytest.mark.integration." + }, + { + "label": "Run pytest (simulation)", + "type": "shell", + "command": "pytest -m sim", + "problemMatcher": [], + "detail": "Runs simulation tests marked with @pytest.mark.sim." + }, + { + "label": "Run pytest with coverage", + "type": "shell", + "command": "pytest --cov=code --cov-report=term-missing", + "problemMatcher": [], + "detail": "Runs all tests and reports coverage for the code workspace." + }, ], "inputs": [ { diff --git a/code/mapping/test/test_mapping_common/test_entity.py b/code/mapping/test/test_mapping_common/test_entity.py index 7bd8d5ff..f0144890 100644 --- a/code/mapping/test/test_mapping_common/test_entity.py +++ b/code/mapping/test/test_mapping_common/test_entity.py @@ -1,5 +1,9 @@ +import pytest + from mapping_common import entity, shape, transform +pytestmark = pytest.mark.unit + def get_shape(): return shape.Rectangle(1.0, 2.0) diff --git a/code/mapping/test/test_mapping_common/test_shape.py b/code/mapping/test/test_mapping_common/test_shape.py index feafcc31..228c6d46 100644 --- a/code/mapping/test/test_mapping_common/test_shape.py +++ b/code/mapping/test/test_mapping_common/test_shape.py @@ -1,6 +1,10 @@ +import pytest + from mapping_common import shape from mapping_common.transform import Transform2D, Vector2, Point2 +pytestmark = pytest.mark.unit + def test_rectangle_conversion(): s = shape.Rectangle(1.0, 2.0) diff --git a/code/perception/tests/test_ego_motion_compensation.py b/code/perception/tests/test_ego_motion_compensation.py index 2cacd499..46ff6d04 100644 --- a/code/perception/tests/test_ego_motion_compensation.py +++ b/code/perception/tests/test_ego_motion_compensation.py @@ -7,6 +7,8 @@ ego_motion_compensation, ) +pytestmark = pytest.mark.unit + # --- Mock Data Structures --- diff --git a/code/requirements_infrastructure.txt b/code/requirements_infrastructure.txt index d0d8784a..f627549a 100644 --- a/code/requirements_infrastructure.txt +++ b/code/requirements_infrastructure.txt @@ -1,2 +1,4 @@ # Ruff version pinned via build/pins/ruff.env ruff +pytest +pytest-cov diff --git a/code/test/conftest.py b/code/test/conftest.py new file mode 100644 index 00000000..f7a32bb9 --- /dev/null +++ b/code/test/conftest.py @@ -0,0 +1,35 @@ +"""Shared pytest fixtures for repository-wide tests.""" + +from __future__ import annotations + +import random +from types import SimpleNamespace + +import pytest + + +@pytest.fixture(autouse=True) +def deterministic_seed() -> None: + """Set deterministic pseudo-random seeds for repeatable test behavior.""" + seed = 1337 + random.seed(seed) + try: + import numpy as np + + np.random.seed(seed) + except ModuleNotFoundError: + pass + + +@pytest.fixture +def fake_ros_clock() -> SimpleNamespace: + """Provide a lightweight ROS-like clock object for pure-python tests.""" + return SimpleNamespace(clock=SimpleNamespace(sec=0, nanosec=0)) + + +@pytest.fixture +def snapshot_dir(tmp_path): + """Return a per-test snapshot directory for golden-file style assertions.""" + snapshots_path = tmp_path / "snapshots" + snapshots_path.mkdir(parents=True, exist_ok=True) + return snapshots_path diff --git a/pytest.ini b/pytest.ini new file mode 100644 index 00000000..4ed12f6f --- /dev/null +++ b/pytest.ini @@ -0,0 +1,12 @@ +[pytest] +testpaths = code +python_files = test_*.py *_test.py +addopts = -ra --strict-config --strict-markers +xfail_strict = true +norecursedirs = .git .github .devcontainer build volumes code-ros1 +markers = + unit: Fast, isolated tests with no ROS graph or external services. + integration: Cross-module tests with multiple components and realistic inputs. + sim: Simulation-backed tests that require CARLA/ROS simulation runtime. + ros: Tests that require ROS middleware/runtime behavior. + slow: Long-running tests not suitable for fast feedback loops. diff --git a/ruff-strict.toml b/ruff-strict.toml new file mode 100644 index 00000000..68d07232 --- /dev/null +++ b/ruff-strict.toml @@ -0,0 +1,13 @@ +extend = "ruff.toml" + +[lint] +extend-select = ["B", "C4", "SIM", "UP", "RUF", "D", "PLR"] + +[lint.pydocstyle] +convention = "google" + +[lint.per-file-ignores] +"__init__.py" = ["F401", "D104"] +"test_*.py" = ["F841", "D", "S101", "PLR2004"] +"**/tests/*.py" = ["D", "S101", "PLR2004"] +"**/test/**/*.py" = ["D", "S101", "PLR2004"] From 818ed66675bd66fea94e7d936fb302aeefa44e28 Mon Sep 17 00:00:00 2001 From: ll7 Date: Fri, 20 Feb 2026 16:09:46 +0000 Subject: [PATCH 06/43] docs+logging: add developer contract, ADR flow, and logging utilities --- .github/pull_request_template.md | 13 +++ agents.md | 7 ++ code/paf_common/paf_common/__init__.py | 3 + code/paf_common/paf_common/logging_utils.py | 96 +++++++++++++++++++ doc/adr/0000-template.md | 33 +++++++ doc/adr/README.md | 15 +++ doc/development/README.md | 4 + doc/development/context_retention.md | 24 +++++ doc/development/developer_contract.md | 37 +++++++ doc/development/documentation_requirements.md | 9 ++ doc/development/linting.md | 21 ++++ doc/development/logging.md | 26 +++++ doc/development/testing_strategy.md | 37 +++++++ 13 files changed, 325 insertions(+) create mode 100644 code/paf_common/paf_common/logging_utils.py create mode 100644 doc/adr/0000-template.md create mode 100644 doc/adr/README.md create mode 100644 doc/development/context_retention.md create mode 100644 doc/development/developer_contract.md create mode 100644 doc/development/logging.md create mode 100644 doc/development/testing_strategy.md diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md index 4c0d9a48..c8c9be85 100644 --- a/.github/pull_request_template.md +++ b/.github/pull_request_template.md @@ -21,6 +21,18 @@ e.g. is old functionality not usable anymore Which files functionalities are most important in this PR. On which part should the reviewer be focussed on? +## Assumptions and constraints + +List assumptions, environment constraints, and any scope limits used for this change. + +## Validation summary + +List commands executed and outcomes (lint, format, tests, simulation checks, etc.). + +## Known gaps / follow-ups + +List remaining risks or follow-up work that is intentionally out of scope. + # Checklist: - [ ] My code follows the style guidelines of this project @@ -30,4 +42,5 @@ Which files functionalities are most important in this PR. On which part should - [ ] My changes generate no new warnings - [ ] I have added tests that prove my fix is effective or that my feature works (might be obsolete with CI later on) - [ ] New and existing unit tests pass locally with my changes (might be obsolete with CI later on) +- [ ] I documented assumptions, validation steps, and known gaps in this PR diff --git a/agents.md b/agents.md index dc3a6806..291b0dda 100644 --- a/agents.md +++ b/agents.md @@ -84,6 +84,7 @@ Do not attempt to fix unrelated failing tests/lints outside the requested scope. - Update docs when behavior, setup, commands, or interfaces change. - For developer-facing changes, prefer updating docs under `doc/development/` or package README files. - Keep Markdown concise, structured, and actionable. +- Capture non-trivial architectural decisions in `doc/adr/` using the ADR template. ## 10) Git and PR hygiene @@ -107,3 +108,9 @@ Before finishing, ensure: - [ ] Relevant docs were updated when needed. - [ ] Any remaining risks or manual follow-ups are clearly noted. +## 13) Quality gates and contributor contract + +- Follow `doc/development/developer_contract.md` for PR assumptions, validation summary, and known gaps. +- Use `pytest` markers defined in `pytest.ini` (`unit`, `integration`, `sim`) to scope validation. +- Use `ruff-strict.toml` for incremental hardening of packages (docstring and maintainability checks). + diff --git a/code/paf_common/paf_common/__init__.py b/code/paf_common/paf_common/__init__.py index e69de29b..ff311d9d 100644 --- a/code/paf_common/paf_common/__init__.py +++ b/code/paf_common/paf_common/__init__.py @@ -0,0 +1,3 @@ +from .logging_utils import configure_logging, with_log_context + +__all__ = ["configure_logging", "with_log_context"] diff --git a/code/paf_common/paf_common/logging_utils.py b/code/paf_common/paf_common/logging_utils.py new file mode 100644 index 00000000..61b7342a --- /dev/null +++ b/code/paf_common/paf_common/logging_utils.py @@ -0,0 +1,96 @@ +"""Utilities for consistent structured logging across agent modules.""" + +from __future__ import annotations + +import json +import logging +import sys +from collections.abc import Mapping +from datetime import UTC, datetime +from typing import Any + + +class JsonLogFormatter(logging.Formatter): + """Format log records as JSON objects with stable top-level fields.""" + + def format(self, record: logging.LogRecord) -> str: + """Convert a log record into a JSON string.""" + payload: dict[str, Any] = { + "timestamp": datetime.now(UTC).isoformat(), + "level": record.levelname, + "logger": record.name, + "message": record.getMessage(), + "module": record.module, + "function": record.funcName, + "line": record.lineno, + } + + if record.exc_info: + payload["exception"] = self.formatException(record.exc_info) + + extra_context = getattr(record, "context", None) + if isinstance(extra_context, Mapping): + payload["context"] = dict(extra_context) + + return json.dumps(payload, ensure_ascii=False) + + +class ContextLoggerAdapter(logging.LoggerAdapter): + """Attach stable context fields to each emitted log record.""" + + def process(self, msg: str, kwargs: dict[str, Any]) -> tuple[str, dict[str, Any]]: + """Inject adapter context into the logging kwargs.""" + extra = kwargs.setdefault("extra", {}) + context = dict(extra.get("context", {})) + context.update(self.extra) + extra["context"] = context + return msg, kwargs + + +def configure_logging( + *, + level: int = logging.INFO, + json_logs: bool = False, + logger_name: str | None = None, +) -> logging.Logger: + """Configure and return a stream logger with project defaults. + + Args: + level: Minimum emitted log level. + json_logs: Emit JSON log entries when set to True. + logger_name: Optional logger name. Uses root logger when omitted. + + Returns: + Configured logger instance. + """ + logger = logging.getLogger(logger_name) + logger.setLevel(level) + logger.handlers.clear() + logger.propagate = False + + handler = logging.StreamHandler(sys.stdout) + if json_logs: + handler.setFormatter(JsonLogFormatter()) + else: + handler.setFormatter( + logging.Formatter( + "%(asctime)s %(levelname)s [%(name)s] %(message)s", + "%Y-%m-%dT%H:%M:%S%z", + ) + ) + + logger.addHandler(handler) + return logger + + +def with_log_context(logger: logging.Logger, **context: Any) -> ContextLoggerAdapter: + """Return a logger adapter that injects stable context keys. + + Args: + logger: Base logger to wrap. + **context: Context values to include in every log record. + + Returns: + Logger adapter with context attached. + """ + return ContextLoggerAdapter(logger, context) diff --git a/doc/adr/0000-template.md b/doc/adr/0000-template.md new file mode 100644 index 00000000..12ea2bb4 --- /dev/null +++ b/doc/adr/0000-template.md @@ -0,0 +1,33 @@ +# ADR NNNN: + +- Status: Proposed | Accepted | Superseded +- Date: YYYY-MM-DD +- Owners: + +## Context + +Describe the problem, constraints, and why this decision is needed now. + +## Options considered + +1. Option A +2. Option B +3. Option C + +## Decision + +State the chosen option and why. + +## Consequences + +- Positive outcomes +- Trade-offs +- Risks + +## Validation + +How this decision will be validated (tests, metrics, rollout criteria). + +## Follow-ups + +List concrete next tasks or migration steps. diff --git a/doc/adr/README.md b/doc/adr/README.md new file mode 100644 index 00000000..11046f58 --- /dev/null +++ b/doc/adr/README.md @@ -0,0 +1,15 @@ +# Architecture Decision Records (ADRs) + +Use ADRs to preserve technical intent and trade-offs over time. + +## Naming + +- File format: `NNNN-short-title.md` +- Example: `0001-standardize-pytest-markers.md` + +## Process + +1. Copy `0000-template.md`. +2. Fill in context, options, decision, and consequences. +3. Link the ADR in the corresponding PR. +4. Mark superseded ADRs when decisions change. diff --git a/doc/development/README.md b/doc/development/README.md index 769b2f94..ff66b9ef 100644 --- a/doc/development/README.md +++ b/doc/development/README.md @@ -26,6 +26,10 @@ If you contribute to this project please read the following guidelines first: 3. [drive action](./drive_action.md) 7. [Install python packages](./installing_python_packages.md) 8. [Discord Webhook Documentation](./discord_webhook.md) +9. [Developer Contract](./developer_contract.md) +10. [Testing Strategy](./testing_strategy.md) +11. [Logging Standard](./logging.md) +12. [Context Retention](./context_retention.md) ## Administrative Guidelines diff --git a/doc/development/context_retention.md b/doc/development/context_retention.md new file mode 100644 index 00000000..e74f9074 --- /dev/null +++ b/doc/development/context_retention.md @@ -0,0 +1,24 @@ +# Context Retention + +## Why + +Long-lived projects lose intent when design decisions only live in chat threads or ephemeral PR comments. + +## Mechanisms + +1. Architecture Decision Records (ADRs) + - Keep ADRs in `doc/adr/`. + - Record decision, alternatives, and consequences. +2. PR quality fields + - Capture assumptions, validation, and known gaps in every PR. +3. Test markers and logs + - Preserve behavior expectations with marker-based tests and structured logs. + +## When to write an ADR + +Write an ADR when you: + +- Change package boundaries or interfaces. +- Introduce a new dependency or framework. +- Make a policy decision (linting, testing, release process). +- Replace an established behavior with a new default. diff --git a/doc/development/developer_contract.md b/doc/development/developer_contract.md new file mode 100644 index 00000000..54589726 --- /dev/null +++ b/doc/development/developer_contract.md @@ -0,0 +1,37 @@ +# Developer Contract + +This page defines the minimum quality bar for all code contributions. + +## 1) Before coding + +- Confirm scope and assumptions in the issue/PR description. +- Identify impacted package boundaries (`code//...`). +- Decide required validation level: `unit`, `integration`, or `sim`. + +## 2) During coding + +- Keep changes small and package-local. +- Avoid public API changes unless required by scope. +- Use structured logging and avoid `print` in runtime nodes. +- Add/adjust tests for behavior changes. + +## 3) Before opening PR + +- Run lint and format checks: + - `ruff check /workspace/code/` + - `ruff format /workspace/code/ --check` +- Run strict lint incrementally (recommended): + - `ruff check /workspace/code/ --config /workspace/ruff-strict.toml` +- Run relevant tests by marker: + - `pytest -m unit` + - `pytest -m integration` + - `pytest -m sim` (when simulation behavior changed) + +## 4) PR quality fields (required) + +Every PR must include: + +- Assumptions and constraints. +- Validation summary (commands + result). +- Known gaps/follow-ups. +- Explicit note if docs were updated or why not. diff --git a/doc/development/documentation_requirements.md b/doc/development/documentation_requirements.md index 9946ac5f..33ac4863 100644 --- a/doc/development/documentation_requirements.md +++ b/doc/development/documentation_requirements.md @@ -85,6 +85,15 @@ Press `Enter` or select the option and it should produce a docstring that looks ![docstring.png](/doc/assets/development/docstring.png) +Docstrings are required for public modules, classes, and functions in maintained ROS2 packages. +Use Google-style docstrings and keep them aligned with actual behavior and parameter names. + +For incremental enforcement use: + +```bash +ruff check /workspace/code/ --config /workspace/ruff-strict.toml +``` + ### 1.5. Readability and Maintainability - **Consistent Formatting:** Code should follow a consistent and readable formatting style. Tools like linters or formatters can help enforce a consistent code style. diff --git a/doc/development/linting.md b/doc/development/linting.md index dc37ea56..a5fc3b52 100644 --- a/doc/development/linting.md +++ b/doc/development/linting.md @@ -13,6 +13,11 @@ We use [Ruff](https://docs.astral.sh/ruff/) for both linting and formatting Python code. +Repository configs: + +- `ruff.toml`: baseline rules used in CI. +- `ruff-strict.toml`: stricter optional profile for gradual hardening (includes docstring checks and additional maintainability rules). + ### Ruff versioning - The pinned version lives in `build/pins/ruff.env` (kept in sync with `build/.env` via `scripts/update-dotenv.sh`, which VS Code runs on folder open). @@ -27,6 +32,12 @@ Helper commands are available in `build/docker/agent-ros2/scripts/devfunctions.b - `ruff.check-format`: run `ruff format --check`. - `ruff.format`: apply formatting with `ruff format`. +Use the strict profile when hardening a package: + +```bash +ruff check /workspace/code/ --config /workspace/ruff-strict.toml +``` + ### Run Ruff via Docker Compose Use the pinned version directly from the host: @@ -37,6 +48,16 @@ docker compose -f build/docker-compose.linter.yaml up The Compose file mounts the repo into the container and runs Ruff against it. +## ✅ Test lint-adjacent workflow + +Use pytest markers to scope feedback loops: + +```bash +pytest -m unit +pytest -m integration +pytest -m sim +``` + ## 💬 Markdown Linter To enforce unified standards in all markdown files, we use [markdownlint-cli](https://github.com/igorshubovych/markdownlint-cli). More details on it can be found in the according documentation. diff --git a/doc/development/logging.md b/doc/development/logging.md new file mode 100644 index 00000000..32fc9bf7 --- /dev/null +++ b/doc/development/logging.md @@ -0,0 +1,26 @@ +# Logging Standard + +## Objectives + +- Keep logs machine-parseable and human-readable. +- Preserve request/scenario context across module boundaries. +- Make failures actionable during autonomous runs. + +## Rules + +- Use Python `logging`; do not use `print` in runtime node logic. +- Use levels consistently: + - `DEBUG`: detailed diagnostics + - `INFO`: lifecycle and state transitions + - `WARNING`: degraded behavior with fallback + - `ERROR`: failed operation requiring attention +- Include stable context keys where available (for example `route_id`, `scenario_id`, `module`, `node_name`). + +## Shared utilities + +Common helpers are available in `paf_common`: + +- `configure_logging(...)` to configure stream/JSON logging. +- `with_log_context(...)` to bind stable context fields. + +Use JSON output for CI and batch processing contexts, and text output for local interactive development. diff --git a/doc/development/testing_strategy.md b/doc/development/testing_strategy.md new file mode 100644 index 00000000..7d570eba --- /dev/null +++ b/doc/development/testing_strategy.md @@ -0,0 +1,37 @@ +# Testing Strategy + +## Goals + +- Fast feedback for local development. +- Confidence in subsystem integration. +- Clear separation between deterministic and simulation-heavy tests. + +## Test levels + +- `unit`: Isolated logic tests with no external services and no ROS graph dependency. +- `integration`: Multi-module behavior tests using realistic data paths. +- `sim`: End-to-end or high-fidelity simulation tests (e.g., CARLA). + +## Markers and commands + +Repository markers are defined in `pytest.ini`. + +- Run unit tests: `pytest -m unit` +- Run integration tests: `pytest -m integration` +- Run simulation tests: `pytest -m sim` +- Run all tests with coverage: `pytest --cov=code --cov-report=term-missing` + +## Submodule strategy + +For each package under `code/`: + +1. Keep `unit` tests near core algorithms and utilities. +2. Add `integration` tests where package boundaries are crossed. +3. Reserve `sim` tests for behavior that needs runtime/simulator fidelity. +4. Prefer deterministic fixtures and fixed random seeds. + +## CI recommendation (phased) + +- On every PR: run `unit` tests. +- On merge to main or scheduled runs: run `integration` tests. +- Nightly or hardware-backed pipeline: run `sim` tests. From b6923620eff19e5c6e59a83793e77a293139d899 Mon Sep 17 00:00:00 2001 From: ll7 Date: Fri, 20 Feb 2026 16:11:41 +0000 Subject: [PATCH 07/43] vscode: add ROS2 python extraPaths workspace settings --- .vscode/settings.json | 49 ++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 48 insertions(+), 1 deletion(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index b1875c79..d14ce26b 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -31,5 +31,52 @@ "task.allowAutomaticTasks": "on", "[markdown]": { "editor.defaultFormatter": "DavidAnson.vscode-markdownlint" - } + }, + "ROS2.distro": "jazzy", + "python.autoComplete.extraPaths": [ + "/internal_workspace/carla_ros2_ws/install/rqt_carla_control/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_waypoint_publisher/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_ackermann_control/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_ros_bridge/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_spawn_objects/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_manual_control/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_ad_agent/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/ros_compatibility/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_waypoint_types/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_walker_agent/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_twist_to_control/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_ros_scenario_runner/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_ros_scenario_runner_types/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_ackermann_msgs/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_msgs/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_common/lib/python3.12/site-packages", + "/internal_workspace/agent_deps_ros2_ws/install/ros2_numpy/lib/python3.12/site-packages", + "/internal_workspace/agent_deps_ros2_ws/install/py_trees_ros_viewer/lib/python3.12/site-packages", + "/internal_workspace/agent_deps_ros2_ws/install/py_trees_ros/lib/python3.12/site-packages", + "/internal_workspace/agent_deps_ros2_ws/install/py_trees_ros_interfaces/lib/python3.12/site-packages", + "/opt/ros/jazzy/lib/python3.12/site-packages" + ], + "python.analysis.extraPaths": [ + "/internal_workspace/carla_ros2_ws/install/rqt_carla_control/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_waypoint_publisher/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_ackermann_control/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_ros_bridge/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_spawn_objects/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_manual_control/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_ad_agent/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/ros_compatibility/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_waypoint_types/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_walker_agent/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_twist_to_control/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_ros_scenario_runner/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_ros_scenario_runner_types/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_ackermann_msgs/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_msgs/lib/python3.12/site-packages", + "/internal_workspace/carla_ros2_ws/install/carla_common/lib/python3.12/site-packages", + "/internal_workspace/agent_deps_ros2_ws/install/ros2_numpy/lib/python3.12/site-packages", + "/internal_workspace/agent_deps_ros2_ws/install/py_trees_ros_viewer/lib/python3.12/site-packages", + "/internal_workspace/agent_deps_ros2_ws/install/py_trees_ros/lib/python3.12/site-packages", + "/internal_workspace/agent_deps_ros2_ws/install/py_trees_ros_interfaces/lib/python3.12/site-packages", + "/opt/ros/jazzy/lib/python3.12/site-packages" + ] } From 89ad26af2aef2d2934e1900e1864b003d63a6e79 Mon Sep 17 00:00:00 2001 From: ll7 Date: Fri, 20 Feb 2026 16:24:20 +0000 Subject: [PATCH 08/43] deps: add container dependency sync/check workflow and docs --- .vscode/tasks.json | 14 ++++ build/docker/agent-ros2/Dockerfile | 1 + .../agent-ros2/scripts/dependency-sync.sh | 51 ++++++++++++++ .../agent-ros2/scripts/devfunctions.bash | 16 +++++ doc/development/README.md | 1 + doc/development/dependency_management.md | 67 +++++++++++++++++++ doc/development/installing_python_packages.md | 39 +++++++---- doc/development/quickstart_contributor.md | 4 ++ 8 files changed, 180 insertions(+), 13 deletions(-) create mode 100644 build/docker/agent-ros2/scripts/dependency-sync.sh create mode 100644 doc/development/dependency_management.md diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 40039625..be01e128 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -127,6 +127,20 @@ "problemMatcher": [], "detail": "Runs all tests and reports coverage for the code workspace." }, + { + "label": "Dependency check in dev container", + "type": "shell", + "command": "bash -lc 'source /internal_workspace/dev.bashrc && dep.check'", + "problemMatcher": [], + "detail": "Validates rosdep and pip dependency health in the running agent-dev container environment." + }, + { + "label": "Synchronize dependencies in dev container", + "type": "shell", + "command": "bash -lc 'source /internal_workspace/dev.bashrc && dep.sync'", + "problemMatcher": [], + "detail": "Installs rosdep and pip dependencies from package.xml and requirements manifests." + }, ], "inputs": [ { diff --git a/build/docker/agent-ros2/Dockerfile b/build/docker/agent-ros2/Dockerfile index f14c4451..cf662f9b 100644 --- a/build/docker/agent-ros2/Dockerfile +++ b/build/docker/agent-ros2/Dockerfile @@ -368,6 +368,7 @@ WORKDIR ${INTERNAL_WORKSPACE_DIR} COPY --chown=${PAF_UID}:${PAF_GID} ${DOCKER_RESOURCE_BASE}/scripts/dev.bashrc ./ COPY --chmod=755 ${DOCKER_RESOURCE_BASE}/scripts/entrypoint-dev.sh \ ${DOCKER_RESOURCE_BASE}/scripts/devfunctions.bash \ + ${DOCKER_RESOURCE_BASE}/scripts/dependency-sync.sh \ ${DOCKER_RESOURCE_BASE}/scripts/reset_env.bash \ ./scripts/ diff --git a/build/docker/agent-ros2/scripts/dependency-sync.sh b/build/docker/agent-ros2/scripts/dependency-sync.sh new file mode 100644 index 00000000..fe2bc0a9 --- /dev/null +++ b/build/docker/agent-ros2/scripts/dependency-sync.sh @@ -0,0 +1,51 @@ +#!/bin/bash +set -euo pipefail + +MODE="${1:-sync}" + +if [[ "${MODE}" != "sync" && "${MODE}" != "check" ]]; then + echo "Usage: dependency-sync.sh [sync|check]" + exit 2 +fi + +if [[ -z "${PAF_ROS_WS:-}" || -z "${INTERNAL_WORKSPACE_DIR:-}" || -z "${ROS_DISTRO:-}" ]]; then + echo "PAF_ROS_WS, INTERNAL_WORKSPACE_DIR and ROS_DISTRO must be set." + exit 1 +fi + +source "${INTERNAL_WORKSPACE_DIR}/env.bash" +cd "${PAF_ROS_WS}" + +echo "Refreshing apt and rosdep indices..." +sudo apt-get update +rosdep update --rosdistro "${ROS_DISTRO}" + +if [[ "${MODE}" == "check" ]]; then + echo "Running rosdep check for workspace package.xml dependencies..." + rosdep check --from-paths src --ignore-src --rosdistro "${ROS_DISTRO}" + + echo "Running pip health check..." + python3 -m pip check + + cat <` in the container shell. +2. Every added Python dependency must be pinned with `==` in the matching requirements file. +3. Every ROS package dependency must be declared in the relevant `package.xml`. +4. After dependency changes, run `dep.sync` and `devbuild` in the container. + +## Standard workflow + +### 1) Modify dependency manifests + +- Python: update `requirements*.txt` with pinned versions. +- ROS: update `package.xml` (`depend`, `exec_depend`, `test_depend` as applicable). + +### 2) Synchronize in container + +In a shell inside the running `agent-dev` container: + +```bash +dep.check # optional pre-check +dep.sync # installs rosdep + pip dependencies from repo files +devbuild # rebuild workspace after dependency changes +``` + +`dep.sync` internally runs: + +- `rosdep install --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y -r` +- `install-python-requirements.sh` (all `requirements*.txt` + `requirements_infrastructure.txt`) +- `python3 -m pip check` + +### 3) Validate + +```bash +ruff check /workspace/code/ +ruff format /workspace/code/ --check +pytest -m unit +``` + +## When container rebuild is required + +Use `Dev Containers: Rebuild Container` when you change image-level inputs such as: + +- `build/docker/agent-ros2/Dockerfile` +- `build/pins/*.env` +- scripts copied into the image under `build/docker/agent-ros2/scripts/` + +For regular `package.xml` / `requirements*.txt` updates, an image rebuild is usually not needed; run `dep.sync` + `devbuild` in the existing container. + +## Troubleshooting + +- Build-time failures: + - `/internal_workspace/rosdep_install.log` + - `/internal_workspace/pip_install.log` +- Runtime dependency drift: + - run `dep.check` + - run `python3 -m pip check` diff --git a/doc/development/installing_python_packages.md b/doc/development/installing_python_packages.md index fc908d9a..9cd6bcd1 100644 --- a/doc/development/installing_python_packages.md +++ b/doc/development/installing_python_packages.md @@ -1,23 +1,36 @@ -# Install python packages +# Install Python packages -(Kept from previous group [paf22]) +**Summary:** Python dependencies are managed from repository manifests and synchronized inside the running development container. -**Summary:** This page gives a short overview how to add python packages to the project. +## Choose the correct requirements file -- [Install python packages](#install-python-packages) - - [Adding packages with pip](#adding-packages-with-pip) +- `code/requirements.txt`: shared runtime dependencies. +- `code/requirements.cpu.txt`, `code/requirements.cuda.txt`, `code/requirements.rocm.txt`: hardware-specific runtime dependencies. +- `code/requirements_infrastructure.txt`: developer tooling (`ruff`, `pytest`, etc.). -## Adding packages with pip +Every dependency must be pinned with `==`. -To have a unified setup every python package has to be added with a fixed version. +## Add a dependency safely -> Please don't install a package (inside a container) with `pip install xxx` since it would then be just installed in your specific container. +1. Edit the matching `requirements*.txt` file. +2. Open a shell in the `agent-dev` container. +3. Run: -Instead, any package should be added to `code/requirements.txt`. Always set the package to a fixed version with `==` to avoid version conflicts. +```bash +dep.sync +devbuild +``` -An example how this file could look like is given below: +4. Validate with: -```text -torch==1.13.0 -torchvision==0.1.9 +```bash +python3 -m pip check +ruff check /workspace/code/ +pytest -m unit ``` + +## Important container note + +Avoid ad-hoc `pip install ` as a long-term fix. It only mutates your current container and is lost for other developers/CI unless the dependency is committed to `requirements*.txt`. + +For the full ROS + Python process (including `package.xml` and rosdep), see [Dependency Management](./dependency_management.md). diff --git a/doc/development/quickstart_contributor.md b/doc/development/quickstart_contributor.md index 5d99770e..c67ac5c8 100644 --- a/doc/development/quickstart_contributor.md +++ b/doc/development/quickstart_contributor.md @@ -78,6 +78,7 @@ pre-commit run --all-files - `Lint current Python file with ruff` - `Format current Python file with ruff` - `Lint active package with ruff` + - `Dependency check in dev container` 5. Commit only related files and open a focused PR. ## 7) Common problems @@ -87,3 +88,6 @@ pre-commit run --all-files - If rosdep/pip setup failed during image build, inspect logs inside container: - `/internal_workspace/rosdep_install.log` - `/internal_workspace/pip_install.log` +- If dependency drift appears after changing `requirements*.txt` or `package.xml`, run: + - `dep.sync` + - `devbuild` From 40996f6894103f966d13ffdf0284f129e0f004b6 Mon Sep 17 00:00:00 2001 From: ll7 <32880741+ll7@users.noreply.github.com> Date: Tue, 31 Mar 2026 12:41:34 +0200 Subject: [PATCH 09/43] Add future work document outlining development goals and principles --- doc/dev_talks/paf25/future_work.md | 297 +++++++++++++++++++++++++++++ 1 file changed, 297 insertions(+) create mode 100644 doc/dev_talks/paf25/future_work.md diff --git a/doc/dev_talks/paf25/future_work.md b/doc/dev_talks/paf25/future_work.md new file mode 100644 index 00000000..9afba536 --- /dev/null +++ b/doc/dev_talks/paf25/future_work.md @@ -0,0 +1,297 @@ +# Future Work + +## Table of Contents + +- [1. Goal](#1-goal) +- [2. Recommended Development Principles](#2-recommended-development-principles) +- [3. Proposed Work Packages](#3-proposed-work-packages) + - [3.1 WP1: Documentation and Interface Consolidation](#31-wp1-documentation-and-interface-consolidation) + - [3.2 WP2: Radar Motion Quality](#32-wp2-radar-motion-quality) + - [3.3 WP3: Lidar, Mapping and Tracking Stabilization](#33-wp3-lidar-mapping-and-tracking-stabilization) + - [3.4 WP4: Collision Prediction and Intersection Logic](#34-wp4-collision-prediction-and-intersection-logic) + - [3.5 WP5: Automated Testing and CI Expansion](#35-wp5-automated-testing-and-ci-expansion) + - [3.6 WP6: Performance, Metrics and Observability](#36-wp6-performance-metrics-and-observability) + - [3.7 WP7: Optional Feature Expansion](#37-wp7-optional-feature-expansion) +- [4. Proposed Execution Order](#4-proposed-execution-order) +- [5. Suggested Team Split for 4 Students](#5-suggested-team-split-for-4-students) +- [6. Definition of Done for the Next Phase](#6-definition-of-done-for-the-next-phase) + +## 1. Goal + +This document proposes how development should continue after the current state summarized in [paf25_review_by_ll7.md](./paf25_review_by_ll7.md) and [improvements_assessment.md](./improvements_assessment.md). + +The core recommendation is simple: + +Do not immediately expand feature breadth again. +First consolidate the new architecture so that perception, mapping, planning and testing become more reliable and easier to maintain. + +## 2. Recommended Development Principles + +The next phase should follow these principles: + +### 2.1 Consolidation Before Expansion + +The repository already has enough technically interesting ideas. + +The next phase should focus on making those ideas stable, testable and well-documented before adding many more features. + +### 2.2 Interfaces Must Become Explicit + +The semantics of messages, topics and intermediate representations should be fixed more clearly. + +That is especially important for: + +- motion data +- tracked entities +- classification data +- planning-relevant map entities + +### 2.3 Development Must Be Evidence-Driven + +Future changes should be justified with measurable effects. + +Examples: + +- fewer false positives in cross-traffic detection +- lower radar velocity error +- more stable track ids +- lower collision rate on selected routes + +### 2.4 Integration Should Happen Weekly + +The project already shows signs of integration complexity. + +That means each work package should include recurring integration checkpoints instead of long isolated branches. + +## 3. Proposed Work Packages + +## 3.1 WP1: Documentation and Interface Consolidation + +### Objective + +Bring the active documentation back in sync with the actual implementation and freeze the most important subsystem contracts. + +### Why this should be first + +Right now some documentation is already stale although the code improved a lot. That is a warning sign that architectural knowledge is still too implicit. + +### Main tasks + +- update active docs so they reflect current code behavior +- fix stale topic and message references in architecture docs +- document the semantics of `TrafficLightImages`, `ClusteredPointsArray`, radar compensated points and tracked entities +- define which topics and messages are considered stable interfaces +- fix malformed or incomplete Markdown pages + +### Expected output + +- current architecture documentation +- current perception and planning docs +- no major active doc contradicts the code +- clear subsystem interfaces for further development + +## 3.2 WP2: Radar Motion Quality + +### Objective + +Improve radar velocity estimation so that moving vs stationary classification becomes more reliable. + +### Why this matters + +This is one of the most important known weaknesses in the current stack. It directly affects cross-traffic detection, entity classification and collision reasoning. + +### Main tasks + +- re-evaluate radial velocity interpretation at different azimuth angles +- include ego angular motion where appropriate +- compare per-point vs per-cluster velocity aggregation +- validate sensor-frame vs vehicle-frame transformation assumptions +- create a benchmark set for stationary and moving reference cases + +### Expected output + +- more stable radar motion estimates +- fewer false positives for stationary objects +- documented benchmark results +- updated radar documentation with verified assumptions + +## 3.3 WP3: Lidar, Mapping and Tracking Stabilization + +### Objective + +Make the fusion and tracking pipeline more robust and easier to maintain. + +### Why this matters + +This is currently one of the strongest areas technically, but also one of the most complex. + +### Main tasks + +- split overly large modules into smaller units where possible +- add tests for tracking behavior across multiple frames +- validate radar-lidar assignment thresholds and association behavior +- improve handling of track loss, re-acquisition and noisy detections +- consider explicit confidence values or track quality indicators + +### Expected output + +- easier-to-review fusion code +- more stable tracked entities +- lower regression risk in mapping and tracking + +## 3.4 WP4: Collision Prediction and Intersection Logic + +### Objective + +Move from conservative area-based cross-traffic checks toward more direction-aware and trajectory-aware intersection behavior. + +### Why this matters + +The current cross-traffic logic is useful, but it is still explicitly documented as conservative and potentially prone to false-positive braking. + +### Main tasks + +- use motion direction more explicitly in cross-traffic decisions +- validate the collision prediction logic in `motion_planning.py` +- connect trajectory conflict reasoning more tightly to behavior decisions +- reduce the dependence on simple rectangular speed-threshold checks +- define scenario-based acceptance tests for blocked, safe and ambiguous intersections + +### Expected output + +- fewer unnecessary emergency stops +- better intersection decisions +- a clearer path to replacing the current conservative cross-traffic guard + +## 3.5 WP5: Automated Testing and CI Expansion + +### Objective + +Expand the current route-level testing into a broader reliability net. + +### Why this matters + +The local automatic test harness is a good start, but the most complex logic still lacks sufficient focused test coverage. + +### Main tasks + +- add unit tests for perception utility functions and compensation math +- add tests for radar-lidar assignment and tracking filters +- add tests for traffic-light state buffering and invalid transitions +- add focused tests for collision and intersection helpers +- define a small smoke-test subset that can run regularly in CI + +### Expected output + +- better protection against regressions +- faster debugging of feature work +- clearer confidence in system changes + +## 3.6 WP6: Performance, Metrics and Observability + +### Objective + +Make technical quality measurable. + +### Why this matters + +Without metrics, it is hard to know whether changes improve the system or only change behavior. + +### Main tasks + +- measure perception and mapping latency +- log basic tracking quality metrics +- measure collision and infraction rates on standard routes +- track traffic-light and radar-specific failure cases +- create lightweight debug dashboards or structured result summaries + +### Expected output + +- repeatable quality indicators +- evidence for technical tradeoffs +- easier prioritization for future work + +## 3.7 WP7: Optional Feature Expansion + +### Objective + +Only after consolidation, expand system capability further. + +### Candidate topics + +- improved semantic classification consistency across sensors +- richer track prediction models +- stronger emergency vehicle handling +- more formal occupancy or risk modeling in planning +- additional route and scenario coverage for testing + +### Important note + +This work package should start only after WP1 to WP5 are in a reasonably good state. + +## 4. Proposed Execution Order + +The recommended order is: + +1. WP1: Documentation and interface consolidation +2. WP5: Automated testing and CI expansion +3. WP2: Radar motion quality +4. WP3: Lidar, mapping and tracking stabilization +5. WP4: Collision prediction and intersection logic +6. WP6: Performance, metrics and observability +7. WP7: Optional feature expansion + +This order is intentional. + +It starts by making the current system understandable and testable, then fixes the most important motion-quality weakness, then strengthens fusion and planning on top of that. + +## 5. Suggested Team Split for 4 Students + +If development continues with 4 students, a practical split would be: + +### Student 1: Perception and Radar + +- radar motion quality +- traffic-light reliability follow-up +- perception-side tests + +### Student 2: Mapping and Tracking + +- tracking filter stabilization +- radar-lidar assignment +- entity confidence and fusion cleanup + +### Student 3: Planning and Intersection Logic + +- collision prediction validation +- cross-traffic behavior refinement +- route-level scenario evaluation + +### Student 4: Testing, Tooling and Documentation + +- interface documentation +- CI and smoke tests +- metrics and evaluation tooling +- architecture consistency checks + +This split should not create silos. + +All four should still meet weekly to review: + +- interface changes +- route-test results +- regressions +- updated priorities + +## 6. Definition of Done for the Next Phase + +The next development phase should be considered successful only if the following conditions are met: + +1. Active documentation matches the current implementation. +2. The most important fusion and planning helpers have focused automated tests. +3. Radar motion quality is improved and backed by explicit benchmark results. +4. Cross-traffic and collision behavior is validated on a defined route set. +5. The largest core modules are easier to understand or split into clearer units. +6. Future feature work can proceed on top of a more stable baseline. + +That would turn the current progress from a strong student milestone into a much more robust development platform. From 0f0f5d939adc8b421b70dfe7ff345cd005587af0 Mon Sep 17 00:00:00 2001 From: ll7 <32880741+ll7@users.noreply.github.com> Date: Tue, 31 Mar 2026 12:41:38 +0200 Subject: [PATCH 10/43] Add PAF25 review document detailing repository evolution and improvements --- doc/dev_talks/paf25/paf25_review_by_ll7.md | 446 +++++++++++++++++++++ 1 file changed, 446 insertions(+) create mode 100644 doc/dev_talks/paf25/paf25_review_by_ll7.md diff --git a/doc/dev_talks/paf25/paf25_review_by_ll7.md b/doc/dev_talks/paf25/paf25_review_by_ll7.md new file mode 100644 index 00000000..17220a00 --- /dev/null +++ b/doc/dev_talks/paf25/paf25_review_by_ll7.md @@ -0,0 +1,446 @@ +# PAF25 Review by LL7 + +## Table of Contents + +- [1. Scope and Method](#1-scope-and-method) +- [2. Executive Summary](#2-executive-summary) +- [3. Repository Evolution](#3-repository-evolution) +- [4. Topic Review](#4-topic-review) + - [4.1 Traffic Light](#41-traffic-light) + - [4.2 Radar and Sensor Message Handling](#42-radar-and-sensor-message-handling) + - [4.3 Auto Tests](#43-auto-tests) + - [4.4 Cross Traffic Check](#44-cross-traffic-check) + - [4.5 Lidar, Tracking and Fusion](#45-lidar-tracking-and-fusion) +- [5. Documentation Review Against the Requirements](#5-documentation-review-against-the-requirements) + - [5.1 What Is Clearly Better Than at paf25.start](#51-what-is-clearly-better-than-at-paf25start) + - [5.2 Where the Guidelines Are Not Fully Met Yet](#52-where-the-guidelines-are-not-fully-met-yet) + - [5.3 Requirement-by-Requirement Verdict](#53-requirement-by-requirement-verdict) +- [6. Overall Verdict](#6-overall-verdict) + +## 1. Scope and Method + +This review compares `paf25.start` against `main`. + +The comparison range contains: + +- 221 commits +- 209 changed files +- 6019 insertions and 4867 deletions +- 52 changed Markdown files + +The review is based on: + +- `git diff` and `git log` for the full `paf25.start..main` range +- targeted inspection of the current implementations in perception, mapping, planning and test tooling +- targeted inspection of the updated documentation in `doc/` +- comparison against the rules defined in `doc/development/documentation_requirements.md` + +The emphasis of this report is on: + +- traffic light handling +- radar and sensor message handling +- auto tests +- cross traffic check +- lidar, tracking and related fusion work + +## 2. Executive Summary + +Compared to `paf25.start`, the repository has evolved from a more fragmented and partly legacy-heavy codebase into a more ROS2-focused, fusion-oriented stack with much stronger perception-to-mapping integration. + +The most important technical shift is that perception is no longer only publishing geometric detections. It now publishes richer intermediate data that includes grouped traffic-light crops, lidar clusters, radar-derived motion and class information, and heading changes. Mapping and planning consume that richer data to do radar-lidar association, entity tracking, motion estimation and collision-aware behavior. + +The most visible organizational changes are: + +- major cleanup of legacy ROS1 components +- migration from Flake8 plus Black to Ruff in CI, docs and local tooling +- significantly expanded documentation effort +- introduction of a local automated route-test harness for ROS2 and CARLA leaderboard runs + +Overall, the repository has clearly matured. The biggest strengths are perception and mapping integration, better traffic-light robustness, better lidar compensation and tracking, and much better route-level testing support. The biggest remaining weaknesses are documentation drift, incomplete test depth outside route tests, and known radar-motion limitations that are already documented by the team. + +## 3. Repository Evolution + +At a repository level, the last months were not only feature work. There was also a broad structural cleanup. + +### 3.1 Technical Direction + +The repository moved further toward a ROS2-centered active stack. + +- Large parts of `code-ros1/` were removed or reduced in relevance. +- Build and container files were simplified and modernized. +- Perception, mapping and planning changed together, not independently. +- The intermediate layer now carries more semantic and motion information than before. + +### 3.2 Tooling and Quality Workflow + +The development workflow became stricter and more consistent. + +- Ruff replaced Flake8 and Black. +- CI, VS Code settings, tasks and documentation were updated for Ruff. +- The repo now has a clearer single source of truth for linting and formatting. + +This is a meaningful quality step because it reduces style drift between local development and automation. + +### 3.3 Documentation Activity + +Documentation effort increased strongly. + +- 52 Markdown files changed in the review range. +- New or substantially updated docs were added for lidar, radar, cross-traffic behavior, local automatic tests, parameters and mapping internals. +- Mapping API documentation was regenerated and updated. + +So the repository did not only change in code. It also attempted to explain the new architecture and workflows. The important caveat is that this documentation effort is uneven: some pages are strong and current, while others are already stale or inconsistent with the code. + +## 4. Topic Review + +### 4.1 Traffic Light + +Traffic-light handling became noticeably more robust and more structured. + +#### What changed + +The key architectural change is the introduction of a grouped traffic-light message. + +- `VisionNode` no longer publishes cropped traffic-light images one by one. +- It now collects all plausible traffic-light crops from one frame and publishes them together via the new `TrafficLightImages` message. +- `TrafficLightNode` subscribes to this grouped message instead of a single `sensor_msgs/Image`. + +On top of that, the traffic-light classifier logic became more defensive. + +- multiple crops can be evaluated per frame +- ambiguous frames are discarded +- impossible state transitions are filtered out +- turn-signal-like lights are filtered with a circle-based heuristic +- final state publication is buffered over time instead of reacting instantly to a single crop +- stale state is reset after a timeout + +This is a significant improvement over a more direct per-image classification flow because it reduces flicker and reduces the chance that a single bad crop immediately changes the final state. + +#### Why it matters + +This change directly targets behavior reliability. + +- The commit history shows explicit work on red-light issues. +- The final implementation now has temporal voting and plausibility filtering. +- A dedicated `routes_traffic_light.xml` route was added, which indicates that traffic-light behavior became important enough to justify isolated testing. + +#### Remaining weaknesses + +The code is ahead of the documentation here. + +- `doc/perception/traffic_light_detection.md` still describes outdated APIs and behavior in several places. +- It still refers to a single image input, outdated helper functions and an older node model. + +So the implementation improved substantially, but the documentation did not keep up fully. + +### 4.2 Radar and Sensor Message Handling + +This area saw one of the largest conceptual changes in the repository. + +#### What changed + +Radar is no longer treated mainly as a standalone clustering source. + +Instead, the pipeline increasingly uses radar as a motion sensor that enriches lidar-based entities. + +Important changes include: + +- `radar_node.py` now subscribes to ego speed in addition to radar and IMU data. +- IMU-based pitch estimation is used for ground reflection filtering. +- The node can buffer radar messages over a configurable time window. +- When clustering is disabled, the node publishes ego-motion-compensated per-point motion on `/paf/hero/Radar/compensated_points`. +- `ClusteredPointsArray` was extended to carry `motion_array` and `object_class`. +- Mapping now consumes those richer messages and can associate radar motion with lidar entities. + +This is a major evolution in sensor-message handling because the interface between perception and mapping became semantically richer. The messages are no longer just containers for point positions and cluster ids. + +#### Fusion direction + +The current direction is clear: + +- lidar provides geometry and object extent +- radar provides motion cues +- mapping performs association and integrates both + +The addition of `RadarPointAssignmentFilter` and the new association buffer parameter in mapping makes that explicit. The system is moving away from treating radar detections as fully independent final objects. + +#### Why it matters + +This improves several downstream behaviors: + +- cross-traffic detection +- collision prediction +- dynamic entity tracking +- classification of moving vs stationary objects + +#### Remaining weaknesses + +This part is improved, but not fully solved. + +- `doc/dev_talks/paf25/radar_velocity_issue.md` documents that stationary objects can still receive unrealistic velocities. +- `doc/perception/radar_node.md` explicitly warns that angular ego motion is still not considered in the velocity estimation. +- The current pipeline is therefore clearly more advanced than before, but it is still an engineering compromise rather than a finished radar-motion solution. + +### 4.3 Auto Tests + +The repository made clear progress on automated regression testing, especially at route level. + +#### What changed + +A local ROS2-oriented test harness was added. + +- `code/test/run_test.py` is a large CARLA leaderboard-based local test runner. +- `code/test/index_dict.py` defines named scenarios with time thresholds. +- `code/routes/test.xml` was expanded heavily. +- `code/leaderboard_launcher/scripts/launch_leaderboard.test.sh` was added. +- `doc/general/ros2_local_test.md` documents how to run and extend the tests. + +The runner supports: + +- route execution through the leaderboard framework +- statistics collection +- checkpoint output +- resume support +- post-run evaluation of infractions such as collisions and red-light violations + +This is a strong practical step because it gives the team a repeatable way to validate end-to-end behavior locally. + +#### Additional testing progress + +There is also at least one focused perception test now: + +- `code/perception/tests/test_ego_motion_compensation.py` + +That test validates the lidar ego-motion compensation math using controlled pose scenarios. + +#### Remaining weaknesses + +Testing improved, but it is still not comprehensive in the sense required by the documentation rules. + +- Most of the new coverage is route-level and scenario-level. +- The codebase still has relatively few focused unit or integration tests for the many changes in planning, radar fusion and tracking. + +So the repository evolved from almost no convincing automation in these new areas to a useful regression harness, but not yet to broad test coverage. + +### 4.4 Cross Traffic Check + +Cross-traffic handling became much more data-driven. + +#### What changed + +The old approach was simplified away and replaced by logic that uses dynamic map entities and their motion. + +Key changes: + +- obsolete helpers like `has_cross_traffic` and `check_cross_traffic` were removed +- `intersection.py` now contains explicit priority cross-traffic checks based on overlapping dynamic entities +- the behavior uses speed thresholds and rectangular check zones in `Wait` and `Enter` +- if fast cross traffic is detected while the ego vehicle is still moving, emergency signaling can be triggered + +This is a clear upgrade from static or heuristic-only intersection logic. + +#### Interaction with planning + +Cross-traffic logic also became closer to collision reasoning. + +- `motion_planning.py` now subscribes to the current map +- it predicts local trajectories for entities +- it checks trajectory collisions using `time_horizon` and `crash_threshold` +- collision trajectories are published for visualization + +This means the repo is moving from simple intersection gating toward actual trajectory-based conflict estimation. + +#### Remaining weaknesses + +The repository itself documents that this logic is still conservative. + +`doc/planning/behaviors/Intersection.md` correctly states that: + +- the current cross-traffic check does not yet use motion direction robustly +- the rectangular check area rotates with the ego vehicle +- false-positive braking is therefore possible +- long term, the dedicated cross-traffic check may be replaced by a sufficiently reliable collision check + +This is an honest and technically correct limitation statement. + +### 4.5 Lidar, Tracking and Fusion + +This is probably the strongest technical growth area in the whole comparison range. + +#### What changed in lidar itself + +`lidar_distance.py` was substantially redesigned. + +The most important step is the strategy-based compensation architecture. + +The node now supports: + +- `NoCompensation` +- `Buffer` +- `EgoMotionCompensation` +- `LocalCompensation` + +Depending on the mode, the node consumes: + +- previous lidar frames +- EKF pose +- ego speed +- IMU heading + +The node also now publishes delta heading for downstream use. + +Clustering improved as well. + +- an additional upper z filter was added +- clustering parameters changed +- the clustering logic moved toward a distance-aware, polar-like representation to better handle lidar point density + +This is much more mature than a plain frame-by-frame DBSCAN over raw points. + +#### What changed in tracking and mapping + +The intermediate layer changed from static integration toward actual temporal tracking. + +Important additions: + +- `TrackingFilter` was added in mapping +- tracking uses two-frame history and Hungarian matching +- tracking supports type persistence across frames +- motion updates can be toggled +- delta heading from lidar compensation is fed into tracking +- sensor ids are propagated and deduplicated + +At the same time, lidar entities can now receive radar-derived motion through `RadarPointAssignmentFilter`. + +This is the clearest example of the repository evolving from "perception publishes detections" to "perception and mapping together build tracked dynamic entities". + +#### Why it matters + +This work directly enables: + +- better dynamic obstacle understanding +- motion-aware planning +- cross-traffic reasoning +- collision prediction +- more stable object identity across frames + +#### Remaining weaknesses + +The technical direction is strong, but some modules are now very large. + +- `lidar_distance.py` +- `radar_node.py` +- parts of mapping integration and filtering + +These files are more capable than before, but also harder to maintain and review. So the architecture improved, while local code complexity also increased. + +## 5. Documentation Review Against the Requirements + +## 5.1 What Is Clearly Better Than at paf25.start + +The repository clearly invested real effort into documentation. + +Positive examples: + +- `doc/perception/lidar_distance.md` is detailed, structured and technically useful. +- `doc/perception/radar_node.md` explains design decisions, sensor placement and known limitations. +- `doc/planning/behaviors/Intersection.md` documents both the behavior and its current limitations. +- `doc/general/ros2_local_test.md` explains how the local automatic test workflow works. +- mapping documentation was refreshed heavily. +- the linting/documentation requirements themselves were improved. + +In other words: the repo did not just accumulate code. It also tried to preserve engineering context. + +## 5.2 Where the Guidelines Are Not Fully Met Yet + +The problem is not lack of documentation effort. The problem is consistency and maintenance. + +### Example 1: Traffic-light documentation is partially stale + +`doc/perception/traffic_light_detection.md` still contains outdated descriptions, for example: + +- it describes older APIs and node behavior +- it still talks about a single image flow where the implementation now uses `TrafficLightImages` +- it references logic that is no longer present in the code in the same form + +This violates the maintenance requirement and partially also the explicit-usage requirement. + +### Example 2: Architecture documentation is stale + +`doc/general/architecture_current.md` still contains outdated topic and message references, for example: + +- old message paths for clustered point messages +- ROS1-style or outdated references around dynamic reconfigure and message types +- a traffic-light pipeline description that no longer reflects the current grouped-image message flow + +This weakens the value of the architecture document because it can mislead new contributors. + +### Example 3: Not all new docs follow the required Markdown structure + +The documentation requirements ask for a table of contents and numbered headings for all documents. + +This is not applied consistently. + +- `doc/perception/lidar_distance.md` and `doc/perception/radar_node.md` follow the structure reasonably well. +- `doc/perception/traffic_light_detection.md` has a linked list but not the same numbering discipline. +- `doc/planning/behaviors/Intersection.md` is clear, but not numbered as required. +- `doc/general/ros2_local_test.md` is useful, but also does not follow the numbering convention from the template. +- `doc/perception/radar_raw_debugger.md` is minimal and does not follow the more complete template structure. + +### Example 4: At least one Markdown file is malformed + +`doc/perception/radar_raw_debugger.md` currently ends with an opening fenced code block and no closing fence. + +That directly violates the documentation-quality requirement. + +### Example 5: In-code documentation improved, but not uniformly + +The new lidar compensation code and some perception code now have proper docstrings and clearer internal structure. +However, some of the largest and most important modules are still very large and multi-purpose. + +This means readability and maintainability improved in parts, but the codebase does not uniformly meet the documented standard. + +## 5.3 Requirement-by-Requirement Verdict + +The following verdict is limited to what can be assessed from the local repository. + +| Requirement Area | Verdict | Assessment | +| --- | --- | --- | +| Remove or clearly mark deprecated code | Partially met | Good progress: large ROS1 cleanup, discontinued docs, removed obsolete logic. Not fully met because some documentation still describes outdated behavior. | +| Python linting and formatting | Met | Ruff was introduced across CI, tasks, docs and local tooling. | +| Python docstrings | Partially met | Better than before, especially in lidar/radar related code, but still uneven. | +| Readability and maintainability | Partially met | Several subsystems became more modular, but some files are now very large and combine too many responsibilities. | +| Code structure and modularity | Partially met | Strategy pattern and filters are improvements, but complexity is still concentrated in a few large modules. | +| Efficiency and performance | Mostly met | The repo shows concrete algorithmic work on compensation, clustering and tracking. I did not find evidence of performance being ignored. | +| Error handling | Partially met | Some nodes improved logging and guarded behavior, but robustness still varies by module. | +| Testing | Partially met | Strong route-level autotest improvement and one focused perception test, but not comprehensive coverage for all major changes. | +| Markdown structure and organization | Partially met | Many docs improved, but TOC and numbered-heading rules are not applied consistently. | +| Content detail and usage docs | Mostly met | Radar, lidar and autotest docs contain substantial technical detail. Traffic-light and architecture docs are weaker because they drifted. | +| Visual aids | Partially met | Some useful images exist, but several new docs omit diagrams or screenshots. | +| Maintenance and updates | Partially met | Documentation was updated often, but not always kept aligned with the final code. | +| Deprecation handling in docs | Partially met | Good use of `discontinued/` and some cleanup, but stale active docs remain. | +| GitHub repository hygiene | Not fully assessable locally | Issues, PR closure and remote branch hygiene cannot be verified from the local checkout alone. | + +## 6. Overall Verdict + +Compared to `paf25.start`, the repository has evolved substantially and in the right direction. + +The strongest improvements are: + +- a much more capable perception-to-mapping pipeline +- better traffic-light robustness +- radar-lidar fusion with motion-aware entity handling +- actual multi-frame tracking in mapping +- new local automatic route testing +- better tooling and linting discipline + +The most important technical evolution is that motion information now matters throughout the stack. Radar, lidar, mapping and planning are more tightly connected than before, and the repository looks much more like a system for dynamic-scene reasoning rather than a collection of independent nodes. + +The main remaining weaknesses are: + +- documentation drift in a few important active docs +- insufficient test depth beyond route-level regression tests +- known radar-motion limitations that are documented but not fully solved +- increasing complexity in a few large core modules + +In summary, the repository did not merely accumulate new features during the last months. It became architecturally more coherent, more ROS2-focused, more fusion-aware and more testable. The remaining work is mostly in consolidation: align the documentation with the final code, keep reducing legacy leftovers, and add more targeted automated tests for the new planning and fusion logic. From 2ffe0a5a984bd349f43813129376bbaad5dbdb3c Mon Sep 17 00:00:00 2001 From: ll7 <32880741+ll7@users.noreply.github.com> Date: Tue, 31 Mar 2026 12:41:42 +0200 Subject: [PATCH 11/43] Add improvements assessment document summarizing project evolution and recommendations --- .../paf25/improvements_assessment.md | 239 ++++++++++++++++++ 1 file changed, 239 insertions(+) create mode 100644 doc/dev_talks/paf25/improvements_assessment.md diff --git a/doc/dev_talks/paf25/improvements_assessment.md b/doc/dev_talks/paf25/improvements_assessment.md new file mode 100644 index 00000000..5dafc280 --- /dev/null +++ b/doc/dev_talks/paf25/improvements_assessment.md @@ -0,0 +1,239 @@ +# Improvements Assessment + +## Table of Contents + +- [Table of Contents](#table-of-contents) +- [1. Context](#1-context) +- [2. Did the Project Improve by a Lot?](#2-did-the-project-improve-by-a-lot) +- [3. Was This Good Work for 4 Students?](#3-was-this-good-work-for-4-students) +- [4. What Was Done Well](#4-what-was-done-well) + - [4.1 Architectural Improvement](#41-architectural-improvement) + - [4.2 Focus on Dynamic Scene Understanding](#42-focus-on-dynamic-scene-understanding) + - [4.3 Traffic-Light Robustness](#43-traffic-light-robustness) + - [4.4 Testing Progress](#44-testing-progress) + - [4.5 Tooling and Cleanup](#45-tooling-and-cleanup) +- [5. What Could Have Been Done Better](#5-what-could-have-been-done-better) + - [5.1 More Consolidation, Less Parallel Breadth](#51-more-consolidation-less-parallel-breadth) + - [5.2 Stronger Interface Discipline](#52-stronger-interface-discipline) + - [5.3 More Focused Automated Tests](#53-more-focused-automated-tests) + - [5.4 Earlier Refactoring of Large Core Modules](#54-earlier-refactoring-of-large-core-modules) + - [5.5 Better Use of Explicit Evaluation Metrics](#55-better-use-of-explicit-evaluation-metrics) +- [6. Overall Assessment](#6-overall-assessment) +- [7. Recommended Development Direction](#7-recommended-development-direction) + +## 1. Context + +This document summarizes the practical implications of the review in [paf25_review_by_ll7.md](./paf25_review_by_ll7.md). + +The key question here is not only whether the repository changed, but whether it changed in a meaningful way relative to the likely available time and team size. + +The comparison range from `paf25.start` to `main` showed: + +- 221 commits +- 209 changed files +- 6019 insertions and 4867 deletions +- large work in perception, mapping, planning, tests, documentation and tooling + +## 2. Did the Project Improve by a Lot? + +Yes. The project improved by a lot. + +The improvement is not just visible in raw commit count or file count. It is visible in the technical level of the stack. + +At `paf25.start`, the repository still looked more fragmented, more legacy-heavy and less integrated across perception, mapping and planning. + +By `main`, the repository had clearly moved toward: + +- a more ROS2-centered active stack +- richer intermediate data between subsystems +- stronger radar-lidar fusion +- actual entity tracking instead of only single-frame detections +- more robust traffic-light handling +- explicit cross-traffic and collision reasoning +- much better route-level regression testing support +- more modern linting and formatting workflow + +This is not a cosmetic improvement. It is a structural improvement. + +The best sign of real progress is that the repository now contains more motion-aware reasoning throughout the pipeline: + +- perception publishes richer data +- mapping tracks and associates entities +- planning starts to consume that motion information for safety decisions + +That is a meaningful evolution in system capability. + +## 3. Was This Good Work for 4 Students? + +Yes, overall this was good work for 4 students. + +More precisely: this looks like strong student engineering work with clear ambition and real technical substance. + +Why this is a good result for 4 students: + +- The team did not only add features. It also cleaned up legacy code and modernized tooling. +- Multiple subsystems changed coherently instead of drifting apart completely. +- The work includes not only perception features, but also mapping, planning, route testing and documentation. +- Some changes are technically non-trivial, especially lidar compensation, tracking and radar-lidar association. + +This does not look like shallow feature accumulation. It looks like a team that tried to improve the actual architecture. + +That said, this is good student work, not finished product work. + +The codebase still shows the typical pattern of a strong student project under time pressure: + +- breadth improved faster than finish quality +- several important ideas are implemented, but not fully consolidated +- documentation effort is real, but not always synchronized with the final code +- testing exists, but is not yet deep enough for all the new logic + +So the right conclusion is: + +- yes, this was good work for 4 students +- no, it is not yet a fully polished or fully validated system + +That is a fair and technically defensible judgment. + +## 4. What Was Done Well + +### 4.1 Architectural Improvement + +The strongest achievement is that the repository became more coherent. + +The project moved away from isolated node improvements and toward a real perception to mapping to planning pipeline. + +That is the most important sign of maturity. + +### 4.2 Focus on Dynamic Scene Understanding + +The team invested in motion-aware functionality rather than only static detection. + +Good examples are: + +- radar-derived motion handling +- lidar compensation strategies +- tracked entities in mapping +- cross-traffic logic +- collision prediction hooks in planning + +These are relevant system-level improvements, not only convenience features. + +### 4.3 Traffic-Light Robustness + +Traffic-light handling became more robust through: + +- grouped per-frame crop handling +- temporal buffering +- plausibility filtering +- timeout-based invalidation + +That is a good example of engineering for reliability instead of only raw detection. + +### 4.4 Testing Progress + +Adding a usable local route-test harness was a good decision. + +For a student team, route-level regression tests are highly valuable because they provide immediate end-to-end feedback on behavior changes. + +### 4.5 Tooling and Cleanup + +The move to Ruff and the cleanup of old ROS1-heavy parts of the repository were good maintenance decisions. + +These changes reduce friction for future contributors and help keep the repo maintainable. + +## 5. What Could Have Been Done Better + +### 5.1 More Consolidation, Less Parallel Breadth + +The team improved many areas at once. That shows ambition, but it also created finish-quality gaps. + +The clearest symptom is that some of the most important active docs already drifted behind the implementation. + +A stronger consolidation phase near the end would likely have helped. + +### 5.2 Stronger Interface Discipline + +The repository would have benefited from freezing some subsystem interfaces earlier. + +Examples: + +- message definitions between perception and mapping +- topic-level architecture documentation +- expected semantics of motion data and classification data + +The code changed faster than the architectural description. + +### 5.3 More Focused Automated Tests + +The route-test work is good, but there should have been more targeted tests for: + +- radar motion compensation +- radar-lidar assignment +- tracking behavior over multiple frames +- collision prediction logic +- traffic-light state transitions + +Without those tests, regression risk remains relatively high in the most complex parts of the system. + +### 5.4 Earlier Refactoring of Large Core Modules + +Some files now carry a lot of responsibility. + +Examples include: + +- lidar processing +- radar processing +- mapping integration +- tracking logic + +These modules are more capable than before, but also harder to reason about, test and review. + +More decomposition into smaller units would have improved maintainability. + +### 5.5 Better Use of Explicit Evaluation Metrics + +The repo shows useful scenario and route testing, but the development process would have been stronger with clearer subsystem-level metrics, for example: + +- tracking stability +- false positive rate for cross-traffic detection +- velocity estimation error for stationary vs moving objects +- traffic-light misclassification rate +- latency and throughput in the perception pipeline + +Those metrics would make technical tradeoffs easier to defend. + +## 6. Overall Assessment + +If this work corresponds to roughly the last 4 months and roughly 4 students, I would judge it as a clearly positive result. + +The team appears to have delivered: + +- substantial technical progress +- meaningful architectural improvement +- useful tooling and testing progress +- visible engineering effort in documentation and cleanup + +I would not judge the result as "finished" or "fully production-ready". +I would judge it as a strong intermediate engineering milestone. + +In plain terms: + +- The project improved a lot. +- The work is good for 4 students. +- The main shortcoming is not lack of effort, but incomplete consolidation. + +## 7. Recommended Development Direction + +The best next step is not to add many new features immediately. + +The best next step is to turn the current progress into a more stable and defensible system. + +The recommended direction is: + +1. Consolidate interfaces and documentation. +2. Strengthen automated tests around the new fusion and planning logic. +3. Fix known radar-motion and cross-traffic limitations. +4. Reduce complexity in the largest modules. +5. Only then expand feature scope further. + +That approach would maximize the value of the work that has already been done. From 336dc747c1fa06c2014e00acaeb51fcfcad769b0 Mon Sep 17 00:00:00 2001 From: ll7 Date: Tue, 31 Mar 2026 13:50:11 +0200 Subject: [PATCH 12/43] feat: add dependency validation workflow and enhance development scripts - Introduced a new GitHub Actions workflow for dependency validation. - Added a pre-PR quality check task in VS Code. - Implemented a dependency doctor script to catch pinning issues. - Updated README and documentation for performance iteration and dependency management. - Added contract tests for ROS interface package manifests and launch files. - Enhanced existing scripts and configurations for better development experience. --- .github/pull_request_template.md | 1 + .github/workflows/dependency-doctor.yml | 27 +++++++ .github/workflows/unit-tests.yml | 28 +++++++ .gitignore | 3 + .vscode/tasks.json | 21 +++++ .../agent-ros2/scripts/devfunctions.bash | 23 ++++++ code/requirements_infrastructure.txt | 6 +- code/test/test_interface_contracts.py | 42 ++++++++++ code/test/test_launch_manifests.py | 57 +++++++++++++ code/test/test_planning_regression.py | 34 ++++++++ doc/development/README.md | 1 + doc/development/dependency_management.md | 20 +++++ doc/development/performance_iteration.md | 47 +++++++++++ doc/development/quickstart_contributor.md | 3 + pytest.ini | 2 +- scripts/dependency-doctor.sh | 79 +++++++++++++++++++ 16 files changed, 390 insertions(+), 4 deletions(-) create mode 100644 .github/workflows/dependency-doctor.yml create mode 100644 .github/workflows/unit-tests.yml create mode 100644 code/test/test_interface_contracts.py create mode 100644 code/test/test_launch_manifests.py create mode 100644 code/test/test_planning_regression.py create mode 100644 doc/development/performance_iteration.md create mode 100755 scripts/dependency-doctor.sh diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md index c8c9be85..00d6d8bd 100644 --- a/.github/pull_request_template.md +++ b/.github/pull_request_template.md @@ -43,4 +43,5 @@ List remaining risks or follow-up work that is intentionally out of scope. - [ ] I have added tests that prove my fix is effective or that my feature works (might be obsolete with CI later on) - [ ] New and existing unit tests pass locally with my changes (might be obsolete with CI later on) - [ ] I documented assumptions, validation steps, and known gaps in this PR +- [ ] I ran dependency validation (`dep.check` or `scripts/dependency-doctor.sh`) when touching dependencies diff --git a/.github/workflows/dependency-doctor.yml b/.github/workflows/dependency-doctor.yml new file mode 100644 index 00000000..bbdd92dd --- /dev/null +++ b/.github/workflows/dependency-doctor.yml @@ -0,0 +1,27 @@ +name: Dependency doctor + +on: + pull_request: + branches: + - main + push: + branches: + - main + +jobs: + dependency-doctor: + name: Validate dependency manifests + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 + with: + python-version: "3.12" + - name: Verify pinned and consistent requirements + run: bash scripts/dependency-doctor.sh + - name: Install infra dependencies + run: | + python -m pip install --upgrade pip + pip install -r code/requirements_infrastructure.txt + - name: Validate Python package dependency graph + run: python -m pip check diff --git a/.github/workflows/unit-tests.yml b/.github/workflows/unit-tests.yml new file mode 100644 index 00000000..6dccc508 --- /dev/null +++ b/.github/workflows/unit-tests.yml @@ -0,0 +1,28 @@ +name: Unit tests (fast) + +on: + pull_request: + branches: + - main + push: + branches: + - main + +jobs: + unit-tests: + name: Run fast unit tests + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 + with: + python-version: "3.12" + - name: Install test dependencies + run: | + python -m pip install --upgrade pip + pip install -r code/requirements_infrastructure.txt + pip install numpy + - name: Run unit tests (plugin autoload disabled) + env: + PYTEST_DISABLE_PLUGIN_AUTOLOAD: "1" + run: pytest code/test -m unit diff --git a/.gitignore b/.gitignore index a5e56948..62839a27 100644 --- a/.gitignore +++ b/.gitignore @@ -22,3 +22,6 @@ __pycache__/ # Auto downloaded models code/*.pt /*.pt + +# VS Code transient browse database files +.vscode/browse.vc.db* diff --git a/.vscode/tasks.json b/.vscode/tasks.json index be01e128..81267a01 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -141,6 +141,27 @@ "problemMatcher": [], "detail": "Installs rosdep and pip dependencies from package.xml and requirements manifests." }, + { + "label": "Build active package with colcon", + "type": "shell", + "command": "bash -lc 'if [[ \"${relativeFile}\" == code/* ]]; then pkg=${relativeFile#code/}; pkg=${pkg%%/*}; source /internal_workspace/dev.bashrc && devbuild.pkg ${pkg}; else echo \"Open a file inside code//... first.\"; exit 1; fi'", + "problemMatcher": [], + "detail": "Builds only the package derived from the currently active file path." + }, + { + "label": "Pre-PR quality check", + "type": "shell", + "command": "bash -lc 'source /internal_workspace/dev.bashrc && dep.check && ruff check /workspace/code/ && ruff format /workspace/code/ --check && PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 pytest code/test -m unit'", + "problemMatcher": [], + "detail": "Runs dependency check, lint, format, and fast unit tests before opening a PR." + }, + { + "label": "Run dependency doctor", + "type": "shell", + "command": "bash ${workspaceFolder}/scripts/dependency-doctor.sh", + "problemMatcher": [], + "detail": "Checks requirements pinning and cross-file version conflicts." + }, ], "inputs": [ { diff --git a/build/docker/agent-ros2/scripts/devfunctions.bash b/build/docker/agent-ros2/scripts/devfunctions.bash index 0cb784b9..cc01c490 100755 --- a/build/docker/agent-ros2/scripts/devfunctions.bash +++ b/build/docker/agent-ros2/scripts/devfunctions.bash @@ -6,6 +6,7 @@ cat <: colcon builds only selected package for faster local iteration - pytrees.viewer: Launches the py-trees tree viewer for behavior trees Note: You need to have a behavior tree running (like the agent) for this to show anything - leaderboard.dev: Starts code/leaderboard_launcher/scripts/launch_leaderboard.dev.sh @@ -50,6 +51,28 @@ EOF } export -f devbuild +devbuild.pkg() { + if [ -z "${1:-}" ]; then + echo "Usage: devbuild.pkg " + return 2 + fi + + ( + cd "${PAF_ROS_WS}" || return $? + + colcon build --symlink-install --continue-on-error --packages-select "$1" || return $? + ) + + cat < None: + """Ensure each interface package.xml contains the expected rosidl contract.""" + assert package_xml_path.exists() + + root = ET.parse(package_xml_path).getroot() + assert root.tag == "package" + + for tag_name, required_values in REQUIRED_TEXT_TAGS.items(): + values = { + node.text.strip() + for node in root.findall(tag_name) + if node.text and node.text.strip() + } + assert required_values.issubset(values), ( + f"Missing {tag_name} entries in {package_xml_path.name}: " + f"expected {sorted(required_values)}, got {sorted(values)}" + ) diff --git a/code/test/test_launch_manifests.py b/code/test/test_launch_manifests.py new file mode 100644 index 00000000..97dbc17d --- /dev/null +++ b/code/test/test_launch_manifests.py @@ -0,0 +1,57 @@ +"""Static smoke tests for ROS launch manifest files.""" + +from __future__ import annotations + +import xml.etree.ElementTree as ET +from pathlib import Path + +import pytest + +pytestmark = pytest.mark.unit + +LAUNCH_XML_FILES = [ + Path("/workspace/code/acting/launch/acting.xml"), + Path("/workspace/code/agent/launch/agent.dev.xml"), + Path("/workspace/code/control/launch/control.xml"), + Path("/workspace/code/localization/launch/localization.xml"), + Path("/workspace/code/mapping/launch/mapping.xml"), + Path("/workspace/code/perception/launch/perception.xml"), + Path("/workspace/code/planning/launch/planning.dev.xml"), +] + + +@pytest.mark.parametrize("launch_path", LAUNCH_XML_FILES) +def test_launch_xml_is_well_formed(launch_path: Path) -> None: + """Validate that core launch XML files are present and parseable.""" + assert launch_path.exists() + root = ET.parse(launch_path).getroot() + assert root.tag == "launch" + + +def test_agent_launch_contains_core_subsystems() -> None: + """Ensure the dev agent launch includes the subsystem launch files.""" + agent_launch = Path("/workspace/code/agent/launch/agent.dev.xml") + root = ET.parse(agent_launch).getroot() + included_files = {include.get("file", "") for include in root.findall("include")} + + expected_fragments = { + "perception.xml", + "planning.dev.xml", + "acting.xml", + "mapping.xml", + } + + for fragment in expected_fragments: + assert any(fragment in file_path for file_path in included_files) + + +def test_agent_persistent_launch_contains_localization() -> None: + """Ensure persistent launch file keeps localization and persistent planning.""" + persistent_launch = Path("/workspace/code/agent/launch/agent.dev.persistent.xml") + root = ET.parse(persistent_launch).getroot() + included_files = {include.get("file", "") for include in root.findall("include")} + + assert any("localization.xml" in file_path for file_path in included_files) + assert any( + "planning.dev.persistent.xml" in file_path for file_path in included_files + ) diff --git a/code/test/test_planning_regression.py b/code/test/test_planning_regression.py new file mode 100644 index 00000000..562f71f2 --- /dev/null +++ b/code/test/test_planning_regression.py @@ -0,0 +1,34 @@ +"""Deterministic regression tests for planning helper behavior.""" + +from __future__ import annotations + +import sys +from pathlib import Path +import importlib + +import pytest + +pytestmark = pytest.mark.unit + +PLANNING_SRC = Path("/workspace/code/planning") +if str(PLANNING_SRC) not in sys.path: + sys.path.insert(0, str(PLANNING_SRC)) + + +@pytest.fixture(scope="module") +def planning_help_functions(): + """Import planning helper module after path setup.""" + return importlib.import_module("planning.global_planner.help_functions") + + +def test_linear_interpolation_snapshot(planning_help_functions) -> None: + """Check stable interpolation output for a known geometry.""" + points = planning_help_functions.linear_interpolation( + (0.0, 0.0), (4.0, 0.0), interval_m=1.5 + ) + assert points == [(0.0, 0.0), (2.0, 0.0), (4.0, 0.0)] + + +def test_scale_vector_zero_is_stable(planning_help_functions) -> None: + """Check behavior for zero vectors remains deterministic and safe.""" + assert planning_help_functions.scale_vector((0.0, 0.0), 5.0) == (0, 0) diff --git a/doc/development/README.md b/doc/development/README.md index d56792fd..71d3f6d3 100644 --- a/doc/development/README.md +++ b/doc/development/README.md @@ -31,6 +31,7 @@ If you contribute to this project please read the following guidelines first: 11. [Logging Standard](./logging.md) 12. [Context Retention](./context_retention.md) 13. [Dependency Management](./dependency_management.md) +14. [Performance Iteration Guide](./performance_iteration.md) ## Administrative Guidelines diff --git a/doc/development/dependency_management.md b/doc/development/dependency_management.md index 5be300ab..0c2e142b 100644 --- a/doc/development/dependency_management.md +++ b/doc/development/dependency_management.md @@ -57,6 +57,26 @@ Use `Dev Containers: Rebuild Container` when you change image-level inputs such For regular `package.xml` / `requirements*.txt` updates, an image rebuild is usually not needed; run `dep.sync` + `devbuild` in the existing container. +### Rebuild vs sync decision table + +| Change type | `dep.sync` | `devbuild` | Rebuild container | +|---|---|---|---| +| `code/**/requirements*.txt` | ✅ | ✅ | ❌ | +| `code/**/package.xml` | ✅ | ✅ | ❌ | +| `build/docker/agent-ros2/scripts/*.sh` | ❌ | ❌ | ✅ | +| `build/docker/agent-ros2/Dockerfile` | ❌ | ❌ | ✅ | +| `build/pins/*.env` | ❌ | ❌ | ✅ | + +## Dependency doctor + +Use the repository doctor script to catch pinning issues and conflicts early: + +```bash +bash scripts/dependency-doctor.sh +``` + +This check is also executed in CI by `.github/workflows/dependency-doctor.yml`. + ## Troubleshooting - Build-time failures: diff --git a/doc/development/performance_iteration.md b/doc/development/performance_iteration.md new file mode 100644 index 00000000..5d5e7572 --- /dev/null +++ b/doc/development/performance_iteration.md @@ -0,0 +1,47 @@ +# Performance Iteration Guide + +## Goal + +Reduce local iteration time while preserving correctness. + +## Fast local build strategy + +Inside the dev container, prefer package-selective builds during development: + +```bash +devbuild.pkg +``` + +Use full rebuilds (`devbuild`) before larger integration checks. + +## Suggested daily loop + +1. Edit one package. +2. Run `devbuild.pkg `. +3. Run `ruff check` and relevant unit tests. +4. Run full workspace checks before PR. + +## Profiling guidance + +Start with reproducible micro/algorithm-level profiling for Python hotspots. + +- Lightweight CPU profiling: + +```bash +python -m cProfile -o /tmp/profile.out +python -m pstats /tmp/profile.out +``` + +- Sampling profiler for running nodes (inside container): + +```bash +py-spy top --pid +``` + +Prioritize fixes only for top hot paths with measurable impact. + +## CI/runtime performance considerations + +- Keep fast unit tests separate from integration/simulation jobs. +- Avoid enabling heavy simulation or profiling in default PR workflows. +- Track benchmark regressions in dedicated scheduled jobs. diff --git a/doc/development/quickstart_contributor.md b/doc/development/quickstart_contributor.md index c67ac5c8..29a0e20f 100644 --- a/doc/development/quickstart_contributor.md +++ b/doc/development/quickstart_contributor.md @@ -79,6 +79,7 @@ pre-commit run --all-files - `Format current Python file with ruff` - `Lint active package with ruff` - `Dependency check in dev container` + - `Pre-PR quality check` 5. Commit only related files and open a focused PR. ## 7) Common problems @@ -91,3 +92,5 @@ pre-commit run --all-files - If dependency drift appears after changing `requirements*.txt` or `package.xml`, run: - `dep.sync` - `devbuild` +- If requirements consistency is unclear across files, run: + - `Run dependency doctor` task or `bash scripts/dependency-doctor.sh` diff --git a/pytest.ini b/pytest.ini index 4ed12f6f..105ad3e8 100644 --- a/pytest.ini +++ b/pytest.ini @@ -1,7 +1,7 @@ [pytest] testpaths = code python_files = test_*.py *_test.py -addopts = -ra --strict-config --strict-markers +addopts = -ra --strict-config --strict-markers --ignore=code/test/run_test.py xfail_strict = true norecursedirs = .git .github .devcontainer build volumes code-ros1 markers = diff --git a/scripts/dependency-doctor.sh b/scripts/dependency-doctor.sh new file mode 100755 index 00000000..e7773e09 --- /dev/null +++ b/scripts/dependency-doctor.sh @@ -0,0 +1,79 @@ +#!/usr/bin/env bash +set -euo pipefail + +REPO_ROOT="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." &>/dev/null && pwd)" +export REPO_ROOT + +python3 - <<'PY' +from __future__ import annotations + +import os +from pathlib import Path + +repo_root = Path(os.environ["REPO_ROOT"]) +code_root = repo_root / "code" + +requirement_files = sorted(code_root.glob("requirements*.txt")) +if not requirement_files: + raise SystemExit("No requirements*.txt files found under code/.") + +pins: dict[str, set[str]] = {} +pin_files: dict[str, set[str]] = {} +unpinned_entries: list[tuple[Path, str]] = [] + +flavour_requirement_files = { + "requirements.cpu.txt", + "requirements.cuda.txt", + "requirements.rocm.txt", +} + +for req_file in requirement_files: + for raw_line in req_file.read_text(encoding="utf-8").splitlines(): + line = raw_line.strip() + if not line or line.startswith("#"): + continue + if line.startswith(("-r", "--")): + continue + + if "==" not in line: + unpinned_entries.append((req_file, line)) + continue + + package, version = line.split("==", maxsplit=1) + package = package.strip().lower() + version = version.strip() + if not package or not version: + unpinned_entries.append((req_file, line)) + continue + pins.setdefault(package, set()).add(version) + pin_files.setdefault(package, set()).add(req_file.name) + +if unpinned_entries: + print("Found unpinned requirement entries:") + for path, line in unpinned_entries: + print(f"- {path.relative_to(repo_root)}: {line}") + raise SystemExit(1) + +conflicts: dict[str, set[str]] = {} +for pkg, versions in pins.items(): + if len(versions) <= 1: + continue + + source_files = pin_files.get(pkg, set()) + if source_files and source_files.issubset(flavour_requirement_files): + continue + + conflicts[pkg] = versions + +if conflicts: + print("Found conflicting package pins across requirements files:") + for pkg, versions in sorted(conflicts.items()): + print(f"- {pkg}: {', '.join(sorted(versions))}") + raise SystemExit(1) + +print("Dependency doctor checks passed:") +print(f"- validated {len(requirement_files)} requirements files") +print(f"- validated {len(pins)} pinned packages") +PY + +echo "dependency-doctor: OK" From b42e91f08c252560f395e91833bb937f64155fb2 Mon Sep 17 00:00:00 2001 From: ll7 Date: Tue, 31 Mar 2026 19:12:54 +0200 Subject: [PATCH 13/43] Improve dev workflow and ROS-backed unit tests --- .github/workflows/ros-unit-tests.yml | 40 +++++++++++++++++++ .github/workflows/unit-tests.yml | 14 +++---- .vscode/tasks.json | 23 +++++++---- agents.md | 1 + .../agent-ros2/scripts/devfunctions.bash | 5 +++ .../test/test_mapping_common/test_general.py | 4 ++ .../test/test_mapping_common/test_shapely.py | 6 ++- .../test_mapping_common/test_transform.py | 6 ++- code/test/test_interface_contracts.py | 8 ++-- code/test/test_launch_manifests.py | 20 +++++----- code/test/test_planning_regression.py | 3 +- doc/development/quickstart_contributor.md | 2 + doc/development/testing_strategy.md | 23 ++++++++++- 13 files changed, 123 insertions(+), 32 deletions(-) create mode 100644 .github/workflows/ros-unit-tests.yml diff --git a/.github/workflows/ros-unit-tests.yml b/.github/workflows/ros-unit-tests.yml new file mode 100644 index 00000000..bc180da4 --- /dev/null +++ b/.github/workflows/ros-unit-tests.yml @@ -0,0 +1,40 @@ +name: ROS backed unit tests container + +on: + pull_request: + branches: + - main + push: + branches: + - main + +jobs: + ros_unit_tests: + name: Run ROS-backed unit tests in agent container + runs-on: + - self-hosted + - build + steps: + - uses: actions/checkout@v4 + - name: Set test image tag + run: echo "TEST_IMAGE=paf-agent-deploy-test:${GITHUB_SHA}" >> "$GITHUB_ENV" + - name: Build agent-deploy test image + run: | + docker build \ + --target agent-deploy \ + --file build/docker/agent-ros2/Dockerfile \ + --build-arg PAF_USERNAME=paf \ + --build-arg PAF_UID=1000 \ + --build-arg PAF_GID=1000 \ + --build-arg BASE_FLAVOUR=cuda \ + --tag "${TEST_IMAGE}" \ + . + - name: Run ROS-backed unit tests + run: | + docker run --rm \ + --entrypoint bash \ + "${TEST_IMAGE}" \ + -lc 'source /internal_workspace/deploy.bashrc && PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest /workspace/code/perception/tests /workspace/code/mapping/test /workspace/code/planning/test -m unit' + - name: Cleanup test image + if: always() + run: docker image rm -f "${TEST_IMAGE}" || true diff --git a/.github/workflows/unit-tests.yml b/.github/workflows/unit-tests.yml index 6dccc508..50c21b0e 100644 --- a/.github/workflows/unit-tests.yml +++ b/.github/workflows/unit-tests.yml @@ -1,4 +1,4 @@ -name: Unit tests (fast) +name: Host smoke tests on: pull_request: @@ -9,8 +9,8 @@ on: - main jobs: - unit-tests: - name: Run fast unit tests + host-smoke-tests: + name: Run host smoke tests runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 @@ -20,9 +20,9 @@ jobs: - name: Install test dependencies run: | python -m pip install --upgrade pip - pip install -r code/requirements_infrastructure.txt - pip install numpy - - name: Run unit tests (plugin autoload disabled) + python -m pip install -r code/requirements_infrastructure.txt + python -m pip install numpy + - name: Run host smoke tests (plugin autoload disabled) env: PYTEST_DISABLE_PLUGIN_AUTOLOAD: "1" - run: pytest code/test -m unit + run: python -m pytest code/test -m unit diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 81267a01..b66a009c 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -100,30 +100,37 @@ "detail": "Checks ruff formatting for the package derived from the currently active file path." }, { - "label": "Run pytest (unit)", + "label": "Run host smoke tests", "type": "shell", - "command": "pytest -m unit", + "command": "PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest code/test -m unit", "problemMatcher": [], - "detail": "Runs fast unit tests marked with @pytest.mark.unit." + "detail": "Runs the host-runnable smoke test subset under code/test." + }, + { + "label": "Run ROS-backed unit tests (dev container)", + "type": "shell", + "command": "bash -lc 'source /internal_workspace/dev.bashrc && colcon build --symlink-install --continue-on-error --packages-up-to mapping perception planning && devsource && PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest /workspace/code/perception/tests /workspace/code/mapping/test /workspace/code/planning/test -m unit'", + "problemMatcher": [], + "detail": "Builds mapping, perception, and planning plus dependencies in the dev container and runs ROS-backed unit tests." }, { "label": "Run pytest (integration)", "type": "shell", - "command": "pytest -m integration", + "command": "PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest -m integration", "problemMatcher": [], "detail": "Runs integration tests marked with @pytest.mark.integration." }, { "label": "Run pytest (simulation)", "type": "shell", - "command": "pytest -m sim", + "command": "PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest -m sim", "problemMatcher": [], "detail": "Runs simulation tests marked with @pytest.mark.sim." }, { "label": "Run pytest with coverage", "type": "shell", - "command": "pytest --cov=code --cov-report=term-missing", + "command": "PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest --cov=code --cov-report=term-missing", "problemMatcher": [], "detail": "Runs all tests and reports coverage for the code workspace." }, @@ -151,9 +158,9 @@ { "label": "Pre-PR quality check", "type": "shell", - "command": "bash -lc 'source /internal_workspace/dev.bashrc && dep.check && ruff check /workspace/code/ && ruff format /workspace/code/ --check && PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 pytest code/test -m unit'", + "command": "bash -lc 'source /internal_workspace/dev.bashrc && dep.check && ruff check /workspace/code/ && ruff format /workspace/code/ --check && PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest code/test -m unit'", "problemMatcher": [], - "detail": "Runs dependency check, lint, format, and fast unit tests before opening a PR." + "detail": "Runs dependency check, lint, format, and the host smoke test subset before opening a PR." }, { "label": "Run dependency doctor", diff --git a/agents.md b/agents.md index 291b0dda..35c649c4 100644 --- a/agents.md +++ b/agents.md @@ -70,6 +70,7 @@ After changes: 1. Run targeted lint/format checks for touched Python files. 2. Run nearest relevant tests (if present) before broad test runs. 3. If Docker/ROS behavior changed, validate with the closest existing launch/build flow. +4. For ROS-backed pytest runs in the dev container, source `/internal_workspace/dev.bashrc`, build the required package closure, and call `devsource` before running tests so generated interfaces are on `PYTHONPATH`. Do not attempt to fix unrelated failing tests/lints outside the requested scope. diff --git a/build/docker/agent-ros2/scripts/devfunctions.bash b/build/docker/agent-ros2/scripts/devfunctions.bash index cc01c490..a0e3fe80 100755 --- a/build/docker/agent-ros2/scripts/devfunctions.bash +++ b/build/docker/agent-ros2/scripts/devfunctions.bash @@ -27,6 +27,11 @@ EOF # This function sources the ROS /workspace devsource() { source "${INTERNAL_WORKSPACE_DIR}/env.bash" + + if [ -f "${PAF_ROS_WS}/install/local_setup.bash" ]; then + source "${PAF_ROS_WS}/install/local_setup.bash" + fi + echo "${INTERNAL_WORKSPACE_DIR}/env.bash sourced." } export -f devsource diff --git a/code/mapping/test/test_mapping_common/test_general.py b/code/mapping/test/test_mapping_common/test_general.py index 036d0024..7b8d5ed5 100644 --- a/code/mapping/test/test_mapping_common/test_general.py +++ b/code/mapping/test/test_mapping_common/test_general.py @@ -1,7 +1,11 @@ +import pytest + from mapping_common.map import Map import test_entity +pytestmark = pytest.mark.unit + def test_map_conversion(): m = Map() diff --git a/code/mapping/test/test_mapping_common/test_shapely.py b/code/mapping/test/test_mapping_common/test_shapely.py index 26f347c5..bb270208 100644 --- a/code/mapping/test/test_mapping_common/test_shapely.py +++ b/code/mapping/test/test_mapping_common/test_shapely.py @@ -1,6 +1,8 @@ +import math from typing import List + +import pytest import shapely -import math import mapping_common.map import mapping_common.mask @@ -11,6 +13,8 @@ import test_entity import test_shape +pytestmark = pytest.mark.unit + def test_circle_shapely(): offset_transl = Vector2.new(1.0, 0.0) diff --git a/code/mapping/test/test_mapping_common/test_transform.py b/code/mapping/test/test_mapping_common/test_transform.py index e88e14b0..90961269 100644 --- a/code/mapping/test/test_mapping_common/test_transform.py +++ b/code/mapping/test/test_mapping_common/test_transform.py @@ -1,8 +1,12 @@ -from mapping_common.transform import Transform2D, Vector2, Point2 import math +import pytest + +from mapping_common.transform import Transform2D, Vector2, Point2 from test_shape import get_polygon +pytestmark = pytest.mark.unit + def test_point_conversion(): p = Point2.new(1.0, 25.0) diff --git a/code/test/test_interface_contracts.py b/code/test/test_interface_contracts.py index 6c66db21..a69c59ea 100644 --- a/code/test/test_interface_contracts.py +++ b/code/test/test_interface_contracts.py @@ -9,10 +9,12 @@ pytestmark = pytest.mark.unit +CODE_ROOT = Path(__file__).resolve().parents[1] + INTERFACE_PACKAGE_XML = [ - Path("/workspace/code/mapping_interfaces/package.xml"), - Path("/workspace/code/perception_interfaces/package.xml"), - Path("/workspace/code/planning_interfaces/package.xml"), + CODE_ROOT / "mapping_interfaces/package.xml", + CODE_ROOT / "perception_interfaces/package.xml", + CODE_ROOT / "planning_interfaces/package.xml", ] REQUIRED_TEXT_TAGS = { diff --git a/code/test/test_launch_manifests.py b/code/test/test_launch_manifests.py index 97dbc17d..15076475 100644 --- a/code/test/test_launch_manifests.py +++ b/code/test/test_launch_manifests.py @@ -9,14 +9,16 @@ pytestmark = pytest.mark.unit +CODE_ROOT = Path(__file__).resolve().parents[1] + LAUNCH_XML_FILES = [ - Path("/workspace/code/acting/launch/acting.xml"), - Path("/workspace/code/agent/launch/agent.dev.xml"), - Path("/workspace/code/control/launch/control.xml"), - Path("/workspace/code/localization/launch/localization.xml"), - Path("/workspace/code/mapping/launch/mapping.xml"), - Path("/workspace/code/perception/launch/perception.xml"), - Path("/workspace/code/planning/launch/planning.dev.xml"), + CODE_ROOT / "acting/launch/acting.xml", + CODE_ROOT / "agent/launch/agent.dev.xml", + CODE_ROOT / "control/launch/control.xml", + CODE_ROOT / "localization/launch/localization.xml", + CODE_ROOT / "mapping/launch/mapping.xml", + CODE_ROOT / "perception/launch/perception.xml", + CODE_ROOT / "planning/launch/planning.dev.xml", ] @@ -30,7 +32,7 @@ def test_launch_xml_is_well_formed(launch_path: Path) -> None: def test_agent_launch_contains_core_subsystems() -> None: """Ensure the dev agent launch includes the subsystem launch files.""" - agent_launch = Path("/workspace/code/agent/launch/agent.dev.xml") + agent_launch = CODE_ROOT / "agent/launch/agent.dev.xml" root = ET.parse(agent_launch).getroot() included_files = {include.get("file", "") for include in root.findall("include")} @@ -47,7 +49,7 @@ def test_agent_launch_contains_core_subsystems() -> None: def test_agent_persistent_launch_contains_localization() -> None: """Ensure persistent launch file keeps localization and persistent planning.""" - persistent_launch = Path("/workspace/code/agent/launch/agent.dev.persistent.xml") + persistent_launch = CODE_ROOT / "agent/launch/agent.dev.persistent.xml" root = ET.parse(persistent_launch).getroot() included_files = {include.get("file", "") for include in root.findall("include")} diff --git a/code/test/test_planning_regression.py b/code/test/test_planning_regression.py index 562f71f2..3cda91f3 100644 --- a/code/test/test_planning_regression.py +++ b/code/test/test_planning_regression.py @@ -10,7 +10,8 @@ pytestmark = pytest.mark.unit -PLANNING_SRC = Path("/workspace/code/planning") +CODE_ROOT = Path(__file__).resolve().parents[1] +PLANNING_SRC = CODE_ROOT / "planning" if str(PLANNING_SRC) not in sys.path: sys.path.insert(0, str(PLANNING_SRC)) diff --git a/doc/development/quickstart_contributor.md b/doc/development/quickstart_contributor.md index 29a0e20f..a1eaa3bc 100644 --- a/doc/development/quickstart_contributor.md +++ b/doc/development/quickstart_contributor.md @@ -78,6 +78,8 @@ pre-commit run --all-files - `Lint current Python file with ruff` - `Format current Python file with ruff` - `Lint active package with ruff` + - `Run host smoke tests` + - `Run ROS-backed unit tests (dev container)` - `Dependency check in dev container` - `Pre-PR quality check` 5. Commit only related files and open a focused PR. diff --git a/doc/development/testing_strategy.md b/doc/development/testing_strategy.md index 7d570eba..1013deea 100644 --- a/doc/development/testing_strategy.md +++ b/doc/development/testing_strategy.md @@ -16,11 +16,28 @@ Repository markers are defined in `pytest.ini`. -- Run unit tests: `pytest -m unit` +- Run host smoke tests: `PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest code/test -m unit` +- Run unit tests in the current environment: `PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest -m unit` - Run integration tests: `pytest -m integration` - Run simulation tests: `pytest -m sim` - Run all tests with coverage: `pytest --cov=code --cov-report=term-missing` +## Host smoke tests vs ROS-backed unit tests + +Use two different fast loops: + +- Host smoke tests: `code/test` only. These must stay runnable outside the dev container and should not depend on `/workspace`, sourced ROS overlays, or generated interfaces. +- ROS-backed unit tests: package-local tests such as `code/perception/tests`, `code/mapping/test`, and `code/planning/test`. These run inside the dev container after building the required package closure. + +Example ROS-backed unit test loop inside the dev container: + +```bash +source /internal_workspace/dev.bashrc +colcon build --symlink-install --continue-on-error --packages-up-to mapping perception planning +devsource +PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest /workspace/code/perception/tests /workspace/code/mapping/test /workspace/code/planning/test -m unit +``` + ## Submodule strategy For each package under `code/`: @@ -32,6 +49,8 @@ For each package under `code/`: ## CI recommendation (phased) -- On every PR: run `unit` tests. +- On every PR: run host smoke tests on `ubuntu-latest`. +- On every PR: run ROS-backed mapping/perception/planning unit tests in the agent container on the self-hosted build runner. +- On every PR: run strict Ruff for starter packages that have opted in. - On merge to main or scheduled runs: run `integration` tests. - Nightly or hardware-backed pipeline: run `sim` tests. From 69c480aedacceed4647ac5aeff5ad1ee40079560 Mon Sep 17 00:00:00 2001 From: ll7 Date: Tue, 31 Mar 2026 19:13:06 +0200 Subject: [PATCH 14/43] Add strict Ruff starter gate for paf_common --- .github/workflows/ruff-strict-rollout.yml | 32 +++++++++++++++++++++++ code/paf_common/paf_common/debugging.py | 20 +++++++------- code/paf_common/paf_common/exceptions.py | 5 +++- code/paf_common/paf_common/parameters.py | 14 +++++----- code/paf_common/setup.py | 2 ++ doc/development/linting.md | 5 ++++ 6 files changed, 61 insertions(+), 17 deletions(-) create mode 100644 .github/workflows/ruff-strict-rollout.yml diff --git a/.github/workflows/ruff-strict-rollout.yml b/.github/workflows/ruff-strict-rollout.yml new file mode 100644 index 00000000..cde799d2 --- /dev/null +++ b/.github/workflows/ruff-strict-rollout.yml @@ -0,0 +1,32 @@ +name: "Ruff strict rollout" + +on: + pull_request: + branches: + - main + push: + branches: + - main + +jobs: + ruff-strict-starter: + name: Lint paf_common with Ruff strict + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - name: Load Ruff version + id: ruff-version + run: | + VERSION_LINE=$(grep -E '^RUFF_VERSION=' build/pins/ruff.env || true) + VERSION=${VERSION_LINE#RUFF_VERSION=} + if [ -z "${VERSION}" ]; then + echo "Ruff version missing in build/pins/ruff.env" >&2 + exit 1 + fi + echo "version=${VERSION}" >> "$GITHUB_OUTPUT" + - name: Install Ruff + uses: astral-sh/ruff-action@v3 + with: + version: ${{ steps.ruff-version.outputs.version }} + - name: Lint starter package with strict profile + run: ruff check code/paf_common --config ruff-strict.toml diff --git a/code/paf_common/paf_common/debugging.py b/code/paf_common/paf_common/debugging.py index 2c72560c..407015a1 100644 --- a/code/paf_common/paf_common/debugging.py +++ b/code/paf_common/paf_common/debugging.py @@ -1,4 +1,5 @@ -from typing import Optional +"""Helpers for attaching a debugpy debugger to running ROS nodes.""" + import inspect import importlib.util @@ -7,10 +8,12 @@ def get_logger() -> RcutilsLogger: + """Return the shared debugger logger.""" return rclpy.logging.get_logger("debugger") def get_caller_file() -> str: + """Return the outermost caller filename when available.""" stack = inspect.stack() if len(stack) < 1: return "unknown" @@ -19,19 +22,18 @@ def get_caller_file() -> str: def start_debugger( - node_module_name: Optional[str] = None, + node_module_name: str | None = None, host: str = "127.0.0.1", port: int = 53000, wait_for_client: bool = False, -): - """_summary_ +)-> None: + """Start a debugpy listener for the current node when debugpy is available. Args: - node_module_name (str): Name of the underlying node. Only used for logging - host (str): host the debugger binds to - port (int): debugger port - wait_for_client (bool, optional): If the debugger should wait - for a client to attach. Defaults to False. + node_module_name: Name of the underlying node. Used for logging only. + host: Host address the debugger binds to. + port: Debugger port. + wait_for_client: Whether to wait until a debugger client attaches. """ debugger_spec = importlib.util.find_spec("debugpy") if debugger_spec is not None: diff --git a/code/paf_common/paf_common/exceptions.py b/code/paf_common/paf_common/exceptions.py index e1b058d3..0ab12928 100644 --- a/code/paf_common/paf_common/exceptions.py +++ b/code/paf_common/paf_common/exceptions.py @@ -1,7 +1,10 @@ +"""Helpers for formatting exceptions for logs and status messages.""" + import traceback -def emsg_with_trace(e: Exception): +def emsg_with_trace(e: Exception) -> str: + """Return a formatted traceback string for the given exception.""" traceback_str_list = traceback.format_exception(e) traceback_str = "".join(traceback_str_list) return f"\n{traceback_str}" diff --git a/code/paf_common/paf_common/parameters.py b/code/paf_common/paf_common/parameters.py index 8f2cde7a..17d91ad2 100644 --- a/code/paf_common/paf_common/parameters.py +++ b/code/paf_common/paf_common/parameters.py @@ -1,4 +1,4 @@ -from typing import List +"""Helpers for mapping ROS parameter updates onto node attributes.""" from rclpy.node import Node from rclpy.parameter import Parameter @@ -7,17 +7,17 @@ ) -def update_attributes(obj: Node, params: List[Parameter]) -> SetParametersResult: - """Update attributes of obj with params +def update_attributes(obj: Node, params: list[Parameter]) -> SetParametersResult: + """Update node attributes from ROS parameter values. - Important: Attribute names must match parameter names + Important: Attribute names must match parameter names. Args: - obj (Node): Node which attributes are updated - params (List[Parameter]): parameters with new values + obj: Node whose attributes are updated. + params: Parameters with new values. Returns: - SetParametersResult: + Result object describing whether all updates succeeded. """ result = SetParametersResult() result.successful = True diff --git a/code/paf_common/setup.py b/code/paf_common/setup.py index 9df790ea..fa3c5d11 100644 --- a/code/paf_common/setup.py +++ b/code/paf_common/setup.py @@ -1,3 +1,5 @@ +"""Setuptools entrypoint for the paf_common package.""" + from setuptools import find_packages, setup package_name = "paf_common" diff --git a/doc/development/linting.md b/doc/development/linting.md index a5fc3b52..5d3d63f5 100644 --- a/doc/development/linting.md +++ b/doc/development/linting.md @@ -38,6 +38,11 @@ Use the strict profile when hardening a package: ruff check /workspace/code/ --config /workspace/ruff-strict.toml ``` +Current rollout: + +- `code/paf_common` is gated in CI with the strict profile. +- Expand strict enforcement package-by-package instead of enabling it repo-wide at once. + ### Run Ruff via Docker Compose Use the pinned version directly from the host: From 87d712d61d3d6d810dc3746b2dafe2623663efe4 Mon Sep 17 00:00:00 2001 From: ll7 Date: Tue, 31 Mar 2026 19:13:36 +0200 Subject: [PATCH 15/43] Refs #826: replace typed ROS parameter accessors --- .../localization/ekf_state_publisher.py | 10 +-- .../localization/odometry_fusion.py | 46 +++++------- .../localization/sensor_covariance_fusion.py | 72 ++++++++----------- .../perception/traffic_light_node.py | 30 +++----- .../planning/behavior_agent/bt_parameters.py | 11 +-- 5 files changed, 55 insertions(+), 114 deletions(-) diff --git a/code/localization/localization/ekf_state_publisher.py b/code/localization/localization/ekf_state_publisher.py index d0e705cb..cc30f591 100755 --- a/code/localization/localization/ekf_state_publisher.py +++ b/code/localization/localization/ekf_state_publisher.py @@ -33,14 +33,8 @@ def __init__(self): super().__init__("ekf_state_publisher") self.get_logger().info(f"{type(self).__name__} node initializing...") # Parameters - self.loop_rate = ( - self.declare_parameter("loop_rate", 0.05).get_parameter_value().double_value - ) - self.role_name = ( - self.declare_parameter("role_name", "hero") - .get_parameter_value() - .string_value - ) + self.loop_rate = self.declare_parameter("loop_rate", 0.05).value + self.role_name = self.declare_parameter("role_name", "hero").value # Publishes global ekf_pos and ekf_heading from hero frame out of tf-graph self.global_position_publisher: Publisher = self.create_publisher( diff --git a/code/localization/localization/odometry_fusion.py b/code/localization/localization/odometry_fusion.py index 901b81c0..d398ffe9 100755 --- a/code/localization/localization/odometry_fusion.py +++ b/code/localization/localization/odometry_fusion.py @@ -31,36 +31,22 @@ def __init__(self): self.get_logger().info(f"{type(self).__name__} node initializing...") # Parameters - self.control_loop_rate = ( - self.declare_parameter("loop_rate", 0.05).get_parameter_value().double_value - ) - self.role_name = ( - self.declare_parameter("role_name", "hero") - .get_parameter_value() - .string_value - ) - self.pose_covariance = ( - self.declare_parameter( - "pose_covariance", - ParameterValue(type=ParameterType.PARAMETER_DOUBLE_ARRAY), - descriptor=ParameterDescriptor( - description="Covariance for Odometry Pose", - ), - ) - .get_parameter_value() - .double_array_value - ) - self.twist_covariance = ( - self.declare_parameter( - "twist_covariance", - ParameterValue(type=ParameterType.PARAMETER_DOUBLE_ARRAY), - descriptor=ParameterDescriptor( - description="Covariance for Odometry Twist", - ), - ) - .get_parameter_value() - .double_array_value - ) + self.control_loop_rate = self.declare_parameter("loop_rate", 0.05).value + self.role_name = self.declare_parameter("role_name", "hero").value + self.pose_covariance = self.declare_parameter( + "pose_covariance", + ParameterValue(type=ParameterType.PARAMETER_DOUBLE_ARRAY), + descriptor=ParameterDescriptor( + description="Covariance for Odometry Pose", + ), + ).value + self.twist_covariance = self.declare_parameter( + "twist_covariance", + ParameterValue(type=ParameterType.PARAMETER_DOUBLE_ARRAY), + descriptor=ParameterDescriptor( + description="Covariance for Odometry Twist", + ), + ).value # Node starts only if at least one steer angle # and one speed message was received diff --git a/code/localization/localization/sensor_covariance_fusion.py b/code/localization/localization/sensor_covariance_fusion.py index 4ecfbd43..53d6b1f5 100755 --- a/code/localization/localization/sensor_covariance_fusion.py +++ b/code/localization/localization/sensor_covariance_fusion.py @@ -31,50 +31,34 @@ def __init__(self): self.get_logger().info(f"{type(self).__name__} node initializing...") # Parameters - self.imu_orientation = ( - self.declare_parameter( - "imu_orientation", - ParameterValue(type=ParameterType.PARAMETER_DOUBLE_ARRAY), - descriptor=ParameterDescriptor( - description="IMU Covariance for Orientation", - ), - ) - .get_parameter_value() - .double_array_value - ) - self.imu_angular_velocity = ( - self.declare_parameter( - "imu_angular_velocity", - ParameterValue(type=ParameterType.PARAMETER_DOUBLE_ARRAY), - descriptor=ParameterDescriptor( - description="IMU Covariance for Angular Velocity", - ), - ) - .get_parameter_value() - .double_array_value - ) - self.imu_linear_acceleration = ( - self.declare_parameter( - "imu_linear_acceleration", - ParameterValue(type=ParameterType.PARAMETER_DOUBLE_ARRAY), - descriptor=ParameterDescriptor( - description="IMU Covariance for Linear Acceleration", - ), - ) - .get_parameter_value() - .double_array_value - ) - self.gps_position = ( - self.declare_parameter( - "gps_position", - ParameterValue(type=ParameterType.PARAMETER_DOUBLE_ARRAY), - descriptor=ParameterDescriptor( - description="Alt,Lat,Long Covariance", - ), - ) - .get_parameter_value() - .double_array_value - ) + self.imu_orientation = self.declare_parameter( + "imu_orientation", + ParameterValue(type=ParameterType.PARAMETER_DOUBLE_ARRAY), + descriptor=ParameterDescriptor( + description="IMU Covariance for Orientation", + ), + ).value + self.imu_angular_velocity = self.declare_parameter( + "imu_angular_velocity", + ParameterValue(type=ParameterType.PARAMETER_DOUBLE_ARRAY), + descriptor=ParameterDescriptor( + description="IMU Covariance for Angular Velocity", + ), + ).value + self.imu_linear_acceleration = self.declare_parameter( + "imu_linear_acceleration", + ParameterValue(type=ParameterType.PARAMETER_DOUBLE_ARRAY), + descriptor=ParameterDescriptor( + description="IMU Covariance for Linear Acceleration", + ), + ).value + self.gps_position = self.declare_parameter( + "gps_position", + ParameterValue(type=ParameterType.PARAMETER_DOUBLE_ARRAY), + descriptor=ParameterDescriptor( + description="Alt,Lat,Long Covariance", + ), + ).value # The publishers (topic names have to coincide with ekf_config.yaml) self.imu_publisher = self.create_publisher(Imu, "/imu/data", qos_profile=10) diff --git a/code/perception/perception/traffic_light_node.py b/code/perception/perception/traffic_light_node.py index d89c9a19..7a84c511 100755 --- a/code/perception/perception/traffic_light_node.py +++ b/code/perception/perception/traffic_light_node.py @@ -25,28 +25,14 @@ def __init__(self): # Parameters - self.control_loop_rate = ( - self.declare_parameter( - "control_loop_rate", - 0.05, - ) - .get_parameter_value() - .double_value - ) - self.role_name = ( - self.declare_parameter("role_name", "hero") - .get_parameter_value() - .string_value - ) - self.side = ( - self.declare_parameter("side", "Center").get_parameter_value().string_value - ) - self.model = ( - self.declare_parameter("model", "").get_parameter_value().string_value - ) - self.tfs_debug = ( - self.declare_parameter("tfs_debug", False).get_parameter_value().bool_value - ) + self.control_loop_rate = self.declare_parameter( + "control_loop_rate", + 0.05, + ).value + self.role_name = self.declare_parameter("role_name", "hero").value + self.side = self.declare_parameter("side", "Center").value + self.model = self.declare_parameter("model", "").value + self.tfs_debug = self.declare_parameter("tfs_debug", False).value # general setup self.bridge = CvBridge() diff --git a/code/planning/planning/behavior_agent/bt_parameters.py b/code/planning/planning/behavior_agent/bt_parameters.py index 48b66552..9c3f7869 100644 --- a/code/planning/planning/behavior_agent/bt_parameters.py +++ b/code/planning/planning/behavior_agent/bt_parameters.py @@ -48,16 +48,7 @@ def _register_parameter( param_desc.floating_point_range = [float_range] parameter = node.declare_parameter(name, default_value, param_desc) - if isinstance(default_value, str): - return_value = parameter.get_parameter_value().string_value - elif isinstance(default_value, bool): - return_value = parameter.get_parameter_value().bool_value - elif isinstance(default_value, int): - return_value = parameter.get_parameter_value().integer_value - elif isinstance(default_value, float): - return_value = parameter.get_parameter_value().double_value - else: - raise RuntimeError(f"Parameter type error on parameter {name}") + return_value = parameter.value blackboard.set(f"/params/{name}", return_value) return return_value From 42cf68c3baa5ed2e4315f4940c6e271daf0233a8 Mon Sep 17 00:00:00 2001 From: ll7 Date: Tue, 31 Mar 2026 19:13:51 +0200 Subject: [PATCH 16/43] Refs #909, Refs #910: improve radar and intersection motion handling --- code/perception/perception/radar_node.py | 181 +++++++++--------- .../tests/test_radar_velocity_compensation.py | 52 +++++ .../behavior_agent/behaviors/intersection.py | 145 +++++++++++--- ...est_intersection_priority_cross_traffic.py | 82 ++++++++ doc/perception/radar_node.md | 9 +- doc/planning/behaviors/Intersection.md | 13 +- 6 files changed, 358 insertions(+), 124 deletions(-) create mode 100644 code/perception/tests/test_radar_velocity_compensation.py create mode 100644 code/planning/test/test_intersection_priority_cross_traffic.py diff --git a/code/perception/perception/radar_node.py b/code/perception/perception/radar_node.py index 5bed536c..e0d1ed46 100755 --- a/code/perception/perception/radar_node.py +++ b/code/perception/perception/radar_node.py @@ -31,6 +31,47 @@ from .perception_utils import array_to_clustered_points +def _vector_dot(a: Vector2, b: Vector2) -> float: + return a.x() * b.x() + a.y() * b.y() + + +def _sensor_position_in_vehicle_frame( + sensor_name: str, sensor_config: dict[str, list[float]] +) -> Vector2: + sensor_x, sensor_y, _ = sensor_config[sensor_name] + return Vector2.new(sensor_x, -sensor_y) + + +def _sensor_ego_velocity( + sensor_position: Vector2, + ego_speed: float = 0.0, + yaw_rate: float = 0.0, +) -> Vector2: + translational_velocity = Vector2.new(ego_speed, 0.0) + rotational_velocity = Vector2.new( + -yaw_rate * sensor_position.y(), + yaw_rate * sensor_position.x(), + ) + return translational_velocity + rotational_velocity + + +def _compensate_radar_radial_velocity( + radial_velocity: float, + azimuth: float, + ego_speed: float = 0.0, + yaw_rate: float = 0.0, + sensor_position: Optional[Vector2] = None, +) -> Vector2: + line_of_sight = Vector2.new(np.cos(azimuth), np.sin(azimuth)) + sensor_velocity = _sensor_ego_velocity( + sensor_position if sensor_position is not None else Vector2.zero(), + ego_speed=ego_speed, + yaw_rate=yaw_rate, + ) + radial_compensation = _vector_dot(sensor_velocity, line_of_sight) + return line_of_sight * (radial_velocity + radial_compensation) + + class RadarNode(Node): """See doc/perception/radar_node.md on how to configure this node.""" @@ -64,82 +105,47 @@ def __init__(self): self.accel_z_buffer = deque(maxlen=5) self.current_pitch = 0.0 + self.current_yaw_rate = 0.0 # Parameters - self.accelerometer_arrow_size = ( - self.declare_parameter( - "accelerometer_arrow_size", - 2.0, - ) - .get_parameter_value() - .double_value - ) - self.accelerometer_factor = ( - self.declare_parameter( - "accelerometer_factor", - 0.05, - ) - .get_parameter_value() - .double_value - ) - self.imu_debug = ( - self.declare_parameter( - "imu_debug", - False, - ) - .get_parameter_value() - .bool_value - ) - - self.dbscan_eps = ( - self.declare_parameter( - "dbscan_eps", - 0.3, - ) - .get_parameter_value() - .double_value - ) - self.dbscan_samples = ( - self.declare_parameter( - "dbscan_samples", - 2, - ) - .get_parameter_value() - .integer_value - ) - self.data_buffered = ( - self.declare_parameter( - "data_buffered", - False, - ) - .get_parameter_value() - .bool_value - ) - self.data_buffer_time = ( - self.declare_parameter( - "data_buffer_time", - 0.1, - ) - .get_parameter_value() - .double_value - ) - self.enable_clustering = ( - self.declare_parameter( - "enable_clustering", - False, - ) - .get_parameter_value() - .bool_value - ) - self.enable_debug_info = ( - self.declare_parameter( - "enable_debug_info", - False, - ) - .get_parameter_value() - .bool_value - ) + self.accelerometer_arrow_size = self.declare_parameter( + "accelerometer_arrow_size", + 2.0, + ).value + self.accelerometer_factor = self.declare_parameter( + "accelerometer_factor", + 0.05, + ).value + self.imu_debug = self.declare_parameter( + "imu_debug", + False, + ).value + + self.dbscan_eps = self.declare_parameter( + "dbscan_eps", + 0.3, + ).value + self.dbscan_samples = self.declare_parameter( + "dbscan_samples", + 2, + ).value + self.data_buffered = self.declare_parameter( + "data_buffered", + False, + ).value + self.data_buffer_time = self.declare_parameter( + "data_buffer_time", + 0.1, + ).value + self.enable_clustering = self.declare_parameter( + "enable_clustering", + False, + ).value + self.enable_debug_info = self.declare_parameter( + "enable_debug_info", + False, + ).value # Publishers self.visualization_radar_publisher = self.create_publisher( @@ -266,6 +272,7 @@ def imu_callback(self, msg): self.accel_x_buffer.append(msg.linear_acceleration.x) self.accel_z_buffer.append(msg.linear_acceleration.z) + self.current_yaw_rate = msg.angular_velocity.z # Ensure that we have enough values (min. 5 measuring points) if len(self.accel_x_buffer) < 5: @@ -726,22 +733,18 @@ def _compensate_motion(self, points: np.ndarray): velocity_per_point = float(point[3]) azimuth = np.arctan2(point[1], point[0]) - cos_az = np.cos(azimuth) - - vx = velocity_per_point * np.cos(azimuth) - vy = velocity_per_point * np.sin(azimuth) - point_motion_vec = Vector2.new(vx, vy) - - motion_vec = point_motion_vec - - # ego motion compensation - if self.hero_speed is not None: - hypspeed = self.hero_speed.speed * cos_az - - xspeed = hypspeed * cos_az - yspeed = hypspeed * np.sin(azimuth) - ego_motion_vec = Vector2.new(xspeed, yspeed) - motion_vec += ego_motion_vec + sensor_name = "RADAR0" if point[0] >= 0 else "RADAR1" + sensor_position = _sensor_position_in_vehicle_frame( + sensor_name, self.sensor_config + ) + ego_speed = 0.0 if self.hero_speed is None else self.hero_speed.speed + motion_vec = _compensate_radar_radial_velocity( + radial_velocity=velocity_per_point, + azimuth=azimuth, + ego_speed=ego_speed, + yaw_rate=self.current_yaw_rate, + sensor_position=sensor_position, + ) motion_array[i] = Motion2D(motion_vec, 0.0) motion_vectors[i] = np.array([motion_vec.x(), motion_vec.y(), point[-1]]) diff --git a/code/perception/tests/test_radar_velocity_compensation.py b/code/perception/tests/test_radar_velocity_compensation.py new file mode 100644 index 00000000..0ef721e4 --- /dev/null +++ b/code/perception/tests/test_radar_velocity_compensation.py @@ -0,0 +1,52 @@ +import numpy as np +import pytest + +from mapping_common.transform import Vector2 +from perception.radar_node import ( + _compensate_radar_radial_velocity, + _sensor_ego_velocity, +) + +pytestmark = pytest.mark.unit + + +def test_sensor_ego_velocity_includes_sensor_offset_rotation(): + sensor_velocity = _sensor_ego_velocity( + Vector2.new(2.0, 1.5), + yaw_rate=1.0, + ) + + assert sensor_velocity.x() == pytest.approx(-1.5) + assert sensor_velocity.y() == pytest.approx(2.0) + + +def test_radar_velocity_compensation_cancels_stationary_translation(): + ego_speed = 8.0 + azimuth = np.pi / 3.0 + radial_velocity = -(ego_speed * np.cos(azimuth)) + + compensated = _compensate_radar_radial_velocity( + radial_velocity=radial_velocity, + azimuth=azimuth, + ego_speed=ego_speed, + ) + + assert np.allclose([compensated.x(), compensated.y()], [0.0, 0.0], atol=1e-6) + + +@pytest.mark.parametrize("azimuth", [0.0, np.pi / 4.0, np.pi / 2.0]) +def test_radar_velocity_compensation_cancels_stationary_yaw_motion(azimuth): + sensor_position = Vector2.new(2.0, 1.5) + sensor_velocity = _sensor_ego_velocity(sensor_position, yaw_rate=1.0) + radial_velocity = -( + sensor_velocity.x() * np.cos(azimuth) + sensor_velocity.y() * np.sin(azimuth) + ) + + compensated = _compensate_radar_radial_velocity( + radial_velocity=radial_velocity, + azimuth=azimuth, + yaw_rate=1.0, + sensor_position=sensor_position, + ) + + assert np.allclose([compensated.x(), compensated.y()], [0.0, 0.0], atol=1e-6) diff --git a/code/planning/planning/behavior_agent/behaviors/intersection.py b/code/planning/planning/behavior_agent/behaviors/intersection.py index f12f647f..80bbd2da 100755 --- a/code/planning/planning/behavior_agent/behaviors/intersection.py +++ b/code/planning/planning/behavior_agent/behaviors/intersection.py @@ -1,5 +1,4 @@ import py_trees -import math from py_trees.common import Status from typing import Optional @@ -8,7 +7,8 @@ from rclpy.clock import Clock from rclpy.duration import Duration -from std_msgs.msg import String, Bool +from std_msgs.msg import String, Bool, Float32 +from geometry_msgs.msg import PoseStamped from perception_interfaces.msg import Waypoint, TrafficLightState from planning_interfaces.srv import OvertakeStatus from carla_msgs.msg import CarlaRoute @@ -18,7 +18,7 @@ from mapping_common.entity import FlagFilter from mapping_common.markers import debug_marker from mapping_common.shape import Rectangle -from mapping_common.transform import Transform2D, Vector2 +from mapping_common.transform import Transform2D, Point2, Vector2 import shapely from planning.behavior_agent.blackboard_utils import Blackboard @@ -99,6 +99,9 @@ def tr_status_str(t: Optional[TrafficLightState]): at least once """ +CURRENT_PRIORITY_CHECK_REFERENCE: Optional[Transform2D] = None +"""Global pose of the hero when the priority-cross-traffic check started.""" + # Cross traffic parameter CROSS_TRAFFIC_SPEED_THRESHOLD = 2.5 # m/s CROSS_CHECK_DISTANCE = 15.0 @@ -110,10 +113,84 @@ def tr_status_str(t: Optional[TrafficLightState]): PRIORITY_CHECK_DISTANCE = 13.0 # further ahead in the direction of travel PRIORITY_CHECK_LENGTH = 25.0 PRIORITY_CHECK_WIDTH = 50.0 +PRIORITY_CLOSING_SPEED_THRESHOLD = 0.5 SELF_EMERGENCY_THRESHOLD = 10 / 3.6 # m/s ≈ 2.78 -def check_priority_cross_traffic(map: Map, tree: MapTree): +def _dot(a: Vector2, b: Vector2) -> float: + return a.x() * b.x() + a.y() * b.y() + + +def _pose_to_transform( + position: Optional[PoseStamped], heading: Optional[Float32] +) -> Optional[Transform2D]: + if position is None or heading is None: + return None + return Transform2D.new_rotation_translation( + heading.data, + Vector2.new(position.pose.position.x, position.pose.position.y), + ) + + +def _get_current_pose_transform(blackboard: Blackboard) -> Optional[Transform2D]: + current_pos: Optional[PoseStamped] = blackboard.try_get( + "/paf/hero/global_current_pos" + ) + current_heading: Optional[Float32] = blackboard.try_get( + "/paf/hero/global_current_heading" + ) + return _pose_to_transform(current_pos, current_heading) + + +def _build_priority_check_mask( + hero, + reference_pose: Optional[Transform2D] = None, + current_pose: Optional[Transform2D] = None, +): + offset = Transform2D.new_translation( + Vector2.new(PRIORITY_CHECK_DISTANCE + hero.get_front_x(), 0.0) + ) + rect = Rectangle( + length=PRIORITY_CHECK_LENGTH, + width=PRIORITY_CHECK_WIDTH, + offset=offset, + ) + target_point = Point2.from_vector(offset.translation()) + + if reference_pose is not None and current_pose is not None: + mask_transform = current_pose.inverse() * reference_pose + else: + mask_transform = hero.transform + + return rect.to_shapely(mask_transform), mask_transform * target_point + + +def _get_entity_velocity_in_hero_frame(entity) -> Optional[Vector2]: + if entity.motion is None: + return None + return entity.transform * entity.motion.linear_motion + + +def _is_priority_cross_traffic_threat(entity, target_point: Point2) -> bool: + velocity = _get_entity_velocity_in_hero_frame(entity) + if velocity is None or velocity.length() <= PRIORITY_SPEED_THRESHOLD: + return False + + entity_position = Point2.from_vector(entity.transform.translation()) + to_target = entity_position.vector_to(target_point) + if to_target.length() == 0.0: + return True + + closing_speed = _dot(velocity, to_target.normalized()) + return closing_speed > PRIORITY_CLOSING_SPEED_THRESHOLD + + +def check_priority_cross_traffic( + map: Map, + tree: MapTree, + reference_pose: Optional[Transform2D] = None, + current_pose: Optional[Transform2D] = None, +): """ Checks whether there are fast vehicles in the larger intersection area. @@ -126,30 +203,17 @@ def check_priority_cross_traffic(map: Map, tree: MapTree): if hero is None: return True, None - # Larger rectangle for approaching emergency vehicles - offset = Transform2D.new_translation( - Vector2.new(PRIORITY_CHECK_DISTANCE + hero.get_front_x(), 0.0) + mask, target_point = _build_priority_check_mask( + hero, + reference_pose=reference_pose, + current_pose=current_pose, ) - rect = Rectangle( - length=PRIORITY_CHECK_LENGTH, - width=PRIORITY_CHECK_WIDTH, - offset=offset, - ) - mask = rect.to_shapely(hero.transform) shapely_entities = tree.get_overlapping_entities(mask) for se in shapely_entities: entity = se.entity - motion = entity.motion - if motion is None: - continue - - v = motion.linear_motion - speed = math.hypot(v.x(), v.y()) - - # Only consider fast objects - if speed > PRIORITY_SPEED_THRESHOLD: + if _is_priority_cross_traffic_threat(entity, target_point): return False, mask return True, mask @@ -245,9 +309,9 @@ def setup(self, **kwargs): self.blackboard = Blackboard() def initialise(self): - global INTERSECTION_HAS_TRAFFIC_LIGHT + global INTERSECTION_HAS_TRAFFIC_LIGHT, CURRENT_PRIORITY_CHECK_REFERENCE INTERSECTION_HAS_TRAFFIC_LIGHT = False - pass + CURRENT_PRIORITY_CHECK_REFERENCE = None def update(self): """ @@ -531,8 +595,18 @@ def update(self): tree = map.build_tree(FlagFilter(is_collider=True, is_hero=False)) if self.intersection_type != CarlaRoute.LEFT: + global CURRENT_PRIORITY_CHECK_REFERENCE + current_pose = _get_current_pose_transform(self.blackboard) + if CURRENT_PRIORITY_CHECK_REFERENCE is None: + CURRENT_PRIORITY_CHECK_REFERENCE = current_pose + # Priority cross traffic check - priority_clear, priority_mask = check_priority_cross_traffic(map, tree) + priority_clear, priority_mask = check_priority_cross_traffic( + map, + tree, + reference_pose=CURRENT_PRIORITY_CHECK_REFERENCE, + current_pose=current_pose, + ) add_debug_entry( self.name, f"[Wait] Priority cross traffic clear: {priority_clear}", @@ -729,7 +803,7 @@ def setup(self, **kwargs): def initialise(self): get_logger().info("Enter Intersection") - global CURRENT_INTERSECTION_WAYPOINT + global CURRENT_INTERSECTION_WAYPOINT, CURRENT_PRIORITY_CHECK_REFERENCE if CURRENT_INTERSECTION_WAYPOINT is None: get_logger().error( "Intersection behavior: CURRENT_INTERSECTION_WAYPOINT not set" @@ -739,6 +813,10 @@ def initialise(self): unset_line_stop(self.stop_client) self.curr_behavior_pub.publish(String(data=bs.int_enter.name)) self.intersection_type = self.waypoint.road_option + if CURRENT_PRIORITY_CHECK_REFERENCE is None: + CURRENT_PRIORITY_CHECK_REFERENCE = _get_current_pose_transform( + self.blackboard + ) def update(self): """ @@ -766,7 +844,17 @@ def update(self): tree = map.build_tree(FlagFilter(is_collider=True, is_hero=False)) - priority_clear, priority_mask = check_priority_cross_traffic(map, tree) + global CURRENT_PRIORITY_CHECK_REFERENCE + current_pose = _get_current_pose_transform(self.blackboard) + if CURRENT_PRIORITY_CHECK_REFERENCE is None: + CURRENT_PRIORITY_CHECK_REFERENCE = current_pose + + priority_clear, priority_mask = check_priority_cross_traffic( + map, + tree, + reference_pose=CURRENT_PRIORITY_CHECK_REFERENCE, + current_pose=current_pose, + ) add_debug_entry( self.name, f"[Enter] Priority cross traffic clear: {priority_clear}", @@ -828,4 +916,5 @@ def update(self): ) def terminate(self, new_status): - pass + global CURRENT_PRIORITY_CHECK_REFERENCE + CURRENT_PRIORITY_CHECK_REFERENCE = None diff --git a/code/planning/test/test_intersection_priority_cross_traffic.py b/code/planning/test/test_intersection_priority_cross_traffic.py new file mode 100644 index 00000000..1b9731ba --- /dev/null +++ b/code/planning/test/test_intersection_priority_cross_traffic.py @@ -0,0 +1,82 @@ +import math +from types import SimpleNamespace + +import pytest + +from mapping_common import entity, map as mapping_map, shape, transform +from planning.behavior_agent.behaviors import intersection + +pytestmark = pytest.mark.unit + + +class FakeTree: + def __init__(self, entities): + self.entities = entities + + def get_overlapping_entities(self, mask): + del mask + return [SimpleNamespace(entity=e) for e in self.entities] + + +def _make_car( + x: float, + y: float, + vx: float = 0.0, + vy: float = 0.0, + *, + is_hero: bool = False, +) -> entity.Car: + flags = entity.Flags(is_collider=not is_hero, is_hero=is_hero) + motion = entity.Motion2D(linear_motion=transform.Vector2.new(vx, vy)) + return entity.Car( + confidence=1.0, + priority=1.0, + shape=shape.Rectangle(4.0, 2.0), + transform=transform.Transform2D.new_rotation_translation( + 0.0, transform.Vector2.new(x, y) + ), + motion=motion, + flags=flags, + ) + + +def test_priority_cross_traffic_blocks_fast_approaching_entity(): + hero = _make_car(0.0, 0.0, is_hero=True) + approaching = _make_car(12.0, 12.0, 0.0, -7.5) + map_obj = mapping_map.Map(entities=[hero, approaching]) + + priority_clear, _ = intersection.check_priority_cross_traffic( + map_obj, FakeTree([approaching]) + ) + + assert not priority_clear + + +def test_priority_cross_traffic_ignores_fast_entity_moving_away(): + hero = _make_car(0.0, 0.0, is_hero=True) + moving_away = _make_car(12.0, 12.0, 0.0, 7.5) + map_obj = mapping_map.Map(entities=[hero, moving_away]) + + priority_clear, _ = intersection.check_priority_cross_traffic( + map_obj, FakeTree([moving_away]) + ) + + assert priority_clear + + +def test_priority_check_mask_stays_world_aligned_while_turning(): + hero = _make_car(0.0, 0.0, is_hero=True) + reference_pose = transform.Transform2D.identity() + current_pose = transform.Transform2D.new_rotation_translation( + math.pi / 2.0, transform.Vector2.zero() + ) + + _, target_point = intersection._build_priority_check_mask( + hero, + reference_pose=reference_pose, + current_pose=current_pose, + ) + + expected_distance = intersection.PRIORITY_CHECK_DISTANCE + hero.get_front_x() + assert target_point.x() == pytest.approx(0.0) + assert target_point.y() == pytest.approx(-expected_distance) diff --git a/doc/perception/radar_node.md b/doc/perception/radar_node.md index bffa16da..742f276e 100644 --- a/doc/perception/radar_node.md +++ b/doc/perception/radar_node.md @@ -127,7 +127,14 @@ The radar node itself does not directly perform the behavioral decision. Instead Radar points are processed to compute ego-motion compensated velocities. -Each radar point contains Doppler velocity information. This velocity is transformed into Cartesian motion components and then compensated using the ego vehicle speed. +Each radar point contains Doppler velocity information. This velocity is interpreted along the radar line of sight and then compensated using the ego vehicle motion. + +The compensation now includes two components: + +- the translational ego speed from `/carla/hero/Speed` +- the yaw-rate-induced sensor motion derived from `/carla/hero/IMU` + +The yaw-rate term is evaluated at the physical sensor offset of each radar. This reduces the false motion that stationary entities would otherwise receive while the ego vehicle rotates, for example when leaving the parking spot. As a result, a motion vector is computed for each radar point. These per-point velocities are used for further processing in the mapping stage, especially for assigning radar-derived motion to lidar-based entities. diff --git a/doc/planning/behaviors/Intersection.md b/doc/planning/behaviors/Intersection.md index 777eba1a..59132eab 100644 --- a/doc/planning/behaviors/Intersection.md +++ b/doc/planning/behaviors/Intersection.md @@ -69,6 +69,10 @@ Fast moving objects (e.g. cross traffic) are detected based on a velocity thresh A velocity threshold is used to filter relevant traffic. Static or slow-moving objects are ignored to reduce false positives. +Fast entities are only considered relevant if their motion points toward the center of the priority-check area. Fast objects inside the box that are already moving away from the conflict region are ignored. + +While the ego vehicle waits and then enters the intersection, the priority-check area is anchored to the pose where the check started. This keeps the red debug box stable in world space even if the ego vehicle starts turning. + ### Emergency Handling If fast cross traffic is detected while the ego vehicle is still moving above a certain speed, an emergency signal is triggered. @@ -77,13 +81,9 @@ This signal is published to notify about a potentially dangerous situation. ### Current Limitations -The current cross traffic check does not yet consider the motion direction of detected objects. - -As a result, the vehicle may brake whenever an object inside the check area moves above the configured speed threshold, even if that object is moving away from the ego vehicle and does not actually pose a risk. - -In addition, the rectangular check area currently rotates together with the ego vehicle. This can affect the relevance of the checked region during turning maneuvers. +The current check is still conservative because it uses a rectangular conflict region and a closing-speed heuristic instead of full trajectory prediction. -The current check should therefore be understood as a conservative safety mechanism. +If the blackboard does not provide global pose and heading, the implementation falls back to the ego-aligned rectangle for that cycle. In the long term, this check may become less relevant or unnecessary if the collision check becomes sufficiently reliable. @@ -97,4 +97,5 @@ The following parameters are used for cross traffic detection: - PRIORITY_SPEED_THRESHOLD: Speed threshold for prioritizing traffic (25.0/3.6 m/s ≈ 6.94 m/s) - PRIORITY_CHECK_DISTANCE: Distance for priority traffic detection (13.0 m) - PRIORITY_CHECK_LENGTH / WIDTH: Size of the priority check area (25.0 m / 50.0 m) +- PRIORITY_CLOSING_SPEED_THRESHOLD: Minimum closing speed toward the conflict region (0.5 m/s) - SELF_EMERGENCY_THRESHOLD: Ego speed above which emergency handling is triggered (10.0/3.6 m/s ≈ 2.78 m/s) From cad570d5f72b463b41d30ec27ec913d10fb5d9ca Mon Sep 17 00:00:00 2001 From: ll7 Date: Tue, 31 Mar 2026 19:14:14 +0200 Subject: [PATCH 17/43] Expand future work document structure --- doc/dev_talks/paf25/future_work.md | 50 +++++++++++++++++++++++++----- 1 file changed, 43 insertions(+), 7 deletions(-) diff --git a/doc/dev_talks/paf25/future_work.md b/doc/dev_talks/paf25/future_work.md index 9afba536..f35acd95 100644 --- a/doc/dev_talks/paf25/future_work.md +++ b/doc/dev_talks/paf25/future_work.md @@ -2,18 +2,54 @@ ## Table of Contents +- [Table of Contents](#table-of-contents) - [1. Goal](#1-goal) - [2. Recommended Development Principles](#2-recommended-development-principles) + - [2.1 Consolidation Before Expansion](#21-consolidation-before-expansion) + - [2.2 Interfaces Must Become Explicit](#22-interfaces-must-become-explicit) + - [2.3 Development Must Be Evidence-Driven](#23-development-must-be-evidence-driven) + - [2.4 Integration Should Happen Weekly](#24-integration-should-happen-weekly) - [3. Proposed Work Packages](#3-proposed-work-packages) - - [3.1 WP1: Documentation and Interface Consolidation](#31-wp1-documentation-and-interface-consolidation) - - [3.2 WP2: Radar Motion Quality](#32-wp2-radar-motion-quality) - - [3.3 WP3: Lidar, Mapping and Tracking Stabilization](#33-wp3-lidar-mapping-and-tracking-stabilization) - - [3.4 WP4: Collision Prediction and Intersection Logic](#34-wp4-collision-prediction-and-intersection-logic) - - [3.5 WP5: Automated Testing and CI Expansion](#35-wp5-automated-testing-and-ci-expansion) - - [3.6 WP6: Performance, Metrics and Observability](#36-wp6-performance-metrics-and-observability) - - [3.7 WP7: Optional Feature Expansion](#37-wp7-optional-feature-expansion) +- [3.1 WP1: Documentation and Interface Consolidation](#31-wp1-documentation-and-interface-consolidation) + - [Objective](#objective) + - [Why this should be first](#why-this-should-be-first) + - [Main tasks](#main-tasks) + - [Expected output](#expected-output) +- [3.2 WP2: Radar Motion Quality](#32-wp2-radar-motion-quality) + - [Objective](#objective-1) + - [Why this matters](#why-this-matters) + - [Main tasks](#main-tasks-1) + - [Expected output](#expected-output-1) +- [3.3 WP3: Lidar, Mapping and Tracking Stabilization](#33-wp3-lidar-mapping-and-tracking-stabilization) + - [Objective](#objective-2) + - [Why this matters](#why-this-matters-1) + - [Main tasks](#main-tasks-2) + - [Expected output](#expected-output-2) +- [3.4 WP4: Collision Prediction and Intersection Logic](#34-wp4-collision-prediction-and-intersection-logic) + - [Objective](#objective-3) + - [Why this matters](#why-this-matters-2) + - [Main tasks](#main-tasks-3) + - [Expected output](#expected-output-3) +- [3.5 WP5: Automated Testing and CI Expansion](#35-wp5-automated-testing-and-ci-expansion) + - [Objective](#objective-4) + - [Why this matters](#why-this-matters-3) + - [Main tasks](#main-tasks-4) + - [Expected output](#expected-output-4) +- [3.6 WP6: Performance, Metrics and Observability](#36-wp6-performance-metrics-and-observability) + - [Objective](#objective-5) + - [Why this matters](#why-this-matters-4) + - [Main tasks](#main-tasks-5) + - [Expected output](#expected-output-5) +- [3.7 WP7: Optional Feature Expansion](#37-wp7-optional-feature-expansion) + - [Objective](#objective-6) + - [Candidate topics](#candidate-topics) + - [Important note](#important-note) - [4. Proposed Execution Order](#4-proposed-execution-order) - [5. Suggested Team Split for 4 Students](#5-suggested-team-split-for-4-students) + - [Student 1: Perception and Radar](#student-1-perception-and-radar) + - [Student 2: Mapping and Tracking](#student-2-mapping-and-tracking) + - [Student 3: Planning and Intersection Logic](#student-3-planning-and-intersection-logic) + - [Student 4: Testing, Tooling and Documentation](#student-4-testing-tooling-and-documentation) - [6. Definition of Done for the Next Phase](#6-definition-of-done-for-the-next-phase) ## 1. Goal From 5c51df0f28f097030399dacf77f4ea5a63a2ed46 Mon Sep 17 00:00:00 2001 From: ll7 Date: Tue, 31 Mar 2026 19:23:50 +0200 Subject: [PATCH 18/43] Refs #826: finish repo-wide ROS parameter .value migration --- code/agent/agent/data_management_node.py | 6 +- .../control/pure_pursuit_controller.py | 110 ++--- code/control/control/vehicle_controller.py | 100 ++-- code/control/control/velocity_controller.py | 136 ++--- .../localization/gps_debug_node.py | 6 +- .../localization/gps_transform.py | 14 +- .../localization/kalman_filter.py | 14 +- .../position_heading_publisher_node.py | 46 +- code/mapping/mapping/data_integration.py | 463 +++++++----------- code/mapping/mapping/visualization.py | 54 +- .../perception/Lanedetection_node.py | 6 +- code/perception/perception/lane_position.py | 230 ++++----- code/perception/perception/lidar_distance.py | 76 +-- code/perception/perception/vision_node.py | 98 ++-- .../planning/behavior_agent/behavior_tree.py | 18 +- .../global_plan_distance_publisher.py | 6 +- .../global_planner/global_planner_node.py | 18 +- code/planning/planning/local_planner/ACC.py | 304 +++++------- .../planning/local_planner/motion_planning.py | 46 +- 19 files changed, 669 insertions(+), 1082 deletions(-) diff --git a/code/agent/agent/data_management_node.py b/code/agent/agent/data_management_node.py index 2bef35c0..a312e062 100644 --- a/code/agent/agent/data_management_node.py +++ b/code/agent/agent/data_management_node.py @@ -21,11 +21,7 @@ def __init__(self): self.global_plan: Optional[CarlaRoute] = None # Parameters - self.role_name = ( - self.declare_parameter("role_name", "hero") - .get_parameter_value() - .string_value - ) + self.role_name = self.declare_parameter("role_name", "hero").value # Services # Get created only after data is available diff --git a/code/control/control/pure_pursuit_controller.py b/code/control/control/pure_pursuit_controller.py index bcfe6452..3afafda8 100755 --- a/code/control/control/pure_pursuit_controller.py +++ b/code/control/control/pure_pursuit_controller.py @@ -36,73 +36,49 @@ def __init__(self): self.get_logger().info(f"{type(self).__name__} node initializing...") # Configuration parameters - self.control_loop_rate = ( - self.declare_parameter("control_loop_rate", 0.05) - .get_parameter_value() - .double_value - ) - self.role_name = ( - self.declare_parameter("role_name", "hero") - .get_parameter_value() - .string_value - ) - - self.k_lad = ( - self.declare_parameter( - "k_lad", - 0.85, - descriptor=ParameterDescriptor( - description="Impact of velocity on lookahead distance", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=10.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.min_la_distance = ( - self.declare_parameter( - "min_la_distance", - 3.0, - descriptor=ParameterDescriptor( - description="Minimal lookahead distance", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=10.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.max_la_distance = ( - self.declare_parameter( - "max_la_distance", - 25.0, - descriptor=ParameterDescriptor( - description="Maximal lookahead distance", - floating_point_range=[ - FloatingPointRange(from_value=10.0, to_value=50.0, step=0.1) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.k_pub = ( - self.declare_parameter( - "k_pub", - 0.8, - descriptor=ParameterDescriptor( - description="Proportional factor of published steer", - floating_point_range=[ - FloatingPointRange(from_value=0.1, to_value=3.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) + self.control_loop_rate = self.declare_parameter("control_loop_rate", 0.05).value + self.role_name = self.declare_parameter("role_name", "hero").value + + self.k_lad = self.declare_parameter( + "k_lad", + 0.85, + descriptor=ParameterDescriptor( + description="Impact of velocity on lookahead distance", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=10.0, step=0.01) + ], + ), + ).value + self.min_la_distance = self.declare_parameter( + "min_la_distance", + 3.0, + descriptor=ParameterDescriptor( + description="Minimal lookahead distance", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=10.0, step=0.01) + ], + ), + ).value + self.max_la_distance = self.declare_parameter( + "max_la_distance", + 25.0, + descriptor=ParameterDescriptor( + description="Maximal lookahead distance", + floating_point_range=[ + FloatingPointRange(from_value=10.0, to_value=50.0, step=0.1) + ], + ), + ).value + self.k_pub = self.declare_parameter( + "k_pub", + 0.8, + descriptor=ParameterDescriptor( + description="Proportional factor of published steer", + floating_point_range=[ + FloatingPointRange(from_value=0.1, to_value=3.0, step=0.01) + ], + ), + ).value self.trajectory_sub: Subscription = self.create_subscription( Path, "/paf/acting/trajectory_local", self.__set_trajectory, qos_profile=1 diff --git a/code/control/control/vehicle_controller.py b/code/control/control/vehicle_controller.py index f0e4a3ea..bbf8e51c 100755 --- a/code/control/control/vehicle_controller.py +++ b/code/control/control/vehicle_controller.py @@ -38,69 +38,45 @@ def __init__(self): self.get_logger().info(f"{type(self).__name__} node initializing...") # Configuration parameters - self.control_loop_rate = ( - self.declare_parameter("control_loop_rate", 0.05) - .get_parameter_value() - .double_value - ) - self.role_name = ( - self.declare_parameter("role_name", "hero") - .get_parameter_value() - .string_value - ) - self.loop_sleep_time = ( - self.declare_parameter( - "loop_sleep_time", - 0.2, - descriptor=ParameterDescriptor( - description="This sleep time is used to slow down the vehicle " - "controller to a reasonable speed", - floating_point_range=[ - FloatingPointRange(from_value=0.05, to_value=0.4, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) + self.control_loop_rate = self.declare_parameter("control_loop_rate", 0.05).value + self.role_name = self.declare_parameter("role_name", "hero").value + self.loop_sleep_time = self.declare_parameter( + "loop_sleep_time", + 0.2, + descriptor=ParameterDescriptor( + description="This sleep time is used to slow down the vehicle " + "controller to a reasonable speed", + floating_point_range=[ + FloatingPointRange(from_value=0.05, to_value=0.4, step=0.01) + ], + ), + ).value # Manual control - self.manual_override_active = ( - self.declare_parameter( - "manual_override_active", - False, - descriptor=ParameterDescriptor(description="Activate Manual Override"), - ) - .get_parameter_value() - .bool_value - ) - self.manual_steer = ( - self.declare_parameter( - "manual_steer", - 0.0, - descriptor=ParameterDescriptor( - description="Steering input sent to carla.", - floating_point_range=[ - FloatingPointRange(from_value=-1.0, to_value=1.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.manual_throttle = ( - self.declare_parameter( - "manual_throttle", - 0.0, - descriptor=ParameterDescriptor( - description="Throttle input sent to carla.", - floating_point_range=[ - FloatingPointRange(from_value=-1.0, to_value=1.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) + self.manual_override_active = self.declare_parameter( + "manual_override_active", + False, + descriptor=ParameterDescriptor(description="Activate Manual Override"), + ).value + self.manual_steer = self.declare_parameter( + "manual_steer", + 0.0, + descriptor=ParameterDescriptor( + description="Steering input sent to carla.", + floating_point_range=[ + FloatingPointRange(from_value=-1.0, to_value=1.0, step=0.01) + ], + ), + ).value + self.manual_throttle = self.declare_parameter( + "manual_throttle", + 0.0, + descriptor=ParameterDescriptor( + description="Throttle input sent to carla.", + floating_point_range=[ + FloatingPointRange(from_value=-1.0, to_value=1.0, step=0.01) + ], + ), + ).value # State variables self.__curr_behavior = None diff --git a/code/control/control/velocity_controller.py b/code/control/control/velocity_controller.py index b219a28e..a5846b7a 100755 --- a/code/control/control/velocity_controller.py +++ b/code/control/control/velocity_controller.py @@ -22,88 +22,60 @@ def __init__(self): super().__init__("velocity_controller") self.get_logger().info(f"{type(self).__name__} node initializing...") - self.control_loop_rate = ( - self.declare_parameter( - "control_loop_rate", - 0.05, - ) - .get_parameter_value() - .double_value - ) - self.role_name = ( - self.declare_parameter("role_name", "hero") - .get_parameter_value() - .string_value - ) - - self.fixed_speed = ( - self.declare_parameter( - "fixed_speed", - 0.0, - descriptor=ParameterDescriptor( - description="Drive with fixed speed / disregard input", - floating_point_range=[ - FloatingPointRange(from_value=-10.0, to_value=10.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.fixed_speed_active = ( - self.declare_parameter( - "fixed_speed_active", - False, - descriptor=ParameterDescriptor( - description="Activate fixed speed mode disregards input" - ), - ) - .get_parameter_value() - .bool_value - ) - - self.pid_p = ( - self.declare_parameter( - "pid_p", - 0.60, - descriptor=ParameterDescriptor( - description="P for PID controller", - floating_point_range=[ - FloatingPointRange(from_value=0.001, to_value=10.0, step=0.001) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.pid_i = ( - self.declare_parameter( - "pid_i", - 0.00076, - descriptor=ParameterDescriptor( - description="I for PID controller", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=0.1, step=0.00001) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.pid_d = ( - self.declare_parameter( - "pid_d", - 0.63, - descriptor=ParameterDescriptor( - description="D for PID controller", - floating_point_range=[ - FloatingPointRange(from_value=0.01, to_value=10.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) + self.control_loop_rate = self.declare_parameter( + "control_loop_rate", + 0.05, + ).value + self.role_name = self.declare_parameter("role_name", "hero").value + + self.fixed_speed = self.declare_parameter( + "fixed_speed", + 0.0, + descriptor=ParameterDescriptor( + description="Drive with fixed speed / disregard input", + floating_point_range=[ + FloatingPointRange(from_value=-10.0, to_value=10.0, step=0.01) + ], + ), + ).value + self.fixed_speed_active = self.declare_parameter( + "fixed_speed_active", + False, + descriptor=ParameterDescriptor( + description="Activate fixed speed mode disregards input" + ), + ).value + + self.pid_p = self.declare_parameter( + "pid_p", + 0.60, + descriptor=ParameterDescriptor( + description="P for PID controller", + floating_point_range=[ + FloatingPointRange(from_value=0.001, to_value=10.0, step=0.001) + ], + ), + ).value + self.pid_i = self.declare_parameter( + "pid_i", + 0.00076, + descriptor=ParameterDescriptor( + description="I for PID controller", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=0.1, step=0.00001) + ], + ), + ).value + self.pid_d = self.declare_parameter( + "pid_d", + 0.63, + descriptor=ParameterDescriptor( + description="D for PID controller", + floating_point_range=[ + FloatingPointRange(from_value=0.01, to_value=10.0, step=0.01) + ], + ), + ).value self.target_velocity_sub: Subscription = self.create_subscription( Float32, diff --git a/code/localization/localization/gps_debug_node.py b/code/localization/localization/gps_debug_node.py index da701d3b..1ce3d2a4 100644 --- a/code/localization/localization/gps_debug_node.py +++ b/code/localization/localization/gps_debug_node.py @@ -16,11 +16,7 @@ class GpsDebug(Node): def __init__(self): super().__init__(type(self).__name__) self.get_logger().info(f"{type(self).__name__} node initializing...") - self.role_name = ( - self.declare_parameter("role_name", "hero") - .get_parameter_value() - .string_value - ) + self.role_name = self.declare_parameter("role_name", "hero").value self.create_subscription( NavSatFix, diff --git a/code/localization/localization/gps_transform.py b/code/localization/localization/gps_transform.py index b9b0c117..98de56ca 100755 --- a/code/localization/localization/gps_transform.py +++ b/code/localization/localization/gps_transform.py @@ -27,16 +27,10 @@ def __init__(self): super().__init__("gps_transform") self.get_logger().info(f"{type(self).__name__} node initializing...") self.transformer = CoordinateTransformer() - self.role_name = ( - self.declare_parameter("role_name", "hero") - .get_parameter_value() - .string_value - ) - self.position_use_ground_truth = ( - self.declare_parameter("position_use_ground_truth", False) - .get_parameter_value() - .bool_value - ) + self.role_name = self.declare_parameter("role_name", "hero").value + self.position_use_ground_truth = self.declare_parameter( + "position_use_ground_truth", False + ).value # Initalize publisher for Odometry data self.odometry_publisher: Publisher = self.create_publisher( diff --git a/code/localization/localization/kalman_filter.py b/code/localization/localization/kalman_filter.py index e0beef1d..a11e4e03 100755 --- a/code/localization/localization/kalman_filter.py +++ b/code/localization/localization/kalman_filter.py @@ -94,16 +94,10 @@ def __init__(self): # basic info self.transformer = None # for coordinate transformation - self.control_loop_rate = ( - self.declare_parameter("control_loop_rate", 0.001) - .get_parameter_value() - .double_value - ) - self.role_name = ( - self.declare_parameter("role_name", "hero") - .get_parameter_value() - .string_value - ) + self.control_loop_rate = self.declare_parameter( + "control_loop_rate", 0.001 + ).value + self.role_name = self.declare_parameter("role_name", "hero").value self.frame_id = "map" self.dt = self.control_loop_rate diff --git a/code/localization/localization/position_heading_publisher_node.py b/code/localization/localization/position_heading_publisher_node.py index ea572c98..b60869df 100755 --- a/code/localization/localization/position_heading_publisher_node.py +++ b/code/localization/localization/position_heading_publisher_node.py @@ -58,44 +58,26 @@ def __init__(self): self.get_logger().info(f"{type(self).__name__} node initializing...") # Configuration parameters - self.control_loop_rate = ( - self.declare_parameter("control_loop_rate", 0.05) - .get_parameter_value() - .double_value - ) - self.role_name = ( - self.declare_parameter("role_name", "hero") - .get_parameter_value() - .string_value - ) + self.control_loop_rate = self.declare_parameter("control_loop_rate", 0.05).value + self.role_name = self.declare_parameter("role_name", "hero").value # Filter used: """ Possible Filters: Pos: EKF, Kalman, RunningAvg, None Heading: EKF, Kalman, None """ - self.pos_filter = ( - self.declare_parameter( - "pos_filter", - "EKF", - descriptor=ParameterDescriptor( - description="Options: EKF, Kalman, RunningAvg, None" - ), - ) - .get_parameter_value() - .string_value - ) - self.heading_filter = ( - self.declare_parameter( - "heading_filter", - "EKF", - descriptor=ParameterDescriptor( - description="Options: EKF, Kalman, None" - ), - ) - .get_parameter_value() - .string_value - ) + self.pos_filter = self.declare_parameter( + "pos_filter", + "EKF", + descriptor=ParameterDescriptor( + description="Options: EKF, Kalman, RunningAvg, None" + ), + ).value + self.heading_filter = self.declare_parameter( + "heading_filter", + "EKF", + descriptor=ParameterDescriptor(description="Options: EKF, Kalman, None"), + ).value self.get_logger().info( f"Pos Filter: {self.pos_filter}, Heading Filter: {self.heading_filter}" ) diff --git a/code/mapping/mapping/data_integration.py b/code/mapping/mapping/data_integration.py index c89257d8..54a3ee84 100755 --- a/code/mapping/mapping/data_integration.py +++ b/code/mapping/mapping/data_integration.py @@ -82,299 +82,206 @@ def __init__(self): # Parameters - self.map_publish_rate = ( - self.declare_parameter("map_publish_rate", 0.05) - .get_parameter_value() - .double_value - ) + self.map_publish_rate = self.declare_parameter("map_publish_rate", 0.05).value # Parameters: Enable entity sources - self.enable_radar_cluster = ( - self.declare_parameter( - "enable_radar_cluster", - True, - descriptor=ParameterDescriptor( - description="Enable Radar Cluster integration", - ), - ) - .get_parameter_value() - .bool_value - ) - self.enable_lidar_cluster = ( - self.declare_parameter( - "enable_lidar_cluster", - True, - descriptor=ParameterDescriptor( - description="Enable Lidar Cluster integration", - ), - ) - .get_parameter_value() - .bool_value - ) - self.enable_vision_cluster = ( - self.declare_parameter( - "enable_vision_cluster", - True, - descriptor=ParameterDescriptor( - description="Enable Vision Node Cluster integration", - ), - ) - .get_parameter_value() - .bool_value - ) - self.enable_raw_lidar_points = ( - self.declare_parameter( - "enable_raw_lidar_points", - False, - descriptor=ParameterDescriptor( - description="Enable raw lidar input", - ), - ) - .get_parameter_value() - .bool_value - ) - self.enable_lane_marker = ( - self.declare_parameter( - "enable_lane_marker", - True, - descriptor=ParameterDescriptor( - description="Enable Lane Mark integration", - ), - ) - .get_parameter_value() - .bool_value - ) - self.enable_stop_marks = ( - self.declare_parameter( - "enable_stop_marks", - True, - descriptor=ParameterDescriptor( - description="Enable stop marks from the UpdateStopMarks service", - ), - ) - .get_parameter_value() - .bool_value - ) - self.radar_lidar_assoc_buffer = ( - self.declare_parameter( - "radar_lidar_assoc_buffer", - 1.5, - descriptor=ParameterDescriptor( - description="Buffer [m] around lidar polygon for radar association" - ), - ) - .get_parameter_value() - .double_value - ) + self.enable_radar_cluster = self.declare_parameter( + "enable_radar_cluster", + True, + descriptor=ParameterDescriptor( + description="Enable Radar Cluster integration", + ), + ).value + self.enable_lidar_cluster = self.declare_parameter( + "enable_lidar_cluster", + True, + descriptor=ParameterDescriptor( + description="Enable Lidar Cluster integration", + ), + ).value + self.enable_vision_cluster = self.declare_parameter( + "enable_vision_cluster", + True, + descriptor=ParameterDescriptor( + description="Enable Vision Node Cluster integration", + ), + ).value + self.enable_raw_lidar_points = self.declare_parameter( + "enable_raw_lidar_points", + False, + descriptor=ParameterDescriptor( + description="Enable raw lidar input", + ), + ).value + self.enable_lane_marker = self.declare_parameter( + "enable_lane_marker", + True, + descriptor=ParameterDescriptor( + description="Enable Lane Mark integration", + ), + ).value + self.enable_stop_marks = self.declare_parameter( + "enable_stop_marks", + True, + descriptor=ParameterDescriptor( + description="Enable stop marks from the UpdateStopMarks service", + ), + ).value + self.radar_lidar_assoc_buffer = self.declare_parameter( + "radar_lidar_assoc_buffer", + 1.5, + descriptor=ParameterDescriptor( + description="Buffer [m] around lidar polygon for radar association" + ), + ).value # Parameters: Filtering - self.filter_enable_lane_index = ( - self.declare_parameter( - "filter_enable_lane_index", - True, - descriptor=ParameterDescriptor( - description="Enable or disable the lane index filter", - ), - ) - .get_parameter_value() - .bool_value - ) - self.filter_enable_pedestrian_grow = ( - self.declare_parameter( - "filter_enable_pedestrian_grow", - True, - descriptor=ParameterDescriptor( - description="Enable or disable the pedestrian grow filter", - ), - ) - .get_parameter_value() - .bool_value - ) - self.filter_enable_merge = ( - self.declare_parameter( - "filter_enable_merge", - True, - descriptor=ParameterDescriptor( - description="Enable or disable the merging filter", - ), - ) - .get_parameter_value() - .bool_value - ) - self.filter_merge_growth_distance = ( - self.declare_parameter( - "filter_merge_growth_distance", - 0.3, - descriptor=ParameterDescriptor( - description="Amount shapes grow before merging in meters", - floating_point_range=[ - FloatingPointRange(from_value=0.01, to_value=5.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.filter_merge_min_overlap_percent = ( - self.declare_parameter( - "filter_merge_min_overlap_percent", - 0.5, - descriptor=ParameterDescriptor( - description="Min overlap of the grown shapes in percent", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=1.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.filter_merge_min_overlap_area = ( - self.declare_parameter( - "filter_merge_min_overlap_area", - 0.5, - descriptor=ParameterDescriptor( - description="Min overlap of the grown shapes in m2", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=5.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.polygon_simplify_tolerance = ( - self.declare_parameter( - "polygon_simplify_tolerance", - 0.1, - descriptor=ParameterDescriptor( - description="The polygon simplify tolerance", - floating_point_range=[ - FloatingPointRange(from_value=0.01, to_value=1.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) + self.filter_enable_lane_index = self.declare_parameter( + "filter_enable_lane_index", + True, + descriptor=ParameterDescriptor( + description="Enable or disable the lane index filter", + ), + ).value + self.filter_enable_pedestrian_grow = self.declare_parameter( + "filter_enable_pedestrian_grow", + True, + descriptor=ParameterDescriptor( + description="Enable or disable the pedestrian grow filter", + ), + ).value + self.filter_enable_merge = self.declare_parameter( + "filter_enable_merge", + True, + descriptor=ParameterDescriptor( + description="Enable or disable the merging filter", + ), + ).value + self.filter_merge_growth_distance = self.declare_parameter( + "filter_merge_growth_distance", + 0.3, + descriptor=ParameterDescriptor( + description="Amount shapes grow before merging in meters", + floating_point_range=[ + FloatingPointRange(from_value=0.01, to_value=5.0, step=0.01) + ], + ), + ).value + self.filter_merge_min_overlap_percent = self.declare_parameter( + "filter_merge_min_overlap_percent", + 0.5, + descriptor=ParameterDescriptor( + description="Min overlap of the grown shapes in percent", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=1.0, step=0.01) + ], + ), + ).value + self.filter_merge_min_overlap_area = self.declare_parameter( + "filter_merge_min_overlap_area", + 0.5, + descriptor=ParameterDescriptor( + description="Min overlap of the grown shapes in m2", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=5.0, step=0.01) + ], + ), + ).value + self.polygon_simplify_tolerance = self.declare_parameter( + "polygon_simplify_tolerance", + 0.1, + descriptor=ParameterDescriptor( + description="The polygon simplify tolerance", + floating_point_range=[ + FloatingPointRange(from_value=0.01, to_value=1.0, step=0.01) + ], + ), + ).value - self.filter_tracking_entities = ( - self.declare_parameter( - "filter_tracking_entities", - True, - descriptor=ParameterDescriptor( - description="Enable or disable the tracking filter", - ), - ) - .get_parameter_value() - .bool_value - ) + self.filter_tracking_entities = self.declare_parameter( + "filter_tracking_entities", + True, + descriptor=ParameterDescriptor( + description="Enable or disable the tracking filter", + ), + ).value - self.update_tracking_velocity = ( - self.declare_parameter( - "update_tracking_velocity", - False, - descriptor=ParameterDescriptor( - description="Enable or disable to update tracking motion data ", - ), - ) - .get_parameter_value() - .bool_value - ) + self.update_tracking_velocity = self.declare_parameter( + "update_tracking_velocity", + False, + descriptor=ParameterDescriptor( + description="Enable or disable to update tracking motion data ", + ), + ).value self.tracking_filter = TrackingFilter() self.radar_point_assignment_filter = RadarPointAssignmentFilter() # Parameters: Lidar (Only relevant for the raw lider point input) - self.lidar_z_min = ( - self.declare_parameter( - "lidar_z_min", - -1.5, - descriptor=ParameterDescriptor( - description="Excludes lidar points below this height", - floating_point_range=[ - FloatingPointRange(from_value=-10.0, to_value=2.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.lidar_z_max = ( - self.declare_parameter( - "lidar_z_max", - 1.0, - descriptor=ParameterDescriptor( - description="Exclude lidar points above this height", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=10.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.lidar_shape_radius = ( - self.declare_parameter( - "lidar_shape_radius", - 0.15, - descriptor=ParameterDescriptor( - description="The radius with which lidar points get added to map", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=1.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.lidar_priority = ( - self.declare_parameter( - "lidar_priority", - 0.25, - descriptor=ParameterDescriptor( - description="The priority lidar points have in the map", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=1.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.lidar_discard_probability = ( - self.declare_parameter( - "lidar_discard_probability", - 0.9, - descriptor=ParameterDescriptor( - description="Discard this many lidar points. " - "Important for performance", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=1.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) + self.lidar_z_min = self.declare_parameter( + "lidar_z_min", + -1.5, + descriptor=ParameterDescriptor( + description="Excludes lidar points below this height", + floating_point_range=[ + FloatingPointRange(from_value=-10.0, to_value=2.0, step=0.01) + ], + ), + ).value + self.lidar_z_max = self.declare_parameter( + "lidar_z_max", + 1.0, + descriptor=ParameterDescriptor( + description="Exclude lidar points above this height", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=10.0, step=0.01) + ], + ), + ).value + self.lidar_shape_radius = self.declare_parameter( + "lidar_shape_radius", + 0.15, + descriptor=ParameterDescriptor( + description="The radius with which lidar points get added to map", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=1.0, step=0.01) + ], + ), + ).value + self.lidar_priority = self.declare_parameter( + "lidar_priority", + 0.25, + descriptor=ParameterDescriptor( + description="The priority lidar points have in the map", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=1.0, step=0.01) + ], + ), + ).value + self.lidar_discard_probability = self.declare_parameter( + "lidar_discard_probability", + 0.9, + descriptor=ParameterDescriptor( + description="Discard this many lidar points. Important for performance", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=1.0, step=0.01) + ], + ), + ).value # Parameter Radar classification - self.classification_threshold = ( - self.declare_parameter( - "classification_threshold", - 1.5, - descriptor=ParameterDescriptor( - description="Threshold when an entity is classified as stationary", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=3.0, step=0.1) - ], - ), - ) - .get_parameter_value() - .double_value - ) + self.classification_threshold = self.declare_parameter( + "classification_threshold", + 1.5, + descriptor=ParameterDescriptor( + description="Threshold when an entity is classified as stationary", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=3.0, step=0.1) + ], + ), + ).value # For the stop marks: self.stop_marks = {} diff --git a/code/mapping/mapping/visualization.py b/code/mapping/mapping/visualization.py index c057a68d..d7cb7d72 100755 --- a/code/mapping/mapping/visualization.py +++ b/code/mapping/mapping/visualization.py @@ -30,35 +30,25 @@ def __init__(self): super().__init__("mapping_visualization") self.get_logger().info(f"{type(self).__name__} node initializing...") - self.map_topic = ( - self.declare_parameter("map_topic", "/paf/hero/mapping/init_data") - .get_parameter_value() - .string_value - ) - - self.show_meta_markers = ( - self.declare_parameter( - "show_meta_markers", - True, - descriptor=ParameterDescriptor( - description="Show meta information for entities", - ), - ) - .get_parameter_value() - .bool_value - ) - - self.show_tracking_info = ( - self.declare_parameter( - "show_tracking_info", - False, - descriptor=ParameterDescriptor( - description="Show tracking information for entities", - ), - ) - .get_parameter_value() - .bool_value - ) + self.map_topic = self.declare_parameter( + "map_topic", "/paf/hero/mapping/init_data" + ).value + + self.show_meta_markers = self.declare_parameter( + "show_meta_markers", + True, + descriptor=ParameterDescriptor( + description="Show meta information for entities", + ), + ).value + + self.show_tracking_info = self.declare_parameter( + "show_tracking_info", + False, + descriptor=ParameterDescriptor( + description="Show tracking information for entities", + ), + ).value self.marker_publisher: Publisher = self.create_publisher( MarkerArray, "/paf/hero/mapping/marker_array", qos_profile=1 @@ -81,11 +71,7 @@ def __init__(self): "flag_ignored", "flag_hero", ]: - value = ( - self.declare_parameter(flag, 0, descriptor=flag_descriptor) - .get_parameter_value() - .integer_value - ) + value = self.declare_parameter(flag, 0, descriptor=flag_descriptor).value setattr(self, flag, value) self.add_on_set_parameters_callback(self._set_parameters_callback) diff --git a/code/perception/perception/Lanedetection_node.py b/code/perception/perception/Lanedetection_node.py index adf87472..154b055d 100755 --- a/code/perception/perception/Lanedetection_node.py +++ b/code/perception/perception/Lanedetection_node.py @@ -25,11 +25,7 @@ def __init__(self): super().__init__(type(self).__name__) self.get_logger().info(f"{type(self).__name__} node initializing...") - self.role_name = ( - self.declare_parameter("role_name", "hero") - .get_parameter_value() - .string_value - ) + self.role_name = self.declare_parameter("role_name", "hero").value # load model self.model = torch.hub.load("hustvl/yolop", "yolop", pretrained=True) diff --git a/code/perception/perception/lane_position.py b/code/perception/perception/lane_position.py index 54ea2ed3..875be8fe 100755 --- a/code/perception/perception/lane_position.py +++ b/code/perception/perception/lane_position.py @@ -33,148 +33,98 @@ def __init__(self): self.dist_arrays = [] # get parameters from launch file - self.line_length = ( - self.declare_parameter( - "line_length", - 15.0, - descriptor=ParameterDescriptor( - description="predefined length of the lanemarkings", - ), - ) - .get_parameter_value() - .double_value - ) - self.line_width = ( - self.declare_parameter( - "line_width", - 0.5, - descriptor=ParameterDescriptor( - description="width of the lanemarkings", - ), - ) - .get_parameter_value() - .double_value - ) - self.epsilon = ( - self.declare_parameter( - "epsilon", - 1.8, - descriptor=ParameterDescriptor( - description="epsilon for clustering algorithm", - ), - ) - .get_parameter_value() - .double_value - ) - self.min_samples = ( - self.declare_parameter( - "min_samples", - 4, - descriptor=ParameterDescriptor( - description="min samples for clustering", - ), - ) - .get_parameter_value() - .integer_value - ) + self.line_length = self.declare_parameter( + "line_length", + 15.0, + descriptor=ParameterDescriptor( + description="predefined length of the lanemarkings", + ), + ).value + self.line_width = self.declare_parameter( + "line_width", + 0.5, + descriptor=ParameterDescriptor( + description="width of the lanemarkings", + ), + ).value + self.epsilon = self.declare_parameter( + "epsilon", + 1.8, + descriptor=ParameterDescriptor( + description="epsilon for clustering algorithm", + ), + ).value + self.min_samples = self.declare_parameter( + "min_samples", + 4, + descriptor=ParameterDescriptor( + description="min samples for clustering", + ), + ).value # confidence parameters: - self.angle_weight = ( - self.declare_parameter( - "angle_weight", - 0.3, - descriptor=ParameterDescriptor( - description="weight for confidence calculation", - ), - ) - .get_parameter_value() - .double_value - ) - self.size_weight = ( - self.declare_parameter( - "size_weight", - 0.3, - descriptor=ParameterDescriptor( - description="weight for confidence calculation", - ), - ) - .get_parameter_value() - .double_value - ) - self.std_dev_weight = ( - self.declare_parameter( - "std_dev_weight", - 0.4, - descriptor=ParameterDescriptor( - description="weight for confidence calculation", - ), - ) - .get_parameter_value() - .double_value - ) - self.angle_normalization = ( - self.declare_parameter( - "angle_normalization", - 25.0, - descriptor=ParameterDescriptor( - description="max acceptable angle for normalization", - ), - ) - .get_parameter_value() - .double_value - ) - self.size_normalization = ( - self.declare_parameter( - "size_normalization", - 15.0, - descriptor=ParameterDescriptor( - description="max acceptable cluster size for normalization", - ), - ) - .get_parameter_value() - .double_value - ) - self.std_dev_normalization = ( - self.declare_parameter( - "std_dev_normalization", - 0.1, - descriptor=ParameterDescriptor( - description="max acceptable standard deviation " - "in linearregression for normalization", - ), - ) - .get_parameter_value() - .double_value - ) - self.angle_prediction_threshold = ( - self.declare_parameter( - "angle_prediction_threshold", - 5.0, - descriptor=ParameterDescriptor( - description="predictions currently disabled", - ), - ) - .get_parameter_value() - .double_value - ) - self.confidence_threshold = ( - self.declare_parameter("confidence_threshold", 0.6) - .get_parameter_value() - .double_value - ) - - self.y_tolerance = ( - self.declare_parameter( - "y_tolerance", - 1.0, - descriptor=ParameterDescriptor( - description="min distance that lanemarkings have to have, " - "new lanemarkings within this distance are ignored", - ), - ) - .get_parameter_value() - .double_value - ) + self.angle_weight = self.declare_parameter( + "angle_weight", + 0.3, + descriptor=ParameterDescriptor( + description="weight for confidence calculation", + ), + ).value + self.size_weight = self.declare_parameter( + "size_weight", + 0.3, + descriptor=ParameterDescriptor( + description="weight for confidence calculation", + ), + ).value + self.std_dev_weight = self.declare_parameter( + "std_dev_weight", + 0.4, + descriptor=ParameterDescriptor( + description="weight for confidence calculation", + ), + ).value + self.angle_normalization = self.declare_parameter( + "angle_normalization", + 25.0, + descriptor=ParameterDescriptor( + description="max acceptable angle for normalization", + ), + ).value + self.size_normalization = self.declare_parameter( + "size_normalization", + 15.0, + descriptor=ParameterDescriptor( + description="max acceptable cluster size for normalization", + ), + ).value + self.std_dev_normalization = self.declare_parameter( + "std_dev_normalization", + 0.1, + descriptor=ParameterDescriptor( + description="max acceptable standard deviation " + "in linearregression for normalization", + ), + ).value + self.angle_prediction_threshold = self.declare_parameter( + "angle_prediction_threshold", + 5.0, + descriptor=ParameterDescriptor( + description="predictions currently disabled", + ), + ).value + self.confidence_threshold = self.declare_parameter( + "confidence_threshold", 0.6 + ).value + + self.y_tolerance = self.declare_parameter( + "y_tolerance", + 1.0, + descriptor=ParameterDescriptor( + description="min distance that lanemarkings have to have, " + "new lanemarkings within this distance are ignored", + ), + ).value self.setup_subscriptions() self.setup_publishers() diff --git a/code/perception/perception/lidar_distance.py b/code/perception/perception/lidar_distance.py index 1b4ab12c..a6f13823 100755 --- a/code/perception/perception/lidar_distance.py +++ b/code/perception/perception/lidar_distance.py @@ -46,55 +46,33 @@ def __init__(self): self.get_logger().info(f"{type(self).__name__} node initializing...") # Parameters - self.clustering_w = ( - self.declare_parameter( - "clustering_w", - 0.0285, - ) - .get_parameter_value() - .double_value - ) - - self.clustering_lidar_z_min = ( - self.declare_parameter( - "clustering_lidar_z_min", - -1.4, - ) - .get_parameter_value() - .double_value - ) - - self.clustering_lidar_z_max = ( - self.declare_parameter( - "clustering_lidar_z_max", - 1.5, - ) - .get_parameter_value() - .double_value - ) - - self.dbscan_eps = ( - self.declare_parameter( - "dbscan_eps", - 0.03375, - ) - .get_parameter_value() - .double_value - ) - self.dbscan_min_samples = ( - self.declare_parameter( - "dbscan_min_samples", - 10, - ) - .get_parameter_value() - .integer_value - ) - - self.compensation_strategy = ( - self.declare_parameter("compensation_strategy", "LocalCompensation") - .get_parameter_value() - .string_value - ) + self.clustering_w = self.declare_parameter( + "clustering_w", + 0.0285, + ).value + + self.clustering_lidar_z_min = self.declare_parameter( + "clustering_lidar_z_min", + -1.4, + ).value + + self.clustering_lidar_z_max = self.declare_parameter( + "clustering_lidar_z_max", + 1.5, + ).value + + self.dbscan_eps = self.declare_parameter( + "dbscan_eps", + 0.03375, + ).value + self.dbscan_min_samples = self.declare_parameter( + "dbscan_min_samples", + 10, + ).value + + self.compensation_strategy = self.declare_parameter( + "compensation_strategy", "LocalCompensation" + ).value compensation_dict = { "NoCompensation": NoCompensation, diff --git a/code/perception/perception/vision_node.py b/code/perception/perception/vision_node.py index 3535dc3c..d3af33b5 100755 --- a/code/perception/perception/vision_node.py +++ b/code/perception/perception/vision_node.py @@ -50,72 +50,40 @@ def __init__(self): self.bridge = CvBridge() # Parameters - self.role_name = ( - self.declare_parameter("role_name", "hero") - .get_parameter_value() - .string_value - ) - self.view_camera = ( - self.declare_parameter("view_camera", False) - .get_parameter_value() - .bool_value - ) - self.camera_resolution = ( - self.declare_parameter("camera_resolution", 1280) - .get_parameter_value() - .integer_value - ) - self.model = ( - self.declare_parameter("model", "yolo11m-seg") - .get_parameter_value() - .string_value - ) + self.role_name = self.declare_parameter("role_name", "hero").value + self.view_camera = self.declare_parameter("view_camera", False).value + self.camera_resolution = self.declare_parameter("camera_resolution", 1280).value + self.model = self.declare_parameter("model", "yolo11m-seg").value # Traffic light parameters - self.min_x: int = ( - self.declare_parameter( - "min_x", - 485, - descriptor=ParameterDescriptor( - description="Left End of Traffic Light bounding box", - ), - ) - .get_parameter_value() - .integer_value - ) - self.max_x: int = ( - self.declare_parameter( - "max_x", - 780, - descriptor=ParameterDescriptor( - description="Right End of Traffic Light bounding box", - ), - ) - .get_parameter_value() - .integer_value - ) - self.max_y: int = ( - self.declare_parameter( - "max_y", - 360, - descriptor=ParameterDescriptor( - description="Lower End of Traffic Light bounding box measuring " - "from the top. (0,0) is the top left corner", - ), - ) - .get_parameter_value() - .integer_value - ) - self.min_prob: float = ( - self.declare_parameter( - "min_prob", - 0.30, - descriptor=ParameterDescriptor( - description="Minimal Probability, that it's a light", - ), - ) - .get_parameter_value() - .double_value - ) + self.min_x: int = self.declare_parameter( + "min_x", + 485, + descriptor=ParameterDescriptor( + description="Left End of Traffic Light bounding box", + ), + ).value + self.max_x: int = self.declare_parameter( + "max_x", + 780, + descriptor=ParameterDescriptor( + description="Right End of Traffic Light bounding box", + ), + ).value + self.max_y: int = self.declare_parameter( + "max_y", + 360, + descriptor=ParameterDescriptor( + description="Lower End of Traffic Light bounding box measuring " + "from the top. (0,0) is the top left corner", + ), + ).value + self.min_prob: float = self.declare_parameter( + "min_prob", + 0.30, + descriptor=ParameterDescriptor( + description="Minimal Probability, that it's a light", + ), + ).value self.depth_images = [] self.lidar_array = None diff --git a/code/planning/planning/behavior_agent/behavior_tree.py b/code/planning/planning/behavior_agent/behavior_tree.py index 86e35d1e..93ac7c83 100755 --- a/code/planning/planning/behavior_agent/behavior_tree.py +++ b/code/planning/planning/behavior_agent/behavior_tree.py @@ -239,19 +239,11 @@ def __init__(self): ) # Parameters - self.control_loop_rate = ( - self.declare_parameter( - "control_loop_rate", - 1.0 / 5.3, - ) - .get_parameter_value() - .double_value - ) - self.role_name = ( - self.declare_parameter("role_name", "hero") - .get_parameter_value() - .string_value - ) + self.control_loop_rate = self.declare_parameter( + "control_loop_rate", + 1.0 / 5.3, + ).value + self.role_name = self.declare_parameter("role_name", "hero").value register_parameters(self, self.blackboard) # Publishers diff --git a/code/planning/planning/global_planner/global_plan_distance_publisher.py b/code/planning/planning/global_planner/global_plan_distance_publisher.py index 82408691..d281c3bd 100755 --- a/code/planning/planning/global_planner/global_plan_distance_publisher.py +++ b/code/planning/planning/global_planner/global_plan_distance_publisher.py @@ -27,11 +27,7 @@ def __init__(self): self.get_logger().info(f"{type(self).__name__} node initializing...") # basic info - self.role_name = ( - self.declare_parameter("role_name", "hero") - .get_parameter_value() - .string_value - ) + self.role_name = self.declare_parameter("role_name", "hero").value self.current_pos = None self.trajectory_local = None diff --git a/code/planning/planning/global_planner/global_planner_node.py b/code/planning/planning/global_planner/global_planner_node.py index 225a960b..4eac2b5a 100755 --- a/code/planning/planning/global_planner/global_planner_node.py +++ b/code/planning/planning/global_planner/global_planner_node.py @@ -65,19 +65,11 @@ def __init__(self): self.speed_limits: Optional[Float32MultiArray] = None # Parameters - self.role_name = ( - self.declare_parameter("role_name", "hero") - .get_parameter_value() - .string_value - ) - self.distance_spawn_to_first_wp = ( - self.declare_parameter( - "distance_spawn_to_first_wp", - 100.0, - ) - .get_parameter_value() - .double_value - ) + self.role_name = self.declare_parameter("role_name", "hero").value + self.distance_spawn_to_first_wp = self.declare_parameter( + "distance_spawn_to_first_wp", + 100.0, + ).value # Services # Get created only after data is available diff --git a/code/planning/planning/local_planner/ACC.py b/code/planning/planning/local_planner/ACC.py index 1ae80109..bc08abdb 100755 --- a/code/planning/planning/local_planner/ACC.py +++ b/code/planning/planning/local_planner/ACC.py @@ -51,184 +51,132 @@ def __init__(self): mapping_common.set_logger(self.get_logger()) # Parameters - self.role_name = ( - self.declare_parameter("role_name", "hero") - .get_parameter_value() - .string_value - ) - - self.k_p = ( - self.declare_parameter( - "k_p", - 0.5, - descriptor=ParameterDescriptor( - description="Kp used for the PI controller", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=3.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.k_i = ( - self.declare_parameter( - "k_i", - 1.2, - descriptor=ParameterDescriptor( - description="Ki used for the PI controller", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=3.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.t_gap = ( - self.declare_parameter( - "t_gap", - 1.9, - descriptor=ParameterDescriptor( - description="Time gap used for the PI controller", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=5.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.d_min = ( - self.declare_parameter( - "d_min", - 0.7, - descriptor=ParameterDescriptor( - description="Minimal distance to the object in front when standing", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=10.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - - self.hard_approach_distance = ( - self.declare_parameter( - "hard_approach_distance", - 1.5, - descriptor=ParameterDescriptor( - description="Minimum distance when closely approaching an obstacle", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=2.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.hard_approach_speed = ( - self.declare_parameter( - "hard_approach_speed", - 1.0, - descriptor=ParameterDescriptor( - description="Minimum speed when closely approaching an obstacle", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=2.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - - self.acceleration_factor = ( - self.declare_parameter( - "acceleration_factor", - 1.0, - descriptor=ParameterDescriptor( - description="Adjusts the acceleration", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=2.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - - self.curve_line_angle = ( - self.declare_parameter( - "curve_line_angle", - 15.0, - descriptor=ParameterDescriptor( - description="Angle (deg!) of the line " - "used to calculate the curve distance", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=90.0, step=0.1) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.min_curve_speed = ( - self.declare_parameter( - "min_curve_speed", - 4.0, - descriptor=ParameterDescriptor( - description="Minimum desired curve speed at min_curve_distance", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=5.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.min_curve_distance = ( - self.declare_parameter( - "min_curve_distance", - 2.0, - descriptor=ParameterDescriptor( - description="Distance to the intersection with the trajectory", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=10.0, step=0.01) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.max_curve_speed = ( - self.declare_parameter( - "max_curve_speed", - 30.0, - descriptor=ParameterDescriptor( - description="Maximum desired curve speed at max_curve_distance", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=50.0, step=0.1) - ], - ), - ) - .get_parameter_value() - .double_value - ) - self.max_curve_distance = ( - self.declare_parameter( - "max_curve_distance", - 50.0, - descriptor=ParameterDescriptor( - description="Distance to the intersection with the trajectory", - floating_point_range=[ - FloatingPointRange(from_value=0.0, to_value=200.0, step=0.1) - ], - ), - ) - .get_parameter_value() - .double_value - ) + self.role_name = self.declare_parameter("role_name", "hero").value + + self.k_p = self.declare_parameter( + "k_p", + 0.5, + descriptor=ParameterDescriptor( + description="Kp used for the PI controller", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=3.0, step=0.01) + ], + ), + ).value + self.k_i = self.declare_parameter( + "k_i", + 1.2, + descriptor=ParameterDescriptor( + description="Ki used for the PI controller", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=3.0, step=0.01) + ], + ), + ).value + self.t_gap = self.declare_parameter( + "t_gap", + 1.9, + descriptor=ParameterDescriptor( + description="Time gap used for the PI controller", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=5.0, step=0.01) + ], + ), + ).value + self.d_min = self.declare_parameter( + "d_min", + 0.7, + descriptor=ParameterDescriptor( + description="Minimal distance to the object in front when standing", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=10.0, step=0.01) + ], + ), + ).value + + self.hard_approach_distance = self.declare_parameter( + "hard_approach_distance", + 1.5, + descriptor=ParameterDescriptor( + description="Minimum distance when closely approaching an obstacle", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=2.0, step=0.01) + ], + ), + ).value + self.hard_approach_speed = self.declare_parameter( + "hard_approach_speed", + 1.0, + descriptor=ParameterDescriptor( + description="Minimum speed when closely approaching an obstacle", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=2.0, step=0.01) + ], + ), + ).value + + self.acceleration_factor = self.declare_parameter( + "acceleration_factor", + 1.0, + descriptor=ParameterDescriptor( + description="Adjusts the acceleration", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=2.0, step=0.01) + ], + ), + ).value + + self.curve_line_angle = self.declare_parameter( + "curve_line_angle", + 15.0, + descriptor=ParameterDescriptor( + description="Angle (deg!) of the line " + "used to calculate the curve distance", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=90.0, step=0.1) + ], + ), + ).value + self.min_curve_speed = self.declare_parameter( + "min_curve_speed", + 4.0, + descriptor=ParameterDescriptor( + description="Minimum desired curve speed at min_curve_distance", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=5.0, step=0.01) + ], + ), + ).value + self.min_curve_distance = self.declare_parameter( + "min_curve_distance", + 2.0, + descriptor=ParameterDescriptor( + description="Distance to the intersection with the trajectory", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=10.0, step=0.01) + ], + ), + ).value + self.max_curve_speed = self.declare_parameter( + "max_curve_speed", + 30.0, + descriptor=ParameterDescriptor( + description="Maximum desired curve speed at max_curve_distance", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=50.0, step=0.1) + ], + ), + ).value + self.max_curve_distance = self.declare_parameter( + "max_curve_distance", + 50.0, + descriptor=ParameterDescriptor( + description="Distance to the intersection with the trajectory", + floating_point_range=[ + FloatingPointRange(from_value=0.0, to_value=200.0, step=0.1) + ], + ), + ).value # Get Map self.map_sub: Subscription = self.create_subscription( diff --git a/code/planning/planning/local_planner/motion_planning.py b/code/planning/planning/local_planner/motion_planning.py index b8006500..ce19b8e4 100755 --- a/code/planning/planning/local_planner/motion_planning.py +++ b/code/planning/planning/local_planner/motion_planning.py @@ -72,35 +72,23 @@ def __init__(self): mapping_common.set_logger(self.get_logger()) - self.role_name = ( - self.declare_parameter("role_name", "hero") - .get_parameter_value() - .string_value - ) - - self.time_horizon = ( - self.declare_parameter( - "time_horizon", - 3.0, - descriptor=ParameterDescriptor( - description="Set time_horizon in seconds for trajectory prediction", - ), - ) - .get_parameter_value() - .double_value - ) - - self.crash_threshold = ( - self.declare_parameter( - "crash_threshold", - 2.0, - descriptor=ParameterDescriptor( - description="Set crash_threshold in seconds", - ), - ) - .get_parameter_value() - .double_value - ) + self.role_name = self.declare_parameter("role_name", "hero").value + + self.time_horizon = self.declare_parameter( + "time_horizon", + 3.0, + descriptor=ParameterDescriptor( + description="Set time_horizon in seconds for trajectory prediction", + ), + ).value + + self.crash_threshold = self.declare_parameter( + "crash_threshold", + 2.0, + descriptor=ParameterDescriptor( + description="Set crash_threshold in seconds", + ), + ).value self.add_on_set_parameters_callback(self._set_parameters_callback) # Overtake related stuff From a112110d0880181d71ae638b506861de03f50ee5 Mon Sep 17 00:00:00 2001 From: ll7 Date: Tue, 31 Mar 2026 19:23:55 +0200 Subject: [PATCH 19/43] Stabilize host smoke pytest collection --- code/test/conftest.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/code/test/conftest.py b/code/test/conftest.py index f7a32bb9..c5362444 100644 --- a/code/test/conftest.py +++ b/code/test/conftest.py @@ -8,6 +8,9 @@ import pytest +collect_ignore = ["run_test.py"] + + @pytest.fixture(autouse=True) def deterministic_seed() -> None: """Set deterministic pseudo-random seeds for repeatable test behavior.""" From eb1174a66dc425cb43db494f5b9bdfef0d8ce5cf Mon Sep 17 00:00:00 2001 From: ll7 Date: Tue, 31 Mar 2026 19:58:26 +0200 Subject: [PATCH 20/43] Fix CARLA simulator startup for non-root runs --- build/docker-compose.carla.base.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build/docker-compose.carla.base.yaml b/build/docker-compose.carla.base.yaml index cfe0ecc6..2d43d787 100644 --- a/build/docker-compose.carla.base.yaml +++ b/build/docker-compose.carla.base.yaml @@ -4,7 +4,7 @@ services: hostname: "carla-simulator" command: | bash -c "\ - /bin/bash CarlaUE4.sh -quality-level=Epic -world-port=2000 -resx=800 -resy=600 -nosound -carla-settings="/CustomCarlaSettings.ini" ${RENDER_OFFSCREEN} \ + exec ./CarlaUE4/Binaries/Linux/CarlaUE4-Linux-Shipping CarlaUE4 -quality-level=Epic -world-port=2000 -resx=800 -resy=600 -nosound -carla-settings="/CustomCarlaSettings.ini" ${RENDER_OFFSCREEN} \ " # The image can be built locally with ./docker/carla/build_carla.sh image: ghcr.io/una-auxme/carla-leaderboard-gpu:2.1 From 107e9f61bc99890fd4c45415d2c4a556d61a810e Mon Sep 17 00:00:00 2001 From: ll7 Date: Wed, 1 Apr 2026 08:49:18 +0200 Subject: [PATCH 21/43] Improve headless CARLA environment bootstrap --- agents.md | 1 + build/.env.sample | 1 + doc/general/execution.md | 3 +++ scripts/update-dotenv.sh | 11 +++++++++++ 4 files changed, 16 insertions(+) diff --git a/agents.md b/agents.md index 35c649c4..d4c8ffae 100644 --- a/agents.md +++ b/agents.md @@ -34,6 +34,7 @@ If guidance conflicts, prefer repository config files and actively used CI/lint - Primary development is Linux + Docker, typically with NVIDIA GPU support. - Python target is **3.12** (`ruff.toml`). - Use containerized workflows when project docs expect them. +- Before compose-based CARLA runs, refresh `build/.env` via `scripts/update-dotenv.sh`; in SSH-forwarded or headless sessions it sets `RENDER_OFFSCREEN=-RenderOffScreen` so the simulator can start without a local desktop renderer. ## 5) Code change rules diff --git a/build/.env.sample b/build/.env.sample index 119832c0..6f1d40a4 100644 --- a/build/.env.sample +++ b/build/.env.sample @@ -4,3 +4,4 @@ PAF_USERNAME=youruser PAF_UID=1000 PAF_GID=1000 +RENDER_OFFSCREEN= diff --git a/doc/general/execution.md b/doc/general/execution.md index a62ee23d..64870496 100644 --- a/doc/general/execution.md +++ b/doc/general/execution.md @@ -30,6 +30,8 @@ This sets up important docker compose environment variables. In order to start the default leaderboard execution simply navigate to the [build](../../build/) folder and select the `Compose up` option in the right-click menu of the `docker-compose.dev..yml` file. As `` `cuda` should be used for the lab PCs. +The helper script [scripts/update-dotenv.sh](../../scripts/update-dotenv.sh) writes the compose environment file [build/.env](../../build/.env). When it detects a headless or SSH-forwarded session, it also sets `RENDER_OFFSCREEN=-RenderOffScreen` so that the CARLA simulator can start without an attached desktop renderer. + ## Directory Structure The `build` directory contains the necessary configuration and setup files for building and running the project services. Below is an overview of the key files: @@ -54,6 +56,7 @@ Defines the configuration for the `carla-simulator` service, which runs the CARL - **Image**: Uses the CARLA simulator image tailored for the project. The image can be built manually with [`build_carla.sh`](../../build/docker/carla/build_carla.sh), but is pulled from the container registry by default. - **Command**: Starts the simulator with specific settings such as resolution, quality level, and disabling sound. - **Environment Variables**: Sets up desktop→docker pass-through +- **Headless fallback**: The compose environment can set `RENDER_OFFSCREEN=-RenderOffScreen` for SSH-forwarded or headless sessions. This keeps CARLA startup working when no usable local display is available. - **Volumes**: - Desktop: X11 UNIX socket + `${XDG_RUNTIME_DIR}` - Custom CARLA settings diff --git a/scripts/update-dotenv.sh b/scripts/update-dotenv.sh index e7148163..6f2f2920 100755 --- a/scripts/update-dotenv.sh +++ b/scripts/update-dotenv.sh @@ -11,6 +11,11 @@ PAF_USERNAME=$(id -u -n) PAF_UID=$(id -u) PAF_GID=$(id -g) RUFF_VERSION="" +RENDER_OFFSCREEN="" + +if [[ -z "${DISPLAY:-}" || "${DISPLAY:-}" == localhost:* || "${DISPLAY:-}" == 127.0.0.1:* ]]; then + RENDER_OFFSCREEN="-RenderOffScreen" +fi if [ -f "${RUFF_VERSION_FILE}" ]; then # shellcheck source=/dev/null @@ -21,12 +26,18 @@ cat >"$DOTENV_FILE" <" +fi if [ -n "${RUFF_VERSION}" ]; then echo "RUFF_VERSION=${RUFF_VERSION}" >>"$DOTENV_FILE" From cecaabc0d1ced51c13d5483bfc8ab95f402cff4b Mon Sep 17 00:00:00 2001 From: ll7 Date: Wed, 1 Apr 2026 08:49:32 +0200 Subject: [PATCH 22/43] Refs #911: add pitch-aware LiDAR ground filtering --- code/perception/launch/perception.xml | 1 + code/perception/perception/lidar_distance.py | 53 ++++++++++----- .../perception/lidar_filter_utility.py | 43 ++++++++++++ .../tests/test_lidar_ground_filter.py | 66 +++++++++++++++++++ doc/perception/lidar_distance.md | 3 +- 5 files changed, 147 insertions(+), 19 deletions(-) create mode 100644 code/perception/tests/test_lidar_ground_filter.py diff --git a/code/perception/launch/perception.xml b/code/perception/launch/perception.xml index 979af9fc..15c6330d 100644 --- a/code/perception/launch/perception.xml +++ b/code/perception/launch/perception.xml @@ -35,6 +35,7 @@ + diff --git a/code/perception/perception/lidar_distance.py b/code/perception/perception/lidar_distance.py index a6f13823..33950048 100755 --- a/code/perception/perception/lidar_distance.py +++ b/code/perception/perception/lidar_distance.py @@ -2,6 +2,7 @@ from joblib import Parallel, delayed import numpy as np +from scipy.spatial.transform import Rotation from sklearn.cluster import DBSCAN from cv_bridge import CvBridge from transforms3d.quaternions import mat2quat @@ -33,7 +34,11 @@ apply_local_motion_compensation, quaternion_to_heading, ) -from .lidar_filter_utility import bounding_box, remove_field_name +from .lidar_filter_utility import ( + bounding_box, + filter_ground_points, + remove_field_name, +) class LidarDistance(Node): @@ -61,6 +66,11 @@ def __init__(self): 1.5, ).value + self.enable_pitch_ground_filter = self.declare_parameter( + "enable_pitch_ground_filter", + True, + ).value + self.dbscan_eps = self.declare_parameter( "dbscan_eps", 0.03375, @@ -84,6 +94,7 @@ def __init__(self): self.Compensation: CompensationStrategy = compensation_dict[ self.compensation_strategy ]() + self.current_pitch = 0.0 self.bridge = CvBridge() # OpenCV bridge for image conversions @@ -147,6 +158,10 @@ def __init__(self): qos_profile=1, ) + if ( + self.compensation_strategy == "LocalCompensation" + or self.enable_pitch_ground_filter + ): self.create_subscription( msg_type=Imu, topic="/carla/hero/IMU", @@ -222,26 +237,27 @@ def speed_callback(self, velocity: CarlaSpeedometer): def imu_callback(self, imu_data: Imu): """ - Receives IMU data and passes the extracted heading - to the LocalCompensation strategy. + Receives IMU data, updates the current pitch estimate for ground filtering, + and passes the extracted heading to the LocalCompensation strategy. :param imu_data: The IMU message containing orientation data. """ - if self.compensation_strategy != "LocalCompensation": - self.get_logger().warn( - f"{type(self).__name__}" - "IMU callback is only active for LocalCompensation." - ) - return - x = imu_data.orientation.x y = imu_data.orientation.y z = imu_data.orientation.z w = imu_data.orientation.w - heading = quaternion_to_heading(x, y, z, w) - self.Compensation.set_motion_data(heading=heading) + try: + self.current_pitch = Rotation.from_quat((x, y, z, w)).as_euler( + "xyz", degrees=False + )[1] + except ValueError: + self.current_pitch = 0.0 + + if self.compensation_strategy == "LocalCompensation": + heading = quaternion_to_heading(x, y, z, w) + self.Compensation.set_motion_data(heading=heading) def start_clustering(self, data): """ @@ -269,13 +285,14 @@ def start_clustering(self, data): & (coordinates["y"] >= -1) & (coordinates["y"] <= 1) # Exclude ego vehicle in y-axis ) - & ( - (coordinates["z"] > self.clustering_lidar_z_min) - & (coordinates["z"] < self.clustering_lidar_z_max) - ) - # Exclude points below a certain height (street) - # Exclude points above a certain height (e.g. leafs of tree) ] + filtered_coordinates = filter_ground_points( + filtered_coordinates, + z_min=self.clustering_lidar_z_min, + z_max=self.clustering_lidar_z_max, + pitch_rad=self.current_pitch, + enable_pitch_compensation=self.enable_pitch_ground_filter, + ) # Perform DBSCAN clustering clustered_points, cluster_labels = cluster_lidar_data_from_pointcloud( diff --git a/code/perception/perception/lidar_filter_utility.py b/code/perception/perception/lidar_filter_utility.py index b84fe86e..821e090e 100755 --- a/code/perception/perception/lidar_filter_utility.py +++ b/code/perception/perception/lidar_filter_utility.py @@ -46,6 +46,49 @@ def bounding_box( return bb_filter +def filter_ground_points( + points, + z_min, + z_max, + pitch_rad=0.0, + enable_pitch_compensation=True, +): + """Filter lidar points using a base height window and an optional pitch-aware + ground plane. + + The existing static cutoff is preserved when pitch compensation is disabled or + when ``pitch_rad`` is zero. When enabled, the lower z-bound is tilted along the + x-axis so that forward road points do not drift into the obstacle set during + ego pitch motion. + + Parameters + ---------- + points: + Structured numpy array with ``x`` and ``z`` fields. + z_min, z_max: + Base vertical bounds for obstacle clustering. + pitch_rad: + Current ego pitch in radians. + enable_pitch_compensation: + Whether to tilt the lower bound with the current pitch. + + Returns + ------- + numpy.ndarray + Filtered structured array containing only points inside the valid region. + """ + + if points.size == 0: + return points + + lower_bound = np.full(points.shape[0], z_min, dtype=float) + if enable_pitch_compensation: + lower_bound = lower_bound + points["x"] * np.tan(pitch_rad) + + mask = (points["z"] > lower_bound) & (points["z"] < z_max) + return points[mask] + + # https://stackoverflow.com/questions/15575878/how-do-you-remove-a-column-from-a-structured-numpy-array def remove_field_name(a, name): """Removes a column from a structured numpy array diff --git a/code/perception/tests/test_lidar_ground_filter.py b/code/perception/tests/test_lidar_ground_filter.py new file mode 100644 index 00000000..577a3754 --- /dev/null +++ b/code/perception/tests/test_lidar_ground_filter.py @@ -0,0 +1,66 @@ +import numpy as np +import pytest + +from perception.lidar_filter_utility import filter_ground_points + +pytestmark = pytest.mark.unit + + +def make_points(rows): + """Create a structured lidar point array with x, y, z, intensity fields.""" + return np.array( + rows, + dtype=[ + ("x", np.float32), + ("y", np.float32), + ("z", np.float32), + ("intensity", np.uint8), + ], + ) + + +def test_filter_ground_points_matches_static_height_window_without_pitch(): + points = make_points( + [ + (1.0, 0.0, -1.5, 1), + (1.0, 0.0, -1.0, 2), + (1.0, 0.0, 1.6, 3), + ] + ) + + filtered = filter_ground_points(points, z_min=-1.4, z_max=1.5, pitch_rad=0.0) + + assert filtered["intensity"].tolist() == [2] + + +def test_filter_ground_points_raises_lower_bound_with_positive_pitch(): + points = make_points( + [ + (2.0, 0.0, -1.0, 1), + (10.0, 0.0, -0.8, 2), + (10.0, 0.0, 0.2, 3), + ] + ) + + filtered = filter_ground_points(points, z_min=-1.4, z_max=1.5, pitch_rad=0.1) + + assert filtered["intensity"].tolist() == [1, 3] + + +def test_filter_ground_points_can_disable_pitch_compensation(): + points = make_points( + [ + (10.0, 0.0, -0.8, 1), + (10.0, 0.0, 0.2, 2), + ] + ) + + filtered = filter_ground_points( + points, + z_min=-1.4, + z_max=1.5, + pitch_rad=0.1, + enable_pitch_compensation=False, + ) + + assert filtered["intensity"].tolist() == [1, 2] diff --git a/doc/perception/lidar_distance.md b/doc/perception/lidar_distance.md index c7806a00..2fb53399 100644 --- a/doc/perception/lidar_distance.md +++ b/doc/perception/lidar_distance.md @@ -183,7 +183,7 @@ LocalCompensation active: - **Topic Name:** `/carla/hero/IMU` - **Data Type:** `sensor_msgs/Imu` -- **Description:** Provides data used for calculating the heading. +- **Description:** Provides orientation data used for calculating the heading and the current pitch angle for pitch-aware ground filtering. ## 4. Processing Pipeline @@ -202,6 +202,7 @@ LocalCompensation active: - Removes points representing the ego vehicle (`start_clustering`). - Filters out points below a certain height (`clustering_lidar_z_min`) to avoid clustering the road surface (`start_clustering`). +- Optionally tilts the lower height bound with the current IMU pitch (`enable_pitch_ground_filter`) so that road points do not drift into the obstacle set during acceleration, braking, or other pitch motion. - Filters out points above a maximum height (`clustering_lidar_z_max`) to exclude high objects like tree leaves or overhead structures. ### 4.4 Clustering the LiDAR Data From d93c4fe9e68292fd17142da48196b060f0dea618 Mon Sep 17 00:00:00 2001 From: ll7 Date: Wed, 1 Apr 2026 09:07:20 +0200 Subject: [PATCH 23/43] Refs #828: document host-auth devcontainer workflow --- .devcontainer/devcontainer.json | 24 ++-- .vscode/extensions.json | 24 ++-- doc/development/README.md | 2 + doc/development/devcontainer.md | 152 ++++++++++++++++++++++ doc/development/quickstart_contributor.md | 1 + 5 files changed, 183 insertions(+), 20 deletions(-) create mode 100644 doc/development/devcontainer.md diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 541f852c..b066abc8 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -19,23 +19,27 @@ "vscode": { "extensions": [ "davidanson.vscode-markdownlint", + "github.copilot", + "github.copilot-chat", "github.vscode-pull-request-github", - "vscode-icons-team.vscode-icons", - "yzhang.markdown-all-in-one", - "njpwerner.autodocstring", "ms-azuretools.vscode-docker", - "bierner.markdown-mermaid", - "richardkotze.git-mob", + "ms-vscode.cmake-tools", "ms-vscode-remote.remote-containers", - "valentjn.vscode-ltex", + "ms-python.python", + "ms-python.vscode-pylance", + "charliermarsh.ruff", + "editorconfig.editorconfig", "augustocdias.tasks-shell-input", - "ktnrg45.vscode-cython", "ranch-hand-robotics.rde-pack", + "ktnrg45.vscode-cython", "timonwong.shellcheck", - "editorconfig.editorconfig", "phil294.git-log--graph", - "ms-vscode.cmake-tools", - "charliermarsh.ruff" + "richardkotze.git-mob", + "bierner.markdown-mermaid", + "valentjn.vscode-ltex", + "vscode-icons-team.vscode-icons", + "yzhang.markdown-all-in-one", + "njpwerner.autodocstring" ] } } diff --git a/.vscode/extensions.json b/.vscode/extensions.json index 23f827fb..d3d3751d 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -1,22 +1,26 @@ { "recommendations": [ "davidanson.vscode-markdownlint", + "github.copilot", + "github.copilot-chat", "github.vscode-pull-request-github", - "vscode-icons-team.vscode-icons", - "yzhang.markdown-all-in-one", - "njpwerner.autodocstring", "ms-azuretools.vscode-docker", - "bierner.markdown-mermaid", - "richardkotze.git-mob", + "ms-vscode.cmake-tools", "ms-vscode-remote.remote-containers", - "valentjn.vscode-ltex", + "ms-python.python", + "ms-python.vscode-pylance", + "charliermarsh.ruff", + "editorconfig.editorconfig", "augustocdias.tasks-shell-input", - "ktnrg45.vscode-cython", "ranch-hand-robotics.rde-pack", + "ktnrg45.vscode-cython", "timonwong.shellcheck", - "editorconfig.editorconfig", "phil294.git-log--graph", - "ms-vscode.cmake-tools", - "charliermarsh.ruff" + "richardkotze.git-mob", + "bierner.markdown-mermaid", + "valentjn.vscode-ltex", + "vscode-icons-team.vscode-icons", + "yzhang.markdown-all-in-one", + "njpwerner.autodocstring" ] } diff --git a/doc/development/README.md b/doc/development/README.md index 71d3f6d3..4edfada3 100644 --- a/doc/development/README.md +++ b/doc/development/README.md @@ -10,6 +10,7 @@ If this is your first time working with the project you can follow the first steps in [/doc/development/first_steps.md](/doc/development/first_steps.md). For a compact setup path see also [/doc/development/quickstart_contributor.md](/doc/development/quickstart_contributor.md). +For the host-auth dev container workflow see also [Dev Container Workflow](./devcontainer.md). ## Development Guidelines @@ -32,6 +33,7 @@ If you contribute to this project please read the following guidelines first: 12. [Context Retention](./context_retention.md) 13. [Dependency Management](./dependency_management.md) 14. [Performance Iteration Guide](./performance_iteration.md) +15. [Dev Container Workflow](./devcontainer.md) ## Administrative Guidelines diff --git a/doc/development/devcontainer.md b/doc/development/devcontainer.md new file mode 100644 index 00000000..e1a12462 --- /dev/null +++ b/doc/development/devcontainer.md @@ -0,0 +1,152 @@ +# Dev Container Workflow + +- [1) Goal](#1-goal) +- [2) Quick Start](#2-quick-start) +- [3) What The Repo Config Does](#3-what-the-repo-config-does) +- [4) Host Auth Workflow](#4-host-auth-workflow) +- [5) Common Auth Tasks](#5-common-auth-tasks) +- [6) Running PAF Inside The Container](#6-running-paf-inside-the-container) +- [7) Troubleshooting](#7-troubleshooting) + +## 1) Goal + +This repository already ships a compose-backed dev container in [.devcontainer/devcontainer.json](../../.devcontainer/devcontainer.json). +The recommended day-to-day workflow is: + +1. Keep the repository on the host. +2. Use VS Code on the host for GitHub, Copilot, and browser-based sign-in. +3. Reopen the workspace in the `agent-dev` container for ROS2, Ruff, and runtime tooling. + +This avoids the most common OAuth pain points of the older "everything happens inside the container" model while keeping the runtime environment aligned with the repository's Docker setup. + +## 2) Quick Start + +1. Open the repository root in VS Code on the host. +2. Install the recommended extensions from [.vscode/extensions.json](../../.vscode/extensions.json). +3. Sign in on the host for the services you need first: + - GitHub in VS Code + - GitHub Copilot in VS Code +4. Run `Dev Containers: Reopen in Container`. +5. Select `PAF Agent Dev (CUDA)` if VS Code prompts for a configuration. +6. Inside the container, verify the toolchain: + +```bash +ros2 --version +ruff --version +``` + +If you only need the container stack without reopening VS Code, you can still use: + +```bash +bash scripts/dev-up.sh +``` + +## 3) What The Repo Config Does + +The current dev container configuration uses the existing development compose stack instead of inventing a separate image. + +- The configuration file is [.devcontainer/devcontainer.json](../../.devcontainer/devcontainer.json). +- It attaches to the `agent-dev` service from [build/docker-compose.dev.cuda.yml](../../build/docker-compose.dev.cuda.yml). +- The workspace inside the container is `/workspace`. +- `initializeCommand` runs `scripts/update-dotenv.sh` on the host before the container starts. + +That `.env` refresh is important because it updates host user IDs and, in headless or SSH-forwarded sessions, writes `RENDER_OFFSCREEN=-RenderOffScreen` so CARLA can start without a local desktop renderer. + +The dev container only starts `agent-dev` on attach. Runtime services such as CARLA stay opt-in and can be launched when needed via scripts or VS Code tasks. + +## 4) Host Auth Workflow + +The supported auth model is split intentionally: + +- VS Code UI sign-in should happen on the host. +- Browser-based OAuth should happen on the host. +- The repository files are bind-mounted into the container, but credentials should not be baked into images. + +This is the path that works best for: + +- GitHub Pull Requests extension +- GitHub Copilot +- GitHub Copilot Chat + +When these extensions are installed and authenticated on the host, VS Code can continue to use them while you edit the mounted workspace inside the container. + +## 5) Common Auth Tasks + +### GitHub and Copilot in VS Code + +1. Open the repository on the host in VS Code. +2. Sign in to GitHub through the Accounts menu. +3. Sign in to Copilot through the Accounts menu or extension prompt. +4. Reopen in container only after host sign-in is complete. + +### GitHub CLI + +For CLI usage, prefer a host terminal when possible: + +```bash +gh auth login --web +``` + +If you specifically need `gh` inside the container, use an explicit device or web flow there as well. That is still a container-local CLI credential, so prefer host-side auth unless container-local CLI access is required. + +### GHCR + +For GHCR pushes or pulls from the host: + +```bash +gh auth token | docker login ghcr.io -u --password-stdin +``` + +Alternatively use a fine-scoped PAT if your workflow requires it. + +## 6) Running PAF Inside The Container + +Once attached to the container, the existing repository workflows continue to apply. + +Useful commands: + +```bash +source /internal_workspace/dev.bashrc +dep.check +ruff check /workspace/code/ +ruff format /workspace/code/ --check +``` + +Useful VS Code tasks: + +- `Update build/.env file` +- `Start dev container stack (CUDA)` +- `Run host smoke tests` +- `Run ROS-backed unit tests (dev container)` +- `Pre-PR quality check` + +## 7) Troubleshooting + +### Dev container starts but CARLA later fails in headless sessions + +Re-run: + +```bash +bash scripts/update-dotenv.sh +``` + +Then verify that `build/.env` contains `RENDER_OFFSCREEN=-RenderOffScreen` for SSH-forwarded or otherwise headless sessions. + +### GitHub or Copilot prompts still appear broken inside the container + +Make sure the sign-in was completed on the host before reopening in container. The supported path is host-auth plus containerized tooling, not a browser installed inside the container. + +### `gh` works on the host but not in the container + +That is expected unless you explicitly log in inside the container as well. Prefer host-side `gh` usage for authentication-heavy flows. + +### GPU or ROS tooling is missing after attach + +Check the usual prerequisites: + +```bash +bash scripts/check-nvidia.sh +bash scripts/update-dotenv.sh +``` + +Then rebuild or reopen the container if needed. \ No newline at end of file diff --git a/doc/development/quickstart_contributor.md b/doc/development/quickstart_contributor.md index a1eaa3bc..d7137a93 100644 --- a/doc/development/quickstart_contributor.md +++ b/doc/development/quickstart_contributor.md @@ -36,6 +36,7 @@ This command: 3. Select the `PAF Agent Dev (CUDA)` configuration if prompted. The devcontainer uses the existing `agent-dev` service and opens `/workspace`. +For host-auth details for GitHub, Copilot, `gh`, and GHCR see [Dev Container Workflow](./devcontainer.md). ## 4) Verify local toolchain From 35fe8f78f6a77b026d8344d6b7be6cbfdcebc41b Mon Sep 17 00:00:00 2001 From: ll7 Date: Wed, 1 Apr 2026 09:09:58 +0200 Subject: [PATCH 24/43] Improve leaderboard.test rerun ergonomics --- .../agent-ros2/scripts/devfunctions.bash | 39 ++++++++++++++++++- .../scripts/launch_leaderboard.test.sh | 3 ++ 2 files changed, 41 insertions(+), 1 deletion(-) diff --git a/build/docker/agent-ros2/scripts/devfunctions.bash b/build/docker/agent-ros2/scripts/devfunctions.bash index a0e3fe80..b4263143 100755 --- a/build/docker/agent-ros2/scripts/devfunctions.bash +++ b/build/docker/agent-ros2/scripts/devfunctions.bash @@ -15,6 +15,8 @@ Development utility functions: Quitting: Ctrl+c does not work on the leaderboard. Use Right-Click->Kill Terminal - agent.dev: Launches the agent (ros2 launch agent agent.dev.xml); Ctrl+c to stop the agent - leaderboard.test: Launches the Simulation test at /code/leaderboard_launcher/scripts/launch_leaderboard.test.sh + Cleans up stale run_test.py process groups first unless PAF_LEADERBOARD_TEST_SKIP_CLEANUP=1 + Override the traffic manager port with PAF_TRAFFIC_MANAGER_PORT= Kill Terminal to end - ruff.lint: Manually trigger the ruff linter to check the python files - ruff.fix-lint: Apply the safe fixes that the ruff linter encounters during linting @@ -100,10 +102,45 @@ export -f pytrees.viewer leaderboard.test(){ ( - /workspace/code/leaderboard_launcher/scripts/launch_leaderboard.test.sh + if [[ "${PAF_LEADERBOARD_TEST_SKIP_CLEANUP:-0}" != "1" ]]; then + leaderboard.test.cleanup + fi + + /workspace/code/leaderboard_launcher/scripts/launch_leaderboard.test.sh "$@" ) } export -f leaderboard.test + +leaderboard.test.cleanup() { + local pid + local pgid + local stale_pids + + stale_pids=$(pgrep -f '/workspace/code/test/run_test.py' || true) + if [[ -z "$stale_pids" ]]; then + return 0 + fi + + echo "Stopping stale leaderboard.test process groups..." + while read -r pid; do + [[ -z "$pid" ]] && continue + pgid=$(ps -o pgid= -p "$pid" | tr -d '[:space:]') + [[ -z "$pgid" ]] && continue + kill -TERM -- "-$pgid" 2>/dev/null || true + done <<< "$stale_pids" + + sleep 2 + + stale_pids=$(pgrep -f '/workspace/code/test/run_test.py' || true) + while read -r pid; do + [[ -z "$pid" ]] && continue + pgid=$(ps -o pgid= -p "$pid" | tr -d '[:space:]') + [[ -z "$pgid" ]] && continue + kill -KILL -- "-$pgid" 2>/dev/null || true + done <<< "$stale_pids" +} +export -f leaderboard.test.cleanup + ruff.lint() { ( ruff check /workspace/code/ diff --git a/code/leaderboard_launcher/scripts/launch_leaderboard.test.sh b/code/leaderboard_launcher/scripts/launch_leaderboard.test.sh index e2d869f2..2853794e 100755 --- a/code/leaderboard_launcher/scripts/launch_leaderboard.test.sh +++ b/code/leaderboard_launcher/scripts/launch_leaderboard.test.sh @@ -13,12 +13,15 @@ source "${INTERNAL_WORKSPACE_DIR}/env.leaderboard.bash" # Source leaderboard specific venv source leaderboard_venv/bin/activate +traffic_manager_port="${PAF_TRAFFIC_MANAGER_PORT:-8000}" + python3 "/workspace/code/leaderboard_launcher/leaderboard_launcher/wait_for_carla.py" # Start leaderboard with arguments # edit "--routes" if you want a different testroute exec python3 /workspace/code/test/run_test.py \ --host="${CARLA_SIM_HOST}" \ --debug=0 \ + --traffic-manager-port="${traffic_manager_port}" \ --routes="/workspace/code/routes/test.xml" \ --agent="/workspace/code/leaderboard_launcher/leaderboard_launcher/agent_deploy.py" \ --track=MAP \ From f29c9380cfaa111bcc8876cfd5602fa07b810f79 Mon Sep 17 00:00:00 2001 From: ll7 Date: Wed, 1 Apr 2026 09:46:00 +0200 Subject: [PATCH 25/43] Refs #913: anchor LiDAR tracking on fresh points --- code/mapping/mapping/data_integration.py | 93 ++++++++++++++----- .../test_data_integration_tracking_anchor.py | 45 +++++++++ .../msg/ClusteredPointsArray.msg | 3 + code/perception/perception/lidar_distance.py | 72 ++++++++++++-- .../perception/lidar_filter_utility.py | 25 ++++- .../perception/perception/perception_utils.py | 7 ++ .../tests/test_clustered_points_message.py | 28 ++++++ 7 files changed, 238 insertions(+), 35 deletions(-) create mode 100644 code/mapping/test/test_data_integration_tracking_anchor.py create mode 100644 code/perception/tests/test_clustered_points_message.py diff --git a/code/mapping/mapping/data_integration.py b/code/mapping/mapping/data_integration.py index 54a3ee84..7ef77bee 100755 --- a/code/mapping/mapping/data_integration.py +++ b/code/mapping/mapping/data_integration.py @@ -1,6 +1,6 @@ from visualization_msgs.msg import Marker import numpy as np -from typing import List, Optional, Dict +from typing import List, Optional, Dict, Tuple from copy import deepcopy import rclpy @@ -39,6 +39,47 @@ import shapely +def _select_tracking_points( + cluster_points_xy: np.ndarray, is_buffered_mask: Optional[np.ndarray] +) -> np.ndarray: + if ( + is_buffered_mask is None + or is_buffered_mask.shape[0] != cluster_points_xy.shape[0] + ): + return cluster_points_xy + + fresh_points_xy = cluster_points_xy[~is_buffered_mask] + if fresh_points_xy.shape[0] == 0: + return cluster_points_xy + + return fresh_points_xy + + +def _build_polygon_shape_and_transform( + cluster_points_xy: np.ndarray, tracking_points_xy: np.ndarray +) -> Optional[Tuple[Polygon, Transform2D]]: + cluster_polygon_hull = MultiPoint(cluster_points_xy).convex_hull + if cluster_polygon_hull.is_empty or not cluster_polygon_hull.is_valid: + return None + if not isinstance(cluster_polygon_hull, shapely.Polygon): + return None + + if tracking_points_xy.shape[0] == 0: + tracking_points_xy = cluster_points_xy + + tracking_anchor = MultiPoint(tracking_points_xy).convex_hull.centroid + anchor_x = tracking_anchor.x + anchor_y = tracking_anchor.y + + hull_coords = np.asarray(cluster_polygon_hull.exterior.coords[:-1], dtype=float) + if hull_coords.shape[0] < 3: + return None + + shape = Polygon([Point2.new(x - anchor_x, y - anchor_y) for x, y in hull_coords]) + transform = Transform2D.new_translation(Vector2.new(anchor_x, anchor_y)) + return shape, transform + + class MappingDataIntegrationNode(Node): """Creates the initial map data frame based on all kinds of sensor data. @@ -580,6 +621,21 @@ def create_entities_from_clusters(self, sensortype="") -> List[Entity]: else None ) + is_buffered_array = ( + np.array(data.is_buffered_array, dtype=bool) + if data.is_buffered_array + else None + ) + if ( + is_buffered_array is not None + and is_buffered_array.shape[0] != clusterpointsarray.shape[0] + ): + self.get_logger().warn( + "Clustered point buffer mask size mismatch. Ignoring mask.", + throttle_duration_sec=2.0, + ) + is_buffered_array = None + objectclassarray = np.array(data.object_class) if data.object_class else None unique_labels = np.unique(indexarray) @@ -594,6 +650,14 @@ def create_entities_from_clusters(self, sensortype="") -> List[Entity]: # Filter points for current cluster cluster_mask = indexarray == label cluster_points_xy = clusterpointsarray[cluster_mask, :2] + cluster_is_buffered = ( + is_buffered_array[cluster_mask] + if is_buffered_array is not None + else None + ) + tracking_points_xy = _select_tracking_points( + cluster_points_xy, cluster_is_buffered + ) # Check if enough points for polygon are available if cluster_points_xy.shape[0] < 3: @@ -605,29 +669,14 @@ def create_entities_from_clusters(self, sensortype="") -> List[Entity]: else: continue else: - if not np.array_equal(cluster_points_xy[0], cluster_points_xy[-1]): - # add startpoint to close polygon - cluster_points_xy = np.vstack( - [cluster_points_xy, cluster_points_xy[0]] - ) - - cluster_polygon = MultiPoint(cluster_points_xy) - cluster_polygon_hull = cluster_polygon.convex_hull - if cluster_polygon_hull.is_empty or not cluster_polygon_hull.is_valid: + polygon_shape = _build_polygon_shape_and_transform( + cluster_points_xy, + tracking_points_xy, + ) + if polygon_shape is None: self.get_logger().debug("Empty hull", throttle_duration_sec=2.0) continue - if not isinstance(cluster_polygon_hull, shapely.Polygon): - self.get_logger().debug( - "Cluster is not polygon, continue", throttle_duration_sec=2.0 - ) - continue - - shape = Polygon.from_shapely( - cluster_polygon_hull, - make_centered=True, # type: ignore - ) - transform = shape.offset - shape.offset = Transform2D.identity() + shape, transform = polygon_shape motion = None if motion_array_converted is not None: diff --git a/code/mapping/test/test_data_integration_tracking_anchor.py b/code/mapping/test/test_data_integration_tracking_anchor.py new file mode 100644 index 00000000..d78210d9 --- /dev/null +++ b/code/mapping/test/test_data_integration_tracking_anchor.py @@ -0,0 +1,45 @@ +import numpy as np +import pytest + +from mapping.data_integration import ( + _build_polygon_shape_and_transform, + _select_tracking_points, +) + +pytestmark = pytest.mark.unit + + +def test_select_tracking_points_prefers_fresh_lidar_points(): + cluster_points_xy = np.array( + [[0.0, 0.0], [0.0, 2.0], [4.0, 0.0], [4.0, 2.0]], + dtype=float, + ) + is_buffered_mask = np.array([True, True, False, False], dtype=bool) + + tracking_points_xy = _select_tracking_points(cluster_points_xy, is_buffered_mask) + + assert np.array_equal(tracking_points_xy, cluster_points_xy[2:]) + + +def test_build_polygon_shape_and_transform_keeps_full_hull_with_fresh_anchor(): + cluster_points_xy = np.array( + [[0.0, 0.0], [0.0, 2.0], [4.0, 2.0], [4.0, 0.0]], + dtype=float, + ) + fresh_points_xy = np.array([[4.0, 0.0], [4.0, 2.0]], dtype=float) + + polygon = _build_polygon_shape_and_transform(cluster_points_xy, fresh_points_xy) + + assert polygon is not None + shape, transform = polygon + + translation = transform.translation() + assert translation.x() == pytest.approx(4.0) + assert translation.y() == pytest.approx(1.0) + + polygon_xy = shape.to_shapely(transform) + assert np.allclose(polygon_xy.bounds, (0.0, 0.0, 4.0, 2.0)) + + local_x = [point.x() for point in shape.points] + assert min(local_x) == pytest.approx(-4.0) + assert max(local_x) == pytest.approx(0.0) diff --git a/code/mapping_interfaces/msg/ClusteredPointsArray.msg b/code/mapping_interfaces/msg/ClusteredPointsArray.msg index aabc62ca..ecb2a339 100644 --- a/code/mapping_interfaces/msg/ClusteredPointsArray.msg +++ b/code/mapping_interfaces/msg/ClusteredPointsArray.msg @@ -9,6 +9,9 @@ float64[] cluster_points_array # Index array for points in clusters int32[] index_array +# Whether each point is buffered from an earlier scan +bool[] is_buffered_array + # Array of motion properties for each cluster Motion2D[] motion_array diff --git a/code/perception/perception/lidar_distance.py b/code/perception/perception/lidar_distance.py index 33950048..ca477e51 100755 --- a/code/perception/perception/lidar_distance.py +++ b/code/perception/perception/lidar_distance.py @@ -36,7 +36,7 @@ ) from .lidar_filter_utility import ( bounding_box, - filter_ground_points, + ground_filter_mask, remove_field_name, ) @@ -278,21 +278,31 @@ def start_clustering(self, data): # Convert PointCloud2 data to a NumPy structured array coordinates = ros2_numpy.point_cloud2.pointcloud2_to_array(data) - filtered_coordinates = coordinates[ - ~( - (coordinates["x"] >= -2) - & (coordinates["x"] <= 2) # Exclude ego vehicle in x-axis - & (coordinates["y"] >= -1) - & (coordinates["y"] <= 1) # Exclude ego vehicle in y-axis - ) - ] - filtered_coordinates = filter_ground_points( + point_buffer_mask = self.Compensation.get_point_buffer_mask() + if ( + point_buffer_mask is None + or point_buffer_mask.shape[0] != coordinates.shape[0] + ): + point_buffer_mask = np.zeros(coordinates.shape[0], dtype=bool) + + non_ego_mask = ~( + (coordinates["x"] >= -2) + & (coordinates["x"] <= 2) # Exclude ego vehicle in x-axis + & (coordinates["y"] >= -1) + & (coordinates["y"] <= 1) # Exclude ego vehicle in y-axis + ) + filtered_coordinates = coordinates[non_ego_mask] + point_buffer_mask = point_buffer_mask[non_ego_mask] + + ground_mask = ground_filter_mask( filtered_coordinates, z_min=self.clustering_lidar_z_min, z_max=self.clustering_lidar_z_max, pitch_rad=self.current_pitch, enable_pitch_compensation=self.enable_pitch_ground_filter, ) + filtered_coordinates = filtered_coordinates[ground_mask] + point_buffer_mask = point_buffer_mask[ground_mask] # Perform DBSCAN clustering clustered_points, cluster_labels = cluster_lidar_data_from_pointcloud( @@ -315,6 +325,7 @@ def start_clustering(self, data): valid_indices = cluster_labels != -1 filtered_xyz = filtered_xyz[valid_indices] cluster_labels = cluster_labels[valid_indices] + point_buffer_mask = point_buffer_mask[valid_indices] # Combine coordinates with their cluster labels points_with_labels = np.hstack((filtered_xyz, cluster_labels.reshape(-1, 1))) @@ -341,6 +352,7 @@ def start_clustering(self, data): self.get_clock().now(), cluster_points_np, index_array, + is_buffered_array=point_buffer_mask, header_id="hero/LIDAR", ) self.clustered_points_publisher.publish(clustered_points_msg) @@ -783,6 +795,7 @@ def __init__(self): super().__init__() self._cur_lidar_data: Optional[PointCloud2] = None self._prev_lidar_data: Optional[PointCloud2] = None + self._point_buffer_mask: Optional[np.ndarray] = None def set_lidar_data(self, data: PointCloud2): """ @@ -816,6 +829,19 @@ def valid_lidar_data(self): return True + def get_point_buffer_mask(self) -> Optional[np.ndarray]: + if self._point_buffer_mask is None: + return None + return self._point_buffer_mask.copy() + + def _set_current_only_mask(self): + if self._cur_lidar_data is None: + self._point_buffer_mask = None + return + + cur_points = ros2_numpy.point_cloud2.pointcloud2_to_array(self._cur_lidar_data) + self._point_buffer_mask = np.zeros(cur_points.shape[0], dtype=bool) + def prepare_data(self): """ Converts buffered PointCloud2 messages to NumPy arrays and segments @@ -829,9 +855,14 @@ def prepare_data(self): self._prev_lidar_data ) + self.cur_is_buffered = np.zeros(self.cur_points.shape[0], dtype=bool) + self.prev_is_buffered = np.ones(self.prev_points.shape[0], dtype=bool) + ego_mask = create_ego_vehicle_mask(self.prev_points) self.prev_ego_points = self.prev_points[ego_mask] self.prev_env_points = self.prev_points[~ego_mask] + self.prev_ego_is_buffered = self.prev_is_buffered[ego_mask] + self.prev_env_is_buffered = self.prev_is_buffered[~ego_mask] @abstractmethod def set_motion_data(self, **kwargs): @@ -869,6 +900,7 @@ def compensate(self) -> PointCloud2: """ self.valid_lidar_data() # Ensures the current data is set + self._set_current_only_mask() return self._cur_lidar_data @@ -891,11 +923,15 @@ def compensate(self) -> PointCloud2: """ if not self.valid_lidar_data(): + self._set_current_only_mask() return self._cur_lidar_data self.prepare_data() lidar_data = np.concatenate([self.cur_points, self.prev_points]) + self._point_buffer_mask = np.concatenate( + [self.cur_is_buffered, self.prev_is_buffered] + ) lidar_cloud = ros2_numpy.point_cloud2.array_to_pointcloud2(lidar_data) lidar_cloud.header = self._cur_lidar_data.header return lidar_cloud @@ -949,6 +985,7 @@ def compensate(self) -> PointCloud2: """ if not self.valid_lidar_data() or not self.valid_ekf_data(): + self._set_current_only_mask() return self._cur_lidar_data self.prepare_data() @@ -959,6 +996,13 @@ def compensate(self) -> PointCloud2: lidar_points = np.concatenate( [self.cur_points, comp_env_points, self.prev_ego_points] ) + self._point_buffer_mask = np.concatenate( + [ + self.cur_is_buffered, + self.prev_env_is_buffered, + self.prev_ego_is_buffered, + ] + ) lidar_cloud = ros2_numpy.point_cloud2.array_to_pointcloud2(lidar_points) lidar_cloud.header = self._cur_lidar_data.header @@ -1035,6 +1079,7 @@ def compensate(self) -> PointCloud2: or not self.valid_heading_data() or not self.valid_velocity_data() ): + self._set_current_only_mask() return self._cur_lidar_data self.prepare_data() @@ -1059,6 +1104,13 @@ def compensate(self) -> PointCloud2: lidar_points = np.concatenate( [self.cur_points, comp_env_points, self.prev_ego_points] ) + self._point_buffer_mask = np.concatenate( + [ + self.cur_is_buffered, + self.prev_env_is_buffered, + self.prev_ego_is_buffered, + ] + ) lidar_cloud = ros2_numpy.point_cloud2.array_to_pointcloud2(lidar_points) lidar_cloud.header = self._cur_lidar_data.header diff --git a/code/perception/perception/lidar_filter_utility.py b/code/perception/perception/lidar_filter_utility.py index 821e090e..60e0b2d0 100755 --- a/code/perception/perception/lidar_filter_utility.py +++ b/code/perception/perception/lidar_filter_utility.py @@ -78,15 +78,34 @@ def filter_ground_points( Filtered structured array containing only points inside the valid region. """ + return points[ + ground_filter_mask( + points, + z_min=z_min, + z_max=z_max, + pitch_rad=pitch_rad, + enable_pitch_compensation=enable_pitch_compensation, + ) + ] + + +def ground_filter_mask( + points, + z_min, + z_max, + pitch_rad=0.0, + enable_pitch_compensation=True, +): + """Return the boolean mask for points that pass the ground filter.""" + if points.size == 0: - return points + return np.array([], dtype=bool) lower_bound = np.full(points.shape[0], z_min, dtype=float) if enable_pitch_compensation: lower_bound = lower_bound + points["x"] * np.tan(pitch_rad) - mask = (points["z"] > lower_bound) & (points["z"] < z_max) - return points[mask] + return (points["z"] > lower_bound) & (points["z"] < z_max) # https://stackoverflow.com/questions/15575878/how-do-you-remove-a-column-from-a-structured-numpy-array diff --git a/code/perception/perception/perception_utils.py b/code/perception/perception/perception_utils.py index 8568041e..50cb3a6f 100644 --- a/code/perception/perception/perception_utils.py +++ b/code/perception/perception/perception_utils.py @@ -12,6 +12,7 @@ def array_to_clustered_points( stamp: Time, points, point_indices, + is_buffered_array=None, object_speed_array=None, object_class_array=None, header_id="hero", @@ -22,6 +23,7 @@ def array_to_clustered_points( Args: points: numpy array with shape (N, 3) point_indices: numpy array with the shape (N,) + is_buffered_array: numpy array with the shape (N,) object_speed_array: numpy array with the shape (N,) object_class_array: numpy array with the shape (N,) header_id: string @@ -40,6 +42,11 @@ def array_to_clustered_points( # Populate the indexArray clustered_points.index_array = point_indices.astype(int).tolist() + if is_buffered_array is not None: + clustered_points.is_buffered_array = np.asarray( + is_buffered_array, dtype=bool + ).tolist() + # Populate the motionArray if object_speed_array is provided if object_speed_array is not None: clustered_points.motion_array = object_speed_array diff --git a/code/perception/tests/test_clustered_points_message.py b/code/perception/tests/test_clustered_points_message.py new file mode 100644 index 00000000..f5971d4b --- /dev/null +++ b/code/perception/tests/test_clustered_points_message.py @@ -0,0 +1,28 @@ +import numpy as np +import pytest + +from perception.perception_utils import array_to_clustered_points +from rclpy.time import Time + +pytestmark = pytest.mark.unit + + +def test_array_to_clustered_points_serializes_buffered_mask(): + points = np.array([[1.0, 2.0, 0.5], [2.5, -1.0, 0.25]]) + point_indices = np.array([3, 3]) + is_buffered_array = np.array([False, True]) + + clustered_points = array_to_clustered_points( + Time(seconds=12), + points, + point_indices, + is_buffered_array=is_buffered_array, + header_id="hero/LIDAR", + ) + + assert clustered_points.header.frame_id == "hero/LIDAR" + assert list(clustered_points.index_array) == [3, 3] + assert list(clustered_points.is_buffered_array) == [False, True] + assert list(clustered_points.cluster_points_array) == pytest.approx( + points.flatten() + ) From bdebed4293b582409c95468372537fc23b6b046f Mon Sep 17 00:00:00 2001 From: ll7 Date: Wed, 1 Apr 2026 11:10:04 +0200 Subject: [PATCH 26/43] Refs #831: use OpenDRIVE projection for GNSS transform --- .../localization/coordinate_transformation.py | 80 ++++++++++++++++++- .../localization/gps_debug_node.py | 14 +++- .../localization/gps_transform.py | 61 +++++++++++++- .../localization/kalman_filter.py | 30 ++----- .../position_heading_publisher_node.py | 32 ++------ .../test/test_coordinate_transformation.py | 75 +++++++++++++++++ code/requirements.txt | 1 + 7 files changed, 244 insertions(+), 49 deletions(-) create mode 100644 code/localization/test/test_coordinate_transformation.py diff --git a/code/localization/localization/coordinate_transformation.py b/code/localization/localization/coordinate_transformation.py index 4fcfa6eb..66cdb10e 100755 --- a/code/localization/localization/coordinate_transformation.py +++ b/code/localization/localization/coordinate_transformation.py @@ -9,10 +9,18 @@ """ import math +from xml.etree import ElementTree as eTree + import numpy as np -from scipy.spatial.transform import Rotation import pymap3d import pymap3d.ellipsoid +from scipy.spatial.transform import Rotation + +try: + from pyproj import CRS, Transformer +except ModuleNotFoundError: + CRS = None + Transformer = None a = 6378137 # EARTH_RADIUS_EQUA in Pylot, used in geodetic_to_enu @@ -23,6 +31,54 @@ STD_LAT = 0.0 STD_LON = 0.0 STD_H = 0.0 +_GEO_REFERENCE_SKIP_PREFIXES = ( + "+geoidgrids=", + "+vunits=", +) + + +def extract_geo_reference_from_opendrive(opendrive: str) -> str: + """Return the OpenDRIVE geoReference string used by CARLA for GNSS projection.""" + root = eTree.fromstring(opendrive) + header = root.find("header") + if header is None: + raise ValueError("OpenDRIVE header missing.") + + geo_reference = header.findtext("geoReference") + if geo_reference is None: + raise ValueError("OpenDRIVE geoReference missing.") + + return geo_reference.strip() + + +def _extract_proj_parameter( + geo_reference: str, parameter: str, default: float +) -> float: + index = geo_reference.find(parameter) + if index == -1: + return default + + end_index = geo_reference.find(" ", index) + if end_index == -1: + end_index = len(geo_reference) + + return float(geo_reference[index + len(parameter) : end_index]) + + +def _sanitize_geo_reference(geo_reference: str) -> str: + return " ".join( + token + for token in geo_reference.replace("\n", " ").split() + if not token.startswith(_GEO_REFERENCE_SKIP_PREFIXES) + ) + + +def _build_geodetic_to_local_transformer(geo_reference: str): + if CRS is None or Transformer is None: + return None + + target_crs = CRS.from_user_input(_sanitize_geo_reference(geo_reference)) + return Transformer.from_crs(CRS.from_epsg(4326), target_crs, always_xy=True) class CoordinateTransformer: @@ -32,14 +88,36 @@ class CoordinateTransformer: la_ref = STD_LAT ln_ref = STD_LON h_ref = STD_H + geo_reference = None + _geodetic_to_local = None ref_set = False def __init__(self): pass + @classmethod + def configure_from_geo_reference(cls, geo_reference: str): + cls.geo_reference = geo_reference.strip() + cls.la_ref = _extract_proj_parameter(cls.geo_reference, "+lat_0=", STD_LAT) + cls.ln_ref = _extract_proj_parameter(cls.geo_reference, "+lon_0=", STD_LON) + cls.h_ref = _extract_proj_parameter(cls.geo_reference, "+h_0=", STD_H) + cls._geodetic_to_local = _build_geodetic_to_local_transformer(cls.geo_reference) + cls.ref_set = True + def gnss_to_xyz(self, lat, lon, h): + if self._geodetic_to_local is not None: + return self.projected_geodetic_to_xyz(lat, lon, h) return self.geodetic_to_enu(lat, lon, h) + def projected_geodetic_to_xyz(self, lat, lon, alt): + """Project WGS84 coordinates into CARLA's local map frame. + + CARLA's projected northing is inverted relative to the localization frame, + so the returned y coordinate keeps the existing localization convention. + """ + x, y = self._geodetic_to_local.transform(lon, lat) + return float(x), float(-y), alt + alt_offset + def geodetic_to_enu(self, lat, lon, alt): """ Method from pylot project to calculate coordinates diff --git a/code/localization/localization/gps_debug_node.py b/code/localization/localization/gps_debug_node.py index 1ce3d2a4..366d6c9b 100644 --- a/code/localization/localization/gps_debug_node.py +++ b/code/localization/localization/gps_debug_node.py @@ -7,7 +7,10 @@ import carla -from localization.coordinate_transformation import CoordinateTransformer +from localization.coordinate_transformation import ( + CoordinateTransformer, + extract_geo_reference_from_opendrive, +) class GpsDebug(Node): @@ -52,6 +55,15 @@ def __init__(self): raise RuntimeError(msg) self.transformer = CoordinateTransformer() + try: + self.transformer.configure_from_geo_reference( + extract_geo_reference_from_opendrive(self.map.to_opendrive()) + ) + except AttributeError: + self.get_logger().warn( + "CARLA map does not expose OpenDRIVE directly; " + "falling back to the legacy GNSS transform." + ) self.get_logger().info(f"{type(self).__name__} node initialized.") diff --git a/code/localization/localization/gps_transform.py b/code/localization/localization/gps_transform.py index 98de56ca..8fd0e7fb 100755 --- a/code/localization/localization/gps_transform.py +++ b/code/localization/localization/gps_transform.py @@ -17,9 +17,14 @@ from rclpy.parameter import Parameter from sensor_msgs.msg import NavSatFix from nav_msgs.msg import Odometry +from std_msgs.msg import Bool from paf_common.parameters import update_attributes +from planning_interfaces.srv import GetOpenDriveString -from localization.coordinate_transformation import CoordinateTransformer +from localization.coordinate_transformation import ( + CoordinateTransformer, + extract_geo_reference_from_opendrive, +) class GpsTransform(Node): @@ -31,6 +36,11 @@ def __init__(self): self.position_use_ground_truth = self.declare_parameter( "position_use_ground_truth", False ).value + self.open_drive_client = self.create_client( + GetOpenDriveString, + f"/paf/{self.role_name}/data/planning/get_open_drive", + ) + self._geo_reference_request_in_flight = False # Initalize publisher for Odometry data self.odometry_publisher: Publisher = self.create_publisher( @@ -44,11 +54,56 @@ def __init__(self): self.gps_callback, qos_profile=10, ) + self.create_subscription( + Bool, + f"/paf/{self.role_name}/data/planning/open_drive_updated", + self.request_geo_reference, + qos_profile=10, + ) self.map = None self.get_logger().info(f"{type(self).__name__} node initialized.") + def request_geo_reference(self, _=None): + if self.position_use_ground_truth or self.transformer.ref_set: + return + if self._geo_reference_request_in_flight: + return + if not self.open_drive_client.wait_for_service(timeout_sec=0.0): + return + + self._geo_reference_request_in_flight = True + future = self.open_drive_client.call_async(GetOpenDriveString.Request()) + future.add_done_callback(self._handle_geo_reference_response) + + def _handle_geo_reference_response(self, future): + self._geo_reference_request_in_flight = False + + try: + response = future.result() + except Exception as exc: + self.get_logger().warn( + f"{self.open_drive_client.service_name} request failed: {exc}" + ) + return + + if response is None: + self.get_logger().warn( + f"{self.open_drive_client.service_name} service returned None." + ) + return + if not response.success: + self.get_logger().warn( + f"{self.open_drive_client.service_name} service failed: {response.msg}." + ) + return + + self.transformer.configure_from_geo_reference( + extract_geo_reference_from_opendrive(response.data) + ) + self.get_logger().info("Geo ref updated.") + def _set_parameters_callback(self, params: List[Parameter]): """Callback for parameter updates.""" return update_attributes(self, params) @@ -59,6 +114,10 @@ def process_data(self, gps: NavSatFix): Args: gps (NavSatFix): GPS Data to process """ + if not self.position_use_ground_truth and not self.transformer.ref_set: + self.request_geo_reference() + return + if self.position_use_ground_truth and self.map is None: import carla import os diff --git a/code/localization/localization/kalman_filter.py b/code/localization/localization/kalman_filter.py index a11e4e03..a22aca88 100755 --- a/code/localization/localization/kalman_filter.py +++ b/code/localization/localization/kalman_filter.py @@ -28,9 +28,11 @@ from sensor_msgs.msg import NavSatFix, Imu from carla_msgs.msg import CarlaSpeedometer import math -from localization.coordinate_transformation import CoordinateTransformer -from localization.coordinate_transformation import quat_to_heading -from xml.etree import ElementTree as eTree +from localization.coordinate_transformation import ( + CoordinateTransformer, + extract_geo_reference_from_opendrive, + quat_to_heading, +) from paf_common.parameters import update_attributes GPS_RUNNING_AVG_ARGS = 10 @@ -424,25 +426,9 @@ def get_geoRef(self, opendrive: String): Args: opendrive (String): OpenDrive Map from Carla """ - root = eTree.fromstring(opendrive.data) - header = root.find("header") - geoRefText = header.find("geoReference").text - - latString = "+lat_0=" - lonString = "+lon_0=" - - indexLat = geoRefText.find(latString) - indexLon = geoRefText.find(lonString) - - indexLatEnd = geoRefText.find(" ", indexLat) - indexLonEnd = geoRefText.find(" ", indexLon) - - latValue = float(geoRefText[indexLat + len(latString) : indexLatEnd]) - lonValue = float(geoRefText[indexLon + len(lonString) : indexLonEnd]) - - CoordinateTransformer.la_ref = latValue - CoordinateTransformer.ln_ref = lonValue - CoordinateTransformer.ref_set = True + CoordinateTransformer.configure_from_geo_reference( + extract_geo_reference_from_opendrive(opendrive.data) + ) self.transformer = CoordinateTransformer() diff --git a/code/localization/localization/position_heading_publisher_node.py b/code/localization/localization/position_heading_publisher_node.py index b60869df..e54d9318 100755 --- a/code/localization/localization/position_heading_publisher_node.py +++ b/code/localization/localization/position_heading_publisher_node.py @@ -35,9 +35,11 @@ from sensor_msgs.msg import NavSatFix, Imu from std_msgs.msg import Float32, Bool -from localization.coordinate_transformation import CoordinateTransformer -from localization.coordinate_transformation import quat_to_heading -from xml.etree import ElementTree as eTree +from localization.coordinate_transformation import ( + CoordinateTransformer, + extract_geo_reference_from_opendrive, + quat_to_heading, +) from paf_common.parameters import update_attributes from rcl_interfaces.msg import ( ParameterDescriptor, @@ -417,27 +419,9 @@ async def get_geoRef(self, b: Bool = Bool(data=True)): f"{self.open_drive_client.service_name} service failed: {response.msg}." ) return False - opendrive: str = response.data - - root = eTree.fromstring(opendrive) - header = root.find("header") - geoRefText = header.find("geoReference").text - - latString = "+lat_0=" - lonString = "+lon_0=" - - indexLat = geoRefText.find(latString) - indexLon = geoRefText.find(lonString) - - indexLatEnd = geoRefText.find(" ", indexLat) - indexLonEnd = geoRefText.find(" ", indexLon) - - latValue = float(geoRefText[indexLat + len(latString) : indexLatEnd]) - lonValue = float(geoRefText[indexLon + len(lonString) : indexLonEnd]) - - self.transformer.la_ref = latValue - self.transformer.ln_ref = lonValue - self.transformer.ref_set = True + self.transformer.configure_from_geo_reference( + extract_geo_reference_from_opendrive(response.data) + ) self.get_logger().info("Geo ref updated.") diff --git a/code/localization/test/test_coordinate_transformation.py b/code/localization/test/test_coordinate_transformation.py new file mode 100644 index 00000000..3e8d47d9 --- /dev/null +++ b/code/localization/test/test_coordinate_transformation.py @@ -0,0 +1,75 @@ +import pytest + +pyproj = pytest.importorskip("pyproj") + +from localization.coordinate_transformation import ( # noqa: E402 + CoordinateTransformer, + alt_offset, + extract_geo_reference_from_opendrive, +) + + +pytestmark = pytest.mark.unit + + +OPEN_DRIVE = """ + +
+ +
+ +""" + + +@pytest.fixture(autouse=True) +def reset_transformer_state(): + CoordinateTransformer.la_ref = 0.0 + CoordinateTransformer.ln_ref = 0.0 + CoordinateTransformer.h_ref = 0.0 + CoordinateTransformer.geo_reference = None + CoordinateTransformer._geodetic_to_local = None + CoordinateTransformer.ref_set = False + + +def test_extract_geo_reference_from_opendrive(): + assert " ".join(extract_geo_reference_from_opendrive(OPEN_DRIVE).split()) == ( + "+proj=tmerc +lat_0=0 +lon_0=0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 " + "+units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs" + ) + + +def test_configure_from_geo_reference_sets_projection_and_reference(): + CoordinateTransformer.configure_from_geo_reference( + extract_geo_reference_from_opendrive(OPEN_DRIVE) + ) + + assert CoordinateTransformer.ref_set is True + assert CoordinateTransformer.la_ref == pytest.approx(0.0) + assert CoordinateTransformer.ln_ref == pytest.approx(0.0) + assert CoordinateTransformer._geodetic_to_local is not None + + +def test_gnss_to_xyz_uses_opendrive_projection(): + geo_reference = extract_geo_reference_from_opendrive(OPEN_DRIVE) + CoordinateTransformer.configure_from_geo_reference(geo_reference) + + sanitized_geo_reference = ( + "+proj=tmerc +lat_0=0 +lon_0=0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 " + "+units=m +no_defs" + ) + projection = pyproj.Transformer.from_crs( + pyproj.CRS.from_epsg(4326), + pyproj.CRS.from_user_input(sanitized_geo_reference), + always_xy=True, + ) + expected_x, expected_y = projection.transform(0.001, 0.0015) + + transformer = CoordinateTransformer() + x, y, z = transformer.gnss_to_xyz(0.0015, 0.001, 12.0) + + assert x == pytest.approx(expected_x) + assert y == pytest.approx(-expected_y) + assert z == pytest.approx(12.0 + alt_offset) diff --git a/code/requirements.txt b/code/requirements.txt index 1d048288..bfdb9467 100644 --- a/code/requirements.txt +++ b/code/requirements.txt @@ -12,6 +12,7 @@ shapely==2.0.6 # Localization pymap3d==3.2.0 +pyproj==3.7.2 # Perception dvclive==3.48.5 From f59f465af9db9daf782336301dc07e8eab697e2a Mon Sep 17 00:00:00 2001 From: ll7 Date: Wed, 1 Apr 2026 15:08:35 +0200 Subject: [PATCH 27/43] Refs #831: harden live GNSS validation and transform fallback --- .vscode/tasks.json | 7 + .../agent-ros2/scripts/dependency-sync.sh | 2 + .../localization/coordinate_transformation.py | 41 ++++- .../test/test_coordinate_transformation.py | 39 +++- doc/development/testing_strategy.md | 14 ++ scripts/validate-gnss-projection.sh | 173 ++++++++++++++++++ 6 files changed, 267 insertions(+), 9 deletions(-) create mode 100755 scripts/validate-gnss-projection.sh diff --git a/.vscode/tasks.json b/.vscode/tasks.json index b66a009c..46687479 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -148,6 +148,13 @@ "problemMatcher": [], "detail": "Installs rosdep and pip dependencies from package.xml and requirements manifests." }, + { + "label": "Validate live GNSS projection", + "type": "shell", + "command": "bash ${workspaceFolder}/scripts/validate-gnss-projection.sh", + "problemMatcher": [], + "detail": "While leaderboard.test is running, starts a sidecar gps_transform with position_use_ground_truth=false and measures its error against CARLA ground truth." + }, { "label": "Build active package with colcon", "type": "shell", diff --git a/build/docker/agent-ros2/scripts/dependency-sync.sh b/build/docker/agent-ros2/scripts/dependency-sync.sh index fe2bc0a9..99fd5fa8 100644 --- a/build/docker/agent-ros2/scripts/dependency-sync.sh +++ b/build/docker/agent-ros2/scripts/dependency-sync.sh @@ -13,7 +13,9 @@ if [[ -z "${PAF_ROS_WS:-}" || -z "${INTERNAL_WORKSPACE_DIR:-}" || -z "${ROS_DIST exit 1 fi +set +u source "${INTERNAL_WORKSPACE_DIR}/env.bash" +set -u cd "${PAF_ROS_WS}" echo "Refreshing apt and rosdep indices..." diff --git a/code/localization/localization/coordinate_transformation.py b/code/localization/localization/coordinate_transformation.py index 66cdb10e..d30195d8 100755 --- a/code/localization/localization/coordinate_transformation.py +++ b/code/localization/localization/coordinate_transformation.py @@ -65,6 +65,20 @@ def _extract_proj_parameter( return float(geo_reference[index + len(parameter) : end_index]) +def _extract_proj_string_parameter( + geo_reference: str, parameter: str, default: str +) -> str: + index = geo_reference.find(parameter) + if index == -1: + return default + + end_index = geo_reference.find(" ", index) + if end_index == -1: + end_index = len(geo_reference) + + return geo_reference[index + len(parameter) : end_index] + + def _sanitize_geo_reference(geo_reference: str) -> str: return " ".join( token @@ -81,6 +95,17 @@ def _build_geodetic_to_local_transformer(geo_reference: str): return Transformer.from_crs(CRS.from_epsg(4326), target_crs, always_xy=True) +def _uses_default_carla_projection(geo_reference: str) -> bool: + return ( + _extract_proj_string_parameter(geo_reference, "+proj=", "") == "tmerc" + and math.isclose(_extract_proj_parameter(geo_reference, "+lat_0=", 0.0), 0.0) + and math.isclose(_extract_proj_parameter(geo_reference, "+lon_0=", 0.0), 0.0) + and math.isclose(_extract_proj_parameter(geo_reference, "+k=", 1.0), 1.0) + and math.isclose(_extract_proj_parameter(geo_reference, "+x_0=", 0.0), 0.0) + and math.isclose(_extract_proj_parameter(geo_reference, "+y_0=", 0.0), 0.0) + ) + + class CoordinateTransformer: """This class can be used to transform coordinates between the x/y/z and GNSS reference frame""" @@ -90,6 +115,7 @@ class CoordinateTransformer: h_ref = STD_H geo_reference = None _geodetic_to_local = None + _use_projected_transform = False ref_set = False def __init__(self): @@ -102,12 +128,19 @@ def configure_from_geo_reference(cls, geo_reference: str): cls.ln_ref = _extract_proj_parameter(cls.geo_reference, "+lon_0=", STD_LON) cls.h_ref = _extract_proj_parameter(cls.geo_reference, "+h_0=", STD_H) cls._geodetic_to_local = _build_geodetic_to_local_transformer(cls.geo_reference) + cls._use_projected_transform = ( + cls._geodetic_to_local is not None + and not _uses_default_carla_projection(cls.geo_reference) + ) cls.ref_set = True def gnss_to_xyz(self, lat, lon, h): - if self._geodetic_to_local is not None: + if self._use_projected_transform and self._geodetic_to_local is not None: return self.projected_geodetic_to_xyz(lat, lon, h) - return self.geodetic_to_enu(lat, lon, h) + # Default CARLA maps still round-trip GNSS XY through the legacy transform + # more accurately than through the advertised zeroed tmerc projection. + x, y, _ = self.geodetic_to_enu(lat, lon, h) + return x, y, float(h) def projected_geodetic_to_xyz(self, lat, lon, alt): """Project WGS84 coordinates into CARLA's local map frame. @@ -116,7 +149,9 @@ def projected_geodetic_to_xyz(self, lat, lon, alt): so the returned y coordinate keeps the existing localization convention. """ x, y = self._geodetic_to_local.transform(lon, lat) - return float(x), float(-y), alt + alt_offset + # CARLA's GNSS altitude already matches the local map frame, so adding the + # legacy map offset here double-counts elevation. + return float(x), float(-y), float(alt) def geodetic_to_enu(self, lat, lon, alt): """ diff --git a/code/localization/test/test_coordinate_transformation.py b/code/localization/test/test_coordinate_transformation.py index 3e8d47d9..b76adaa2 100644 --- a/code/localization/test/test_coordinate_transformation.py +++ b/code/localization/test/test_coordinate_transformation.py @@ -4,7 +4,6 @@ from localization.coordinate_transformation import ( # noqa: E402 CoordinateTransformer, - alt_offset, extract_geo_reference_from_opendrive, ) @@ -24,6 +23,19 @@ """ +OPEN_DRIVE_NON_DEFAULT = """ + +
+ +
+ +""" + + @pytest.fixture(autouse=True) def reset_transformer_state(): CoordinateTransformer.la_ref = 0.0 @@ -31,6 +43,7 @@ def reset_transformer_state(): CoordinateTransformer.h_ref = 0.0 CoordinateTransformer.geo_reference = None CoordinateTransformer._geodetic_to_local = None + CoordinateTransformer._use_projected_transform = False CoordinateTransformer.ref_set = False @@ -52,12 +65,26 @@ def test_configure_from_geo_reference_sets_projection_and_reference(): assert CoordinateTransformer._geodetic_to_local is not None -def test_gnss_to_xyz_uses_opendrive_projection(): +def test_gnss_to_xyz_uses_legacy_xy_for_default_carla_projection(): geo_reference = extract_geo_reference_from_opendrive(OPEN_DRIVE) CoordinateTransformer.configure_from_geo_reference(geo_reference) + transformer = CoordinateTransformer() + expected_x, expected_y, _ = transformer.geodetic_to_enu(0.0015, 0.001, 12.0) + + x, y, z = transformer.gnss_to_xyz(0.0015, 0.001, 12.0) + + assert x == pytest.approx(expected_x) + assert y == pytest.approx(expected_y) + assert z == pytest.approx(12.0) + + +def test_gnss_to_xyz_uses_opendrive_projection_for_non_default_reference(): + geo_reference = extract_geo_reference_from_opendrive(OPEN_DRIVE_NON_DEFAULT) + CoordinateTransformer.configure_from_geo_reference(geo_reference) + sanitized_geo_reference = ( - "+proj=tmerc +lat_0=0 +lon_0=0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 " + "+proj=tmerc +lat_0=1 +lon_0=2 +k=0.9996 +x_0=500000 +y_0=1000000 +datum=WGS84 " "+units=m +no_defs" ) projection = pyproj.Transformer.from_crs( @@ -65,11 +92,11 @@ def test_gnss_to_xyz_uses_opendrive_projection(): pyproj.CRS.from_user_input(sanitized_geo_reference), always_xy=True, ) - expected_x, expected_y = projection.transform(0.001, 0.0015) + expected_x, expected_y = projection.transform(2.001, 1.0015) transformer = CoordinateTransformer() - x, y, z = transformer.gnss_to_xyz(0.0015, 0.001, 12.0) + x, y, z = transformer.gnss_to_xyz(1.0015, 2.001, 12.0) assert x == pytest.approx(expected_x) assert y == pytest.approx(-expected_y) - assert z == pytest.approx(12.0 + alt_offset) + assert z == pytest.approx(12.0) diff --git a/doc/development/testing_strategy.md b/doc/development/testing_strategy.md index 1013deea..bdb50b74 100644 --- a/doc/development/testing_strategy.md +++ b/doc/development/testing_strategy.md @@ -38,6 +38,20 @@ devsource PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest /workspace/code/perception/tests /workspace/code/mapping/test /workspace/code/planning/test -m unit ``` +## Live GNSS validation during CARLA runs + +Use the live GNSS projection check only while `leaderboard.test` is already running in the dev container. + +- Task: `Validate live GNSS projection` +- Script: `bash scripts/validate-gnss-projection.sh` + +The script starts a sidecar `gps_transform` with `position_use_ground_truth=false`, waits for `/odometry/gps_projection_check`, and compares the output against CARLA ground truth for the live `hero` actor. If `pyproj` is missing in the dev container, it first runs the dependency synchronization workflow. + +Optional thresholds can be supplied through environment variables: + +- `PAF_GNSS_MEAN_THRESHOLD_M` +- `PAF_GNSS_MAX_THRESHOLD_M` + ## Submodule strategy For each package under `code/`: diff --git a/scripts/validate-gnss-projection.sh b/scripts/validate-gnss-projection.sh new file mode 100755 index 00000000..9ea0980f --- /dev/null +++ b/scripts/validate-gnss-projection.sh @@ -0,0 +1,173 @@ +#!/usr/bin/env bash +set -euo pipefail + +CONTAINER_NAME="${PAF_DEV_CONTAINER:-build-agent-dev-1}" +CARLA_HOST="${CARLA_SIM_HOST:-carla-simulator}" +ROLE_NAME="${PAF_ROLE_NAME:-hero}" +SIDE_NODE_NAME="${PAF_GNSS_SIDE_NODE_NAME:-gps_transform_projection_check}" +OUTPUT_TOPIC="${PAF_GNSS_OUTPUT_TOPIC:-/odometry/gps_projection_check}" +SAMPLE_COUNT="${PAF_GNSS_SAMPLE_COUNT:-10}" +TIMEOUT_SEC="${PAF_GNSS_TIMEOUT_SEC:-20}" +MEAN_THRESHOLD_M="${PAF_GNSS_MEAN_THRESHOLD_M:-}" +MAX_THRESHOLD_M="${PAF_GNSS_MAX_THRESHOLD_M:-}" + +cleanup() { + docker exec "${CONTAINER_NAME}" bash -lc \ + "pkill -f '${SIDE_NODE_NAME}|gps_transform --ros-args.*${SIDE_NODE_NAME}' || true" \ + >/dev/null 2>&1 || true +} + +trap cleanup EXIT + +if ! docker inspect "${CONTAINER_NAME}" >/dev/null 2>&1; then + echo "Dev container '${CONTAINER_NAME}' is not running. Start the dev stack first." >&2 + exit 1 +fi + +if ! docker exec "${CONTAINER_NAME}" bash -lc \ + "pgrep -f '/workspace/code/test/run_test.py' >/dev/null"; then + echo "No live leaderboard route detected in ${CONTAINER_NAME}. Start leaderboard.test first." >&2 + exit 1 +fi + +if ! docker exec "${CONTAINER_NAME}" bash -lc \ + "python3 -c 'import pyproj'" >/dev/null 2>&1; then + echo "pyproj is missing in ${CONTAINER_NAME}; synchronizing dependencies..." + docker exec "${CONTAINER_NAME}" bash -lc \ + "source /internal_workspace/dev.bashrc >/dev/null 2>&1 && bash /workspace/build/docker/agent-ros2/scripts/dependency-sync.sh sync" +fi + +cleanup + +docker exec -d "${CONTAINER_NAME}" bash -lc " + source /internal_workspace/dev.bashrc >/dev/null 2>&1 + export CARLA_SIM_HOST='${CARLA_HOST}' + nohup ros2 run localization gps_transform \ + --ros-args \ + -r __node:='${SIDE_NODE_NAME}' \ + -p role_name:='${ROLE_NAME}' \ + -p position_use_ground_truth:=false \ + -p use_sim_time:=True \ + -r /odometry/gps:='${OUTPUT_TOPIC}' \ + >/tmp/${SIDE_NODE_NAME}.log 2>&1 & +" + +docker exec "${CONTAINER_NAME}" bash -lc " + export PAF_GNSS_SIDE_OUTPUT_TOPIC='${OUTPUT_TOPIC}' + export PAF_GNSS_SAMPLE_COUNT='${SAMPLE_COUNT}' + export PAF_GNSS_TIMEOUT_SEC='${TIMEOUT_SEC}' + export PAF_GNSS_CARLA_HOST='${CARLA_HOST}' + export PAF_GNSS_MEAN_THRESHOLD_M='${MEAN_THRESHOLD_M}' + export PAF_GNSS_MAX_THRESHOLD_M='${MAX_THRESHOLD_M}' + source /internal_workspace/dev.bashrc >/dev/null 2>&1 + python3 - <<'PY' +import json +import math +import os +import time + +import carla +import rclpy +from nav_msgs.msg import Odometry +from rclpy.node import Node + + +class Probe(Node): + def __init__(self, world): + super().__init__('gps_projection_probe') + self.samples = [] + self.world = world + self.create_subscription( + Odometry, + os.environ['PAF_GNSS_SIDE_OUTPUT_TOPIC'], + self.odom_callback, + 10, + ) + + def odom_callback(self, msg): + hero = None + for actor in self.world.get_actors(): + if actor.attributes.get('role_name') == '${ROLE_NAME}': + hero = actor + break + if hero is None: + return + + location = hero.get_location() + carla_enu = (location.x, -location.y, location.z) + odom_xyz = ( + msg.pose.pose.position.x, + msg.pose.pose.position.y, + msg.pose.pose.position.z, + ) + self.samples.append( + { + 'error_m': math.dist(odom_xyz, carla_enu), + 'projection_xyz': odom_xyz, + 'carla_xyz': carla_enu, + } + ) + + +def maybe_threshold(name: str): + value = os.environ.get(name) + return None if value in (None, '') else float(value) + + +def connect_world(deadline: float): + last_error = None + while time.time() < deadline: + try: + client = carla.Client(os.environ['PAF_GNSS_CARLA_HOST'], 2000) + client.set_timeout(10.0) + return client.get_world() + except Exception as exc: + last_error = exc + time.sleep(0.5) + + raise RuntimeError(last_error or 'Timed out while connecting to CARLA.') + + +def main(): + rclpy.init() + deadline = time.time() + float(os.environ['PAF_GNSS_TIMEOUT_SEC']) + probe = Probe(connect_world(deadline)) + target_samples = int(os.environ['PAF_GNSS_SAMPLE_COUNT']) + + while time.time() < deadline and len(probe.samples) < target_samples: + rclpy.spin_once(probe, timeout_sec=0.5) + + if not probe.samples: + print('No samples collected from the sidecar GNSS projection node.', flush=True) + probe.destroy_node() + rclpy.shutdown() + raise SystemExit(1) + + errors = [sample['error_m'] for sample in probe.samples] + result = { + 'sample_count': len(probe.samples), + 'gps_err_mean_m': sum(errors) / len(errors), + 'gps_err_max_m': max(errors), + 'last_projection_xyz': probe.samples[-1]['projection_xyz'], + 'last_carla_xyz': probe.samples[-1]['carla_xyz'], + } + print(json.dumps(result, indent=2), flush=True) + + mean_threshold = maybe_threshold('PAF_GNSS_MEAN_THRESHOLD_M') + max_threshold = maybe_threshold('PAF_GNSS_MAX_THRESHOLD_M') + if mean_threshold is not None and result['gps_err_mean_m'] > mean_threshold: + probe.destroy_node() + rclpy.shutdown() + raise SystemExit(2) + if max_threshold is not None and result['gps_err_max_m'] > max_threshold: + probe.destroy_node() + rclpy.shutdown() + raise SystemExit(3) + + probe.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() +PY" From 3f1e2ce378339036adafd649dfcec4ecfe91f10d Mon Sep 17 00:00:00 2001 From: ll7 Date: Tue, 28 Apr 2026 10:32:08 +0200 Subject: [PATCH 28/43] Fix formatting and add missing newline in launch_leaderboard.test.sh and lidar_distance.md --- .../scripts/launch_leaderboard.test.sh | 2 +- doc/development/devcontainer.md | 2 +- doc/perception/lidar_distance.md | 520 +++++++++--------- 3 files changed, 262 insertions(+), 262 deletions(-) diff --git a/code/leaderboard_launcher/scripts/launch_leaderboard.test.sh b/code/leaderboard_launcher/scripts/launch_leaderboard.test.sh index 2853794e..bdd0b0a4 100755 --- a/code/leaderboard_launcher/scripts/launch_leaderboard.test.sh +++ b/code/leaderboard_launcher/scripts/launch_leaderboard.test.sh @@ -17,7 +17,7 @@ traffic_manager_port="${PAF_TRAFFIC_MANAGER_PORT:-8000}" python3 "/workspace/code/leaderboard_launcher/leaderboard_launcher/wait_for_carla.py" -# Start leaderboard with arguments # edit "--routes" if you want a different testroute +# Start leaderboard with arguments # edit "--routes" if you want a different testroute exec python3 /workspace/code/test/run_test.py \ --host="${CARLA_SIM_HOST}" \ --debug=0 \ diff --git a/doc/development/devcontainer.md b/doc/development/devcontainer.md index e1a12462..78899c03 100644 --- a/doc/development/devcontainer.md +++ b/doc/development/devcontainer.md @@ -149,4 +149,4 @@ bash scripts/check-nvidia.sh bash scripts/update-dotenv.sh ``` -Then rebuild or reopen the container if needed. \ No newline at end of file +Then rebuild or reopen the container if needed. diff --git a/doc/perception/lidar_distance.md b/doc/perception/lidar_distance.md index 2fb53399..bf753f2b 100644 --- a/doc/perception/lidar_distance.md +++ b/doc/perception/lidar_distance.md @@ -1,260 +1,260 @@ -# Documentation: Lidar Distance Node - -## Table of Contents - -- [1. Integration into the Perception Pipeline](#1-integration-into-the-perception-pipeline) -- [2. Point Cloud Compensation](#2-point-cloud-compensation) -- [3. Input: Received Data](#3-input-received-data) -- [4. Processing Pipeline](#4-processing-pipeline) -- [5. Output: Published Topics](#5-output-published-topics) - -## 1. Integration into the Perception Pipeline - -The `lidar_distance.py` node is part of the perception pipeline. This node processes LiDAR data to provide precise distance information. The extracted and processed data serves as a foundation for subsequent layers such as the Intermediate Layer and the Planning module. - -## 2. Point Cloud Compensation - -In the simulation environment (CARLA/Leaderboard), there is a mismatch between the lidar rotation frequency (10Hz) and the simulation frequency (20Hz), resulting in a 180 degree lidar data per simulation frame and a blindspot on missing data. - -To achieve better spatial coverage, the node can buffer the current and previous frame. The core challenge is that the environment points from the previous scan must be compensated (transformed) to account for the ego vehicle's motion that occurred between the two frames. - -### Modular Compensation - -The node uses the Strategy pattern to allow switching between different compensation modes via configuration: - -- `CompensationStrategy` (Abstract Base class) defines the interface for all modes. -- The Compensation object handles buffering, data preparation and returns the compensated point cloud. - -The available compensation modes are: - -**Default Mode:** The default compensation strategy is `LocalCompensation`, which uses local vehicle dynamics for simplified motion correction. - -### 2.1 NoCompensation (Baseline) - -This strategy represents the simplest case, where no buffering, registration, or compensation is performed on the incoming LiDAR data. - -The system simply passes the current point cloud ($P_{cur}$) as the final result, discarding all previous frame data. - -$$ - P_{comp} = P_{cur} -$$ - -### 2.2 Buffer - -This strategy performs basic point cloud accumulation across two consecutive frames without applying any geometric transformation or motion compensation. - -The LiDAR data from the current frame ($P_{cur}$) and the previous frame ($P_{prev}$) are buffered and directly concatenated (joined). -Since no transformation is applied, the previous cloud remains in its original, outdated coordinate frame, leading to motion misaligned points if the ego vehicle moved between frames. - -$$ - P_{comp} = P_{cur} \cup P_{prev} -$$ - -### 2.3 EgoMotionCompensation - -This strategy uses external state information (Estimated Kalman Filter, EKF) to determine the vehicle's transformation between the current frame ($f_i$) and the previous frame ($f_{i-1}$). -Since LiDAR data is always relative to the sensor, we must apply a transformation that describes the positional delta between the vehicle's current and previous pose to correctly align the point clouds. - -2.3.1 Point Cloud Separation - -We first divide the previous point cloud, $P_{prev}$, into points belonging to the ego vehicle, $P_{ego}$, and points belonging to the static environment, $P_{env}$, such that: - -$$P_{prev} = P_{ego} \cup P_{env}$$ - -2.3.2 Homogeneous Transformation Matrix ($T_i$) - -The EKF provides both the translation components and rotation components, allowing us to define the homogeneous transformation matrix $T_i$ for a frame $f_i$ relative to the local position. The transformation matrix $T_i$ is defined as following: - -$$T_{i} = \begin{pmatrix} -R_{i} & t_{i} \\ -\mathbf{0}^T & 1 -\end{pmatrix}$$ - -Components: - -$R_i$ is the $3 \times 3$ rotation matrix representing the orientation (roll, pitch, yaw) of the ego vehicle: - -$$R_{i} = \begin{pmatrix} - r_{11} & r_{12} & r_{13} \\ - r_{21} & r_{22} & r_{23} \\ - r_{31} & r_{32} & r_{33} - \end{pmatrix}$$ - -$t_{i}$ is the $3 \times 1$ translation vector, representing the vehicle's position $(x, y, z)$: - -$$t_{i} = \begin{pmatrix} - t_x \\ - t_y \\ - t_z - \end{pmatrix}$$ - -$\mathbf{0}^T$ is a $1 \times 3$ row vector of zeros: $\begin{pmatrix} 0 & 0 & 0 \end{pmatrix}$. - -2.3.3 Delta Transformation Matrix ($\Delta T$) - -Having defined the homogenous transformation matrices for both $T_i$ (current pose) and $T_{i-1}$ (previous pose), we calculate the positional delta between the two frames. -This delta transformation matrix, $\Delta T$, transforms points from the current sensor frame ($f_i$) back to the coordinates of the previous frame ($f_{i-1}$): - -$$ - \Delta T = T_{i-1} T_i^{-1} -$$ - -2.3.4 Point Cloud Compensation - -The $\Delta T$ allows us to transform all environment points ($P_{env}$ from the previous frame), compensating for the ego-motion that occurred between $f_{i-1}$ and $f_i$. For any point $(x, y, z)^T \in P_{env}$, the compensated position $P'_{env}$ is calculated using homogeneous coordinates: - -$$ - P'_{env} = \Delta T \begin{pmatrix} x \\ y \\ z \\ 1 \end{pmatrix} -$$ - -The final compensated point cloud, $P_{comp}$, is the union of the environment points from the previous frame ($P'_{env}$), the current frame's point cloud ($P_{cur}$), and the ego vehicle points ($P_{ego}$): - -$$ - P_{comp} = P_{cur} \cup P'_{env} \cup P_{ego} -$$ - -### 2.4 LocalCompensation - -This strategy uses the vehicle's local speed and heading change between the previous frame ($f_{i-1}$) and the current frame ($f_i$) to correct the static points. - -2.4.1 Point Cloud Separation - -First, the previous point cloud, $P_{prev}$, is split into two groups: - -$$P_{prev} = P_{ego} \cup P_{env}$$ - -2.4.2 Motion Compensation Components - -We determine how much the vehicle moved during the time interval $\Delta t = t_i - t_{i-1}$. - -Translation Component ($d_x$) - -The main movement is the displacement along the vehicle's forward (X) axis. This distance, $d_x$, is calculated from the average speed ($\bar{v}$) over $\Delta t$. This is the distance we need to undo on the static points. - -$$d_x \approx \bar{v} \cdot \Delta t$$ - -Rotation Component ($R_{\Delta}$) (Optional) - -This is the $3 \times 3$ rotation matrix used to correct for the change in heading ($\Delta \theta$). This correction is only applied if the heading change is significant enough to warrant it, which is controlled by a flag in the implementation. The rotation is around the Z-axis (yaw): - -$$R_{\Delta} = \begin{pmatrix} -\cos(-\Delta \theta) & -\sin(-\Delta \theta) & 0 \\ -\sin(-\Delta \theta) & \cos(-\Delta \theta) & 0 \\ -0 & 0 & 1 -\end{pmatrix}$$ - -Where $\Delta \theta = \theta_i - \theta_{i-1}$. - -2.4.3 Compensation Procedure - -The correction is applied to the static points ($P_{env}$) in two steps: - -Translation (Required): The points are moved backward along the X-axis by subtracting the calculated distance $d_x$: - -$$x_{comp} = x - d_x$$ - -Rotation (Optional): If the account_heading flag is set to True, the resulting coordinates are rotated using the $R_{\Delta}$ matrix to correct for the vehicle's turning. - -The final compensated point cloud, $P_{comp}$, is the combined set of current points, corrected static points, and ego points: - -$$P_{comp} = P_{cur} \cup P'_{env} \cup P_{ego}$$ - -## 3. Input: Received Data - -### Incoming Topics - -- **Topic Name:** `/carla/hero/LIDAR` -- **Data Type:** `sensor_msgs/PointCloud2` -- **Description:** Contains point cloud data from the LiDAR sensor, serving as raw input for distance measurement. - -### Incoming Topics (Compensation Mode Dependent) - -EgoMotionCompensation active: - -- **Topic Name:** `/paf/hero/local_current_pos` -- **Data Type:** `geometry_msgs/PoseStamped` -- **Description:** Provides the pose (position and orientation) of the ego vehicle in the local coordinate frame. - -LocalCompensation active: - -- **Topic Name:** `/carla/hero/Speed` -- **Data Type:** `carla_msgs/CarlaSpeedometer` -- **Description:** Provides the linear speed of the vehicle. - -- **Topic Name:** `/carla/hero/IMU` -- **Data Type:** `sensor_msgs/Imu` -- **Description:** Provides orientation data used for calculating the heading and the current pitch angle for pitch-aware ground filtering. - -## 4. Processing Pipeline - -### Processing Steps - -### 4.1 Reception of PointCloud2 Data - -- ROS subscriber receives the LiDAR point cloud (`callback` method). - -### 4.2 Motion Compensation of Point Cloud - -- Compensates point cloud based on the compensation mode (NoCompensation, Buffer, EgoMotionCompensation, LocalCompensation) -- If LocalCompensation is active, publishes the calculated delta heading to `/paf/hero/delta_heading` - -### 4.3 Filtering and Preprocessing - -- Removes points representing the ego vehicle (`start_clustering`). -- Filters out points below a certain height (`clustering_lidar_z_min`) to avoid clustering the road surface (`start_clustering`). -- Optionally tilts the lower height bound with the current IMU pitch (`enable_pitch_ground_filter`) so that road points do not drift into the obstacle set during acceleration, braking, or other pitch motion. -- Filters out points above a maximum height (`clustering_lidar_z_max`) to exclude high objects like tree leaves or overhead structures. - -### 4.4 Clustering the LiDAR Data - -- Uses DBSCAN to group spatially related points (`start_clustering`). -- Applies coordinate normalization by projecting points onto a unit sphere and augmenting with weighted distance components, creating a polar-coordinate-like clustering approach for better handling of varying point densities in LiDAR sweeps. -- Removes noise points classified by DBSCAN (`cluster_labels != -1`). -- Generates bounding boxes for identified clusters (`generate_bounding_boxes`). -- **Publishes:** - - Visualization markers → `self.marker_visualization_lidar_publisher.publish(marker_array)` - - Clustered data → `self.clustered_points_publisher.publish(clustered_points_msg)` - -### 4.5 Distance Image Calculation - -- Computes distance images for various directions (`start_image_calculation`). -- Filters LiDAR points for specific viewpoints (`calculate_image`). -- Reconstructs distance images from LiDAR data (`reconstruct_img_from_lidar`). -- **Publishes:** - - Distance images for different viewpoints → `self.publish_images(processed_images, data.header)` - -## 5. Output: Published Topics - -### Filtered Point Clouds - -- **Topic Name:** `/carla/hero/LIDAR_filtered` -- **Data Type:** `sensor_msgs/PointCloud2` -- **Description:** Contains filtered LiDAR data after noise suppression. - -### Distance Images in Various Directions - -- **Center:** `~image_distance_topic` (Default: `/paf/hero/Center/dist_array`) -- **Back:** `~image_distance_topic` (Default: `/paf/hero/Back/dist_array`) -- **Left:** `~image_distance_topic` (Default: `/paf/hero/Left/dist_array`) -- **Right:** `~image_distance_topic` (Default: `/paf/hero/Right/dist_array`) -- **Data Type:** `sensor_msgs/Image` -- **Description:** Contains the calculated minimum distance to objects in various directions. Although the _Back_, _Left_, and _Right_ directions are still actively processed in this node's image pipeline from PAF23, the current vision node only subscribes to and utilizes the _Center_S image. - Support for the other directions has been intentionally preserved to allow future teams to easily extend the system with additional camera perspectives if needed. - -### Marker Visualization - -- **Topic Name:** `~marker_topic` (Default: `/paf/hero/Lidar/Marker`) -- **Data Type:** `visualization_msgs/MarkerArray` -- **Description:** Displays the LiDAR point clouds as RViz markers for visualization. - -### Clustered Points - -- **Topic Name:** `~clustered_points_lidar_topic` (Default: `/paf/hero/Lidar/clustered_points`) -- **Data Type:** `ClusteredPointsArray` -- **Description:** Clusters LiDAR data for further downstream analysis in the intermediate layer. - -### Delta Heading - -- **Topic Name:** `/paf/hero/delta_heading` -- **Data Type:** `std_msgs/Float32` -- **Description:** Publishes the change in vehicle heading (delta heading) calculated during LocalCompensation mode. +# Documentation: Lidar Distance Node + +## Table of Contents + +- [1. Integration into the Perception Pipeline](#1-integration-into-the-perception-pipeline) +- [2. Point Cloud Compensation](#2-point-cloud-compensation) +- [3. Input: Received Data](#3-input-received-data) +- [4. Processing Pipeline](#4-processing-pipeline) +- [5. Output: Published Topics](#5-output-published-topics) + +## 1. Integration into the Perception Pipeline + +The `lidar_distance.py` node is part of the perception pipeline. This node processes LiDAR data to provide precise distance information. The extracted and processed data serves as a foundation for subsequent layers such as the Intermediate Layer and the Planning module. + +## 2. Point Cloud Compensation + +In the simulation environment (CARLA/Leaderboard), there is a mismatch between the lidar rotation frequency (10Hz) and the simulation frequency (20Hz), resulting in a 180 degree lidar data per simulation frame and a blindspot on missing data. + +To achieve better spatial coverage, the node can buffer the current and previous frame. The core challenge is that the environment points from the previous scan must be compensated (transformed) to account for the ego vehicle's motion that occurred between the two frames. + +### Modular Compensation + +The node uses the Strategy pattern to allow switching between different compensation modes via configuration: + +- `CompensationStrategy` (Abstract Base class) defines the interface for all modes. +- The Compensation object handles buffering, data preparation and returns the compensated point cloud. + +The available compensation modes are: + +**Default Mode:** The default compensation strategy is `LocalCompensation`, which uses local vehicle dynamics for simplified motion correction. + +### 2.1 NoCompensation (Baseline) + +This strategy represents the simplest case, where no buffering, registration, or compensation is performed on the incoming LiDAR data. + +The system simply passes the current point cloud ($P_{cur}$) as the final result, discarding all previous frame data. + +$$ + P_{comp} = P_{cur} +$$ + +### 2.2 Buffer + +This strategy performs basic point cloud accumulation across two consecutive frames without applying any geometric transformation or motion compensation. + +The LiDAR data from the current frame ($P_{cur}$) and the previous frame ($P_{prev}$) are buffered and directly concatenated (joined). +Since no transformation is applied, the previous cloud remains in its original, outdated coordinate frame, leading to motion misaligned points if the ego vehicle moved between frames. + +$$ + P_{comp} = P_{cur} \cup P_{prev} +$$ + +### 2.3 EgoMotionCompensation + +This strategy uses external state information (Estimated Kalman Filter, EKF) to determine the vehicle's transformation between the current frame ($f_i$) and the previous frame ($f_{i-1}$). +Since LiDAR data is always relative to the sensor, we must apply a transformation that describes the positional delta between the vehicle's current and previous pose to correctly align the point clouds. + +2.3.1 Point Cloud Separation + +We first divide the previous point cloud, $P_{prev}$, into points belonging to the ego vehicle, $P_{ego}$, and points belonging to the static environment, $P_{env}$, such that: + +$$P_{prev} = P_{ego} \cup P_{env}$$ + +2.3.2 Homogeneous Transformation Matrix ($T_i$) + +The EKF provides both the translation components and rotation components, allowing us to define the homogeneous transformation matrix $T_i$ for a frame $f_i$ relative to the local position. The transformation matrix $T_i$ is defined as following: + +$$T_{i} = \begin{pmatrix} +R_{i} & t_{i} \\ +\mathbf{0}^T & 1 +\end{pmatrix}$$ + +Components: + +$R_i$ is the $3 \times 3$ rotation matrix representing the orientation (roll, pitch, yaw) of the ego vehicle: + +$$R_{i} = \begin{pmatrix} + r_{11} & r_{12} & r_{13} \\ + r_{21} & r_{22} & r_{23} \\ + r_{31} & r_{32} & r_{33} + \end{pmatrix}$$ + +$t_{i}$ is the $3 \times 1$ translation vector, representing the vehicle's position $(x, y, z)$: + +$$t_{i} = \begin{pmatrix} + t_x \\ + t_y \\ + t_z + \end{pmatrix}$$ + +$\mathbf{0}^T$ is a $1 \times 3$ row vector of zeros: $\begin{pmatrix} 0 & 0 & 0 \end{pmatrix}$. + +2.3.3 Delta Transformation Matrix ($\Delta T$) + +Having defined the homogenous transformation matrices for both $T_i$ (current pose) and $T_{i-1}$ (previous pose), we calculate the positional delta between the two frames. +This delta transformation matrix, $\Delta T$, transforms points from the current sensor frame ($f_i$) back to the coordinates of the previous frame ($f_{i-1}$): + +$$ + \Delta T = T_{i-1} T_i^{-1} +$$ + +2.3.4 Point Cloud Compensation + +The $\Delta T$ allows us to transform all environment points ($P_{env}$ from the previous frame), compensating for the ego-motion that occurred between $f_{i-1}$ and $f_i$. For any point $(x, y, z)^T \in P_{env}$, the compensated position $P'_{env}$ is calculated using homogeneous coordinates: + +$$ + P'_{env} = \Delta T \begin{pmatrix} x \\ y \\ z \\ 1 \end{pmatrix} +$$ + +The final compensated point cloud, $P_{comp}$, is the union of the environment points from the previous frame ($P'_{env}$), the current frame's point cloud ($P_{cur}$), and the ego vehicle points ($P_{ego}$): + +$$ + P_{comp} = P_{cur} \cup P'_{env} \cup P_{ego} +$$ + +### 2.4 LocalCompensation + +This strategy uses the vehicle's local speed and heading change between the previous frame ($f_{i-1}$) and the current frame ($f_i$) to correct the static points. + +2.4.1 Point Cloud Separation + +First, the previous point cloud, $P_{prev}$, is split into two groups: + +$$P_{prev} = P_{ego} \cup P_{env}$$ + +2.4.2 Motion Compensation Components + +We determine how much the vehicle moved during the time interval $\Delta t = t_i - t_{i-1}$. + +Translation Component ($d_x$) + +The main movement is the displacement along the vehicle's forward (X) axis. This distance, $d_x$, is calculated from the average speed ($\bar{v}$) over $\Delta t$. This is the distance we need to undo on the static points. + +$$d_x \approx \bar{v} \cdot \Delta t$$ + +Rotation Component ($R_{\Delta}$) (Optional) + +This is the $3 \times 3$ rotation matrix used to correct for the change in heading ($\Delta \theta$). This correction is only applied if the heading change is significant enough to warrant it, which is controlled by a flag in the implementation. The rotation is around the Z-axis (yaw): + +$$R_{\Delta} = \begin{pmatrix} +\cos(-\Delta \theta) & -\sin(-\Delta \theta) & 0 \\ +\sin(-\Delta \theta) & \cos(-\Delta \theta) & 0 \\ +0 & 0 & 1 +\end{pmatrix}$$ + +Where $\Delta \theta = \theta_i - \theta_{i-1}$. + +2.4.3 Compensation Procedure + +The correction is applied to the static points ($P_{env}$) in two steps: + +Translation (Required): The points are moved backward along the X-axis by subtracting the calculated distance $d_x$: + +$$x_{comp} = x - d_x$$ + +Rotation (Optional): If the account_heading flag is set to True, the resulting coordinates are rotated using the $R_{\Delta}$ matrix to correct for the vehicle's turning. + +The final compensated point cloud, $P_{comp}$, is the combined set of current points, corrected static points, and ego points: + +$$P_{comp} = P_{cur} \cup P'_{env} \cup P_{ego}$$ + +## 3. Input: Received Data + +### Incoming Topics + +- **Topic Name:** `/carla/hero/LIDAR` +- **Data Type:** `sensor_msgs/PointCloud2` +- **Description:** Contains point cloud data from the LiDAR sensor, serving as raw input for distance measurement. + +### Incoming Topics (Compensation Mode Dependent) + +EgoMotionCompensation active: + +- **Topic Name:** `/paf/hero/local_current_pos` +- **Data Type:** `geometry_msgs/PoseStamped` +- **Description:** Provides the pose (position and orientation) of the ego vehicle in the local coordinate frame. + +LocalCompensation active: + +- **Topic Name:** `/carla/hero/Speed` +- **Data Type:** `carla_msgs/CarlaSpeedometer` +- **Description:** Provides the linear speed of the vehicle. + +- **Topic Name:** `/carla/hero/IMU` +- **Data Type:** `sensor_msgs/Imu` +- **Description:** Provides orientation data used for calculating the heading and the current pitch angle for pitch-aware ground filtering. + +## 4. Processing Pipeline + +### Processing Steps + +### 4.1 Reception of PointCloud2 Data + +- ROS subscriber receives the LiDAR point cloud (`callback` method). + +### 4.2 Motion Compensation of Point Cloud + +- Compensates point cloud based on the compensation mode (NoCompensation, Buffer, EgoMotionCompensation, LocalCompensation) +- If LocalCompensation is active, publishes the calculated delta heading to `/paf/hero/delta_heading` + +### 4.3 Filtering and Preprocessing + +- Removes points representing the ego vehicle (`start_clustering`). +- Filters out points below a certain height (`clustering_lidar_z_min`) to avoid clustering the road surface (`start_clustering`). +- Optionally tilts the lower height bound with the current IMU pitch (`enable_pitch_ground_filter`) so that road points do not drift into the obstacle set during acceleration, braking, or other pitch motion. +- Filters out points above a maximum height (`clustering_lidar_z_max`) to exclude high objects like tree leaves or overhead structures. + +### 4.4 Clustering the LiDAR Data + +- Uses DBSCAN to group spatially related points (`start_clustering`). +- Applies coordinate normalization by projecting points onto a unit sphere and augmenting with weighted distance components, creating a polar-coordinate-like clustering approach for better handling of varying point densities in LiDAR sweeps. +- Removes noise points classified by DBSCAN (`cluster_labels != -1`). +- Generates bounding boxes for identified clusters (`generate_bounding_boxes`). +- **Publishes:** + - Visualization markers → `self.marker_visualization_lidar_publisher.publish(marker_array)` + - Clustered data → `self.clustered_points_publisher.publish(clustered_points_msg)` + +### 4.5 Distance Image Calculation + +- Computes distance images for various directions (`start_image_calculation`). +- Filters LiDAR points for specific viewpoints (`calculate_image`). +- Reconstructs distance images from LiDAR data (`reconstruct_img_from_lidar`). +- **Publishes:** + - Distance images for different viewpoints → `self.publish_images(processed_images, data.header)` + +## 5. Output: Published Topics + +### Filtered Point Clouds + +- **Topic Name:** `/carla/hero/LIDAR_filtered` +- **Data Type:** `sensor_msgs/PointCloud2` +- **Description:** Contains filtered LiDAR data after noise suppression. + +### Distance Images in Various Directions + +- **Center:** `~image_distance_topic` (Default: `/paf/hero/Center/dist_array`) +- **Back:** `~image_distance_topic` (Default: `/paf/hero/Back/dist_array`) +- **Left:** `~image_distance_topic` (Default: `/paf/hero/Left/dist_array`) +- **Right:** `~image_distance_topic` (Default: `/paf/hero/Right/dist_array`) +- **Data Type:** `sensor_msgs/Image` +- **Description:** Contains the calculated minimum distance to objects in various directions. Although the _Back_, _Left_, and _Right_ directions are still actively processed in this node's image pipeline from PAF23, the current vision node only subscribes to and utilizes the _Center_S image. + Support for the other directions has been intentionally preserved to allow future teams to easily extend the system with additional camera perspectives if needed. + +### Marker Visualization + +- **Topic Name:** `~marker_topic` (Default: `/paf/hero/Lidar/Marker`) +- **Data Type:** `visualization_msgs/MarkerArray` +- **Description:** Displays the LiDAR point clouds as RViz markers for visualization. + +### Clustered Points + +- **Topic Name:** `~clustered_points_lidar_topic` (Default: `/paf/hero/Lidar/clustered_points`) +- **Data Type:** `ClusteredPointsArray` +- **Description:** Clusters LiDAR data for further downstream analysis in the intermediate layer. + +### Delta Heading + +- **Topic Name:** `/paf/hero/delta_heading` +- **Data Type:** `std_msgs/Float32` +- **Description:** Publishes the change in vehicle heading (delta heading) calculated during LocalCompensation mode. From d4636d262795aaa2a1967e32c9d2eafa58aeac64 Mon Sep 17 00:00:00 2001 From: ll7 Date: Tue, 28 Apr 2026 12:29:13 +0200 Subject: [PATCH 29/43] Add planning and reasoning documentation; enhance intersection behavior logic and tests Co-authored-by: Copilot --- .agent/PLANS.md | 58 +++++++++++ .../docs-and-reasoning.instructions.md | 12 +++ .../repo-consolidation.instructions.md | 12 +++ .gitignore | 3 + CLAUDE.md | 11 +++ agents.md | 6 ++ .../behavior_agent/behaviors/intersection.py | 96 ++++++++++++++----- ...est_intersection_priority_cross_traffic.py | 20 ++++ doc/README.md | 5 + doc/development/context_retention.md | 3 + .../2026-04-28-development-hardening.md | 43 +++++++++ doc/reasoning/README.md | 23 +++++ tmp/.gitignore | 3 + 13 files changed, 271 insertions(+), 24 deletions(-) create mode 100644 .agent/PLANS.md create mode 100644 .github/instructions/docs-and-reasoning.instructions.md create mode 100644 .github/instructions/repo-consolidation.instructions.md create mode 100644 CLAUDE.md create mode 100644 doc/reasoning/2026-04-28-development-hardening.md create mode 100644 doc/reasoning/README.md create mode 100644 tmp/.gitignore diff --git a/.agent/PLANS.md b/.agent/PLANS.md new file mode 100644 index 00000000..869a816b --- /dev/null +++ b/.agent/PLANS.md @@ -0,0 +1,58 @@ +# Planning Convention + +Use this file when the work is large enough that an agent should externalize its plan before or during implementation. + +## When To Write A Plan + +Write a plan when the task: + +- spans multiple packages or mixes code, docs, and workflow changes, +- changes repository behavior, contributor workflow, or validation expectations, +- updates active interfaces, launch flows, or runtime assumptions, +- or is likely to require follow-up work after the current change. + +Skip formal planning only for narrow, obviously local edits. + +## Plan Template + +Keep plans short and operational: + +```md +# Goal +- One or two sentences on the desired outcome. + +# Boundaries +- What is in scope. +- What is explicitly out of scope. + +# Evidence +- Files, docs, tests, configs, or upstream references that define the contract. + +# Steps +- Ordered implementation steps. + +# Validation +- Commands to run. +- Evidence that will prove the change works in this repository. + +# Risks / Follow-ups +- Remaining uncertainty, deferred scope, or issue candidates. +``` + +## Required Behaviors + +- Restate the repository goal in PAF terms, not generic assistant language. +- Separate observed evidence from assumptions. +- Prefer committed scripts, VS Code tasks, and documented workflows over ad-hoc commands. +- For non-trivial development work, state the proof obligation before implementation. +- Prefer the consolidation order from `doc/dev_talks/paf25/future_work.md`: documentation and interfaces first, then tests and CI, then subsystem fixes. +- If scope expands, capture a follow-up in `doc/reasoning/` or the issue tracker instead of silently broadening the change. + +## Review Expectations + +A good plan makes it easy for a reviewer to answer: + +- what changed, +- why that scope is correct, +- how it was validated, +- and what risk remains. diff --git a/.github/instructions/docs-and-reasoning.instructions.md b/.github/instructions/docs-and-reasoning.instructions.md new file mode 100644 index 00000000..2603fe8b --- /dev/null +++ b/.github/instructions/docs-and-reasoning.instructions.md @@ -0,0 +1,12 @@ +--- +description: "Use when updating documentation, architecture notes, contributor guidance, or saved reasoning. Covers doc sync with code, canonical docs vs reasoning notes, and the doc/reasoning workflow." +name: "Docs And Reasoning" +applyTo: "doc/**/*.md,README.md,agents.md" +--- +# Docs And Reasoning + +- Active documentation must match the current implementation. If code and docs disagree, either fix the doc in the same change or call out the gap explicitly. +- Keep canonical behavior and interface docs in their domain folders under `doc/` or the package docs. Use `doc/reasoning/` for analysis notes, comparisons, migration thoughts, and development output that should not become the source of truth. +- When saving a reasoning note, include the motivating task, the source files or repositories inspected, the main conclusion, and the remaining follow-ups. +- Link new documentation from `doc/README.md` or the most relevant existing index page so it stays discoverable. +- Prefer short, actionable Markdown over long narrative dumps. Remove or update stale statements instead of piling on contradictory notes. diff --git a/.github/instructions/repo-consolidation.instructions.md b/.github/instructions/repo-consolidation.instructions.md new file mode 100644 index 00000000..7b36453a --- /dev/null +++ b/.github/instructions/repo-consolidation.instructions.md @@ -0,0 +1,12 @@ +--- +description: "Use when doing repository-wide cleanup, development workflow changes, cross-package fixes, or quality hardening. Covers consolidation-before-expansion, interface discipline, and proof-first validation for PAF." +name: "Repository Consolidation" +--- +# Repository Consolidation + +- Use `doc/dev_talks/paf25/future_work.md` and `doc/dev_talks/paf25/improvements_assessment.md` as the direction-setting documents for repo-wide changes. +- Prefer consolidation work before new feature breadth: sync active docs, clarify interfaces, strengthen tests and CI, and reduce friction in development workflows. +- When a change touches more than one package or mixes workflow plus runtime behavior, make the controlling contract explicit: name the package boundary, topic/message, task, or doc that defines the expected behavior. +- Keep changes small and reversible. If the best answer is larger than one reviewable slice, implement the smallest slice now and record the rest as follow-up work. +- State proof before implementation. Typical proof in this repo is one of: Ruff lint/format, host smoke tests, ROS-backed unit tests, route-level validation, or docs/link verification. +- If a change updates an interface or development rule, update the canonical documentation in the same change or record the gap in `doc/reasoning/`. diff --git a/.gitignore b/.gitignore index 62839a27..ff8275a0 100644 --- a/.gitignore +++ b/.gitignore @@ -25,3 +25,6 @@ code/*.pt # VS Code transient browse database files .vscode/browse.vc.db* + +tmp/* +!tmp/.gitignore diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 00000000..d9653e96 --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,11 @@ +# Claude Code Instructions + +Use `agents.md` as the canonical repository instruction source. + +For non-trivial work, also read: + +- `.agent/PLANS.md` for plan structure and proof obligations +- `doc/dev_talks/paf25/future_work.md` for the recommended execution order +- `doc/dev_talks/paf25/improvements_assessment.md` for the consolidation-first development direction + +When work produces analysis, comparisons, or handoff notes that should survive the chat session, store them under `doc/reasoning/` and keep the canonical behavior documentation in the appropriate `doc/` or package folder. diff --git a/agents.md b/agents.md index d4c8ffae..8bbfa04f 100644 --- a/agents.md +++ b/agents.md @@ -85,9 +85,15 @@ Do not attempt to fix unrelated failing tests/lints outside the requested scope. - Update docs when behavior, setup, commands, or interfaces change. - For developer-facing changes, prefer updating docs under `doc/development/` or package README files. +- Use `doc/reasoning/` for preserved analysis, comparisons, and handoff notes that should not become the canonical behavior documentation. - Keep Markdown concise, structured, and actionable. - Capture non-trivial architectural decisions in `doc/adr/` using the ADR template. +## 9.1) Planning and reasoning support + +- For non-trivial work, use `.agent/PLANS.md` to make scope, evidence, validation, and follow-ups explicit. +- Treat `doc/dev_talks/paf25/future_work.md` and `doc/dev_talks/paf25/improvements_assessment.md` as direction-setting documents for repository-wide cleanup and development workflow changes. + ## 10) Git and PR hygiene - Use feature-branch style consistent with `doc/development/git_workflow.md`. diff --git a/code/planning/planning/behavior_agent/behaviors/intersection.py b/code/planning/planning/behavior_agent/behaviors/intersection.py index 80bbd2da..4d7a5758 100755 --- a/code/planning/planning/behavior_agent/behaviors/intersection.py +++ b/code/planning/planning/behavior_agent/behaviors/intersection.py @@ -114,6 +114,10 @@ def tr_status_str(t: Optional[TrafficLightState]): PRIORITY_CHECK_LENGTH = 25.0 PRIORITY_CHECK_WIDTH = 50.0 PRIORITY_CLOSING_SPEED_THRESHOLD = 0.5 +PRIORITY_CONFLICT_MARGIN = 1.5 +PRIORITY_PASS_JUDGE_DECELERATION = 3.0 +PRIORITY_PASS_JUDGE_RESPONSE_TIME = 0.5 +PRIORITY_PASS_JUDGE_MARGIN = 1.0 SELF_EMERGENCY_THRESHOLD = 10 / 3.6 # m/s ≈ 2.78 @@ -171,7 +175,9 @@ def _get_entity_velocity_in_hero_frame(entity) -> Optional[Vector2]: return entity.transform * entity.motion.linear_motion -def _is_priority_cross_traffic_threat(entity, target_point: Point2) -> bool: +def _is_priority_cross_traffic_threat( + entity, target_point: Point2, hero_width: float +) -> bool: velocity = _get_entity_velocity_in_hero_frame(entity) if velocity is None or velocity.length() <= PRIORITY_SPEED_THRESHOLD: return False @@ -181,8 +187,32 @@ def _is_priority_cross_traffic_threat(entity, target_point: Point2) -> bool: if to_target.length() == 0.0: return True + direction = velocity.normalized() closing_speed = _dot(velocity, to_target.normalized()) - return closing_speed > PRIORITY_CLOSING_SPEED_THRESHOLD + if closing_speed <= PRIORITY_CLOSING_SPEED_THRESHOLD: + return False + + along_path = _dot(direction, to_target) + if along_path <= 0.0: + return False + + miss_vector = to_target - (direction * along_path) + conflict_radius = ( + PRIORITY_CONFLICT_MARGIN + (entity.get_width() * 0.5) + (hero_width * 0.5) + ) + return miss_vector.length() <= conflict_radius + + +def _is_over_priority_pass_judge_line( + distance_to_conflict: float, ego_speed: float +) -> bool: + ego_speed = max(ego_speed, 0.0) + stopping_distance = ( + (ego_speed * ego_speed) / (2.0 * PRIORITY_PASS_JUDGE_DECELERATION) + + ego_speed * PRIORITY_PASS_JUDGE_RESPONSE_TIME + + PRIORITY_PASS_JUDGE_MARGIN + ) + return distance_to_conflict <= stopping_distance def check_priority_cross_traffic( @@ -202,6 +232,7 @@ def check_priority_cross_traffic( hero = map.hero() if hero is None: return True, None + hero_width = hero.get_width() mask, target_point = _build_priority_check_mask( hero, @@ -213,7 +244,7 @@ def check_priority_cross_traffic( for se in shapely_entities: entity = se.entity - if _is_priority_cross_traffic_threat(entity, target_point): + if _is_priority_cross_traffic_threat(entity, target_point, hero_width): return False, mask return True, mask @@ -849,6 +880,18 @@ def update(self): if CURRENT_PRIORITY_CHECK_REFERENCE is None: CURRENT_PRIORITY_CHECK_REFERENCE = current_pose + speedometer = self.blackboard.try_get("/carla/hero/Speed") + ego_speed = speedometer.speed if speedometer is not None else 0.0 + hero = map.hero() + pass_judge_distance = None + if hero is not None: + _, target_point = _build_priority_check_mask( + hero, + reference_pose=CURRENT_PRIORITY_CHECK_REFERENCE, + current_pose=current_pose, + ) + pass_judge_distance = target_point.vector().length() + priority_clear, priority_mask = check_priority_cross_traffic( map, tree, @@ -863,31 +906,36 @@ def update(self): add_debug_marker(debug_marker(priority_mask, color=(1.0, 0.0, 0.0, 0.3))) if not priority_clear: - # priority cross traffic detected - self.curr_behavior_pub.publish(String(data=bs.int_wait.name)) - set_line_stop(self.stop_client, 0.0) + if pass_judge_distance is not None and _is_over_priority_pass_judge_line( + pass_judge_distance, ego_speed + ): + add_debug_entry( + self.name, + "[Enter] Over priority pass judge line, ignore new stop", + ) + else: + # priority cross traffic detected + self.curr_behavior_pub.publish(String(data=bs.int_wait.name)) + set_line_stop(self.stop_client, 0.0) - speedometer = self.blackboard.try_get("/carla/hero/Speed") - ego_speed = speedometer.speed if speedometer is not None else 0.0 + if ego_speed > SELF_EMERGENCY_THRESHOLD: + self.emergency_pub.publish(Bool(data=True)) + reason = ( + "ENTER: EMERGENCY – fast cross traffic, " + f"ego_speed={ego_speed:.2f} m/s" + ) - if ego_speed > SELF_EMERGENCY_THRESHOLD: - self.emergency_pub.publish(Bool(data=True)) - reason = ( - "ENTER: EMERGENCY – fast cross traffic, " - f"ego_speed={ego_speed:.2f} m/s" - ) + else: + reason = ( + f"ENTER: fast cross traffic, but ego_speed={ego_speed:.2f} m/s " + "(no emergency brake)" + ) - else: - reason = ( - f"ENTER: fast cross traffic, but ego_speed={ego_speed:.2f} m/s " - "(no emergency brake)" + return debug_status( + self.name, + py_trees.common.Status.RUNNING, + reason, ) - - return debug_status( - self.name, - py_trees.common.Status.RUNNING, - reason, - ) unset_line_stop(self.stop_client) self.emergency_pub.publish(Bool(data=False)) diff --git a/code/planning/test/test_intersection_priority_cross_traffic.py b/code/planning/test/test_intersection_priority_cross_traffic.py index 1b9731ba..331ac389 100644 --- a/code/planning/test/test_intersection_priority_cross_traffic.py +++ b/code/planning/test/test_intersection_priority_cross_traffic.py @@ -64,6 +64,18 @@ def test_priority_cross_traffic_ignores_fast_entity_moving_away(): assert priority_clear +def test_priority_cross_traffic_ignores_fast_entity_missing_conflict_area(): + hero = _make_car(0.0, 0.0, is_hero=True) + offset_vehicle = _make_car(4.0, 24.0, 0.0, -7.5) + map_obj = mapping_map.Map(entities=[hero, offset_vehicle]) + + priority_clear, _ = intersection.check_priority_cross_traffic( + map_obj, FakeTree([offset_vehicle]) + ) + + assert priority_clear + + def test_priority_check_mask_stays_world_aligned_while_turning(): hero = _make_car(0.0, 0.0, is_hero=True) reference_pose = transform.Transform2D.identity() @@ -80,3 +92,11 @@ def test_priority_check_mask_stays_world_aligned_while_turning(): expected_distance = intersection.PRIORITY_CHECK_DISTANCE + hero.get_front_x() assert target_point.x() == pytest.approx(0.0) assert target_point.y() == pytest.approx(-expected_distance) + + +def test_enter_pass_judge_line_triggers_once_braking_is_no_longer_comfortable(): + assert intersection._is_over_priority_pass_judge_line(4.0, 5.0) + + +def test_enter_pass_judge_line_allows_recheck_when_conflict_is_still_far(): + assert not intersection._is_over_priority_pass_judge_line(10.0, 2.0) diff --git a/doc/README.md b/doc/README.md index bdfc66d9..120a7a8b 100644 --- a/doc/README.md +++ b/doc/README.md @@ -13,6 +13,7 @@ This document provides an overview of the structure of the documentation. - [`dev_talks`](#dev_talks) - [`localization`](#localization) - [`control`](#control) +- [`reasoning`](#reasoning) ## `general` @@ -60,3 +61,7 @@ The [`localization`](./localization/) folder contains documentation for the whol ## `control` The [`control`](./control/) folder contains documentation for all the controllers. + +## `reasoning` + +The [`reasoning`](./reasoning/) folder contains development analyses, adaptation notes, and other non-canonical reasoning artifacts that should stay in the repository without replacing the main subsystem or contributor documentation. diff --git a/doc/development/context_retention.md b/doc/development/context_retention.md index e74f9074..39e50f53 100644 --- a/doc/development/context_retention.md +++ b/doc/development/context_retention.md @@ -13,6 +13,9 @@ Long-lived projects lose intent when design decisions only live in chat threads - Capture assumptions, validation, and known gaps in every PR. 3. Test markers and logs - Preserve behavior expectations with marker-based tests and structured logs. +4. Reasoning notes for non-trivial improvement work + - Keep analysis and comparison notes in `doc/reasoning/` when they are worth preserving but are not the canonical source of truth. + - Promote the stable parts into `doc/development/`, `doc//`, or ADRs when the behavior or policy is finalized. ## When to write an ADR diff --git a/doc/reasoning/2026-04-28-development-hardening.md b/doc/reasoning/2026-04-28-development-hardening.md new file mode 100644 index 00000000..fa05b897 --- /dev/null +++ b/doc/reasoning/2026-04-28-development-hardening.md @@ -0,0 +1,43 @@ +# Development Hardening Notes + +## Task + +Improve the repository for better day-to-day development by strengthening agent-facing guidance, preserving debugging scratch space, and capturing reasoning in-repo. + +## Sources inspected + +- `doc/dev_talks/paf25/future_work.md` +- `doc/dev_talks/paf25/improvements_assessment.md` +- `agents.md` +- `doc/development/context_retention.md` +- `tmp/robot_sf_ll7/AGENTS.md` +- `tmp/robot_sf_ll7/.agent/PLANS.md` +- `tmp/robot_sf_ll7/.github/copilot-instructions.md` + +## Main conclusions + +1. The next useful repository-wide work is consolidation, not more feature breadth. +2. PAF already has a strong root `agents.md`, but it lacked smaller agent-facing support files for planning, doc sync, and repository-wide cleanup work. +3. A tracked `tmp/` folder should exist for ignored scratch outputs such as cloned reference repositories, debug notes, or local experiments. +4. Reasoning and adaptation notes should live in a dedicated folder so they are preserved without diluting canonical docs. + +## Adapted ideas from `robot_sf_ll7` + +Safe to port directly: + +- a lightweight cross-agent entrypoint (`CLAUDE.md`) +- a planning convention file for non-trivial work (`.agent/PLANS.md`) +- scoped agent instructions instead of expanding the top-level rules endlessly + +Used only as inspiration: + +- a large skill tree under `.agents/skills/` +- a second canonical instruction file under `.github/copilot-instructions.md` + +The second point was intentionally not copied because this repository already uses `agents.md` as its canonical instruction file. + +## Recommended next follow-ups + +1. Audit stale docs in `doc/perception/`, `doc/planning/`, and `doc/mapping/` against the current ROS2 interfaces. +2. Add more focused tests around the known weak points called out in the PAF25 notes: radar motion quality, cross-traffic logic, and tracking stability. +3. Consider adding a small repo-local skill set later if the team wants repeatable AI workflows beyond the current instruction files. diff --git a/doc/reasoning/README.md b/doc/reasoning/README.md new file mode 100644 index 00000000..eb98c488 --- /dev/null +++ b/doc/reasoning/README.md @@ -0,0 +1,23 @@ +# Reasoning Notes + +This folder stores development reasoning that is useful to keep in the repository but should not become the canonical source of truth for runtime behavior or contributor policy. + +Use this folder for: + +- repository comparisons and adaptation notes +- analysis that informs future cleanup work +- temporary design writeups that still need to be turned into canonical docs +- handoff notes for non-trivial repository improvement tasks + +Do not use this folder for: + +- stable architecture or interface documentation +- contributor workflow rules that belong in `doc/development/` +- subsystem behavior docs that belong in `doc//` + +Each note should identify: + +1. The task or motivation. +2. The main sources inspected. +3. The current conclusion. +4. Any follow-up work that remains. diff --git a/tmp/.gitignore b/tmp/.gitignore new file mode 100644 index 00000000..58b913f5 --- /dev/null +++ b/tmp/.gitignore @@ -0,0 +1,3 @@ +# Keep this folder in the repository, but ignore local scratch data. +* +!.gitignore From 29ffdaa6a40df0bce4dcbf269daf6ea0574e27c2 Mon Sep 17 00:00:00 2001 From: ll7 Date: Tue, 28 Apr 2026 16:02:17 +0200 Subject: [PATCH 30/43] planning: refine priority intersection decisions --- .../planning/behavior_agent/behavior_tree.py | 11 + .../behavior_agent/behaviors/intersection.py | 222 +++++++++++++++--- ...est_intersection_priority_cross_traffic.py | 42 ++++ ...-04-28-intersection-prediction-strategy.md | 56 +++++ 4 files changed, 296 insertions(+), 35 deletions(-) create mode 100644 doc/reasoning/2026-04-28-intersection-prediction-strategy.md diff --git a/code/planning/planning/behavior_agent/behavior_tree.py b/code/planning/planning/behavior_agent/behavior_tree.py index 93ac7c83..ca4da111 100755 --- a/code/planning/planning/behavior_agent/behavior_tree.py +++ b/code/planning/planning/behavior_agent/behavior_tree.py @@ -5,9 +5,11 @@ import rclpy.callback_groups from rclpy.callback_groups import CallbackGroup from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile from std_msgs.msg import String, Bool from paf_common.parameters import update_attributes from paf_common.exceptions import emsg_with_trace +from paf_common.sync import startup_topic from rclpy.parameter import Parameter from rcl_interfaces.msg import ( SetParametersResult, @@ -104,6 +106,7 @@ def grow_a_tree( ), intersection.Enter( "Enter Intersection", + node.get_clock(), node.curr_behavior_pub, node.stop_marks_client, node.emergency_pub, @@ -264,6 +267,13 @@ def __init__(self): f"/paf/{self.role_name}/emergency", 1, ) + self.startup_ready_pub = self.create_publisher( + Bool, + startup_topic(self.role_name, "behavior_tree"), + qos_profile=QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL + ), + ) # Service clients self.speed_alteration_client = self.create_client( @@ -307,6 +317,7 @@ def __init__(self): self.control_loop_rate, self.tick_tree_handler ) self.add_on_set_parameters_callback(self._set_parameters_callback) + self.startup_ready_pub.publish(Bool(data=True)) self.get_logger().info(f"{type(self).__name__} node initialized.") def _set_parameters_callback(self, params: List[Parameter]) -> SetParametersResult: diff --git a/code/planning/planning/behavior_agent/behaviors/intersection.py b/code/planning/planning/behavior_agent/behaviors/intersection.py index 4d7a5758..167c4ba8 100755 --- a/code/planning/planning/behavior_agent/behaviors/intersection.py +++ b/code/planning/planning/behavior_agent/behaviors/intersection.py @@ -20,6 +20,7 @@ from mapping_common.shape import Rectangle from mapping_common.transform import Transform2D, Point2, Vector2 import shapely +from shapely.ops import nearest_points from planning.behavior_agent.blackboard_utils import Blackboard from . import behavior_names as bs @@ -115,6 +116,12 @@ def tr_status_str(t: Optional[TrafficLightState]): PRIORITY_CHECK_WIDTH = 50.0 PRIORITY_CLOSING_SPEED_THRESHOLD = 0.5 PRIORITY_CONFLICT_MARGIN = 1.5 +PRIORITY_CONFLICT_CORE_LENGTH = 15.0 +PRIORITY_TTC_THRESHOLD = 1.5 +PRIORITY_TTC_EGO_SPEED_FLOOR = 3.0 +PRIORITY_TTC_GATE_MIN_EGO_SPEED = 1.0 +PRIORITY_UNSAFE_HOLD_TIME = 0.2 +PRIORITY_SAFE_HOLD_TIME = 0.6 PRIORITY_PASS_JUDGE_DECELERATION = 3.0 PRIORITY_PASS_JUDGE_RESPONSE_TIME = 0.5 PRIORITY_PASS_JUDGE_MARGIN = 1.0 @@ -169,6 +176,26 @@ def _build_priority_check_mask( return rect.to_shapely(mask_transform), mask_transform * target_point +def _build_priority_check_centerline( + hero, + reference_pose: Optional[Transform2D] = None, + current_pose: Optional[Transform2D] = None, +): + offset_x = PRIORITY_CHECK_DISTANCE + hero.get_front_x() + half_length = PRIORITY_CONFLICT_CORE_LENGTH / 2.0 + start_point = Point2.new(offset_x - half_length, 0.0) + end_point = Point2.new(offset_x + half_length, 0.0) + + if reference_pose is not None and current_pose is not None: + mask_transform = current_pose.inverse() * reference_pose + else: + mask_transform = hero.transform + + start_point = mask_transform * start_point + end_point = mask_transform * end_point + return shapely.LineString([start_point.to_shapely(), end_point.to_shapely()]) + + def _get_entity_velocity_in_hero_frame(entity) -> Optional[Vector2]: if entity.motion is None: return None @@ -178,29 +205,109 @@ def _get_entity_velocity_in_hero_frame(entity) -> Optional[Vector2]: def _is_priority_cross_traffic_threat( entity, target_point: Point2, hero_width: float ) -> bool: + return ( + _get_priority_conflict_point( + entity, + target_point, + shapely.LineString([[0.0, 0.0], [target_point.x(), target_point.y()]]), + hero_width, + ) + is not None + ) + + +def _get_priority_conflict_point( + entity, + target_point: Point2, + priority_centerline: shapely.LineString, + hero_width: float, +) -> Optional[Point2]: velocity = _get_entity_velocity_in_hero_frame(entity) if velocity is None or velocity.length() <= PRIORITY_SPEED_THRESHOLD: - return False + return None entity_position = Point2.from_vector(entity.transform.translation()) to_target = entity_position.vector_to(target_point) if to_target.length() == 0.0: - return True + return target_point direction = velocity.normalized() closing_speed = _dot(velocity, to_target.normalized()) if closing_speed <= PRIORITY_CLOSING_SPEED_THRESHOLD: - return False + return None along_path = _dot(direction, to_target) if along_path <= 0.0: - return False + return None - miss_vector = to_target - (direction * along_path) conflict_radius = ( PRIORITY_CONFLICT_MARGIN + (entity.get_width() * 0.5) + (hero_width * 0.5) ) - return miss_vector.length() <= conflict_radius + entity_path_end = entity_position + ( + direction * max(along_path + PRIORITY_CHECK_LENGTH, PRIORITY_CHECK_LENGTH) + ) + entity_path = shapely.LineString( + [entity_position.to_shapely(), entity_path_end.to_shapely()] + ) + hero_point, entity_point = nearest_points( + priority_centerline, + entity_path, + ) + if hero_point.distance(entity_point) >= conflict_radius: + return None + + return Point2.new(hero_point.x, hero_point.y) + + +def _is_priority_ttc_conflict( + entity, + target_point: Point2, + ego_speed: float, +) -> bool: + velocity = _get_entity_velocity_in_hero_frame(entity) + if velocity is None or ego_speed < PRIORITY_TTC_GATE_MIN_EGO_SPEED: + return True + + entity_position = Point2.from_vector(entity.transform.translation()) + to_target = entity_position.vector_to(target_point) + along_path = _dot(velocity.normalized(), to_target) + if along_path <= 0.0: + return False + + entity_ttc = along_path / velocity.length() + ego_ttc = target_point.vector().length() / max( + ego_speed, PRIORITY_TTC_EGO_SPEED_FLOOR + ) + return abs(entity_ttc - ego_ttc) <= PRIORITY_TTC_THRESHOLD + + +def _apply_priority_decision_hysteresis( + raw_clear: bool, filtered_clear: bool, state_age_seconds: float +) -> bool: + if raw_clear == filtered_clear: + return filtered_clear + + if raw_clear: + return state_age_seconds >= PRIORITY_SAFE_HOLD_TIME + + return not (state_age_seconds >= PRIORITY_UNSAFE_HOLD_TIME) + + +def _update_priority_decision_state(behavior, raw_clear: bool) -> bool: + now = behavior.clock.now() + if behavior.priority_raw_clear != raw_clear: + behavior.priority_raw_clear = raw_clear + behavior.priority_raw_state_since = now + + state_age_seconds = ( + now - behavior.priority_raw_state_since + ).nanoseconds / 1_000_000_000 + behavior.priority_filtered_clear = _apply_priority_decision_hysteresis( + raw_clear, + behavior.priority_filtered_clear, + state_age_seconds, + ) + return behavior.priority_filtered_clear def _is_over_priority_pass_judge_line( @@ -215,23 +322,15 @@ def _is_over_priority_pass_judge_line( return distance_to_conflict <= stopping_distance -def check_priority_cross_traffic( +def _find_priority_cross_traffic_threat( map: Map, tree: MapTree, reference_pose: Optional[Transform2D] = None, current_pose: Optional[Transform2D] = None, ): - """ - Checks whether there are fast vehicles in the larger intersection area. - - Returns: - (priority_clear, mask_polygon) - priority_clear = True → no fast cross traffic detected - priority_clear = False → fast cross traffic detected - """ hero = map.hero() if hero is None: - return True, None + return None, None, None hero_width = hero.get_width() mask, target_point = _build_priority_check_mask( @@ -239,15 +338,50 @@ def check_priority_cross_traffic( reference_pose=reference_pose, current_pose=current_pose, ) + priority_centerline = _build_priority_check_centerline( + hero, + reference_pose=reference_pose, + current_pose=current_pose, + ) shapely_entities = tree.get_overlapping_entities(mask) for se in shapely_entities: entity = se.entity - if _is_priority_cross_traffic_threat(entity, target_point, hero_width): - return False, mask + conflict_point = _get_priority_conflict_point( + entity, + target_point, + priority_centerline, + hero_width, + ) + if conflict_point is not None: + return entity, mask, conflict_point - return True, mask + return None, mask, target_point + + +def check_priority_cross_traffic( + map: Map, + tree: MapTree, + reference_pose: Optional[Transform2D] = None, + current_pose: Optional[Transform2D] = None, +): + """ + Checks whether there are fast vehicles in the larger intersection area. + + Returns: + (priority_clear, mask_polygon) + priority_clear = True → no fast cross traffic detected + priority_clear = False → fast cross traffic detected + """ + threat_entity, mask, _ = _find_priority_cross_traffic_threat( + map, + tree, + reference_pose=reference_pose, + current_pose=current_pose, + ) + + return threat_entity is None, mask def set_line_stop(client: Client, distance: float): @@ -606,6 +740,9 @@ def initialise(self): self.was_red = False self.intersection_type = self.waypoint.road_option self.left_marker_set = False + self.priority_raw_clear = True + self.priority_filtered_clear = True + self.priority_raw_state_since = self.clock.now() def update(self): """ @@ -631,16 +768,18 @@ def update(self): if CURRENT_PRIORITY_CHECK_REFERENCE is None: CURRENT_PRIORITY_CHECK_REFERENCE = current_pose - # Priority cross traffic check - priority_clear, priority_mask = check_priority_cross_traffic( + threat_entity, priority_mask, _ = _find_priority_cross_traffic_threat( map, tree, reference_pose=CURRENT_PRIORITY_CHECK_REFERENCE, current_pose=current_pose, ) + raw_priority_clear = threat_entity is None + priority_clear = _update_priority_decision_state(self, raw_priority_clear) add_debug_entry( self.name, - f"[Wait] Priority cross traffic clear: {priority_clear}", + f"[Wait] Priority cross traffic clear: {priority_clear}" + f" (raw={raw_priority_clear})", ) if priority_mask is not None: add_debug_marker( @@ -820,11 +959,13 @@ class Enter(py_trees.behaviour.Behaviour): def __init__( self, name: str, + clock: Clock, curr_behavior_pub: Publisher, stop_client: Client, emergency_pub: Publisher, ): super().__init__(name) + self.clock = clock self.curr_behavior_pub = curr_behavior_pub self.stop_client = stop_client self.emergency_pub = emergency_pub @@ -848,6 +989,9 @@ def initialise(self): CURRENT_PRIORITY_CHECK_REFERENCE = _get_current_pose_transform( self.blackboard ) + self.priority_raw_clear = True + self.priority_filtered_clear = True + self.priority_raw_state_since = self.clock.now() def update(self): """ @@ -882,25 +1026,30 @@ def update(self): speedometer = self.blackboard.try_get("/carla/hero/Speed") ego_speed = speedometer.speed if speedometer is not None else 0.0 - hero = map.hero() - pass_judge_distance = None - if hero is not None: - _, target_point = _build_priority_check_mask( - hero, + threat_entity, priority_mask, conflict_point = ( + _find_priority_cross_traffic_threat( + map, + tree, reference_pose=CURRENT_PRIORITY_CHECK_REFERENCE, current_pose=current_pose, ) - pass_judge_distance = target_point.vector().length() - - priority_clear, priority_mask = check_priority_cross_traffic( - map, - tree, - reference_pose=CURRENT_PRIORITY_CHECK_REFERENCE, - current_pose=current_pose, ) + pass_judge_distance = ( + conflict_point.vector().length() if conflict_point is not None else None + ) + raw_priority_clear = threat_entity is None + if not raw_priority_clear and conflict_point is not None: + raw_priority_clear = not _is_priority_ttc_conflict( + threat_entity, + conflict_point, + ego_speed, + ) + + priority_clear = _update_priority_decision_state(self, raw_priority_clear) add_debug_entry( self.name, - f"[Enter] Priority cross traffic clear: {priority_clear}", + f"[Enter] Priority cross traffic clear: {priority_clear}" + f" (raw={raw_priority_clear})", ) if priority_mask is not None: add_debug_marker(debug_marker(priority_mask, color=(1.0, 0.0, 0.0, 0.3))) @@ -909,6 +1058,9 @@ def update(self): if pass_judge_distance is not None and _is_over_priority_pass_judge_line( pass_judge_distance, ego_speed ): + self.priority_raw_clear = True + self.priority_filtered_clear = True + self.priority_raw_state_since = self.clock.now() add_debug_entry( self.name, "[Enter] Over priority pass judge line, ignore new stop", diff --git a/code/planning/test/test_intersection_priority_cross_traffic.py b/code/planning/test/test_intersection_priority_cross_traffic.py index 331ac389..4bb3a8e4 100644 --- a/code/planning/test/test_intersection_priority_cross_traffic.py +++ b/code/planning/test/test_intersection_priority_cross_traffic.py @@ -100,3 +100,45 @@ def test_enter_pass_judge_line_triggers_once_braking_is_no_longer_comfortable(): def test_enter_pass_judge_line_allows_recheck_when_conflict_is_still_far(): assert not intersection._is_over_priority_pass_judge_line(10.0, 2.0) + + +def test_priority_ttc_conflict_blocks_close_arrival_times(): + target_point = transform.Point2.new(15.0, 0.0) + approaching = _make_car(12.0, 12.0, 0.0, -7.5) + + assert intersection._is_priority_ttc_conflict(approaching, target_point, 6.0) + + +def test_priority_conflict_point_tracks_actual_crossing_location(): + hero = _make_car(0.0, 0.0, is_hero=True) + approaching = _make_car(12.0, 12.0, 0.0, -7.5) + centerline = intersection._build_priority_check_centerline(hero) + _, target_point = intersection._build_priority_check_mask(hero) + + conflict_point = intersection._get_priority_conflict_point( + approaching, + target_point, + centerline, + hero.get_width(), + ) + + assert conflict_point is not None + assert conflict_point.x() == pytest.approx(12.0) + assert conflict_point.y() == pytest.approx(0.0) + + +def test_priority_ttc_conflict_ignores_late_arrival_when_ego_is_committed(): + target_point = transform.Point2.new(15.0, 0.0) + delayed = _make_car(24.0, 24.0, 0.0, -7.5) + + assert not intersection._is_priority_ttc_conflict(delayed, target_point, 10.0) + + +def test_priority_hysteresis_delays_new_blocking_decision(): + assert intersection._apply_priority_decision_hysteresis(False, True, 0.1) + assert not intersection._apply_priority_decision_hysteresis(False, True, 0.3) + + +def test_priority_hysteresis_requires_stable_clear_before_release(): + assert not intersection._apply_priority_decision_hysteresis(True, False, 0.3) + assert intersection._apply_priority_decision_hysteresis(True, False, 0.7) diff --git a/doc/reasoning/2026-04-28-intersection-prediction-strategy.md b/doc/reasoning/2026-04-28-intersection-prediction-strategy.md new file mode 100644 index 00000000..60281cd7 --- /dev/null +++ b/doc/reasoning/2026-04-28-intersection-prediction-strategy.md @@ -0,0 +1,56 @@ +# Intersection Prediction Strategy + +## Task + +Evaluate whether PAF should improve intersection handling with stronger motion prediction now, then implement the next useful vehicle-quality step. + +## Local code inspected + +- `code/planning/planning/behavior_agent/behaviors/intersection.py` +- `code/planning/planning/local_planner/motion_planning.py` +- `code/planning/test/test_intersection_priority_cross_traffic.py` +- `doc/planning/behaviors/Intersection.md` +- `doc/planning/motion_planning.md` + +## External references checked + +- Motion forecasting survey: +- Autoware intersection module: +- Autoware intersection collision checker: +- Autoware object collision estimator: + +## Main conclusion + +The practical next step for PAF is not a heavier learned or multimodal motion predictor. + +State-of-the-art motion forecasting depends on stronger tracking quality, confidence handling, lane context, and interaction modeling than the current PAF stack reliably provides. + +The better fit is the production-style pattern seen in Autoware: + +1. keep the prediction model simple and explicit, +2. use geometry plus TTC-style time margins, +3. add hysteresis to suppress stop/go chatter from noisy detections, +4. add a pass-judge or commit rule so the ego does not re-decide to stop after it is too committed to brake comfortably. + +## What was implemented + +- tightened priority cross-traffic filtering so fast vehicles that miss the conflict area do not trigger false stops, +- added a pass-judge rule in intersection `Enter`, +- added a simple TTC gate for `Enter` based on current tracked motion, +- added unsafe/safe hysteresis so noisy detections do not toggle the decision every cycle, +- refined the TTC gate to use the nearest conflict point inside the central priority corridor instead of the mask-center proxy, so edge overlaps do not trigger the same stop logic as a real path conflict. + +## Why this is the right intermediate step + +- It improves vehicle behavior quality immediately. +- It uses signals the stack already has: tracked position and velocity. +- It does not pretend the current perception can support reliable high-order prediction. +- It keeps the door open for later upgrades once perception/tracking quality improves. + +## Recommended next follow-up + +If more intersection quality work is needed, the next useful step is not a neural predictor. It is a slightly richer deterministic model: + +- derive TTC from a better conflict point than the current mask center, +- reuse the existing `motion_planning.py` collision geometry where possible, +- add hold-time parameters and scenario tests for chattering, late detections, and committed entry. From 34379e20365eed44b1dbeb1c5ada0672b5d404e4 Mon Sep 17 00:00:00 2001 From: ll7 Date: Tue, 28 Apr 2026 16:02:46 +0200 Subject: [PATCH 31/43] control: add deterministic startup and frame sync --- code/acting/acting/passthrough.py | 13 +- code/agent/agent/data_management_node.py | 27 +- code/agent/agent/startup_coordinator.py | 90 ++++++ code/agent/launch/agent.dev.persistent.xml | 4 + code/agent/setup.py | 1 + code/control/config/control.yaml | 4 +- .../control/pure_pursuit_controller.py | 28 +- code/control/control/vehicle_controller.py | 190 ++++++++--- code/control/control/velocity_controller.py | 28 +- code/control/launch/control.xml | 12 - .../launch/ros_bridge.dev.xml | 2 +- code/mapping/mapping/data_integration.py | 28 +- code/paf_common/paf_common/sync.py | 105 ++++++ code/planning/planning/local_planner/ACC.py | 36 ++- .../planning/local_planner/motion_planning.py | 29 +- code/test/test_deterministic_sync.py | 107 +++++++ code/test/test_launch_manifests.py | 2 + doc/control/architecture_documentation.md | 8 +- doc/control/vehicle_controller.md | 30 +- ...eterministic-simulation-synchronization.md | 298 ++++++++++++++++++ 20 files changed, 937 insertions(+), 105 deletions(-) create mode 100644 code/agent/agent/startup_coordinator.py create mode 100644 code/paf_common/paf_common/sync.py create mode 100644 code/test/test_deterministic_sync.py create mode 100644 doc/reasoning/2026-04-28-deterministic-simulation-synchronization.md diff --git a/code/acting/acting/passthrough.py b/code/acting/acting/passthrough.py index add9c289..1d3fa468 100755 --- a/code/acting/acting/passthrough.py +++ b/code/acting/acting/passthrough.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -from std_msgs.msg import Float32 +from std_msgs.msg import Bool, Float32 from geometry_msgs.msg import PoseStamped from nav_msgs.msg import Path @@ -11,8 +11,11 @@ import rclpy from rclpy.node import Node from rclpy.publisher import Publisher +from rclpy.qos import DurabilityPolicy, QoSProfile from rclpy.subscription import Subscription +from paf_common.sync import startup_topic + @dataclass class TopicMapping: @@ -74,6 +77,13 @@ def __init__(self): self.pt_publishers: Dict[str, Publisher] = {} self.pt_subscribers: Dict[str, Subscription] = {} + self.startup_ready_pub = self.create_publisher( + Bool, + startup_topic(self.role_name, "passthrough"), + qos_profile=QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL + ), + ) for topic in self.mapped_topics: self.pt_publishers[topic.pub_name] = self.create_publisher( topic.topic_type, topic.pub_name, qos_profile=1 @@ -86,6 +96,7 @@ def __init__(self): qos_profile=1, ) + self.startup_ready_pub.publish(Bool(data=True)) self.get_logger().info(f"{type(self).__name__} node initialized.") diff --git a/code/agent/agent/data_management_node.py b/code/agent/agent/data_management_node.py index a312e062..d11affae 100644 --- a/code/agent/agent/data_management_node.py +++ b/code/agent/agent/data_management_node.py @@ -1,7 +1,6 @@ from typing import Optional import rclpy -import rclpy.clock from rclpy.node import Node from rclpy.service import Service from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy @@ -9,6 +8,7 @@ from std_msgs.msg import Bool, String from carla_msgs.msg import CarlaRoute from planning_interfaces.srv import GetOpenDriveString, GetCarlaRoute +from paf_common.sync import startup_topic class DataManagement(Node): @@ -57,26 +57,16 @@ def __init__(self): Bool, f"/paf/{self.role_name}/data/planning/global_plan_updated", 10 ) - # Carla status publisher - self.status_pub = self.create_publisher( + self.startup_ready_pub = self.create_publisher( Bool, - f"/carla/{self.role_name}/status", + startup_topic(self.role_name, "data_management"), qos_profile=QoSProfile( depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL ), ) - # Periodically send out the status signal, - # because otherwise, the leaderboard does not start the simulation. - # This has to use system time, because the leaderboard - # only sends out clock signals AFTER the simulation has started. - system_clock = rclpy.clock.Clock(clock_type=rclpy.clock.ClockType.SYSTEM_TIME) - self.create_timer(0.5, self.publish_status, clock=system_clock) self.get_logger().info(f"{type(self).__name__} node initialized.") - def publish_status(self): - self.status_pub.publish(Bool(data=True)) - def open_drive_callback(self, data: String): self.get_logger().info("Received open drive data.") self.open_drive_string = data.data @@ -92,6 +82,7 @@ def open_drive_callback(self, data: String): f"Started {self.open_drive_service.service_name} service." ) self.open_drive_updated_pub.publish(Bool(data=True)) + self._publish_startup_ready_if_available() def get_open_drive_service( self, req: GetOpenDriveString.Request, res: GetOpenDriveString.Response @@ -118,6 +109,16 @@ def global_plan_callback(self, data: CarlaRoute): f"Started {self.global_plan_service.service_name} service." ) self.global_plan_updated_pub.publish(Bool(data=True)) + self._publish_startup_ready_if_available() + + def _publish_startup_ready_if_available(self) -> None: + if ( + self.open_drive_string is not None + and self.global_plan is not None + and self.open_drive_service is not None + and self.global_plan_service is not None + ): + self.startup_ready_pub.publish(Bool(data=True)) def get_global_plan_service( self, req: GetCarlaRoute.Request, res: GetCarlaRoute.Response diff --git a/code/agent/agent/startup_coordinator.py b/code/agent/agent/startup_coordinator.py new file mode 100644 index 00000000..f16674c6 --- /dev/null +++ b/code/agent/agent/startup_coordinator.py @@ -0,0 +1,90 @@ +"""Coordinate deterministic startup readiness before unblocking CARLA.""" + +from __future__ import annotations + +import rclpy +import rclpy.clock +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile +from std_msgs.msg import Bool + +from paf_common.sync import StartupReadinessTracker, normalize_sync_id, startup_topic + + +DEFAULT_REQUIRED_NODES = [ + "data_management", + "mapping", + "motion_planning", + "acc", + "passthrough", + "pure_pursuit", + "velocity_controller", + "vehicle_controller", + "behavior_tree", +] + + +class StartupCoordinator(Node): + """Wait for the required startup nodes before publishing CARLA status.""" + + def __init__(self): + super().__init__("startup_coordinator") + self.get_logger().info(f"{type(self).__name__} node initializing...") + + self.role_name = self.declare_parameter("role_name", "hero").value + required_nodes = self.declare_parameter( + "required_nodes", DEFAULT_REQUIRED_NODES + ).value + self.tracker = StartupReadinessTracker(required_nodes) + self._published_ready_once = False + + self.status_pub = self.create_publisher( + Bool, + f"/carla/{self.role_name}/status", + qos_profile=QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL + ), + ) + + startup_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) + for node_id in self.tracker.required_nodes: + self.create_subscription( + Bool, + startup_topic(self.role_name, node_id), + lambda msg, current_node_id=node_id: self._startup_callback( + current_node_id, msg + ), + startup_qos, + ) + + system_clock = rclpy.clock.Clock(clock_type=rclpy.clock.ClockType.SYSTEM_TIME) + self.create_timer(0.5, self._republish_status_if_ready, clock=system_clock) + self.get_logger().info(f"{type(self).__name__} node initialized.") + + def _startup_callback(self, node_id: str, msg: Bool) -> None: + normalized = normalize_sync_id(node_id) + self.tracker.update(normalized, msg.data) + if self.tracker.all_ready() and not self._published_ready_once: + self.get_logger().info( + "All required startup nodes are ready. Releasing CARLA status." + ) + self.status_pub.publish(Bool(data=True)) + self._published_ready_once = True + + def _republish_status_if_ready(self) -> None: + if self.tracker.all_ready(): + self.status_pub.publish(Bool(data=True)) + + +def main(args=None): + rclpy.init(args=args) + + try: + node = StartupCoordinator() + rclpy.spin(node) + except KeyboardInterrupt: + pass + + +if __name__ == "__main__": + main() diff --git a/code/agent/launch/agent.dev.persistent.xml b/code/agent/launch/agent.dev.persistent.xml index 749a8863..b10b80cd 100644 --- a/code/agent/launch/agent.dev.persistent.xml +++ b/code/agent/launch/agent.dev.persistent.xml @@ -11,6 +11,10 @@ + + + + diff --git a/code/agent/setup.py b/code/agent/setup.py index aa390b70..a55313ae 100644 --- a/code/agent/setup.py +++ b/code/agent/setup.py @@ -24,6 +24,7 @@ entry_points={ "console_scripts": [ "data_management_node = agent.data_management_node:main", + "startup_coordinator = agent.startup_coordinator:main", ], }, ) diff --git a/code/control/config/control.yaml b/code/control/config/control.yaml index a769bece..ec397a30 100644 --- a/code/control/config/control.yaml +++ b/code/control/config/control.yaml @@ -3,8 +3,8 @@ vehicle_controller: manual_override_active: False manual_steer: 0.0 manual_throttle: 0.0 - - loop_sleep_time: 0.2 + sync_frame_delta_seconds: 0.05 + frame_barrier_timeout: 1.0 velocity_controller: ros__parameters: diff --git a/code/control/control/pure_pursuit_controller.py b/code/control/control/pure_pursuit_controller.py index 3afafda8..de136649 100755 --- a/code/control/control/pure_pursuit_controller.py +++ b/code/control/control/pure_pursuit_controller.py @@ -5,11 +5,12 @@ import rclpy from rclpy.node import Node from rclpy.publisher import Publisher +from rclpy.qos import DurabilityPolicy, QoSProfile from rclpy.subscription import Subscription from carla_msgs.msg import CarlaSpeedometer from nav_msgs.msg import Path -from std_msgs.msg import Float32 +from std_msgs.msg import Bool, Float32, UInt64 from visualization_msgs.msg import MarkerArray from rclpy.parameter import Parameter from rcl_interfaces.msg import ParameterDescriptor, FloatingPointRange @@ -21,6 +22,7 @@ from paf_common.parameters import update_attributes from paf_common.exceptions import emsg_with_trace +from paf_common.sync import frame_complete_topic, frame_id_from_time_ns, startup_topic # Constant: wheelbase of car @@ -38,6 +40,9 @@ def __init__(self): # Configuration parameters self.control_loop_rate = self.declare_parameter("control_loop_rate", 0.05).value self.role_name = self.declare_parameter("role_name", "hero").value + self.sync_frame_delta_seconds = self.declare_parameter( + "sync_frame_delta_seconds", 0.05 + ).value self.k_lad = self.declare_parameter( "k_lad", @@ -101,12 +106,25 @@ def __init__(self): f"/paf/{self.role_name}/control/pp_debug_markers", qos_profile=1, ) + self.frame_complete_pub: Publisher = self.create_publisher( + UInt64, + frame_complete_topic(self.role_name, "pure_pursuit"), + qos_profile=10, + ) + self.startup_ready_pub: Publisher = self.create_publisher( + Bool, + startup_topic(self.role_name, "pure_pursuit"), + qos_profile=QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL + ), + ) self.__path: Optional[Path] = None self.__velocity: Optional[float] = None self.loop_timer = self.create_timer(self.control_loop_rate, self.loop_handler) self.add_on_set_parameters_callback(self._set_parameters_callback) + self.startup_ready_pub.publish(Bool(data=True)) self.get_logger().info(f"{type(self).__name__} node initialized.") def _set_parameters_callback(self, params: List[Parameter]): @@ -140,6 +158,14 @@ def loop(self): ) else: self.pure_pursuit_steer_pub.publish(Float32(data=steering_angle)) + self.frame_complete_pub.publish( + UInt64( + data=frame_id_from_time_ns( + self.get_clock().now().nanoseconds, + self.sync_frame_delta_seconds, + ) + ) + ) def loop_handler(self): try: diff --git a/code/control/control/vehicle_controller.py b/code/control/control/vehicle_controller.py index bbf8e51c..6c9ac795 100755 --- a/code/control/control/vehicle_controller.py +++ b/code/control/control/vehicle_controller.py @@ -1,9 +1,10 @@ import math import time -from typing import List +from typing import List, Optional from carla_msgs.msg import CarlaEgoVehicleControl, CarlaSpeedometer import rclpy +import rclpy.clock import rclpy.time from rclpy.node import Node from rclpy.qos import QoSProfile, DurabilityPolicy @@ -12,25 +13,36 @@ ParameterDescriptor, FloatingPointRange, ) -from std_msgs.msg import Bool, Float32, String +from std_msgs.msg import Bool, Float32, String, UInt64 from rosgraph_msgs.msg import Clock from paf_common.parameters import update_attributes from paf_common.exceptions import emsg_with_trace +from paf_common.sync import ( + FrameBarrier, + frame_complete_topic, + frame_id_from_time_ns, + startup_topic, +) + + +DEFAULT_REQUIRED_SYNC_STAGES = [ + "mapping", + "motion_planning", + "acc", + "pure_pursuit", + "velocity_controller", +] class VehicleController(Node): """ This node is responsible for collecting all data needed for the - vehicle_control_cmd and sending it. + vehicle_control_cmd and sending it only after the required stages + completed the current simulation frame. The node uses the pure pursuit controller for steering. If the node receives an emergency msg, it will bring the vehicle to a stop and send an emergency msg with data = False back, after the velocity of the vehicle has reached 0. - INFO: Currently the loop of the node has a sleep command in it. The control - command triggers the carla simulator to render the next frame. If the loop - does not have the time to sleep the simulator will run as fast as the system - allows it to run. If your system is too slow to run with the 0.2 loop_sleep_time - you could slow it down by setting the loop_sleep_time to a higher value. """ def __init__(self): @@ -40,17 +52,23 @@ def __init__(self): # Configuration parameters self.control_loop_rate = self.declare_parameter("control_loop_rate", 0.05).value self.role_name = self.declare_parameter("role_name", "hero").value - self.loop_sleep_time = self.declare_parameter( - "loop_sleep_time", - 0.2, + self.sync_frame_delta_seconds = self.declare_parameter( + "sync_frame_delta_seconds", 0.05 + ).value + self.frame_barrier_timeout = self.declare_parameter( + "frame_barrier_timeout", + 1.0, descriptor=ParameterDescriptor( - description="This sleep time is used to slow down the vehicle " - "controller to a reasonable speed", + description="Max wall-clock time to wait for the current frame's " + "stage completions before publishing a safe stop command.", floating_point_range=[ - FloatingPointRange(from_value=0.05, to_value=0.4, step=0.01) + FloatingPointRange(from_value=0.1, to_value=10.0, step=0.1) ], ), ).value + self.required_sync_stages = self.declare_parameter( + "required_sync_stages", DEFAULT_REQUIRED_SYNC_STAGES + ).value # Manual control self.manual_override_active = self.declare_parameter( "manual_override_active", @@ -85,6 +103,9 @@ def __init__(self): self.__brake = 0.0 self.__throttle = 0.0 self._p_steer = 0.0 + self.frame_barrier = FrameBarrier(self.required_sync_stages) + self.pending_clock: Optional[Clock] = None + self.last_published_frame_id: int = -1 # Initialize publishers self.control_publisher = self.create_publisher( @@ -99,6 +120,13 @@ def __init__(self): depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL ), ) + self.startup_ready_pub = self.create_publisher( + Bool, + startup_topic(self.role_name, "vehicle_controller"), + qos_profile=QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL + ), + ) self.emergency_pub = self.create_publisher( Bool, f"/paf/{self.role_name}/emergency", @@ -145,28 +173,38 @@ def __init__(self): qos_profile=1, ) - # Control message - self.message = CarlaEgoVehicleControl() - self.clock_sub = self.create_subscription(Clock, "/clock", self.loop_handler, 1) + for stage_id in self.frame_barrier.required_stages: + self.create_subscription( + UInt64, + frame_complete_topic(self.role_name, stage_id), + lambda msg, current_stage_id=stage_id: self._stage_complete_callback( + current_stage_id, msg + ), + qos_profile=1, + ) + system_clock = rclpy.clock.Clock(clock_type=rclpy.clock.ClockType.SYSTEM_TIME) + self.create_timer(0.05, self._frame_timeout_handler, clock=system_clock) self.add_on_set_parameters_callback(self._set_parameters_callback) + self.startup_ready_pub.publish(Bool(data=True)) self.get_logger().info(f"{type(self).__name__} node initialized.") def _set_parameters_callback(self, params: List[Parameter]): """Callback for parameter updates.""" return update_attributes(self, params) - def update_control_message(self): - """Update the control message based on the current state.""" + def build_control_message(self) -> CarlaEgoVehicleControl: + """Build the control message based on the current state.""" + message = CarlaEgoVehicleControl() if self.manual_override_active: - self.message.reverse = self.manual_throttle < 0 - self.message.throttle = abs(self.manual_throttle) - self.message.steer = self.manual_steer - self.message.brake = 0.0 - self.message.hand_brake = False + message.reverse = self.manual_throttle < 0 + message.throttle = abs(self.manual_throttle) + message.steer = self.manual_steer + message.brake = 0.0 + message.hand_brake = False elif self.__emergency: - self.__emergency_brake(True) + self._apply_emergency_brake(message) else: steer = ( self._p_steer @@ -174,12 +212,14 @@ def update_control_message(self): else -self._p_steer ) - self.message.reverse = self.__reverse - self.message.throttle = self.__throttle - self.message.brake = self.__brake - self.message.steer = steer - self.message.hand_brake = False - self.message.manual_gear_shift = False + message.reverse = self.__reverse + message.throttle = self.__throttle + message.brake = self.__brake + message.steer = steer + message.hand_brake = False + message.manual_gear_shift = False + + return message # Subscriber callbacks def __set_curr_behavior(self, data: String): @@ -193,7 +233,7 @@ def __set_emergency(self, data: Bool): def __get_velocity(self, data: CarlaSpeedometer): self.__velocity = data.speed if self.__emergency and data.speed < 0.1: - self.__emergency_brake(False) + self.__emergency = False for _ in range(7): self.emergency_pub.publish(Bool(data=False)) self.get_logger().info("Emergency braking disengaged") @@ -210,27 +250,79 @@ def __set_reverse(self, data: Bool): def __set_pure_pursuit_steer(self, data: Float32): self._p_steer = data.data / (math.pi / 2) - def __emergency_brake(self, active: bool): - if active: - self.message.throttle = 0.0 - self.message.steer = 0.0 - self.message.brake = 1.0 - self.message.reverse = False - self.message.hand_brake = True - else: - self.__emergency = False - self.message.brake = 0.0 - self.message.hand_brake = False + def _apply_emergency_brake(self, message: CarlaEgoVehicleControl) -> None: + message.throttle = 0.0 + message.steer = 0.0 + message.brake = 1.0 + message.reverse = False + message.hand_brake = True + + def _build_safe_stop_message(self) -> CarlaEgoVehicleControl: + message = CarlaEgoVehicleControl() + message.throttle = 0.0 + message.steer = 0.0 + message.brake = 1.0 + message.reverse = False + message.hand_brake = True + message.manual_gear_shift = False + return message def loop(self, clock: Clock): - """Main control loop""" - self.update_control_message() - self.message.header.stamp = rclpy.time.Time( + """Begin a new pending simulation frame and wait for stage completion.""" + clock_ns = clock.clock.sec * 1_000_000_000 + clock.clock.nanosec + frame_id = frame_id_from_time_ns(clock_ns, self.sync_frame_delta_seconds) + if frame_id <= self.last_published_frame_id: + return + + self.pending_clock = clock + self.frame_barrier.begin_frame(frame_id, time.monotonic()) + self._try_publish_pending_frame() + + def _stage_complete_callback(self, stage_id: str, data: UInt64) -> None: + self.frame_barrier.mark_stage_complete(stage_id, int(data.data)) + self._try_publish_pending_frame() + + def _try_publish_pending_frame(self) -> None: + pending_frame_id = self.frame_barrier.pending_frame_id + if pending_frame_id is None or self.pending_clock is None: + return + if not self.frame_barrier.is_ready(pending_frame_id): + return + + self._publish_control_message(self.build_control_message(), self.pending_clock) + + def _frame_timeout_handler(self) -> None: + pending_frame_id = self.frame_barrier.pending_frame_id + if pending_frame_id is None or self.pending_clock is None: + return + if not self.frame_barrier.timed_out( + time.monotonic(), self.frame_barrier_timeout + ): + return + + missing_stages = self.frame_barrier.missing_stages(pending_frame_id) + self.get_logger().warn( + "Frame barrier timeout for frame " + f"{pending_frame_id}: missing stages {missing_stages}. " + "Publishing safe stop command.", + throttle_duration_sec=1.0, + ) + self._publish_control_message( + self._build_safe_stop_message(), + self.pending_clock, + ) + + def _publish_control_message( + self, message: CarlaEgoVehicleControl, clock: Clock + ) -> None: + message.header.stamp = rclpy.time.Time( seconds=clock.clock.sec, nanoseconds=clock.clock.nanosec ).to_msg() - self.control_publisher.publish(self.message) - - time.sleep(self.loop_sleep_time) + self.control_publisher.publish(message) + if self.frame_barrier.pending_frame_id is not None: + self.last_published_frame_id = self.frame_barrier.pending_frame_id + self.frame_barrier.clear_pending() + self.pending_clock = None def loop_handler(self, clock: Clock): try: diff --git a/code/control/control/velocity_controller.py b/code/control/control/velocity_controller.py index a5846b7a..8c285383 100755 --- a/code/control/control/velocity_controller.py +++ b/code/control/control/velocity_controller.py @@ -3,12 +3,14 @@ import rclpy from rclpy.node import Node from rclpy.publisher import Publisher +from rclpy.qos import DurabilityPolicy, QoSProfile from rclpy.subscription import Subscription from simple_pid import PID -from std_msgs.msg import Float32, Bool +from std_msgs.msg import Float32, Bool, UInt64 from rcl_interfaces.msg import ParameterDescriptor, FloatingPointRange from paf_common.parameters import update_attributes from paf_common.exceptions import emsg_with_trace +from paf_common.sync import frame_complete_topic, frame_id_from_time_ns, startup_topic from rclpy.parameter import Parameter @@ -27,6 +29,9 @@ def __init__(self): 0.05, ).value self.role_name = self.declare_parameter("role_name", "hero").value + self.sync_frame_delta_seconds = self.declare_parameter( + "sync_frame_delta_seconds", 0.05 + ).value self.fixed_speed = self.declare_parameter( "fixed_speed", @@ -102,6 +107,18 @@ def __init__(self): self.reverse_pub: Publisher = self.create_publisher( Bool, f"/paf/{self.role_name}/reverse", qos_profile=1 ) + self.frame_complete_pub: Publisher = self.create_publisher( + UInt64, + frame_complete_topic(self.role_name, "velocity_controller"), + qos_profile=10, + ) + self.startup_ready_pub: Publisher = self.create_publisher( + Bool, + startup_topic(self.role_name, "velocity_controller"), + qos_profile=QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL + ), + ) self.__current_velocity: Optional[float] = None self.__target_velocity: Optional[float] = None @@ -115,6 +132,7 @@ def __init__(self): self.loop_timer = self.create_timer(self.control_loop_rate, self.loop_handler) self.add_on_set_parameters_callback(self._set_parameters_callback) + self.startup_ready_pub.publish(Bool(data=True)) self.get_logger().info(f"{type(self).__name__} node initialized.") def _set_parameters_callback(self, params: List[Parameter]): @@ -182,6 +200,14 @@ def loop(self): self.reverse_pub.publish(Bool(data=reverse)) self.brake_pub.publish(Float32(data=float(brake))) self.throttle_pub.publish(Float32(data=float(throttle))) + self.frame_complete_pub.publish( + UInt64( + data=frame_id_from_time_ns( + self.get_clock().now().nanoseconds, + self.sync_frame_delta_seconds, + ) + ) + ) def loop_handler(self): try: diff --git a/code/control/launch/control.xml b/code/control/launch/control.xml index fbdf8ba5..79b9092f 100644 --- a/code/control/launch/control.xml +++ b/code/control/launch/control.xml @@ -1,17 +1,6 @@ - - @@ -27,7 +16,6 @@ - diff --git a/code/leaderboard_launcher/launch/ros_bridge.dev.xml b/code/leaderboard_launcher/launch/ros_bridge.dev.xml index 73799a1c..1f0a3ead 100644 --- a/code/leaderboard_launcher/launch/ros_bridge.dev.xml +++ b/code/leaderboard_launcher/launch/ros_bridge.dev.xml @@ -14,7 +14,7 @@ - + diff --git a/code/mapping/mapping/data_integration.py b/code/mapping/mapping/data_integration.py index 7ef77bee..b212f8bd 100755 --- a/code/mapping/mapping/data_integration.py +++ b/code/mapping/mapping/data_integration.py @@ -6,10 +6,12 @@ import rclpy from rclpy.node import Node from rclpy.parameter import Parameter +from rclpy.qos import DurabilityPolicy, QoSProfile import ros2_numpy from paf_common.parameters import update_attributes from paf_common.exceptions import emsg_with_trace +from paf_common.sync import frame_complete_topic, frame_id_from_time_ns, startup_topic import mapping_common.map import mapping_common.hero from mapping_common.entity import Entity, Flags, Car, Motion2D, Pedestrian, StopMark @@ -31,7 +33,7 @@ ParameterDescriptor, FloatingPointRange, ) -from std_msgs.msg import Float32 +from std_msgs.msg import Bool, Float32, UInt64 from geometry_msgs.msg import PoseStamped from sensor_msgs.msg import PointCloud2 from carla_msgs.msg import CarlaSpeedometer @@ -124,6 +126,9 @@ def __init__(self): # Parameters self.map_publish_rate = self.declare_parameter("map_publish_rate", 0.05).value + self.sync_frame_delta_seconds = self.declare_parameter( + "sync_frame_delta_seconds", 0.05 + ).value # Parameters: Enable entity sources @@ -412,9 +417,22 @@ def __init__(self): topic="/paf/hero/mapping/clusterpoints", qos_profile=1, ) + self.startup_ready_pub = self.create_publisher( + Bool, + startup_topic("hero", "mapping"), + qos_profile=QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL + ), + ) + self.frame_complete_pub = self.create_publisher( + UInt64, + frame_complete_topic("hero", "mapping"), + qos_profile=10, + ) self.create_timer(self.map_publish_rate, self.publish_new_map_handler) self.add_on_set_parameters_callback(self._set_parameters_callback) + self.startup_ready_pub.publish(Bool(data=True)) self.get_logger().info(f"{type(self).__name__} node initialized.") def _set_parameters_callback(self, params: List[Parameter]): @@ -835,6 +853,14 @@ def publish_new_map(self): msg = map.to_ros_msg() self.map_publisher.publish(msg) + self.frame_complete_pub.publish( + UInt64( + data=frame_id_from_time_ns( + self.get_clock().now().nanoseconds, + self.sync_frame_delta_seconds, + ) + ) + ) def get_current_map_filters(self) -> List[MapFilter]: """Creates an array of filters for the Map diff --git a/code/paf_common/paf_common/sync.py b/code/paf_common/paf_common/sync.py new file mode 100644 index 00000000..9aaf9584 --- /dev/null +++ b/code/paf_common/paf_common/sync.py @@ -0,0 +1,105 @@ +"""Helpers for deterministic startup and frame synchronization.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Iterable, Optional + + +def normalize_sync_id(value: str) -> str: + """Normalize node and stage ids so topic names stay stable.""" + return value.strip().lower().replace(" ", "_").replace("-", "_") + + +def startup_topic(role_name: str, node_id: str) -> str: + """Topic on which a node publishes its startup readiness.""" + normalized = normalize_sync_id(node_id) + return f"/paf/{role_name}/sync/startup/{normalized}" + + +def frame_complete_topic(role_name: str, stage_id: str) -> str: + """Topic on which a stage reports its current completed frame id.""" + normalized = normalize_sync_id(stage_id) + return f"/paf/{role_name}/sync/frame/{normalized}/completed" + + +def frame_id_from_time_ns(time_ns: int, frame_delta_seconds: float) -> int: + """Map a simulation time value onto a monotonic frame id.""" + if frame_delta_seconds <= 0.0: + raise ValueError("frame_delta_seconds must be positive") + + frame_delta_ns = max(1, int(frame_delta_seconds * 1_000_000_000)) + if time_ns <= 0: + return 0 + return time_ns // frame_delta_ns + + +@dataclass +class StartupReadinessTracker: + """Track whether the required startup nodes have reported readiness.""" + + required_nodes: Iterable[str] + ready_by_node: dict[str, bool] = field(default_factory=dict) + + def __post_init__(self) -> None: + self.required_nodes = tuple( + normalize_sync_id(node) for node in self.required_nodes + ) + self.ready_by_node = {node: False for node in self.required_nodes} + + def update(self, node_id: str, is_ready: bool) -> None: + self.ready_by_node[normalize_sync_id(node_id)] = is_ready + + def missing_nodes(self) -> list[str]: + return [node for node, ready in self.ready_by_node.items() if not ready] + + def all_ready(self) -> bool: + return all(self.ready_by_node.get(node, False) for node in self.required_nodes) + + +@dataclass +class FrameBarrier: + """Track which stages completed for the currently pending frame.""" + + required_stages: Iterable[str] + completed_by_stage: dict[str, int] = field(default_factory=dict) + pending_frame_id: Optional[int] = None + pending_started_at: Optional[float] = None + + def __post_init__(self) -> None: + self.required_stages = tuple( + normalize_sync_id(stage) for stage in self.required_stages + ) + self.completed_by_stage = {stage: -1 for stage in self.required_stages} + + def begin_frame(self, frame_id: int, started_at: float) -> None: + if self.pending_frame_id is None or frame_id > self.pending_frame_id: + self.pending_frame_id = frame_id + self.pending_started_at = started_at + + def mark_stage_complete(self, stage_id: str, frame_id: int) -> None: + normalized = normalize_sync_id(stage_id) + last_completed = self.completed_by_stage.get(normalized, -1) + self.completed_by_stage[normalized] = max(last_completed, frame_id) + + def missing_stages(self, frame_id: Optional[int] = None) -> list[str]: + current_frame = self.pending_frame_id if frame_id is None else frame_id + if current_frame is None: + return list(self.required_stages) + return [ + stage + for stage in self.required_stages + if self.completed_by_stage.get(stage, -1) < current_frame + ] + + def is_ready(self, frame_id: Optional[int] = None) -> bool: + return not self.missing_stages(frame_id) + + def timed_out(self, now: float, timeout_seconds: float) -> bool: + if self.pending_frame_id is None or self.pending_started_at is None: + return False + return (now - self.pending_started_at) >= timeout_seconds + + def clear_pending(self) -> None: + self.pending_frame_id = None + self.pending_started_at = None diff --git a/code/planning/planning/local_planner/ACC.py b/code/planning/planning/local_planner/ACC.py index bc08abdb..699ab904 100755 --- a/code/planning/planning/local_planner/ACC.py +++ b/code/planning/planning/local_planner/ACC.py @@ -13,7 +13,7 @@ from paf_common.parameters import update_attributes from nav_msgs.msg import Path -from std_msgs.msg import Float32, String, Bool +from std_msgs.msg import Float32, String, Bool, UInt64 from visualization_msgs.msg import Marker, MarkerArray import mapping_common.mask @@ -24,6 +24,7 @@ from mapping_interfaces.msg import Map as MapMsg from planning_interfaces.srv import SpeedAlteration +from paf_common.sync import frame_complete_topic, frame_id_from_time_ns, startup_topic MARKER_NAMESPACE: str = "acc" ACC_MARKER_COLOR = (0.0, 1.0, 1.0, 0.5) @@ -52,6 +53,9 @@ def __init__(self): # Parameters self.role_name = self.declare_parameter("role_name", "hero").value + self.sync_frame_delta_seconds = self.declare_parameter( + "sync_frame_delta_seconds", 0.05 + ).value self.k_p = self.declare_parameter( "k_p", @@ -222,6 +226,18 @@ def __init__(self): self.velocity_pub: Publisher = self.create_publisher( Float32, f"/paf/{self.role_name}/acc_velocity", qos_profile=1 ) + self.frame_complete_pub: Publisher = self.create_publisher( + UInt64, + frame_complete_topic(self.role_name, "acc"), + qos_profile=10, + ) + self.startup_ready_pub: Publisher = self.create_publisher( + Bool, + startup_topic(self.role_name, "acc"), + qos_profile=QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL + ), + ) # Publish to emergency break if needed self.emergency_pub = self.create_publisher( @@ -244,6 +260,7 @@ def __init__(self): ) self.add_on_set_parameters_callback(self._set_parameters_callback) + self.startup_ready_pub.publish(Bool(data=True)) self.get_logger().info(f"{type(self).__name__} node initialized.") def _set_parameters_callback(self, params: List[Parameter]): @@ -288,7 +305,7 @@ def update_velocity(self): """ if self.map is None or self.trajectory_local is None: # We don't have the necessary data to drive safely - self.velocity_pub.publish(Float32(data=0.0)) + self._publish_velocity(0.0) return hero = self.map.hero() @@ -296,7 +313,7 @@ def update_velocity(self): # We currenly have no hero data. # -> cannot drive safely self.get_logger().error("ACC: No hero with motion found in map!") - self.velocity_pub.publish(Float32(data=0.0)) + self._publish_velocity(0.0) return hero_width = max(1.0, hero.get_width()) @@ -429,13 +446,24 @@ def filter_fn(e: Entity) -> bool: ) ) - self.velocity_pub.publish(Float32(data=desired_speed)) + self._publish_velocity(desired_speed) marker_array = debug_marker_array( MARKER_NAMESPACE, debug_markers, self.get_clock().now().to_msg() ) self.marker_publisher.publish(marker_array) + def _publish_velocity(self, desired_speed: float) -> None: + self.velocity_pub.publish(Float32(data=desired_speed)) + self.frame_complete_pub.publish( + UInt64( + data=frame_id_from_time_ns( + self.get_clock().now().nanoseconds, + self.sync_frame_delta_seconds, + ) + ) + ) + def calculate_velocity_based_on_lead( self, hero_velocity: float, lead_distance: float, delta_v: float ) -> float: diff --git a/code/planning/planning/local_planner/motion_planning.py b/code/planning/planning/local_planner/motion_planning.py index ce19b8e4..870779c3 100755 --- a/code/planning/planning/local_planner/motion_planning.py +++ b/code/planning/planning/local_planner/motion_planning.py @@ -11,10 +11,11 @@ import rclpy.callback_groups from rclpy.node import Node from rclpy.publisher import Publisher +from rclpy.qos import DurabilityPolicy, QoSProfile from geometry_msgs.msg import Pose, PoseStamped from nav_msgs.msg import Path -from std_msgs.msg import Float32, Float32MultiArray, Bool +from std_msgs.msg import Float32, Float32MultiArray, Bool, UInt64 from planning_interfaces.srv import ( StartOvertake, EndOvertake, @@ -28,6 +29,7 @@ from rclpy.duration import Duration from paf_common.parameters import update_attributes +from paf_common.sync import frame_complete_topic, frame_id_from_time_ns, startup_topic import mapping_common.hero import mapping_common.mask @@ -73,6 +75,9 @@ def __init__(self): mapping_common.set_logger(self.get_logger()) self.role_name = self.declare_parameter("role_name", "hero").value + self.sync_frame_delta_seconds = self.declare_parameter( + "sync_frame_delta_seconds", 0.05 + ).value self.time_horizon = self.declare_parameter( "time_horizon", @@ -185,6 +190,18 @@ def __init__(self): self.marker_publisher: Publisher = self.create_publisher( MarkerArray, "/paf/hero/planning/collision_trajectories", qos_profile=1 ) + self.frame_complete_pub: Publisher = self.create_publisher( + UInt64, + frame_complete_topic(self.role_name, "motion_planning"), + qos_profile=10, + ) + self.startup_ready_pub: Publisher = self.create_publisher( + Bool, + startup_topic(self.role_name, "motion_planning"), + qos_profile=QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL + ), + ) # Service clients self.client_callback_group = ( @@ -210,6 +227,7 @@ def __init__(self): self.counter = 0 self.current_map = None self.hero_transform: Transform2D | None = None + self.startup_ready_pub.publish(Bool(data=True)) self.get_logger().info(f"{type(self).__name__} node initialized.") def _set_parameters_callback(self, params: List[Parameter]): @@ -389,6 +407,15 @@ async def publish_local_trajectory(self): throttle_duration_sec=1.0, ) + self.frame_complete_pub.publish( + UInt64( + data=frame_id_from_time_ns( + self.get_clock().now().nanoseconds, + self.sync_frame_delta_seconds, + ) + ) + ) + def check_trajectory_collisions( self, ego_vehicle_trajectory: LineString, diff --git a/code/test/test_deterministic_sync.py b/code/test/test_deterministic_sync.py new file mode 100644 index 00000000..1e10c712 --- /dev/null +++ b/code/test/test_deterministic_sync.py @@ -0,0 +1,107 @@ +"""Unit tests for deterministic startup and frame synchronization helpers.""" + +from __future__ import annotations + +import sys +from pathlib import Path +import xml.etree.ElementTree as ET +import importlib + +import pytest + +pytestmark = pytest.mark.unit + +CODE_ROOT = Path(__file__).resolve().parents[1] +PAF_COMMON_SRC = CODE_ROOT / "paf_common" +if str(PAF_COMMON_SRC) not in sys.path: + sys.path.insert(0, str(PAF_COMMON_SRC)) + + +@pytest.fixture(scope="module") +def sync_helpers(): + return importlib.import_module("paf_common.sync") + + +def test_startup_readiness_tracker_requires_all_nodes(sync_helpers) -> None: + tracker = sync_helpers.StartupReadinessTracker(["data_management", "mapping"]) + + assert not tracker.all_ready() + assert tracker.missing_nodes() == ["data_management", "mapping"] + + tracker.update("data_management", True) + assert not tracker.all_ready() + assert tracker.missing_nodes() == ["mapping"] + + tracker.update("mapping", True) + assert tracker.all_ready() + assert tracker.missing_nodes() == [] + + +def test_frame_barrier_requires_current_frame_completion(sync_helpers) -> None: + barrier = sync_helpers.FrameBarrier(["mapping", "acc", "velocity_controller"]) + barrier.begin_frame(12, started_at=1.0) + + barrier.mark_stage_complete("mapping", 12) + barrier.mark_stage_complete("acc", 11) + barrier.mark_stage_complete("velocity_controller", 12) + + assert not barrier.is_ready() + assert barrier.missing_stages() == ["acc"] + + barrier.mark_stage_complete("acc", 12) + assert barrier.is_ready() + + +def test_frame_barrier_timeout_and_clear_pending(sync_helpers) -> None: + barrier = sync_helpers.FrameBarrier(["mapping"]) + barrier.begin_frame(3, started_at=5.0) + + assert not barrier.timed_out(5.4, 0.5) + assert barrier.timed_out(5.5, 0.5) + + barrier.clear_pending() + assert barrier.pending_frame_id is None + assert barrier.pending_started_at is None + + +@pytest.mark.parametrize( + ("time_ns", "expected_frame_id"), + [ + (0, 0), + (50_000_000, 1), + (99_999_999, 1), + (100_000_000, 2), + ], +) +def test_frame_id_from_time_ns_uses_fixed_delta( + sync_helpers, time_ns: int, expected_frame_id: int +) -> None: + assert sync_helpers.frame_id_from_time_ns(time_ns, 0.05) == expected_frame_id + + +def test_sync_contract_files_reflect_barrier_design() -> None: + control_xml = (CODE_ROOT / "control/launch/control.xml").read_text() + control_yaml = (CODE_ROOT / "control/config/control.yaml").read_text() + ros_bridge_root = ET.parse( + CODE_ROOT / "leaderboard_launcher/launch/ros_bridge.dev.xml" + ).getroot() + persistent_launch_root = ET.parse( + CODE_ROOT / "agent/launch/agent.dev.persistent.xml" + ).getroot() + + assert "loop_sleep_time" not in control_xml + assert "loop_sleep_time" not in control_yaml + assert "frame_barrier_timeout" in control_yaml + assert "sync_frame_delta_seconds" in control_yaml + + wait_for_command_arg = next( + arg + for arg in ros_bridge_root.findall("arg") + if arg.get("name") == "synchronous_mode_wait_for_vehicle_control_command" + ) + assert wait_for_command_arg.get("default") == "True" + + startup_nodes = { + node.get("exec", "") for node in persistent_launch_root.findall("node") + } + assert "startup_coordinator" in startup_nodes diff --git a/code/test/test_launch_manifests.py b/code/test/test_launch_manifests.py index 15076475..b9796d21 100644 --- a/code/test/test_launch_manifests.py +++ b/code/test/test_launch_manifests.py @@ -52,8 +52,10 @@ def test_agent_persistent_launch_contains_localization() -> None: persistent_launch = CODE_ROOT / "agent/launch/agent.dev.persistent.xml" root = ET.parse(persistent_launch).getroot() included_files = {include.get("file", "") for include in root.findall("include")} + node_execs = {node.get("exec", "") for node in root.findall("node")} assert any("localization.xml" in file_path for file_path in included_files) assert any( "planning.dev.persistent.xml" in file_path for file_path in included_files ) + assert "startup_coordinator" in node_execs diff --git a/doc/control/architecture_documentation.md b/doc/control/architecture_documentation.md index cc4a420f..9946eb31 100644 --- a/doc/control/architecture_documentation.md +++ b/doc/control/architecture_documentation.md @@ -3,8 +3,8 @@ **Summary**: The control component applies control theory based on a local trajectory provided by the [acting component](./../acting/README.md). It uses knowledge of the current state -of the vehicle in order to send [CarlaEgoVehicleControl](https://carla.readthedocs.io/en/0.9.8/ros_msgs/#CarlaEgoVehicleControlmsg) commands to the Simulator. This component also sends the [/carla/hero/status](https://leaderboard.carla.org/get_started/) command, -which starts the simulation. +of the vehicle in order to send [CarlaEgoVehicleControl](https://carla.readthedocs.io/en/0.9.8/ros_msgs/#CarlaEgoVehicleControlmsg) commands to the Simulator. +Startup release for [/carla/hero/status](https://leaderboard.carla.org/get_started/) is now handled by the agent-side startup coordinator after the required persistent nodes report readiness. - [Control Architecture](#control-architecture) - [Summary of Control Components](#summary-of-control-components) @@ -65,7 +65,9 @@ which starts the simulation. - **throttle**: Float32 - **brake**: Float32 - **pure_pursuit_steer**: Float32 + - **frame completion side-channel**: UInt64 topics from mapping, motion planning, ACC, pure pursuit, and velocity controller - Outputs: - **vehicle_control_cmd**: [CarlaEgoVehicleControl](https://carla.readthedocs.io/en/0.9.8/ros_msgs/#CarlaEgoVehicleControlmsg) - - **status**: Bool - **emergency**: Bool + +The vehicle controller now acts as the final frame barrier for synchronous simulation. It releases one control command per simulation frame after the required upstream stages reported completion for that frame, and falls back to a safe stop if the barrier times out. diff --git a/doc/control/vehicle_controller.md b/doc/control/vehicle_controller.md index 585c10f1..8a1fc69b 100644 --- a/doc/control/vehicle_controller.md +++ b/doc/control/vehicle_controller.md @@ -10,12 +10,12 @@ ## General Introduction to the Vehicle Controller Component -The [Vehicle Controller](../../code/control/src/vehicle_controller.py) collects all information from the other controllers in Control ```throttle```, ```brake```, ```reverse```, ```pure_puresuit_steer``` -to fill them into the CARLA-Vehicle Command Message ```vehicle_control_cmd``` and send this to the CARLA simulator. +The [Vehicle Controller](../../code/control/control/vehicle_controller.py) collects the control outputs ```throttle```, ```brake```, ```reverse```, and ```pure_pursuit_steer``` +to fill the CARLA vehicle command message ```vehicle_control_cmd``` and send it to the CARLA simulator. -Currently the loop of the node has a sleep command in it. The control command triggers the carla simulator to render the next frame. -If the loop does not have the time to sleep the simulator will run as fast as the system allows it to run. -By default its set to 0.2 to run the controller at a reasonable speed +The controller no longer uses a sleep-based hotfix to pace the simulator. In the current synchronous setup, it waits until the critical upstream stages for the current frame report completion and only then publishes the final command for that frame. The required stages are currently mapping, motion planning, ACC, pure pursuit, and the velocity controller. + +If the barrier does not complete within the configured ```frame_barrier_timeout```, the controller publishes a safe stop command instead of releasing a stale or partial command. It also reacts to some special case - Messages from Planning, such as emergency-braking or executing the unstuck-routine. @@ -23,11 +23,11 @@ It also reacts to some special case - Messages from Planning, such as emergency- As the ```vehicle_control_cmd```-Message requires all 4 Inputs to be in the range of 0 to 1, the Vehicle Controller has to convert the steering signal ```pure_puresuit_steer``` from Radians to [0,1]. -The ```throttle``` and ```brake``` are already calculated in the correct range by the PID Controller of the [Velocity Controller](../../code/control/src/velocity_controller.py). +The ```throttle``` and ```brake``` are already calculated in the correct range by the PID controller of the [Velocity Controller](../../code/control/control/velocity_controller.py). -This output (vehicle command) has to be sent in the same frequency the leaderboard is expecting them, which currently is about ```20 Hz``` (every 0.05 seconds). +This output still has to be sent in the same frequency the leaderboard expects, which currently is about ```20 Hz``` (every 0.05 seconds). -If we send these commands in a lower frequency the leaderboard keeps waiting for an output, which leads to massive lags! +The difference is that the release condition is now frame completion on the critical path, not an arbitrary sleep delay inside the controller. ## Emergency Brake @@ -35,15 +35,15 @@ The Vehicle Controller also reacts to ```emergency```-Messages, published by Pla Once the ```emergency_sub``` receives an emergency message from ```paf/hero/emergency```, the ```__emergency``` attribute gets set to either True or stays False. -In case the ```__emergency``` attribute is set to True, the main loop of the vehicle controller ignores any other vehicle command and goes straight into the ```__emergency_brake``` method until the emergency is resolved. +In case the ```__emergency``` attribute is set to True, the main loop of the vehicle controller ignores any other vehicle command and publishes a full stop command until the emergency is resolved. -If an emergency is triggered the ```__emergency_brake``` method uses a little braking bug abuse, sending the following vehicle command: +If an emergency is triggered, the controller sends the following vehicle command: ```Python - message.throttle = 1 - message.steer = 1 + message.throttle = 0.0 + message.steer = 0.0 message.brake = 1 - message.reverse = True + message.reverse = False message.hand_brake = True message.manual_gear_shift = False ``` @@ -56,14 +56,12 @@ Comparison between normal braking and emergency braking: ![Braking Comparison](/doc/assets/control/emergency_brake_stats_graph.png) -_Please be aware, that this bug abuse might not work in newer updates!_ - ## Unstuck Routine The Vehicle Controller also reads ```current_behavior```-Messages, published by Planning, currently reacting to the **unstuck-behavior**: This is done to drive in a specific way whenever we get into a stuck situation and the [Unstuck Behavior](/doc/planning/behaviors/Unstuck.md) is persued. -Inside the Unstuck Behavior we want to drive backwards with inverted steering, which is why the steering angle published by [Pure Pursuit Controller](../../code/control/src/pure_pursuit_controller.py) gets inverted. +Inside the Unstuck Behavior we want to drive backwards with inverted steering, which is why the steering angle published by [Pure Pursuit Controller](../../code/control/control/pure_pursuit_controller.py) gets inverted. ### Last updated 22.03.2025 diff --git a/doc/reasoning/2026-04-28-deterministic-simulation-synchronization.md b/doc/reasoning/2026-04-28-deterministic-simulation-synchronization.md new file mode 100644 index 00000000..a8a0b7a2 --- /dev/null +++ b/doc/reasoning/2026-04-28-deterministic-simulation-synchronization.md @@ -0,0 +1,298 @@ +# Deterministic Simulation Synchronization + +## Task + +Design a deterministic replacement for the current sleep-based CARLA and ROS synchronization approach so the project can prove that the command sent to the simulator was computed from up-to-date data. + +## Sources inspected + +- `code/control/control/vehicle_controller.py` +- `code/control/control/velocity_controller.py` +- `code/control/control/pure_pursuit_controller.py` +- `code/control/launch/control.xml` +- `code/agent/agent/data_management_node.py` +- `code/mapping/mapping/data_integration.py` +- `code/planning/planning/behavior_agent/behavior_tree.py` +- `code/agent/launch/agent.dev.xml` +- `code/agent/launch/agent.dev.persistent.xml` +- `code/leaderboard_launcher/launch/ros_bridge.dev.xml` +- `doc/control/vehicle_controller.md` +- `doc/control/architecture_documentation.md` +- `doc/research/overhaul25/improvements/README.md` +- `https://github.com/una-auxme/paf/issues/471` +- `https://github.com/una-auxme/paf/issues/701` + +## Current diagnosis + +The current system has two different timing problems. + +### 1. Startup readiness is not deterministic + +`DataManagement` publishes `/carla//status` every 0.5 seconds on system time. That means the simulation is started by a periodic heartbeat, not by a proof that the required node graph is ready. + +### 2. Runtime stepping is not deterministic + +The critical runtime path mixes multiple timing models: + +- `vehicle_controller` subscribes to `/clock` and publishes `vehicle_control_cmd` immediately from the clock callback, +- `vehicle_controller` then sleeps with `loop_sleep_time` as a hotfix, +- `velocity_controller`, `pure_pursuit_controller`, `behavior_tree`, `mapping/data_integration`, and several localization and perception nodes use `create_timer(...)`, +- the ROS bridge is configured with `synchronous_mode_wait_for_vehicle_control_command=False` in the checked launch file. + +This means there is no single proof that "frame N is complete" before the next command is sent. + +The practical consequence is that planning and control can act on a mix of fresh and stale inputs from different ticks. + +## Design principle + +Do **not** try to prove that every node in the workspace ran exactly once. + +That is the wrong contract. + +The correct contract is: + +1. identify the critical path that produces the control command, +2. define which outputs on that path must be valid for frame `N`, +3. send the command for frame `N` only after those outputs are complete, +4. keep visualization, logging, and other non-critical helpers outside the barrier. + +## Proposed architecture + +Split the problem into two planes. + +### A. Startup readiness plane + +Introduce a dedicated `StartupCoordinator`. + +Responsibilities: + +- collect readiness from required nodes, +- verify required static data has arrived, +- publish `/carla//status` only after the required set is ready, +- stop using the periodic status timer as the readiness contract. + +Each required node should publish a transient-local `NodeState` message with at least: + +- `node_name` +- `stage` +- `state`: `BOOTING | READY | DEGRADED | ERROR` +- `reason` +- `required_inputs_ready` +- `last_completed_frame` + +For startup, `READY` should mean: + +- subscriptions are created, +- required services are available, +- required static inputs have arrived, +- the node can participate in the first synchronous frame. + +Examples: + +- `DataManagement` is ready only after OpenDRIVE and global plan were received and the services were created. +- `mapping` is ready only after hero pose and the required perception sources are present. +- `planning` is ready only after the map and persistent route data are available. + +### B. Runtime frame synchronization plane + +Introduce a dedicated `FrameCoordinator` for the synchronous control path. + +Responsibilities: + +- derive a monotonic `frame_id` from the CARLA tick, +- track which stages have completed for the current frame, +- enforce freshness rules, +- publish the final `vehicle_control_cmd`, +- emit metrics and faults when the graph misses deadlines. + +The coordinator should be the **only** component allowed to release the final control command that advances the simulation. + +## Critical path + +The critical synchronous path should be treated as staged. + +Suggested stages: + +1. `FrameSource` + CARLA tick plus sensor data and ego state for frame `N`. +2. `Perception + Localization` + Produce frame-tagged outputs for frame `N`. +3. `Mapping` + Publish map `N` once all required inputs for `N` are available. +4. `Planning` + Produce behavior, target velocity, and local trajectory for `N`. +5. `Control` + Produce steering, throttle, brake, reverse for `N`. +6. `FrameCoordinator` + Validate completeness and freshness, then publish `vehicle_control_cmd` for `N`. + +## Freshness and validity contract + +Every stage output on the critical path should declare provenance. + +This can be done either by extending messages directly or by publishing a side-channel `FrameResult` topic per stage during migration. + +Minimum fields: + +- `stage` +- `frame_id` +- `input_frame_ids` +- `valid` +- `degraded` +- `reason` +- `processing_latency_ms` + +The coordinator should reject or downgrade outputs that violate the contract. + +Examples: + +- planning output for frame `N` must not be accepted if it was computed from map `N-1` unless that lag is explicitly allowed, +- map `N` may optionally allow some slower inputs with a configured max age in frames, +- control `N` must be based on planning `N` and ego state `N`. + +Recommended validity states: + +- `VALID` +- `STALE` +- `MISSING` +- `DEGRADED` +- `ERROR` + +## Failure policy + +The system must stay deterministic even when a node is slow or broken. + +That means the policy must be explicit. + +Recommended policy: + +1. missing optional data: allow degraded continuation, +2. missing required data for one frame: publish a safe fallback command and flag the frame, +3. repeated failure over threshold: abort the route or switch into a controlled safe-stop state. + +Do not let failure handling be implicit in sleep times. + +## Recommended control of CARLA stepping + +After the final command barrier exists, enable the bridge option that waits for the control command before the next synchronous step. + +That means the desired end state is: + +- `synchronous_mode_wait_for_vehicle_control_command=True` +- no `loop_sleep_time` in the control contract +- `vehicle_controller` no longer publishes the final CARLA command directly from the `/clock` callback + +`vehicle_controller` should become a compute node or command assembler, not the simulation step trigger. + +## Sequence diagram + +```mermaid +sequenceDiagram + participant CARLA + participant Bridge as ROS Bridge + participant Startup as StartupCoordinator + participant Frame as FrameCoordinator + participant P as Perception+Localization + participant M as Mapping + participant PL as Planning + participant C as Control + + Note over Startup: startup phase + P-->>Startup: NodeState READY + M-->>Startup: NodeState READY + PL-->>Startup: NodeState READY + C-->>Startup: NodeState READY + Startup->>Bridge: /carla/hero/status = true + + loop for each frame N + CARLA->>Bridge: synchronous tick N + Bridge-->>Frame: tick N and sensor data + P-->>Frame: FrameResult N VALID + Frame-->>M: release frame N + M-->>Frame: MapResult N VALID + Frame-->>PL: release frame N + PL-->>Frame: PlanResult N VALID + Frame-->>C: release frame N + C-->>Frame: ControlResult N VALID + Frame->>Bridge: vehicle_control_cmd for frame N + Bridge->>CARLA: apply command and advance to N+1 + end +``` + +## What should stay asynchronous + +Keep these out of the strict barrier unless they directly affect the command: + +- visualization +- RViz marker publishing +- debug topics +- logging and telemetry export +- developer-only instrumentation + +This prevents the barrier from becoming fragile or too broad. + +## Minimal migration plan + +### Slice 1. Fix startup determinism + +- add `NodeState` and `StartupCoordinator`, +- stop using the periodic `/carla//status` timer as the readiness proof, +- document the required startup set. + +### Slice 2. Make runtime freshness observable + +- add `frame_id` and `FrameResult` side-channel topics for the critical stages, +- log when any stage publishes stale or mismatched data, +- keep the old timers temporarily. + +### Slice 3. Introduce final command barrier + +- add `FrameCoordinator`, +- move publication of the final `vehicle_control_cmd` behind the barrier, +- enable bridge waiting for vehicle control command. + +### Slice 4. Remove timer-driven critical loops + +- convert critical timer loops into frame-triggered computations, +- keep timers only where background work is truly independent of the synchronous path, +- remove `loop_sleep_time` from the control contract. + +### Slice 5. Tighten validation + +- add latency and stale-data tests, +- add route-level checks that verify one command per completed frame, +- record degraded or fallback frames during evaluation. + +## Recommended first implementation target + +The smallest high-value slice is not the full barrier. + +It is: + +1. make startup readiness explicit, +2. add `frame_id` plus `FrameResult` observability for the critical stages, +3. move the final control publish behind a coordinator. + +That gives a deterministic contract without rewriting every node immediately. + +## Validation strategy + +Proof should include both correctness and observability. + +Recommended checks: + +- unit tests for `StartupCoordinator` readiness aggregation, +- unit tests for `FrameCoordinator` freshness and timeout rules, +- integration test that delays one critical stage and verifies no stale command is sent, +- route-level run that confirms exactly one control release per completed frame, +- explicit logs or metrics for stale, degraded, and fallback frames. + +## Main conclusion + +The project should move from a timer-shaped graph to a contract-shaped graph. + +The contract should not be "all nodes slept long enough" and it should not be "every node ran once". + +The contract should be: + +"The command for frame `N` is only sent after the required stages produced a valid output for frame `N`, and startup only begins after the required graph is ready." From 8de6739a9a2952aa00fb47fbf6f42edf80764da7 Mon Sep 17 00:00:00 2001 From: ll7 Date: Tue, 28 Apr 2026 16:03:17 +0200 Subject: [PATCH 32/43] docs: add saved evaluation notes --- .../2026-04-28-roboflow-evaluation.md | 59 +++++++++++++++++++ doc/reasoning/README.md | 7 +++ 2 files changed, 66 insertions(+) create mode 100644 doc/reasoning/2026-04-28-roboflow-evaluation.md diff --git a/doc/reasoning/2026-04-28-roboflow-evaluation.md b/doc/reasoning/2026-04-28-roboflow-evaluation.md new file mode 100644 index 00000000..bebdbf38 --- /dev/null +++ b/doc/reasoning/2026-04-28-roboflow-evaluation.md @@ -0,0 +1,59 @@ +# Roboflow Evaluation Notes + +## Task + +Evaluate whether the public Roboflow organization repositories are useful for PAF's current perception work and create concrete follow-up issues only where the fit is strong enough. + +## Sources inspected + +- `code/perception/perception/vision_node.py` +- `code/perception/traffic_light_detection/dataset.dvc` +- `code/perception/traffic_light_detection/dvc.yaml` +- `doc/perception/vision_node.md` +- `doc/perception/traffic_light_detection.md` +- `doc/perception/experiments/object-detection-model_evaluation/README.md` +- `https://github.com/roboflow` +- `https://github.com/roboflow/supervision` +- `https://github.com/roboflow/notebooks` +- `https://github.com/roboflow/inference` +- `https://github.com/una-auxme/paf/issues/918` + +## Main conclusion + +Roboflow is only partially helpful for PAF right now. + +The useful part is not adopting another runtime inference stack. The useful part is borrowing tooling and workflows that reduce friction around perception evaluation and dataset iteration. + +The current PAF perception stack already has a direct ROS2 + Ultralytics path: + +- the vision node runs Ultralytics YOLO segmentation models, +- traffic-light handling already has a dedicated local training pipeline, +- traffic-light data is already stored in a local DVC-managed dataset. + +That means the main bottleneck is dataset quality, evaluation speed, and debugging support, not model serving. + +## What looks useful + +1. `roboflow/supervision` looks like the best fit. + It is model-agnostic, works with common detection outputs, and would be most useful as an offline helper library for overlays, dataset utilities, simple tracking experiments, and perception debugging around the existing Ultralytics outputs. +2. Selected `roboflow/notebooks` are useful as experiment references. + The strongest candidates are the notebooks around auto-annotation, Grounding DINO plus SAM style dataset bootstrapping, and YOLO fine-tuning workflows. + +## What does not look useful yet + +`roboflow/inference` is not a good near-term integration target. + +Reasons: + +- PAF already has a ROS-native runtime stack and direct model loading. +- Adding another inference server and workflow layer increases system complexity. +- Cloud-connected features add account, licensing, and deployment concerns that do not address the current bottleneck. +- The present need is better local evaluation and better datasets, not a new serving boundary. + +## Recommended follow-up work + +1. Evaluate `supervision` as an experiments-only dependency for offline perception benchmarking, visual debugging, and tracker-style analysis on recorded or generated data. + This is now tracked in `una-auxme/paf#919`. +2. Evaluate a semi-automatic labeling workflow inspired by Roboflow notebooks for expanding PAF datasets while keeping data local and DVC-managed. + This is now tracked in `una-auxme/paf#920`. +3. Defer any `roboflow/inference` adoption unless PAF later develops a clear need for standalone CV microservices or remote model-serving workflows. diff --git a/doc/reasoning/README.md b/doc/reasoning/README.md index eb98c488..23a0a043 100644 --- a/doc/reasoning/README.md +++ b/doc/reasoning/README.md @@ -21,3 +21,10 @@ Each note should identify: 2. The main sources inspected. 3. The current conclusion. 4. Any follow-up work that remains. + +## Current Notes + +- [2026-04-28-development-hardening.md](./2026-04-28-development-hardening.md) +- [2026-04-28-intersection-prediction-strategy.md](./2026-04-28-intersection-prediction-strategy.md) +- [2026-04-28-roboflow-evaluation.md](./2026-04-28-roboflow-evaluation.md) +- [2026-04-28-deterministic-simulation-synchronization.md](./2026-04-28-deterministic-simulation-synchronization.md) From 844e2a8b2a4be9643d534d08813b0d083e203216 Mon Sep 17 00:00:00 2001 From: ll7 Date: Wed, 29 Apr 2026 10:07:34 +0200 Subject: [PATCH 33/43] control: harden route validation and add map lane context --- build/docker-compose.carla.cuda.yaml | 2 + code/agent/agent/data_management_node.py | 38 +-- code/agent/agent/startup_coordinator.py | 10 +- code/control/control/vehicle_controller.py | 5 + .../leaderboard_launcher/paf_agent_base.py | 121 ++++++++- .../scripts/launch_leaderboard.sh | 42 ++- code/mapping/mapping_common/map.py | 254 +++++++++++++++--- .../test/test_mapping_common/test_map.py | 106 ++++++++ code/paf_common/paf_common/route_metrics.py | 106 ++++++++ .../behavior_agent/behaviors/intersection.py | 12 + code/test/run_test.py | 16 ++ code/test/test_deterministic_sync.py | 21 ++ code/test/test_route_metrics.py | 62 +++++ doc/README.md | 5 + doc/dev/README.md | 7 + ...oute-validation-and-map-context-handoff.md | 127 +++++++++ doc/mapping/README.md | 1 + 17 files changed, 858 insertions(+), 77 deletions(-) create mode 100644 code/mapping/test/test_mapping_common/test_map.py create mode 100644 code/paf_common/paf_common/route_metrics.py create mode 100644 code/test/test_route_metrics.py create mode 100644 doc/dev/README.md create mode 100644 doc/dev/progress/2026-04-29-route-validation-and-map-context-handoff.md diff --git a/build/docker-compose.carla.cuda.yaml b/build/docker-compose.carla.cuda.yaml index 23a26318..bf9a6fd7 100644 --- a/build/docker-compose.carla.cuda.yaml +++ b/build/docker-compose.carla.cuda.yaml @@ -13,6 +13,8 @@ services: count: "all" capabilities: [ gpu ] # https://github.com/carla-simulator/carla/issues/6234#issuecomment-1458372639 + environment: + - VK_ICD_FILENAMES=/usr/share/vulkan/icd.d/nvidia_icd.json volumes: # This should not be necessary: https://github.com/NVIDIA/nvidia-container-toolkit/issues/16 - /usr/share/vulkan/icd.d/nvidia_icd.json:/usr/share/vulkan/icd.d/nvidia_icd.json diff --git a/code/agent/agent/data_management_node.py b/code/agent/agent/data_management_node.py index d11affae..721c6aa5 100644 --- a/code/agent/agent/data_management_node.py +++ b/code/agent/agent/data_management_node.py @@ -24,10 +24,20 @@ def __init__(self): self.role_name = self.declare_parameter("role_name", "hero").value # Services - # Get created only after data is available self.open_drive_service: Optional[Service] = None self.global_plan_service: Optional[Service] = None + self.open_drive_service = self.create_service( + GetOpenDriveString, + f"/paf/{self.role_name}/data/planning/get_open_drive", + self.get_open_drive_service, + ) + self.global_plan_service = self.create_service( + GetCarlaRoute, + f"/paf/{self.role_name}/data/planning/get_global_plan", + self.get_global_plan_service, + ) + # Subscriptions self.create_subscription( msg_type=String, @@ -72,15 +82,6 @@ def open_drive_callback(self, data: String): self.open_drive_string = data.data # with open("/workspace/OpenDriveString.xml", "w") as text_file: # text_file.write(self.open_drive_string) - if self.open_drive_service is None: - self.open_drive_service = self.create_service( - GetOpenDriveString, - f"/paf/{self.role_name}/data/planning/get_open_drive", - self.get_open_drive_service, - ) - self.get_logger().info( - f"Started {self.open_drive_service.service_name} service." - ) self.open_drive_updated_pub.publish(Bool(data=True)) self._publish_startup_ready_if_available() @@ -99,25 +100,10 @@ def get_open_drive_service( def global_plan_callback(self, data: CarlaRoute): self.get_logger().info("Received global plan data.") self.global_plan = data - if self.global_plan_service is None: - self.global_plan_service = self.create_service( - GetCarlaRoute, - f"/paf/{self.role_name}/data/planning/get_global_plan", - self.get_global_plan_service, - ) - self.get_logger().info( - f"Started {self.global_plan_service.service_name} service." - ) self.global_plan_updated_pub.publish(Bool(data=True)) - self._publish_startup_ready_if_available() def _publish_startup_ready_if_available(self) -> None: - if ( - self.open_drive_string is not None - and self.global_plan is not None - and self.open_drive_service is not None - and self.global_plan_service is not None - ): + if self.open_drive_string is not None and self.open_drive_service is not None: self.startup_ready_pub.publish(Bool(data=True)) def get_global_plan_service( diff --git a/code/agent/agent/startup_coordinator.py b/code/agent/agent/startup_coordinator.py index f16674c6..55b2d4ff 100644 --- a/code/agent/agent/startup_coordinator.py +++ b/code/agent/agent/startup_coordinator.py @@ -12,15 +12,9 @@ DEFAULT_REQUIRED_NODES = [ + # Route-dependent nodes become ready only after the leaderboard publishes + # the global plan, which happens after /carla//status is released. "data_management", - "mapping", - "motion_planning", - "acc", - "passthrough", - "pure_pursuit", - "velocity_controller", - "vehicle_controller", - "behavior_tree", ] diff --git a/code/control/control/vehicle_controller.py b/code/control/control/vehicle_controller.py index 6c9ac795..fd7dc970 100755 --- a/code/control/control/vehicle_controller.py +++ b/code/control/control/vehicle_controller.py @@ -17,6 +17,7 @@ from rosgraph_msgs.msg import Clock from paf_common.parameters import update_attributes from paf_common.exceptions import emsg_with_trace +from paf_common.route_metrics import increment_route_metric from paf_common.sync import ( FrameBarrier, frame_complete_topic, @@ -32,6 +33,9 @@ "pure_pursuit", "velocity_controller", ] +FRAME_BARRIER_FALLBACK_METRIC = ( + "control.vehicle_controller.frame_barrier_fallback_frames" +) class VehicleController(Node): @@ -301,6 +305,7 @@ def _frame_timeout_handler(self) -> None: return missing_stages = self.frame_barrier.missing_stages(pending_frame_id) + increment_route_metric(FRAME_BARRIER_FALLBACK_METRIC) self.get_logger().warn( "Frame barrier timeout for frame " f"{pending_frame_id}: missing stages {missing_stages}. " diff --git a/code/leaderboard_launcher/leaderboard_launcher/paf_agent_base.py b/code/leaderboard_launcher/leaderboard_launcher/paf_agent_base.py index 0180759c..a1caea26 100755 --- a/code/leaderboard_launcher/leaderboard_launcher/paf_agent_base.py +++ b/code/leaderboard_launcher/leaderboard_launcher/paf_agent_base.py @@ -1,13 +1,94 @@ +import math +import queue +import threading + +from carla_msgs.msg import CarlaEgoVehicleControl, CarlaGnssRoute, CarlaRoute +from carla_msgs.srv import DestroyObject, SpawnObject +from leaderboard.autoagents.autonomous_agent import AutonomousAgent, Track from leaderboard.autoagents.ros2_agent import ROS2Agent -from leaderboard.autoagents.autonomous_agent import Track +from leaderboard.autoagents.ros_base_agent import ROSLauncher import math +import rclpy +from rclpy.qos import DurabilityPolicy, QoSProfile + def get_entry_point(): return "PAFAgent" class PAFAgent(ROS2Agent): + def __init__(self, carla_host, carla_port, debug=False): + if rclpy.ok(): + rclpy.shutdown() + + try: + AutonomousAgent.__init__(self, carla_host, carla_port, debug) + + self._bridge_process = ROSLauncher( + "bridge", ros_version=self.ROS_VERSION, debug=debug + ) + self._bridge_process.run( + package="carla_ros_bridge", + launch_file="carla_ros_bridge.launch.py", + parameters={ + "host": carla_host, + "port": carla_port, + "timeout": 60, + "synchronous_mode": True, + "passive": True, + "register_all_sensors": False, + "ego_vehicle_role_name": "\"['hero']\"", + }, + wait=True, + ) + + self._agent_process = ROSLauncher( + "agent", ros_version=self.ROS_VERSION, debug=debug + ) + self._agent_process.run(**self.get_ros_entrypoint(), wait=True) + + self._control_queue = queue.Queue(1) + self._last_control_timestamp = None + + rclpy.init(args=None) + self.ros_node = rclpy.create_node("leaderboard_node") + + self._spawn_object_service = self.ros_node.create_client( + SpawnObject, "/carla/spawn_object" + ) + self._destroy_object_service = self.ros_node.create_client( + DestroyObject, "/carla/destroy_object" + ) + + self._path_publisher = self.ros_node.create_publisher( + CarlaRoute, + "/carla/hero/global_plan", + qos_profile=QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL + ), + ) + self._path_gnss_publisher = self.ros_node.create_publisher( + CarlaGnssRoute, + "/carla/hero/global_plan_gnss", + qos_profile=QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL + ), + ) + + self.ctrl_subscriber = self.ros_node.create_subscription( + CarlaEgoVehicleControl, + "/carla/hero/vehicle_control_cmd", + self._vehicle_control_cmd_callback, + qos_profile=QoSProfile(depth=1), + ) + + self.spin_thread = threading.Thread(target=rclpy.spin, args=(self.ros_node,)) + self.spin_thread.start() + except Exception: + self._cleanup_partial_ros2_setup() + raise + def setup(self, path_to_conf_file): self.track = Track.MAP @@ -88,5 +169,41 @@ def sensors(self): ] return sensors + def spawn_object(self, type_, id_, transform, attributes, attach_to=0): + self._spawn_object_service.wait_for_service() + return super().spawn_object(type_, id_, transform, attributes, attach_to) + + def destroy_object(self, uid): + self._destroy_object_service.wait_for_service() + return super().destroy_object(uid) + def destroy(self): - super().destroy() + self._cleanup_partial_ros2_setup() + + def _cleanup_partial_ros2_setup(self): + spin_thread = getattr(self, "spin_thread", None) + ros_node = getattr(self, "ros_node", None) + agent_process = getattr(self, "_agent_process", None) + bridge_process = getattr(self, "_bridge_process", None) + + if ros_node is not None: + try: + ros_node.destroy_node() + except Exception: + pass + + if rclpy.ok(): + try: + rclpy.shutdown() + except Exception: + pass + + if spin_thread is not None and spin_thread.is_alive(): + spin_thread.join(timeout=5.0) + + if agent_process is not None and agent_process.is_alive(): + agent_process.terminate() + + if bridge_process is not None and bridge_process.is_alive(): + bridge_process.terminate() + diff --git a/code/leaderboard_launcher/scripts/launch_leaderboard.sh b/code/leaderboard_launcher/scripts/launch_leaderboard.sh index 4ab1b18e..5379a3ea 100755 --- a/code/leaderboard_launcher/scripts/launch_leaderboard.sh +++ b/code/leaderboard_launcher/scripts/launch_leaderboard.sh @@ -13,10 +13,50 @@ source "${INTERNAL_WORKSPACE_DIR}/env.leaderboard.bash" # Source leaderboard specific venv source leaderboard_venv/bin/activate +checkpoint_path="./simulation_results.json" +for ((i = 1; i <= $#; ++i)); do + arg="${!i}" + if [[ "${arg}" == "--checkpoint" ]]; then + next_index=$((i + 1)) + checkpoint_path="${!next_index}" + elif [[ "${arg}" == --checkpoint=* ]]; then + checkpoint_path="${arg#--checkpoint=}" + fi +done + +python3 - <<'PY' +import sys + +sys.path.insert(0, "/workspace/code/paf_common") + +from paf_common.route_metrics import reset_route_metrics_file + +reset_route_metrics_file() +PY + python3 "/workspace/code/leaderboard_launcher/leaderboard_launcher/wait_for_carla.py" # Start leaderboard with arguments -exec python3 "${LEADERBOARD_ROOT}/leaderboard/leaderboard_evaluator.py" \ +set +e +python3 "${LEADERBOARD_ROOT}/leaderboard/leaderboard_evaluator.py" \ --host="${CARLA_SIM_HOST}" \ --track=MAP \ "${@}" +exit_code=$? +set -e + +python3 - < Optional[bool]: + if self is LanePresence.UNKNOWN: + return None + return self is LanePresence.PRESENT + + +@dataclass +class LaneContext: + direction: LaneFreeDirection + presence: LanePresence + marking_state: LaneFreeState + lane_state: LaneFreeState + lane_box: Optional[shapely.Geometry] = None + + def has_lane(self) -> Optional[bool]: + return self.presence.as_optional_bool() + + def is_free(self) -> Optional[bool]: + if self.lane_state is LaneFreeState.FREE: + return True + if self.lane_state is LaneFreeState.BLOCKED: + return False + return None + + +@dataclass +class AdjacentLaneContext: + left: LaneContext + right: LaneContext + + def has_left_lane(self) -> Optional[bool]: + return self.left.has_lane() + + def has_right_lane(self) -> Optional[bool]: + return self.right.has_lane() + + +def _lane_presence_from_marking_state(marking_state: LaneFreeState) -> LanePresence: + if marking_state in [ + LaneFreeState.TO_BE_CHECKED, + LaneFreeState.FREE, + LaneFreeState.BLOCKED, + ]: + return LanePresence.PRESENT + if marking_state is LaneFreeState.MISSING_LANEMARK_ERR: + return LanePresence.ABSENT + return LanePresence.UNKNOWN + + @dataclass class Map: """2 dimensional map for the intermediate layer @@ -500,6 +554,158 @@ def get_entity_in_front_or_back(self, in_front=True) -> Optional[ShapelyEntity]: else: return None + def _get_lane_marking_pair( + self, + right_lane: bool = False, + lane_angle: float = 5.0, + ) -> Tuple[ + LaneFreeState, + Optional[LineString], + Optional[Entity], + Optional[Entity], + ]: + lane_pos = -1 if right_lane else 1 + y_axis_line = LineString([[0, 0], [0, lane_pos * 8]]) + + lane_tree = self.map.build_tree(f=FlagFilter(is_lanemark=True)) + lanemark_y_axis_intersection = lane_tree.query( + geo=y_axis_line, + predicate="intersects", + ) + if len(lanemark_y_axis_intersection) < 2: + return LaneFreeState.MISSING_LANEMARK_ERR, None, None, None + + lane_close_hero = None + lane_further_hero = None + + for ent in lanemark_y_axis_intersection: + if ent.entity.position_index == lane_pos * 1: + lane_close_hero = ent.entity + if ent.entity.position_index == lane_pos * 2: + lane_further_hero = ent.entity + + if lane_close_hero is None or lane_further_hero is None: + return LaneFreeState.MISSING_LANEMARK_ERR, None, None, None + + close_rotation = lane_close_hero.transform.rotation() + further_rotation = lane_further_hero.transform.rotation() + lanemark_angle = np.rad2deg(abs(close_rotation - further_rotation)) + if lanemark_angle > lane_angle: + get_logger().warn( + f"Lane free check: Lanemarkings angle {lanemark_angle} too big, " + f"should be < {lane_angle}°. Aborting check." + ) + return ( + LaneFreeState.LANEMARK_ANGLE_ERR, + y_axis_line, + lane_close_hero, + lane_further_hero, + ) + + return ( + LaneFreeState.TO_BE_CHECKED, + y_axis_line, + lane_close_hero, + lane_further_hero, + ) + + def get_lane_presence( + self, + right_lane: bool = False, + lane_angle: float = 5.0, + ) -> LanePresence: + """Returns whether an adjacent lane is present, absent, or unknown.""" + + marking_state, _, _, _ = self._get_lane_marking_pair( + right_lane=right_lane, + lane_angle=lane_angle, + ) + return _lane_presence_from_marking_state(marking_state) + + def get_lane_context( + self, + right_lane: bool = False, + lane_length: float = 20.0, + lane_transform: float = 0.0, + reduce_lane: float = 1.5, + check_method: Literal[ + "rectangle", + "lanemarking", + "fallback", + ] = "fallback", + min_coverage_percent: float = 0.0, + min_coverage_area: float = 0.0, + lane_angle: float = 5.0, + motion_aware: bool = True, + ) -> LaneContext: + """Returns lane presence and occupancy information for one adjacent side.""" + + direction = LaneFreeDirection.RIGHT if right_lane else LaneFreeDirection.LEFT + marking_state, _, _, _ = self._get_lane_marking_pair( + right_lane=right_lane, + lane_angle=lane_angle, + ) + lane_state, lane_box = self.is_lane_free( + right_lane=right_lane, + lane_length=lane_length, + lane_transform=lane_transform, + reduce_lane=reduce_lane, + check_method=check_method, + min_coverage_percent=min_coverage_percent, + min_coverage_area=min_coverage_area, + lane_angle=lane_angle, + motion_aware=motion_aware, + ) + return LaneContext( + direction=direction, + presence=_lane_presence_from_marking_state(marking_state), + marking_state=marking_state, + lane_state=lane_state, + lane_box=lane_box, + ) + + def get_adjacent_lane_context( + self, + lane_length: float = 20.0, + lane_transform: float = 0.0, + reduce_lane: float = 1.5, + check_method: Literal[ + "rectangle", + "lanemarking", + "fallback", + ] = "fallback", + min_coverage_percent: float = 0.0, + min_coverage_area: float = 0.0, + lane_angle: float = 5.0, + motion_aware: bool = True, + ) -> AdjacentLaneContext: + """Returns lane presence and occupancy information for both adjacent sides.""" + + return AdjacentLaneContext( + left=self.get_lane_context( + right_lane=False, + lane_length=lane_length, + lane_transform=lane_transform, + reduce_lane=reduce_lane, + check_method=check_method, + min_coverage_percent=min_coverage_percent, + min_coverage_area=min_coverage_area, + lane_angle=lane_angle, + motion_aware=motion_aware, + ), + right=self.get_lane_context( + right_lane=True, + lane_length=lane_length, + lane_transform=lane_transform, + reduce_lane=reduce_lane, + check_method=check_method, + min_coverage_percent=min_coverage_percent, + min_coverage_area=min_coverage_area, + lane_angle=lane_angle, + motion_aware=motion_aware, + ), + ) + def is_lane_free( self, right_lane: bool = False, @@ -691,47 +897,15 @@ def is_lane_free_lanemarking( Tuple[LaneFreeState, Optional[shapely.Geometry]]: return if lane is free and the checkbox shape """ - # checks which lane should be checked and set the multiplier for - # the lane entity translation(>0 = left from car) - lane_pos = 1 - if right_lane: - lane_pos = -1 - - # create y-axis line for intersection with lanemarks - y_axis_line = LineString([[0, 0], [0, lane_pos * 8]]) - # build map STRtree from map with filter - lane_tree = self.map.build_tree(f=FlagFilter(is_lanemark=True)) - # get entities that intersect with the y-axis line - lanemark_y_axis_intersection = lane_tree.query( - geo=y_axis_line, predicate="intersects" - ) - # Abort when not enough lane marks got detected - if len(lanemark_y_axis_intersection) < 2: - return LaneFreeState.MISSING_LANEMARK_ERR, None - - lane_close_hero = None - lane_further_hero = None - - # Choose two lanes nearby car - for ent in lanemark_y_axis_intersection: - if ent.entity.position_index == lane_pos * 1: - lane_close_hero = ent.entity - if ent.entity.position_index == lane_pos * 2: - lane_further_hero = ent.entity - - if lane_close_hero is None or lane_further_hero is None: - return LaneFreeState.MISSING_LANEMARK_ERR, None - - # Check if two lanes has a plausible angle to each pother - close_rotation = lane_close_hero.transform.rotation() - further_rotation = lane_further_hero.transform.rotation() - lanemark_angle = np.rad2deg(abs(close_rotation - further_rotation)) - if lanemark_angle > lane_angle: - get_logger().warn( - f"Lane free check: Lanemarkings angle {lanemark_angle} too big, \ - should be < {lane_angle}°. Aborting check." + lane_pos = -1 if right_lane else 1 + lane_state, y_axis_line, lane_close_hero, lane_further_hero = ( + self._get_lane_marking_pair( + right_lane=right_lane, + lane_angle=lane_angle, ) - return LaneFreeState.LANEMARK_ANGLE_ERR, None + ) + if lane_state.is_error(): + return lane_state, None # create the lane ckeckbox shape lane_box = mapping_common.mask.create_lane_box( diff --git a/code/mapping/test/test_mapping_common/test_map.py b/code/mapping/test/test_mapping_common/test_map.py new file mode 100644 index 00000000..9bb8b40d --- /dev/null +++ b/code/mapping/test/test_mapping_common/test_map.py @@ -0,0 +1,106 @@ +import pytest + +from mapping_common import entity, shape, transform +from mapping_common.map import ( + LaneFreeState, + LanePresence, + Map, +) + +pytestmark = pytest.mark.unit + + +def get_hero() -> entity.Car: + return entity.Car( + confidence=1.0, + priority=1.0, + shape=shape.Rectangle(4.5, 2.0), + transform=transform.Transform2D.identity(), + flags=entity.Flags(is_collider=True, is_hero=True), + ) + + +def get_lanemark(position_index: int, y: float, rotation: float = 0.0): + return entity.Lanemarking( + confidence=1.0, + priority=1.0, + shape=shape.Rectangle(30.0, 0.15), + transform=transform.Transform2D.new_rotation_translation( + rotation, + transform.Vector2.new(0.0, y), + ), + flags=entity.Flags(is_lanemark=True), + style=entity.Lanemarking.Style.SOLID, + position_index=position_index, + predicted=False, + ) + + +def get_blocking_car(x: float, y: float) -> entity.Car: + return entity.Car( + confidence=1.0, + priority=1.0, + shape=shape.Rectangle(4.5, 2.0), + transform=transform.Transform2D.new_translation(transform.Vector2.new(x, y)), + flags=entity.Flags(is_collider=True), + ) + + +def test_get_lane_presence_detects_present_absent_and_unknown(): + present_map = Map( + entities=[ + get_hero(), + get_lanemark(position_index=1, y=1.6), + get_lanemark(position_index=2, y=4.6), + ] + ) + absent_map = Map(entities=[get_hero()]) + unknown_map = Map( + entities=[ + get_hero(), + get_lanemark(position_index=1, y=1.6, rotation=0.0), + get_lanemark(position_index=2, y=4.6, rotation=0.4), + ] + ) + + assert present_map.build_tree().get_lane_presence() is LanePresence.PRESENT + assert absent_map.build_tree().get_lane_presence() is LanePresence.ABSENT + assert unknown_map.build_tree().get_lane_presence() is LanePresence.UNKNOWN + + +def test_get_adjacent_lane_context_reports_presence_and_blocking(): + road_map = Map( + entities=[ + get_hero(), + get_lanemark(position_index=1, y=1.6), + get_lanemark(position_index=2, y=4.6), + get_blocking_car(x=6.0, y=3.1), + ] + ) + + lane_context = road_map.build_tree( + entity.FlagFilter(is_collider=True, is_hero=False) + ).get_adjacent_lane_context(check_method="lanemarking") + + assert lane_context.has_left_lane() is True + assert lane_context.left.lane_state is LaneFreeState.BLOCKED + assert lane_context.left.marking_state is LaneFreeState.TO_BE_CHECKED + assert lane_context.has_right_lane() is False + assert lane_context.right.lane_state is LaneFreeState.MISSING_LANEMARK_ERR + + +def test_get_lane_context_preserves_absent_lane_information_with_fallback(): + road_map = Map( + entities=[ + get_hero(), + get_blocking_car(x=4.0, y=2.5), + ] + ) + + lane_context = road_map.build_tree( + entity.FlagFilter(is_collider=True, is_hero=False) + ).get_lane_context(check_method="fallback") + + assert lane_context.has_lane() is False + assert lane_context.presence is LanePresence.ABSENT + assert lane_context.lane_state is LaneFreeState.BLOCKED diff --git a/code/paf_common/paf_common/route_metrics.py b/code/paf_common/paf_common/route_metrics.py new file mode 100644 index 00000000..8ddbfaf7 --- /dev/null +++ b/code/paf_common/paf_common/route_metrics.py @@ -0,0 +1,106 @@ +"""Helpers for lightweight route-level metrics recorded across ROS nodes.""" + +from __future__ import annotations + +import fcntl +import json +import os +from pathlib import Path +from typing import Any, Optional + + +DEFAULT_ROUTE_METRICS_PATH = Path("/tmp/paf_route_metrics.json") +ROUTE_METRICS_ENV_VAR = "PAF_ROUTE_METRICS_PATH" + + +def get_route_metrics_path(path: Optional[str | os.PathLike[str]] = None) -> Path: + """Resolve the route metrics file path.""" + if path is not None: + return Path(path) + + env_path = os.environ.get(ROUTE_METRICS_ENV_VAR) + if env_path: + return Path(env_path) + + return DEFAULT_ROUTE_METRICS_PATH + + +def reset_route_metrics_file( + path: Optional[str | os.PathLike[str]] = None, +) -> Path: + """Delete any stale route metrics file before a fresh run.""" + metrics_path = get_route_metrics_path(path) + metrics_path.parent.mkdir(parents=True, exist_ok=True) + metrics_path.unlink(missing_ok=True) + return metrics_path + + +def increment_route_metric( + metric_name: str, + *, + amount: int = 1, + path: Optional[str | os.PathLike[str]] = None, +) -> int: + """Atomically increment a route metric and return the new counter value.""" + metrics_path = get_route_metrics_path(path) + metrics_path.parent.mkdir(parents=True, exist_ok=True) + + with metrics_path.open("a+", encoding="utf-8") as metrics_file: + fcntl.flock(metrics_file.fileno(), fcntl.LOCK_EX) + metrics_file.seek(0) + raw_content = metrics_file.read().strip() + metrics_data: dict[str, Any] = ( + json.loads(raw_content) if raw_content else {"metrics": {}} + ) + counters = metrics_data.setdefault("metrics", {}) + counters[metric_name] = int(counters.get(metric_name, 0)) + amount + + metrics_file.seek(0) + metrics_file.truncate() + json.dump(metrics_data, metrics_file, indent=4, sort_keys=True) + metrics_file.write("\n") + metrics_file.flush() + os.fsync(metrics_file.fileno()) + fcntl.flock(metrics_file.fileno(), fcntl.LOCK_UN) + + return int(counters[metric_name]) + + +def load_route_metrics( + path: Optional[str | os.PathLike[str]] = None, +) -> dict[str, Any]: + """Load the current route metrics snapshot from disk.""" + metrics_path = get_route_metrics_path(path) + if not metrics_path.exists(): + return {"metrics": {}} + + raw_content = metrics_path.read_text(encoding="utf-8").strip() + if not raw_content: + return {"metrics": {}} + return json.loads(raw_content) + + +def merge_route_metrics_into_checkpoint( + checkpoint_path: str | os.PathLike[str], + *, + metrics_path: Optional[str | os.PathLike[str]] = None, +) -> dict[str, Any]: + """Merge route metrics into the leaderboard checkpoint json.""" + metrics = load_route_metrics(metrics_path) + if not metrics.get("metrics"): + return metrics + + checkpoint_file = Path(checkpoint_path) + if not checkpoint_file.exists(): + return metrics + + checkpoint = json.loads(checkpoint_file.read_text(encoding="utf-8")) + checkpoint["paf_metrics"] = metrics + checkpoint.setdefault("_checkpoint", {}).setdefault("global_record", {})[ + "paf_metrics" + ] = metrics + checkpoint_file.write_text( + json.dumps(checkpoint, indent=4, sort_keys=True) + "\n", + encoding="utf-8", + ) + return metrics diff --git a/code/planning/planning/behavior_agent/behaviors/intersection.py b/code/planning/planning/behavior_agent/behaviors/intersection.py index 167c4ba8..22c784d4 100755 --- a/code/planning/planning/behavior_agent/behaviors/intersection.py +++ b/code/planning/planning/behavior_agent/behaviors/intersection.py @@ -21,6 +21,7 @@ from mapping_common.transform import Transform2D, Point2, Vector2 import shapely from shapely.ops import nearest_points +from paf_common.route_metrics import increment_route_metric from planning.behavior_agent.blackboard_utils import Blackboard from . import behavior_names as bs @@ -126,6 +127,9 @@ def tr_status_str(t: Optional[TrafficLightState]): PRIORITY_PASS_JUDGE_RESPONSE_TIME = 0.5 PRIORITY_PASS_JUDGE_MARGIN = 1.0 SELF_EMERGENCY_THRESHOLD = 10 / 3.6 # m/s ≈ 2.78 +UNNECESSARY_INTERSECTION_STOP_CANDIDATE_METRIC = ( + "planning.intersection.unnecessary_stop_candidates" +) def _dot(a: Vector2, b: Vector2) -> float: @@ -992,6 +996,7 @@ def initialise(self): self.priority_raw_clear = True self.priority_filtered_clear = True self.priority_raw_state_since = self.clock.now() + self.priority_stop_metric_active = False def update(self): """ @@ -1058,6 +1063,7 @@ def update(self): if pass_judge_distance is not None and _is_over_priority_pass_judge_line( pass_judge_distance, ego_speed ): + self.priority_stop_metric_active = False self.priority_raw_clear = True self.priority_filtered_clear = True self.priority_raw_state_since = self.clock.now() @@ -1066,6 +1072,11 @@ def update(self): "[Enter] Over priority pass judge line, ignore new stop", ) else: + if not self.priority_stop_metric_active: + increment_route_metric( + UNNECESSARY_INTERSECTION_STOP_CANDIDATE_METRIC + ) + self.priority_stop_metric_active = True # priority cross traffic detected self.curr_behavior_pub.publish(String(data=bs.int_wait.name)) set_line_stop(self.stop_client, 0.0) @@ -1088,6 +1099,7 @@ def update(self): py_trees.common.Status.RUNNING, reason, ) + self.priority_stop_metric_active = False unset_line_stop(self.stop_client) self.emergency_pub.publish(Bool(data=False)) diff --git a/code/test/run_test.py b/code/test/run_test.py index 41adc403..ee0dea34 100644 --- a/code/test/run_test.py +++ b/code/test/run_test.py @@ -40,6 +40,16 @@ from leaderboard.utils.statistics_manager import StatisticsManager, FAILURE_MESSAGES from leaderboard.utils.route_indexer import RouteIndexer +PAF_COMMON_SRC = "/workspace/code/paf_common" +if PAF_COMMON_SRC not in sys.path: + sys.path.insert(0, PAF_COMMON_SRC) + +from paf_common.route_metrics import ( + load_route_metrics, + merge_route_metrics_into_checkpoint, + reset_route_metrics_file, +) + sensors_to_icons = { "sensor.camera.rgb": "carla_camera", @@ -568,14 +578,20 @@ def main(): ) arguments = parser.parse_args() + reset_route_metrics_file() statistics_manager = StatisticsManager( arguments.checkpoint, arguments.debug_checkpoint ) test_evaluator = TestScenario(arguments, statistics_manager) crashed = test_evaluator.run(arguments) + route_metrics = merge_route_metrics_into_checkpoint(arguments.checkpoint) route_records = test_evaluator.statistics_manager._results.checkpoint.records + if route_metrics.get("metrics"): + print(Fore.CYAN + "PAF ROUTE METRICS") + for metric_name, metric_value in sorted(route_metrics["metrics"].items()): + print(f"{metric_name}: {metric_value}") flags = [] for i, route_record in enumerate(route_records): flag = False diff --git a/code/test/test_deterministic_sync.py b/code/test/test_deterministic_sync.py index 1e10c712..a451700f 100644 --- a/code/test/test_deterministic_sync.py +++ b/code/test/test_deterministic_sync.py @@ -82,6 +82,21 @@ def test_frame_id_from_time_ns_uses_fixed_delta( def test_sync_contract_files_reflect_barrier_design() -> None: control_xml = (CODE_ROOT / "control/launch/control.xml").read_text() control_yaml = (CODE_ROOT / "control/config/control.yaml").read_text() + data_management_source = ( + CODE_ROOT / "agent/agent/data_management_node.py" + ).read_text() + startup_coordinator_source = ( + CODE_ROOT / "agent/agent/startup_coordinator.py" + ).read_text() + paf_agent_source = ( + CODE_ROOT / "leaderboard_launcher/leaderboard_launcher/paf_agent_base.py" + ).read_text() + startup_ready_block = data_management_source.split( + "def _publish_startup_ready_if_available(self) -> None:\n", maxsplit=1 + )[1].split("\n\n def get_global_plan_service", maxsplit=1)[0] + startup_required_nodes_block = startup_coordinator_source.split( + "DEFAULT_REQUIRED_NODES = [\n", maxsplit=1 + )[1].split("]\n\n\nclass StartupCoordinator", maxsplit=1)[0] ros_bridge_root = ET.parse( CODE_ROOT / "leaderboard_launcher/launch/ros_bridge.dev.xml" ).getroot() @@ -93,6 +108,12 @@ def test_sync_contract_files_reflect_barrier_design() -> None: assert "loop_sleep_time" not in control_yaml assert "frame_barrier_timeout" in control_yaml assert "sync_frame_delta_seconds" in control_yaml + assert "self.global_plan is not None" not in startup_ready_block + assert "self.open_drive_string is not None" in startup_ready_block + assert '"ego_vehicle_role_name": "\\"[\'hero\']\\""' in paf_agent_source + assert 'wait_for_message(self.ros_node, "/carla/hero/status"' not in paf_agent_source + assert '"data_management"' in startup_required_nodes_block + assert '"motion_planning"' not in startup_required_nodes_block wait_for_command_arg = next( arg diff --git a/code/test/test_route_metrics.py b/code/test/test_route_metrics.py new file mode 100644 index 00000000..426b8617 --- /dev/null +++ b/code/test/test_route_metrics.py @@ -0,0 +1,62 @@ +"""Unit tests for lightweight route metrics helpers.""" + +from __future__ import annotations + +import json +import sys +from pathlib import Path + +import pytest + +pytestmark = pytest.mark.unit + +CODE_ROOT = Path(__file__).resolve().parents[1] +PAF_COMMON_SRC = CODE_ROOT / "paf_common" +if str(PAF_COMMON_SRC) not in sys.path: + sys.path.insert(0, str(PAF_COMMON_SRC)) + +from paf_common.route_metrics import ( + increment_route_metric, + load_route_metrics, + merge_route_metrics_into_checkpoint, + reset_route_metrics_file, +) + + +def test_increment_route_metric_accumulates_counts(tmp_path: Path) -> None: + metrics_path = tmp_path / "route_metrics.json" + + assert increment_route_metric("alpha", path=metrics_path) == 1 + assert increment_route_metric("alpha", amount=2, path=metrics_path) == 3 + assert load_route_metrics(metrics_path)["metrics"] == {"alpha": 3} + + +def test_reset_route_metrics_file_removes_previous_snapshot(tmp_path: Path) -> None: + metrics_path = tmp_path / "route_metrics.json" + increment_route_metric("alpha", path=metrics_path) + + reset_route_metrics_file(metrics_path) + + assert load_route_metrics(metrics_path) == {"metrics": {}} + + +def test_merge_route_metrics_into_checkpoint_adds_summary(tmp_path: Path) -> None: + checkpoint_path = tmp_path / "simulation_results.json" + metrics_path = tmp_path / "route_metrics.json" + checkpoint_path.write_text( + json.dumps({"_checkpoint": {"global_record": {}}, "labels": [], "values": []}), + encoding="utf-8", + ) + increment_route_metric("alpha", amount=2, path=metrics_path) + + merged_metrics = merge_route_metrics_into_checkpoint( + checkpoint_path, + metrics_path=metrics_path, + ) + checkpoint = json.loads(checkpoint_path.read_text(encoding="utf-8")) + + assert merged_metrics["metrics"] == {"alpha": 2} + assert checkpoint["paf_metrics"]["metrics"] == {"alpha": 2} + assert checkpoint["_checkpoint"]["global_record"]["paf_metrics"]["metrics"] == { + "alpha": 2 + } diff --git a/doc/README.md b/doc/README.md index 120a7a8b..17d8f9de 100644 --- a/doc/README.md +++ b/doc/README.md @@ -4,6 +4,7 @@ This document provides an overview of the structure of the documentation. - [`general`](#general) - [`development`](#development) +- [`dev`](#dev) - [`research`](#research) - [`perception`](#perception) - [`mapping`](#mapping) @@ -23,6 +24,10 @@ The [`general`](./general/) folder contains installation instructions for the pr The [`development`](./development/) folder contains guidelines for developing inside the project. It also provides templates for documentation files and python classes. Further information can be found in the [README](development/README.md). +## `dev` + +The [`dev`](./dev/README.md) folder contains development handoff and progress notes that capture in-flight work without replacing the canonical subsystem or contributor documentation. The current chat snapshot is stored in [2026-04-29-route-validation-and-map-context-handoff.md](./dev/progress/2026-04-29-route-validation-and-map-context-handoff.md). + ## `research` The [`research`](./research/) folder contains the findings of each group during the initial phase of the project. diff --git a/doc/dev/README.md b/doc/dev/README.md new file mode 100644 index 00000000..4a226822 --- /dev/null +++ b/doc/dev/README.md @@ -0,0 +1,7 @@ +# Development Notes + +This folder contains non-canonical development handoff and progress notes that preserve in-flight work without replacing the main subsystem or contributor documentation. + +## Progress Notes + +- [2026-04-29-route-validation-and-map-context-handoff.md](./progress/2026-04-29-route-validation-and-map-context-handoff.md) diff --git a/doc/dev/progress/2026-04-29-route-validation-and-map-context-handoff.md b/doc/dev/progress/2026-04-29-route-validation-and-map-context-handoff.md new file mode 100644 index 00000000..b878dd4a --- /dev/null +++ b/doc/dev/progress/2026-04-29-route-validation-and-map-context-handoff.md @@ -0,0 +1,127 @@ +Created: 2026-04-29T08:50:23+02:00 +Last updated: 2026-04-29T08:50:23+02:00 + +# Route Validation And Map Context Handoff + +## Motivation + +Capture the current chat state as a handoff and progress snapshot for the active branch work. The session currently spans three threads: + +- deterministic ROS2 route validation and startup/runtime synchronization, +- lightweight route metrics and evidence collection for intersection stops and frame-barrier fallbacks, +- richer intermediate-layer map queries so planning and perception can ask for adjacent-lane presence and traversability. + +## Sources inspected + +- `agents.md` +- `.github/instructions/docs-and-reasoning.instructions.md` +- current chat state and compacted conversation summary from 2026-04-28 to 2026-04-29 +- `build/docker-compose.carla.cuda.yaml` +- `code/paf_common/paf_common/route_metrics.py` +- `code/control/control/vehicle_controller.py` +- `code/planning/planning/behavior_agent/behaviors/intersection.py` +- `code/agent/agent/data_management_node.py` +- `code/agent/agent/startup_coordinator.py` +- `code/leaderboard_launcher/leaderboard_launcher/paf_agent_base.py` +- `code/leaderboard_launcher/scripts/launch_leaderboard.sh` +- `code/planning/planning/global_planner/global_planner_node.py` +- `code/test/run_test.py` +- `code/test/test_route_metrics.py` +- `code/test/test_deterministic_sync.py` +- `code/mapping/mapping_common/map.py` +- `code/mapping/test/test_mapping_common/test_map.py` +- `code/mapping/README.md` +- planning call sites in `lane_change.py`, `overtake.py`, and `leave_parking_space.py` +- runtime evidence from `/internal_workspace/log/ros/agent.log`, `/internal_workspace/simulation_results.json`, and `/tmp/paf_route_metrics.json` + +## Progress Summary + +### 1. Route metrics instrumentation is in place + +The branch now records route-level evidence for two concrete behaviors: + +- `planning.intersection.unnecessary_stop_candidates` +- `control.vehicle_controller.frame_barrier_fallback_frames` + +The shared helper lives in `code/paf_common/paf_common/route_metrics.py`. Metrics are reset before route runs and merged back into leaderboard checkpoint output afterward. + +### 2. ROS2 leaderboard startup deadlocks were removed + +The ROS2 route path was pushed from repeated agent setup failure into actual route execution. The main fixes already applied in this chat were: + +- preserve the CARLA CUDA Vulkan ICD override in `build/docker-compose.carla.cuda.yaml`, +- publish startup readiness earlier from `data_management_node.py`, +- reduce startup coordinator requirements to a route-independent minimum in `startup_coordinator.py`, +- locally override the ROS2 leaderboard wrapper in `paf_agent_base.py` so the bridge launches with the hero ego role and avoids constructor-time waits that deadlocked before the world was ticking, +- reset, merge, and print route metrics in `launch_leaderboard.sh`. + +### 3. Live route validation now reaches route execution but not route completion + +The authoritative ROS2 validation path is `leaderboard.dev`, not the legacy ROS1 `leaderboard.test` harness. + +The last live validation run reached `> Running the route`, registered sensors in the checkpoint, and showed active CARLA ticks and agent wallclock/game-time output. That is enough to confirm that the original startup deadlocks were removed. + +The remaining runtime blocker is later in the stack: + +- `PrePlanner` in `code/planning/planning/global_planner/global_planner_node.py` repeatedly logs `Waiting for agent position to stabilize`, +- no global trajectory is published, +- the agent keeps returning timestamp `0` vehicle commands, +- the route therefore does not yet produce a meaningful completed-drive metrics snapshot. + +### 4. Intermediate-layer map knowledge was extended + +`mapping_common.map.MapTree` now exposes a richer lane-query surface: + +- `LanePresence` +- `LaneContext` +- `AdjacentLaneContext` +- `MapTree.get_lane_presence()` +- `MapTree.get_lane_context()` +- `MapTree.get_adjacent_lane_context()` + +This separates three cases that were previously collapsed together by `is_lane_free()` call sites: + +- adjacent lane exists and is free, +- adjacent lane exists but is blocked, +- adjacent lane is absent or lane availability is unknown. + +At this point, the new API exists in the intermediate layer and is documented/tested, but downstream planning behaviors still need to consume it. + +### 5. Branch-review meta-status + +The higher-level branch review work is partially progressed: + +- branch delta inspected, +- core guidance docs read, +- branch narrative docs partially read, +- branch goal summary still pending. + +## Validation Completed + +- Focused deterministic-sync regression tests passed earlier in the session: `code/test/test_deterministic_sync.py` (`8 passed`) +- Narrow ROS2 package builds passed earlier in the session for the touched route-validation packages +- `colcon build --symlink-install --packages-select mapping` passed in `build-agent-dev-1` +- `PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest /workspace/code/mapping/test/test_mapping_common/test_map.py` passed (`3 passed`) +- Narrow Ruff checks for the touched mapping files passed via `python3 -m ruff check ...` and `python3 -m ruff format --check ...` + +## Open Blockers And Risks + +- `leaderboard.dev` still does not complete a real driving route end to end. +- `global_planner_node.py` stabilization logic is the current local bottleneck. +- The last live bridge logs still indicated `synchronous_mode_wait_for_vehicle_control_command: False`, which is likely invalid for the intended deterministic frame-barrier contract. +- Route metrics are implemented but not yet proven by a completed ROS2 route that actually drives through the environment. +- The new lane-context API is not yet wired into lane-change, overtake, or other planning decisions. + +## Recommended Next Actions + +1. Update `code/leaderboard_launcher/leaderboard_launcher/paf_agent_base.py` so the bridge runs with `synchronous_mode_wait_for_vehicle_control_command=True`, then rerun `leaderboard.dev`. +2. Inspect and likely adjust the startup stabilization rule in `code/planning/planning/global_planner/global_planner_node.py` against the observed live localization stream. +3. Once the route drives, inspect `/tmp/paf_route_metrics.json` and the merged metrics in `/internal_workspace/simulation_results.json`. +4. Consume `MapTree.get_lane_context()` or `get_adjacent_lane_context()` from lane-change and overtake behaviors so absent-lane and blocked-lane cases are handled distinctly. +5. Finish reading the branch narrative documents and write the branch goal summary. + +## Handoff Notes + +- The latest stuck live route terminals were intentionally killed after collecting enough evidence to prove that route startup had been unblocked. +- The current branch state already contains the route-metrics helper, ROS2 startup fixes, and the new map lane-context query surface. +- If this work resumes later, start from the ROS2 `leaderboard.dev` path and the `global_planner_node.py` stabilization gate rather than revisiting the earlier startup-deadlock surfaces first. diff --git a/doc/mapping/README.md b/doc/mapping/README.md index b0df8bbb..b09ef2f9 100644 --- a/doc/mapping/README.md +++ b/doc/mapping/README.md @@ -46,6 +46,7 @@ To do intersection checks on the map: - [`map_tree.get_overlapping_entities()`](/doc/mapping/generated/mapping_common/map.md#mapping_common.map.MapTree.get_overlapping_entities) - [`map_tree.get_nearest_entity()`](/doc/mapping/generated/mapping_common/map.md#mapping_common.map.MapTree.get_nearest_entity) - [`map_tree.is_lane_free()`](/doc/mapping/generated/mapping_common/map.md#mapping_common.map.MapTree.is_lane_free) + - [`map_tree.get_lane_context()`](/doc/mapping/generated/mapping_common/map.md#mapping_common.map.MapTree.get_lane_context) and [`map_tree.get_adjacent_lane_context()`](/doc/mapping/generated/mapping_common/map.md#mapping_common.map.MapTree.get_adjacent_lane_context) to query whether adjacent lanes exist and whether they are currently traversable - Functions for creating collision masks can be found in the [mapping_common.mask](/doc/mapping/generated/mapping_common/mask.md) module For intersection-related traffic checks, dynamic entities can also be evaluated using motion information and speed thresholds. From 0c870dedd716f6d11198a4d90ac485b95efbe3cf Mon Sep 17 00:00:00 2001 From: ll7 Date: Wed, 29 Apr 2026 10:09:59 +0200 Subject: [PATCH 34/43] docs: require chat handoff progress notes --- .../docs-and-reasoning.instructions.md | 4 +- CLAUDE.md | 4 +- agents.md | 5 ++- doc/README.md | 2 +- doc/dev/README.md | 1 + .../2026-04-29-agent-handoff-rules.md | 40 +++++++++++++++++++ doc/development/context_retention.md | 5 ++- 7 files changed, 56 insertions(+), 5 deletions(-) create mode 100644 doc/dev/progress/2026-04-29-agent-handoff-rules.md diff --git a/.github/instructions/docs-and-reasoning.instructions.md b/.github/instructions/docs-and-reasoning.instructions.md index 2603fe8b..ad55fc7f 100644 --- a/.github/instructions/docs-and-reasoning.instructions.md +++ b/.github/instructions/docs-and-reasoning.instructions.md @@ -6,7 +6,9 @@ applyTo: "doc/**/*.md,README.md,agents.md" # Docs And Reasoning - Active documentation must match the current implementation. If code and docs disagree, either fix the doc in the same change or call out the gap explicitly. -- Keep canonical behavior and interface docs in their domain folders under `doc/` or the package docs. Use `doc/reasoning/` for analysis notes, comparisons, migration thoughts, and development output that should not become the source of truth. +- Keep canonical behavior and interface docs in their domain folders under `doc/` or the package docs. Use `doc/dev/progress/` for timestamped progress and handoff notes that capture the current chat state, and use `doc/reasoning/` for deeper analysis notes, comparisons, migration thoughts, and development output that should not become the source of truth. +- When saving a progress or handoff note, include the timestamp, motivating task, current chat state, source files or repositories inspected, validation status, remaining blockers, and recommended follow-ups. - When saving a reasoning note, include the motivating task, the source files or repositories inspected, the main conclusion, and the remaining follow-ups. - Link new documentation from `doc/README.md` or the most relevant existing index page so it stays discoverable. +- Add documentation hints for new progress notes by updating `doc/dev/README.md`, `doc/README.md`, or the closest relevant module or development doc. - Prefer short, actionable Markdown over long narrative dumps. Remove or update stale statements instead of piling on contradictory notes. diff --git a/CLAUDE.md b/CLAUDE.md index d9653e96..81439c1d 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -8,4 +8,6 @@ For non-trivial work, also read: - `doc/dev_talks/paf25/future_work.md` for the recommended execution order - `doc/dev_talks/paf25/improvements_assessment.md` for the consolidation-first development direction -When work produces analysis, comparisons, or handoff notes that should survive the chat session, store them under `doc/reasoning/` and keep the canonical behavior documentation in the appropriate `doc/` or package folder. +When work produces progress or handoff notes that should preserve the current chat state, store them as timestamped files under `doc/dev/progress/` and keep them discoverable via `doc/dev/README.md` and the relevant documentation index. + +When work produces deeper analysis, comparisons, or longer-form reasoning that should survive the chat session, store them under `doc/reasoning/` and keep the canonical behavior documentation in the appropriate `doc/` or package folder. diff --git a/agents.md b/agents.md index 8bbfa04f..c989ef45 100644 --- a/agents.md +++ b/agents.md @@ -85,13 +85,16 @@ Do not attempt to fix unrelated failing tests/lints outside the requested scope. - Update docs when behavior, setup, commands, or interfaces change. - For developer-facing changes, prefer updating docs under `doc/development/` or package README files. -- Use `doc/reasoning/` for preserved analysis, comparisons, and handoff notes that should not become the canonical behavior documentation. +- Use `doc/dev/progress/` for timestamped progress and handoff notes that preserve the current chat or implementation state without becoming the canonical behavior documentation. +- Use `doc/reasoning/` for preserved analysis, comparisons, migration notes, and longer-form reasoning that should survive the chat session but should not replace canonical docs. - Keep Markdown concise, structured, and actionable. - Capture non-trivial architectural decisions in `doc/adr/` using the ADR template. +- When a progress or handoff note is created, add or update documentation hints that make it discoverable from `doc/dev/README.md`, `doc/README.md`, or the most relevant nearby package or development doc. ## 9.1) Planning and reasoning support - For non-trivial work, use `.agent/PLANS.md` to make scope, evidence, validation, and follow-ups explicit. +- For non-trivial or multi-step work, always create or update a timestamped note under `doc/dev/progress/` before finishing or handing off. The note should capture the current chat state, the motivating task, files or systems inspected, validation performed, remaining blockers, and recommended next actions. - Treat `doc/dev_talks/paf25/future_work.md` and `doc/dev_talks/paf25/improvements_assessment.md` as direction-setting documents for repository-wide cleanup and development workflow changes. ## 10) Git and PR hygiene diff --git a/doc/README.md b/doc/README.md index 17d8f9de..96151263 100644 --- a/doc/README.md +++ b/doc/README.md @@ -26,7 +26,7 @@ The [`development`](./development/) folder contains guidelines for developing in ## `dev` -The [`dev`](./dev/README.md) folder contains development handoff and progress notes that capture in-flight work without replacing the canonical subsystem or contributor documentation. The current chat snapshot is stored in [2026-04-29-route-validation-and-map-context-handoff.md](./dev/progress/2026-04-29-route-validation-and-map-context-handoff.md). +The [`dev`](./dev/README.md) folder contains development handoff and progress notes that capture in-flight work without replacing the canonical subsystem or contributor documentation. Use the [Development Notes index](./dev/README.md) to find the latest handoff and progress snapshots. ## `research` diff --git a/doc/dev/README.md b/doc/dev/README.md index 4a226822..6b7162d4 100644 --- a/doc/dev/README.md +++ b/doc/dev/README.md @@ -4,4 +4,5 @@ This folder contains non-canonical development handoff and progress notes that p ## Progress Notes +- [2026-04-29-agent-handoff-rules.md](./progress/2026-04-29-agent-handoff-rules.md) - [2026-04-29-route-validation-and-map-context-handoff.md](./progress/2026-04-29-route-validation-and-map-context-handoff.md) diff --git a/doc/dev/progress/2026-04-29-agent-handoff-rules.md b/doc/dev/progress/2026-04-29-agent-handoff-rules.md new file mode 100644 index 00000000..a0fcbd06 --- /dev/null +++ b/doc/dev/progress/2026-04-29-agent-handoff-rules.md @@ -0,0 +1,40 @@ +Created: 2026-04-29T10:09:26+02:00 +Last updated: 2026-04-29T10:09:26+02:00 + +# Agent Handoff Rules Update + +## Motivation + +Codify a single repository rule for preserving current chat state at handoff time so future agent sessions always leave behind a timestamped progress note plus documentation hints that point to it. + +## Sources inspected + +- `agents.md` +- `CLAUDE.md` +- `.github/instructions/docs-and-reasoning.instructions.md` +- `doc/development/context_retention.md` +- `doc/dev/README.md` +- `doc/README.md` + +## Current conclusion + +The repository previously had conflicting guidance: + +- `agents.md` and `CLAUDE.md` treated `doc/reasoning/` as the home for handoff notes, +- the newer workflow already introduced `doc/dev/progress/` for timestamped chat-state handoffs, +- `doc/README.md` pointed directly to one specific handoff note instead of the `doc/dev/` index. + +The rule set is now aligned so that: + +1. timestamped progress and handoff notes live in `doc/dev/progress/`, +2. deeper analysis and comparison notes stay in `doc/reasoning/`, +3. new progress notes must be discoverable through documentation hints such as `doc/dev/README.md`, `doc/README.md`, or the nearest relevant development or module documentation. + +## Validation + +- Checked the edited instruction and documentation files for editor-reported errors. + +## Follow-up + +- Future non-trivial chats should either create or update a timestamped note in `doc/dev/progress/` before the agent finishes. +- Keep `doc/README.md` pointing to the `doc/dev/` index rather than rotating a hard-coded link to a single note. \ No newline at end of file diff --git a/doc/development/context_retention.md b/doc/development/context_retention.md index 39e50f53..3ac8fcee 100644 --- a/doc/development/context_retention.md +++ b/doc/development/context_retention.md @@ -13,7 +13,10 @@ Long-lived projects lose intent when design decisions only live in chat threads - Capture assumptions, validation, and known gaps in every PR. 3. Test markers and logs - Preserve behavior expectations with marker-based tests and structured logs. -4. Reasoning notes for non-trivial improvement work +4. Progress and handoff notes for non-trivial work + - Keep timestamped progress and handoff notes in `doc/dev/progress/` when the current chat state, validation state, blockers, or next actions need to survive the session. + - Add documentation hints in `doc/dev/README.md`, `doc/README.md`, or the closest relevant development or module doc so the latest handoff is easy to find. +5. Reasoning notes for non-trivial improvement work - Keep analysis and comparison notes in `doc/reasoning/` when they are worth preserving but are not the canonical source of truth. - Promote the stable parts into `doc/development/`, `doc//`, or ADRs when the behavior or policy is finalized. From 735f07d9eb458693d643dea767b0ab62bf35a619 Mon Sep 17 00:00:00 2001 From: ll7 Date: Wed, 29 Apr 2026 10:13:11 +0200 Subject: [PATCH 35/43] docs: ensure newline at end of agent handoff rules document --- doc/dev/progress/2026-04-29-agent-handoff-rules.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/dev/progress/2026-04-29-agent-handoff-rules.md b/doc/dev/progress/2026-04-29-agent-handoff-rules.md index a0fcbd06..f261be97 100644 --- a/doc/dev/progress/2026-04-29-agent-handoff-rules.md +++ b/doc/dev/progress/2026-04-29-agent-handoff-rules.md @@ -37,4 +37,4 @@ The rule set is now aligned so that: ## Follow-up - Future non-trivial chats should either create or update a timestamped note in `doc/dev/progress/` before the agent finishes. -- Keep `doc/README.md` pointing to the `doc/dev/` index rather than rotating a hard-coded link to a single note. \ No newline at end of file +- Keep `doc/README.md` pointing to the `doc/dev/` index rather than rotating a hard-coded link to a single note. From 98b7068c9e0ca217bafd139802e754838f2010d0 Mon Sep 17 00:00:00 2001 From: ll7 Date: Thu, 30 Apr 2026 22:48:39 +0200 Subject: [PATCH 36/43] control: bound route startup stabilization --- agents.md | 2 +- .../leaderboard_launcher/paf_agent_base.py | 7 +- .../global_planner/global_planner_node.py | 50 ++++++------- .../global_planner/position_stability.py | 41 +++++++++++ code/test/test_deterministic_sync.py | 7 +- code/test/test_planning_regression.py | 16 +++++ doc/dev/README.md | 1 + .../progress/2026-04-30-route-sync-bridge.md | 71 +++++++++++++++++++ doc/planning/Global_Planner.md | 6 ++ 9 files changed, 166 insertions(+), 35 deletions(-) create mode 100644 code/planning/planning/global_planner/position_stability.py create mode 100644 doc/dev/progress/2026-04-30-route-sync-bridge.md diff --git a/agents.md b/agents.md index c989ef45..e048c799 100644 --- a/agents.md +++ b/agents.md @@ -35,6 +35,7 @@ If guidance conflicts, prefer repository config files and actively used CI/lint - Python target is **3.12** (`ruff.toml`). - Use containerized workflows when project docs expect them. - Before compose-based CARLA runs, refresh `build/.env` via `scripts/update-dotenv.sh`; in SSH-forwarded or headless sessions it sets `RENDER_OFFSCREEN=-RenderOffScreen` so the simulator can start without a local desktop renderer. +- If you run CARLA, always stop the CARLA simulator, compose stack, and any related route-validation processes before handing off or finishing. ## 5) Code change rules @@ -124,4 +125,3 @@ Before finishing, ensure: - Follow `doc/development/developer_contract.md` for PR assumptions, validation summary, and known gaps. - Use `pytest` markers defined in `pytest.ini` (`unit`, `integration`, `sim`) to scope validation. - Use `ruff-strict.toml` for incremental hardening of packages (docstring and maintainability checks). - diff --git a/code/leaderboard_launcher/leaderboard_launcher/paf_agent_base.py b/code/leaderboard_launcher/leaderboard_launcher/paf_agent_base.py index a1caea26..61b3daa9 100755 --- a/code/leaderboard_launcher/leaderboard_launcher/paf_agent_base.py +++ b/code/leaderboard_launcher/leaderboard_launcher/paf_agent_base.py @@ -7,7 +7,6 @@ from leaderboard.autoagents.autonomous_agent import AutonomousAgent, Track from leaderboard.autoagents.ros2_agent import ROS2Agent from leaderboard.autoagents.ros_base_agent import ROSLauncher -import math import rclpy from rclpy.qos import DurabilityPolicy, QoSProfile @@ -36,6 +35,7 @@ def __init__(self, carla_host, carla_port, debug=False): "port": carla_port, "timeout": 60, "synchronous_mode": True, + "synchronous_mode_wait_for_vehicle_control_command": True, "passive": True, "register_all_sensors": False, "ego_vehicle_role_name": "\"['hero']\"", @@ -83,7 +83,9 @@ def __init__(self, carla_host, carla_port, debug=False): qos_profile=QoSProfile(depth=1), ) - self.spin_thread = threading.Thread(target=rclpy.spin, args=(self.ros_node,)) + self.spin_thread = threading.Thread( + target=rclpy.spin, args=(self.ros_node,) + ) self.spin_thread.start() except Exception: self._cleanup_partial_ros2_setup() @@ -206,4 +208,3 @@ def _cleanup_partial_ros2_setup(self): if bridge_process is not None and bridge_process.is_alive(): bridge_process.terminate() - diff --git a/code/planning/planning/global_planner/global_planner_node.py b/code/planning/planning/global_planner/global_planner_node.py index 4eac2b5a..3ffdb4a4 100755 --- a/code/planning/planning/global_planner/global_planner_node.py +++ b/code/planning/planning/global_planner/global_planner_node.py @@ -1,5 +1,4 @@ -from collections import deque -from typing import Deque, Optional +from typing import Optional from xml.etree import ElementTree as eTree import rclpy @@ -19,10 +18,9 @@ GetSpeedLimits, ) -from mapping_common.transform import Point2 - from paf_common.exceptions import emsg_with_trace +from .position_stability import PositionStabilityGate from .preplanning_trajectory import OpenDriveConverter # TODO: These definition do not align with the CarlaRoute.RIGHT, etc.. definitions. @@ -54,9 +52,6 @@ def __init__(self): # Working variables self.odc = None self.route_recalculation_required: bool = True - self.position_stabilized: bool = False - self.last_agent_positions: Deque[Point] = deque() - self.last_agent_positions_count_target = 5 self.agent_pos = None self.agent_ori = None @@ -70,6 +65,23 @@ def __init__(self): "distance_spawn_to_first_wp", 100.0, ).value + position_stabilization_samples = self.declare_parameter( + "position_stabilization_samples", + 5, + ).value + position_stabilization_distance_m = self.declare_parameter( + "position_stabilization_distance_m", + 0.5, + ).value + position_stabilization_max_unstable_samples = self.declare_parameter( + "position_stabilization_max_unstable_samples", + 50, + ).value + self.position_stability_gate = PositionStabilityGate( + sample_count_target=position_stabilization_samples, + stable_distance_m=position_stabilization_distance_m, + max_unstable_samples=position_stabilization_max_unstable_samples, + ) # Services # Get created only after data is available @@ -379,30 +391,8 @@ async def position_callback(self, data: PoseStamped): (needed for the trajectory preplanning) :param data: updated CarlaWorldInformation """ - if len(self.last_agent_positions) < self.last_agent_positions_count_target: - self.get_logger().info( - "Waiting for agent positions", throttle_duration_sec=2 - ) - self.last_agent_positions.append(data.pose.position) - self.agent_pos = None - self.agent_ori = None - return - agent_pos = data.pose.position - agent_point = Point2.new(agent_pos.x, agent_pos.y) - - # Check if our position has stabilized - if not self.position_stabilized: - self.position_stabilized = True - for pos in self.last_agent_positions: - pos_point = Point2.new(pos.x, pos.y) - if pos_point.distance_to(agent_point) > 0.5: - self.position_stabilized = False - - self.last_agent_positions.popleft() - self.last_agent_positions.append(agent_pos) - - if not self.position_stabilized: + if not self.position_stability_gate.update(agent_pos.x, agent_pos.y): self.get_logger().info( "Waiting for agent position to stabilize", throttle_duration_sec=2, diff --git a/code/planning/planning/global_planner/position_stability.py b/code/planning/planning/global_planner/position_stability.py new file mode 100644 index 00000000..7bfd55af --- /dev/null +++ b/code/planning/planning/global_planner/position_stability.py @@ -0,0 +1,41 @@ +from collections import deque +from dataclasses import dataclass, field +from math import hypot +from typing import Deque + + +@dataclass +class PositionStabilityGate: + sample_count_target: int + stable_distance_m: float + max_unstable_samples: int + _samples: Deque[tuple[float, float]] = field(default_factory=deque, init=False) + _unstable_samples: int = field(default=0, init=False) + _accepted: bool = field(default=False, init=False) + + def update(self, x: float, y: float) -> bool: + if self._accepted: + return True + + current = (x, y) + if len(self._samples) < self.sample_count_target: + self._samples.append(current) + return False + + stable = all( + hypot(x - previous_x, y - previous_y) <= self.stable_distance_m + for previous_x, previous_y in self._samples + ) + self._samples.popleft() + self._samples.append(current) + + if stable: + self._accepted = True + return True + + self._unstable_samples += 1 + if self._unstable_samples >= self.max_unstable_samples: + self._accepted = True + return True + + return False diff --git a/code/test/test_deterministic_sync.py b/code/test/test_deterministic_sync.py index a451700f..db931672 100644 --- a/code/test/test_deterministic_sync.py +++ b/code/test/test_deterministic_sync.py @@ -111,7 +111,12 @@ def test_sync_contract_files_reflect_barrier_design() -> None: assert "self.global_plan is not None" not in startup_ready_block assert "self.open_drive_string is not None" in startup_ready_block assert '"ego_vehicle_role_name": "\\"[\'hero\']\\""' in paf_agent_source - assert 'wait_for_message(self.ros_node, "/carla/hero/status"' not in paf_agent_source + assert '"synchronous_mode_wait_for_vehicle_control_command": True' in ( + paf_agent_source + ) + assert ( + 'wait_for_message(self.ros_node, "/carla/hero/status"' not in paf_agent_source + ) assert '"data_management"' in startup_required_nodes_block assert '"motion_planning"' not in startup_required_nodes_block diff --git a/code/test/test_planning_regression.py b/code/test/test_planning_regression.py index 3cda91f3..dd0ba8a0 100644 --- a/code/test/test_planning_regression.py +++ b/code/test/test_planning_regression.py @@ -33,3 +33,19 @@ def test_linear_interpolation_snapshot(planning_help_functions) -> None: def test_scale_vector_zero_is_stable(planning_help_functions) -> None: """Check behavior for zero vectors remains deterministic and safe.""" assert planning_help_functions.scale_vector((0.0, 0.0), 5.0) == (0, 0) + + +def test_position_stability_gate_accepts_after_bounded_unstable_window() -> None: + """Prevent startup preplanning from waiting forever on drifting localization.""" + position_stability = importlib.import_module( + "planning.global_planner.position_stability" + ) + gate = position_stability.PositionStabilityGate( + sample_count_target=3, + stable_distance_m=0.5, + max_unstable_samples=4, + ) + + decisions = [gate.update(float(x), 0.0) for x in range(7)] + + assert decisions == [False, False, False, False, False, False, True] diff --git a/doc/dev/README.md b/doc/dev/README.md index 6b7162d4..a0bbb2dc 100644 --- a/doc/dev/README.md +++ b/doc/dev/README.md @@ -4,5 +4,6 @@ This folder contains non-canonical development handoff and progress notes that p ## Progress Notes +- [2026-04-30-route-sync-bridge.md](./progress/2026-04-30-route-sync-bridge.md) - [2026-04-29-agent-handoff-rules.md](./progress/2026-04-29-agent-handoff-rules.md) - [2026-04-29-route-validation-and-map-context-handoff.md](./progress/2026-04-29-route-validation-and-map-context-handoff.md) diff --git a/doc/dev/progress/2026-04-30-route-sync-bridge.md b/doc/dev/progress/2026-04-30-route-sync-bridge.md new file mode 100644 index 00000000..111ab824 --- /dev/null +++ b/doc/dev/progress/2026-04-30-route-sync-bridge.md @@ -0,0 +1,71 @@ +Created: 2026-04-30T22:29:21+02:00 +Last updated: 2026-04-30T22:48:01+02:00 + +# Route Sync Bridge Progress + +## Motivation + +Continue the route-validation implementation from +`2026-04-29-route-validation-and-map-context-handoff.md`, starting with the +recorded blocker that the overridden ROS2 leaderboard bridge did not pass +`synchronous_mode_wait_for_vehicle_control_command=True`. + +## Sources inspected + +- `doc/dev/progress/2026-04-29-route-validation-and-map-context-handoff.md` +- `code/leaderboard_launcher/leaderboard_launcher/paf_agent_base.py` +- `code/leaderboard_launcher/launch/ros_bridge.dev.xml` +- `code/test/test_deterministic_sync.py` +- `code/planning/planning/global_planner/global_planner_node.py` + +## Current chat state + +The wrapper launch parameters in `paf_agent_base.py` now explicitly set +`synchronous_mode_wait_for_vehicle_control_command` to `True`, matching the +existing `ros_bridge.dev.xml` contract and the control frame-barrier design. + +The deterministic sync regression test now checks both launch surfaces: + +- the XML `synchronous_mode_wait_for_vehicle_control_command` default, +- the Python leaderboard wrapper parameter passed to `ROSLauncher.run()`. + +The `PrePlanner` startup position gate now uses a pure +`PositionStabilityGate`. It keeps the existing stable-window behavior, but adds +a bounded unstable sample window through +`position_stabilization_max_unstable_samples` so route preplanning cannot wait +forever on a drifting localization stream. The existing +`distance_spawn_to_first_wp` check still rejects poses that are too far away +from the route start before a global trajectory is generated. + +`agents.md` now also records the operational cleanup rule that any CARLA run +must be stopped, including the simulator, compose stack, and related +route-validation processes, before handoff or completion. + +## Validation + +- Red check: the focused deterministic sync contract test failed before the + Python wrapper parameter was added. +- Green check: + `uv run --with pytest==9.0.2 python -m pytest code/test/test_deterministic_sync.py -q` + passed with `8 passed`. +- Red check: the focused planning regression test failed while + `planning.global_planner.position_stability` did not exist. +- Green check: + `uv run --with pytest==9.0.2 --with numpy python -m pytest code/test/test_planning_regression.py -q` + passed with `3 passed`. + +## Remaining blockers + +- `leaderboard.dev` still needs a live rerun with CARLA to verify the bridge + now waits for vehicle-control commands in the actual route. +- Route metrics still need proof from a completed ROS2 drive. +- Planning behaviors still need to consume the new `MapTree` lane-context API. + +## Recommended next actions + +1. Run the documented `leaderboard.dev` route validation path with CARLA. +2. Inspect bridge logs for + `synchronous_mode_wait_for_vehicle_control_command: True`. +3. If the route still does not publish a global trajectory, inspect the + `distance_spawn_to_first_wp` check and the OpenDRIVE/global-plan service + responses in `PrePlanner.process_global_plan()`. diff --git a/doc/planning/Global_Planner.md b/doc/planning/Global_Planner.md index 0928e7b3..5186b010 100644 --- a/doc/planning/Global_Planner.md +++ b/doc/planning/Global_Planner.md @@ -53,6 +53,12 @@ The received agent spawn position is valid if it´s closer to the first waypoint parameter expresses. This is necessary to prevent unwanted behaviour in the startup phase where the current agent position is faulty. +Before this distance check runs, the planner waits for the incoming `/paf/{role_name}/global_current_pos` +stream to stabilize. The stabilization gate is controlled by `position_stabilization_samples`, +`position_stabilization_distance_m`, and `position_stabilization_max_unstable_samples`. If localization keeps +drifting for the bounded unstable sample window, preplanning continues with the latest pose and still relies on +`distance_spawn_to_first_wp` to reject poses that are too far away from the route start. + When the ODC is initialised, the current agent position is received and the global plan is obtained from the leaderboard. The trajectory can be calculated by iterating through the global route and passing it to the ODC. After smaller outliners are removed the x and y coordinates as well as the yaw-orientation and the prevailing From 7160f20e88064a6622a5409289506500aca67e7f Mon Sep 17 00:00:00 2001 From: ll7 Date: Thu, 30 Apr 2026 22:55:42 +0200 Subject: [PATCH 37/43] ci: fix checkout-safe issue sweep --- .github/workflows/drive.yml | 14 ++++++------ .github/workflows/markdownlint.yml | 2 +- .../test/test_coordinate_transformation.py | 11 +++++++--- code/paf_common/paf_common/debugging.py | 2 +- .../global_planner/global_planner_node.py | 11 ++++++++-- code/test/run_test.py | 8 +++---- code/test/test_planning_regression.py | 18 +++++++++++++++ code/test/test_route_metrics.py | 12 +++++----- .../progress/2026-04-30-route-sync-bridge.md | 22 ++++++++++++++++++- doc/development/build_action.md | 4 ++-- doc/development/distributed_simulation.md | 8 +++---- doc/development/drive_action.md | 20 ++++++++--------- doc/development/first_steps.md | 4 ++-- doc/general/tests.md | 2 +- 14 files changed, 93 insertions(+), 45 deletions(-) diff --git a/.github/workflows/drive.yml b/.github/workflows/drive.yml index 9b4d4059..648ea975 100644 --- a/.github/workflows/drive.yml +++ b/.github/workflows/drive.yml @@ -16,13 +16,13 @@ jobs: if: ${{ github.event.workflow_run.conclusion == 'success' }} steps: - name: Checkout repository - uses: actions/checkout@v3 + uses: actions/checkout@v5 - name: Print environment variables (DEBUG) run: | echo "AGENT_VERSION=${AGENT_VERSION}" echo "COMPOSE_FILE=${COMPOSE_FILE}" - name: Download artifact - uses: actions/github-script@v6 + uses: actions/github-script@v8 with: script: | let allArtifacts = await github.rest.actions.listWorkflowRunArtifacts({ @@ -49,27 +49,27 @@ jobs: run: unzip artifact.zip - name: Return artifact JSON id: return-artifact-json - uses: actions/github-script@v6 + uses: actions/github-script@v8 with: script: | let fs = require('fs'); let data = JSON.parse(fs.readFileSync(`${process.env.GITHUB_WORKSPACE}/artifact.json`)); return data; - - name: Run docker-compose + - name: Run Docker Compose run: | xhost +local: USERNAME=$(whoami) USER_UID=$(id -u) USER_GID=$(id -g) RENDER_OFFSCREEN=-RenderOffScreen docker compose up --quiet-pull --exit-code-from agent - name: Copy results run: docker compose cp agent:/tmp/simulation_results.json . - - name: Stop docker-compose + - name: Stop Docker Compose # always run this step, to clean up even on error if: always() run: docker compose down -v # add rendered JSON as comment to the pull request - name: Create simulation results table id: simulation-results - uses: actions/github-script@v6 + uses: actions/github-script@v8 with: script: | const fs = require('fs'); @@ -94,7 +94,7 @@ jobs: echo "${{ steps.simulation-results.outputs.result }}" - name: Add simulation results as comment if: ${{ steps.return-artifact-json.outputs.result.is_pr }} - uses: actions/github-script@v6 + uses: actions/github-script@v8 with: github-token: ${{ secrets.GITHUB_TOKEN }} # this script reads the simulation_results.json and creates a comment on the pull request with the results. diff --git a/.github/workflows/markdownlint.yml b/.github/workflows/markdownlint.yml index c330bf42..bf4b6c85 100644 --- a/.github/workflows/markdownlint.yml +++ b/.github/workflows/markdownlint.yml @@ -14,7 +14,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out the repo - uses: actions/checkout@v2 + uses: actions/checkout@v5 # Execute the markdown linter - name: Run the markdown linter uses: addnab/docker-run-action@v3 diff --git a/code/localization/test/test_coordinate_transformation.py b/code/localization/test/test_coordinate_transformation.py index b76adaa2..f9bc4223 100644 --- a/code/localization/test/test_coordinate_transformation.py +++ b/code/localization/test/test_coordinate_transformation.py @@ -1,10 +1,15 @@ +import importlib + import pytest pyproj = pytest.importorskip("pyproj") -from localization.coordinate_transformation import ( # noqa: E402 - CoordinateTransformer, - extract_geo_reference_from_opendrive, +coordinate_transformation = importlib.import_module( + "localization.coordinate_transformation" +) +CoordinateTransformer = coordinate_transformation.CoordinateTransformer +extract_geo_reference_from_opendrive = ( + coordinate_transformation.extract_geo_reference_from_opendrive ) diff --git a/code/paf_common/paf_common/debugging.py b/code/paf_common/paf_common/debugging.py index 407015a1..266db9bc 100644 --- a/code/paf_common/paf_common/debugging.py +++ b/code/paf_common/paf_common/debugging.py @@ -26,7 +26,7 @@ def start_debugger( host: str = "127.0.0.1", port: int = 53000, wait_for_client: bool = False, -)-> None: +) -> None: """Start a debugpy listener for the current node when debugpy is available. Args: diff --git a/code/planning/planning/global_planner/global_planner_node.py b/code/planning/planning/global_planner/global_planner_node.py index 3ffdb4a4..15ac7df2 100755 --- a/code/planning/planning/global_planner/global_planner_node.py +++ b/code/planning/planning/global_planner/global_planner_node.py @@ -4,6 +4,7 @@ import rclpy import rclpy.callback_groups from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile from rclpy.service import Service from transforms3d.euler import euler2quat @@ -113,13 +114,19 @@ def __init__(self): self.global_trajectory_updated_pub = self.create_publisher( msg_type=Bool, topic=f"/paf/{self.role_name}/data/planning/global_trajectory_updated", - qos_profile=1, + qos_profile=QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ), ) self.speed_limit_updated_pub = self.create_publisher( msg_type=Bool, topic=f"/paf/{self.role_name}/data/planning/speed_limits_updated", - qos_profile=1, + qos_profile=QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ), ) # Service clients diff --git a/code/test/run_test.py b/code/test/run_test.py index ee0dea34..51104078 100644 --- a/code/test/run_test.py +++ b/code/test/run_test.py @@ -44,11 +44,9 @@ if PAF_COMMON_SRC not in sys.path: sys.path.insert(0, PAF_COMMON_SRC) -from paf_common.route_metrics import ( - load_route_metrics, - merge_route_metrics_into_checkpoint, - reset_route_metrics_file, -) +route_metrics = importlib.import_module("paf_common.route_metrics") +merge_route_metrics_into_checkpoint = route_metrics.merge_route_metrics_into_checkpoint +reset_route_metrics_file = route_metrics.reset_route_metrics_file sensors_to_icons = { diff --git a/code/test/test_planning_regression.py b/code/test/test_planning_regression.py index dd0ba8a0..716c7998 100644 --- a/code/test/test_planning_regression.py +++ b/code/test/test_planning_regression.py @@ -49,3 +49,21 @@ def test_position_stability_gate_accepts_after_bounded_unstable_window() -> None decisions = [gate.update(float(x), 0.0) for x in range(7)] assert decisions == [False, False, False, False, False, False, True] + + +def test_global_planner_update_notifications_are_transient_local() -> None: + """Late subscribers must still see route data availability notifications.""" + source = ( + CODE_ROOT / "planning/planning/global_planner/global_planner_node.py" + ).read_text(encoding="utf-8") + trajectory_publisher_block = source.split( + "self.global_trajectory_updated_pub = self.create_publisher(", + maxsplit=1, + )[1].split("\n\n self.speed_limit_updated_pub", maxsplit=1)[0] + speed_limit_publisher_block = source.split( + "self.speed_limit_updated_pub = self.create_publisher(", + maxsplit=1, + )[1].split("\n\n # Service clients", maxsplit=1)[0] + + assert "DurabilityPolicy.TRANSIENT_LOCAL" in trajectory_publisher_block + assert "DurabilityPolicy.TRANSIENT_LOCAL" in speed_limit_publisher_block diff --git a/code/test/test_route_metrics.py b/code/test/test_route_metrics.py index 426b8617..4d2c2eea 100644 --- a/code/test/test_route_metrics.py +++ b/code/test/test_route_metrics.py @@ -3,6 +3,7 @@ from __future__ import annotations import json +import importlib import sys from pathlib import Path @@ -15,12 +16,11 @@ if str(PAF_COMMON_SRC) not in sys.path: sys.path.insert(0, str(PAF_COMMON_SRC)) -from paf_common.route_metrics import ( - increment_route_metric, - load_route_metrics, - merge_route_metrics_into_checkpoint, - reset_route_metrics_file, -) +route_metrics = importlib.import_module("paf_common.route_metrics") +increment_route_metric = route_metrics.increment_route_metric +load_route_metrics = route_metrics.load_route_metrics +merge_route_metrics_into_checkpoint = route_metrics.merge_route_metrics_into_checkpoint +reset_route_metrics_file = route_metrics.reset_route_metrics_file def test_increment_route_metric_accumulates_counts(tmp_path: Path) -> None: diff --git a/doc/dev/progress/2026-04-30-route-sync-bridge.md b/doc/dev/progress/2026-04-30-route-sync-bridge.md index 111ab824..e1c9acec 100644 --- a/doc/dev/progress/2026-04-30-route-sync-bridge.md +++ b/doc/dev/progress/2026-04-30-route-sync-bridge.md @@ -1,5 +1,5 @@ Created: 2026-04-30T22:29:21+02:00 -Last updated: 2026-04-30T22:48:01+02:00 +Last updated: 2026-04-30T22:54:59+02:00 # Route Sync Bridge Progress @@ -41,6 +41,17 @@ from the route start before a global trajectory is generated. must be stopped, including the simulator, compose stack, and related route-validation processes, before handoff or completion. +The follow-up issue sweep handled the checkout-safe subset of open GitHub +issues: + +- local Ruff/code-format issues from `code/` were fixed, +- stale GitHub Action versions in `drive.yml` and `markdownlint.yml` were + updated, +- Docker Compose wording was updated where it referred to the old binary name + rather than compose file names, +- global-planner update notifications now use transient-local QoS so late + subscribers can observe route-data availability. + ## Validation - Red check: the focused deterministic sync contract test failed before the @@ -53,6 +64,13 @@ route-validation processes, before handoff or completion. - Green check: `uv run --with pytest==9.0.2 --with numpy python -m pytest code/test/test_planning_regression.py -q` passed with `3 passed`. +- Green check: + `uv run --with ruff==0.14.8 ruff check code` passed. +- Green check: + `uv run --with ruff==0.14.8 ruff format --check code` passed. +- Green check: + `uv run --with pytest==9.0.2 --with numpy python -m pytest code/test -m unit -q` + passed with `27 passed`. ## Remaining blockers @@ -60,6 +78,8 @@ route-validation processes, before handoff or completion. now waits for vehicle-control commands in the actual route. - Route metrics still need proof from a completed ROS2 drive. - Planning behaviors still need to consume the new `MapTree` lane-context API. +- Several open GitHub issues remain intentionally unclaimed because they need + CARLA validation, perception/planning research, or larger architecture work. ## Recommended next actions diff --git a/doc/development/build_action.md b/doc/development/build_action.md index ec534253..3ae86928 100644 --- a/doc/development/build_action.md +++ b/doc/development/build_action.md @@ -6,7 +6,7 @@ - [General](#general) - [The `build-and-push-image` job](#the-build-and-push-image-job) - - [1. Checkout repository (`actions/checkout@v3`)](#1-checkout-repository-actionscheckoutv3) + - [1. Checkout repository (`actions/checkout@v5`)](#1-checkout-repository-actionscheckoutv5) - [2. Set up Docker Buildx (`docker/setup-buildx-action@v2`)](#2-set-up-docker-buildx-dockersetup-buildx-actionv2) - [3. Cache Docker layers](#3-cache-docker-layers) - [4. Log in to the Container registry (`docker/login-action@v2`)](#4-log-in-to-the-container-registry-dockerlogin-actionv2) @@ -32,7 +32,7 @@ After the action is finished the `drive` action is triggered. ## The `build-and-push-image` job -### 1. Checkout repository ([`actions/checkout@v3`](https://github.com/actions/checkout)) +### 1. Checkout repository ([`actions/checkout@v5`](https://github.com/actions/checkout)) Trivial, just checks out the repo. diff --git a/doc/development/distributed_simulation.md b/doc/development/distributed_simulation.md index a285e8a5..3ab1977e 100644 --- a/doc/development/distributed_simulation.md +++ b/doc/development/distributed_simulation.md @@ -7,8 +7,8 @@ - [General](#general) - [Remote Machine Setup](#remote-machine-setup) - [Local Machine Setup](#local-machine-setup) - - [Ensure similarity between normal docker-compose and distributed docker-compose files](#ensure-similarity-between-normal-docker-compose-and-distributed-docker-compose-files) - - [Set the `` of the carla simulator in docker-compose distributed files](#set-the-ip-address-of-the-carla-simulator-in-docker-compose-distributed-files) + - [Ensure similarity between normal Docker Compose and distributed Docker Compose files](#ensure-similarity-between-normal-docker-compose-and-distributed-docker-compose-files) + - [Set the `` of the carla simulator in Docker Compose distributed files](#set-the-ip-address-of-the-carla-simulator-in-docker-compose-distributed-files) - [Start the agent on your local machine](#start-the-agent-on-your-local-machine) - [How do you know that you do not have enough compute resources?](#how-do-you-know-that-you-do-not-have-enough-compute-resources) @@ -32,12 +32,12 @@ As far as we know, you need more than **10 GB of VRAM** to run the server and th - set the host ip address from the remote machine as the new carla-ip address - start the agent on your local machine -### Ensure similarity between normal docker-compose and distributed docker-compose files +### Ensure similarity between normal Docker Compose and distributed Docker Compose files Carefully compare that their are no major differences between the `docker-compose.*.yaml` and `docker-compose.*-distributed.yaml` files. Mainly, the `carla-simulator` service will not be executed in the non-distributed version. -### Set the `` of the carla simulator in docker-compose distributed files +### Set the `` of the carla simulator in Docker Compose distributed files Replace the argument `` with the ip address of the remote machine. You can find the ip address of the remote machine by executing the following command on the remote machine: diff --git a/doc/development/drive_action.md b/doc/development/drive_action.md index b734105b..510e8a4c 100644 --- a/doc/development/drive_action.md +++ b/doc/development/drive_action.md @@ -3,16 +3,16 @@ **Summary:** This page explains the GitHub build action we use to evaluate our agent. - [The drive job](#the-drive-job) - - [1. Checkout repository (`actions/checkout@v3`)](#1-checkout-repository-actionscheckoutv3) + - [1. Checkout repository (`actions/checkout@v5`)](#1-checkout-repository-actionscheckoutv5) - [2. Download artifact](#2-download-artifact) - [3. Unzip artifact](#3-unzip-artifact) - [4. Return artifact JSON](#4-return-artifact-json) - - [5. Run agent with docker-compose](#5-run-agent-with-docker-compose) + - [5. Run agent with Docker Compose](#5-run-agent-with-docker-compose) - [6. Copy simulation results file out of container](#6-copy-simulation-results-file-out-of-container) - - [7. Stop docker-compose stack](#7-stop-docker-compose-stack) + - [7. Stop Docker Compose stack](#7-stop-docker-compose-stack) - [8. Create simulation results table](#8-create-simulation-results-table) - [9. Print simulation results](#9-print-simulation-results) - - [10. Comment result in pull request `actions/github-script@v6`](#10-comment-result-in-pull-request-actionsgithub-scriptv6) + - [10. Comment result in pull request `actions/github-script@v8`](#10-comment-result-in-pull-request-actionsgithub-scriptv8) - [Simulation results](#simulation-results) - [11. Prune all images older than one day](#11-prune-all-images-older-than-one-day) @@ -22,9 +22,9 @@ The `drive` job is executed conditionally on `pull_request`, after the build suc > Warning: Always start the GitHub runner that handles the `drive` action through direct access to the machine. Do not use remote access like `ssh` or `xrdp`. -### 1. Checkout repository ([`actions/checkout@v3`](https://github.com/actions/checkout)) +### 1. Checkout repository ([`actions/checkout@v5`](https://github.com/actions/checkout)) -Same step as in the [build job](#1-checkout-repository--actionscheckoutv3-) +Same step as in the [build job](build_action.md#1-checkout-repository-actionscheckoutv5) ### 2. Download artifact @@ -38,7 +38,7 @@ Extracts the files of the downloaded artifact. Parses the extracted file in the JSON format to read the information inside the file. -### 5. Run agent with docker-compose +### 5. Run agent with Docker Compose Runs the agent with the [`build/docker-compose.cicd.yaml`](../../build/docker-compose.cicd.yaml) that only contains the bare minimum components for test execution: @@ -52,10 +52,10 @@ bare minimum components for test execution: Copies the created `simulation_results.json` file out of the agent container into the current container -### 7. Stop docker-compose stack +### 7. Stop Docker Compose stack Stops the remaining containers (Carla, roscore) and removes the volumes with: -`$ docker-compose down -v`. +`docker compose down -v`. This step is important to clean up the remaining containers to have a clean run everytime. This is also the reason for the `if: always()`, that ensures step execution. @@ -68,7 +68,7 @@ Reads the simulation results an creates a table for better readability. Prints the simulation results table to the action. -### 10. Comment result in pull request [`actions/github-script@v6`](https://github.com/marketplace/actions/github-script) +### 10. Comment result in pull request [`actions/github-script@v8`](https://github.com/marketplace/actions/github-script) This steps uses a JS script to parse the simulation results and add a comment with a results table to the corresponding pull request. diff --git a/doc/development/first_steps.md b/doc/development/first_steps.md index 7af5d940..933a409f 100644 --- a/doc/development/first_steps.md +++ b/doc/development/first_steps.md @@ -24,11 +24,11 @@ bash scripts/dev-up.sh After that use `Dev Containers: Reopen in Container` in VS Code. This uses `.devcontainer/devcontainer.json` and opens `/workspace` in the `agent-dev` service. -Manual alternative: head to the `/build` folder and execute the `docker-compose.dev.yaml` file via right-click and selecting `Compose Up` in the menu: +Manual alternative: head to the `/build` folder and execute the `docker-compose.dev.yaml` Docker Compose file via right-click and selecting `Compose Up` in the menu: ![devcontainer.png](/doc/assets/development/devcontainer.png) -> This is the default way to execute every docker-compose file. If you try to start a container without it things may break! +> This is the default way to execute every Docker Compose file. If you try to start a container without it things may break! Then navigate to the `Docker` tab in VS Code and attach a VS Code window to the `build-agent-dev` container: diff --git a/doc/general/tests.md b/doc/general/tests.md index 701a34e6..6dcb203e 100644 --- a/doc/general/tests.md +++ b/doc/general/tests.md @@ -37,7 +37,7 @@ For creating a new route, the following must be considered: ## Start Test -To start a test, simply run the file build/docker-compose.test.yaml with compose up. +To start a test, run `docker compose -f build/docker-compose.test.yaml up`. Make sure that test.xml is set as the ROUTE variable in build/agent_service_test.yaml. ## Output From f5ab4f73612e1d176068a1f196f20a41cb18a013 Mon Sep 17 00:00:00 2001 From: ll7 Date: Fri, 1 May 2026 08:05:41 +0200 Subject: [PATCH 38/43] build: bound gpu ppa setup --- build/docker/agent-ros2/Dockerfile | 9 +++- .../progress/2026-05-01-build-validation.md | 49 +++++++++++++++++++ 2 files changed, 56 insertions(+), 2 deletions(-) create mode 100644 doc/dev/progress/2026-05-01-build-validation.md diff --git a/build/docker/agent-ros2/Dockerfile b/build/docker/agent-ros2/Dockerfile index cf662f9b..52eb9249 100644 --- a/build/docker/agent-ros2/Dockerfile +++ b/build/docker/agent-ros2/Dockerfile @@ -19,8 +19,13 @@ RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ # GPU vulkan driver install (amd + intel) RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ --mount=type=cache,target=/var/lib/apt,sharing=locked \ - add-apt-repository -y ppa:kisak/turtle && apt-get update && \ - apt-get upgrade -y && apt-get install -y libvulkan1 vulkan-tools mesa-vulkan-drivers + if timeout 60s add-apt-repository -y ppa:kisak/turtle; then \ + apt-get update && apt-get upgrade -y; \ + else \ + echo "WARNING: ppa:kisak/turtle unavailable, using Ubuntu Mesa/Vulkan packages"; \ + apt-get update; \ + fi && \ + apt-get install -y libvulkan1 vulkan-tools mesa-vulkan-drivers FROM agent-base-gpu AS agent-base-rocm # No changes needed here, only pytorch installation requires changes diff --git a/doc/dev/progress/2026-05-01-build-validation.md b/doc/dev/progress/2026-05-01-build-validation.md new file mode 100644 index 00000000..8ac85aa0 --- /dev/null +++ b/doc/dev/progress/2026-05-01-build-validation.md @@ -0,0 +1,49 @@ +Created: 2026-05-01T08:14:00+02:00 +Last updated: 2026-05-01T08:14:00+02:00 + +# Build Validation Progress + +## Motivation + +Run the full local build path after the route-sync and issue-sweep changes, and +remove the Docker image build blocker observed in the GPU base stage. + +## Current chat state + +The local ROS workspace build passed inside `build-agent-dev-1` with +`devbuild`, completing all 12 project packages. + +The CUDA dev image initially blocked in the GPU base stage while +`add-apt-repository -y ppa:kisak/turtle` waited on the Launchpad PPA. The +Dockerfile now keeps the Kisak PPA as the preferred source, but bounds that +step with `timeout 60s` and falls back to the Ubuntu Mesa/Vulkan packages when +the PPA is unavailable. + +After that change, both Docker image builds completed: + +- `agent-dev-luttkule` +- `agent-deploy-luttkule` + +No CARLA simulator service was launched for this build pass. + +## Validation + +- Green check: + `docker exec build-agent-dev-1 bash -lc 'source /internal_workspace/dev.bashrc && devbuild'` + passed with `Summary: 12 packages finished [17.0s]`. +- Green check: + `docker compose --env-file build/.env -f build/docker-compose.dev.cuda.yml build agent-dev` + built `agent-dev-luttkule`. +- Green check: + `docker compose --env-file build/.env -f build/docker-compose.deploy.cuda.yml build agent-deploy` + built `agent-deploy-luttkule`; its deploy-stage `colcon build` passed with + `Summary: 12 packages finished [1min 16s]`. +- CARLA cleanup check: + no `carla-simulator`, `CarlaUE4`, `leaderboard`, or `run_test.py` runtime + processes were left running. The only matching Docker container name was the + pre-existing BuildKit helper `buildx_buildkit_carla-builder0`, not a CARLA + simulator. + +## Remaining blockers + +- CARLA route validation remains unrun in this build pass. From 628a0ee43e75d312a0b689de2fe9fb757b50a153 Mon Sep 17 00:00:00 2001 From: ll7 Date: Fri, 1 May 2026 08:28:23 +0200 Subject: [PATCH 39/43] fix: align route startup and lane context decisions --- code/mapping/mapping_common/map.py | 8 ++ .../test/test_mapping_common/test_map.py | 16 ++++ .../behavior_agent/behaviors/lane_change.py | 39 +++++---- .../behaviors/leave_parking_space.py | 22 +++-- .../behavior_agent/behaviors/overtake.py | 43 +++++----- .../global_planner/global_planner_node.py | 80 +++++++++++++++---- .../global_planner/route_alignment.py | 29 +++++++ code/test/test_planning_regression.py | 47 +++++++++++ .../progress/2026-05-01-build-validation.md | 44 +++++++++- 9 files changed, 268 insertions(+), 60 deletions(-) create mode 100644 code/planning/planning/global_planner/route_alignment.py diff --git a/code/mapping/mapping_common/map.py b/code/mapping/mapping_common/map.py index 9dd22ab2..d8105da2 100644 --- a/code/mapping/mapping_common/map.py +++ b/code/mapping/mapping_common/map.py @@ -85,6 +85,14 @@ def is_free(self) -> Optional[bool]: return False return None + def is_traversable(self) -> Optional[bool]: + """Return whether the adjacent lane exists and is currently free.""" + if self.presence is LanePresence.ABSENT: + return False + if self.presence is LanePresence.UNKNOWN: + return None + return self.is_free() + @dataclass class AdjacentLaneContext: diff --git a/code/mapping/test/test_mapping_common/test_map.py b/code/mapping/test/test_mapping_common/test_map.py index 9bb8b40d..339d0847 100644 --- a/code/mapping/test/test_mapping_common/test_map.py +++ b/code/mapping/test/test_mapping_common/test_map.py @@ -104,3 +104,19 @@ def test_get_lane_context_preserves_absent_lane_information_with_fallback(): assert lane_context.has_lane() is False assert lane_context.presence is LanePresence.ABSENT assert lane_context.lane_state is LaneFreeState.BLOCKED + + +def test_lane_context_only_traversable_when_lane_exists_and_is_free(): + road_map = Map( + entities=[ + get_hero(), + ] + ) + + lane_context = road_map.build_tree( + entity.FlagFilter(is_collider=True, is_hero=False) + ).get_lane_context(check_method="rectangle") + + assert lane_context.has_lane() is False + assert lane_context.lane_state is LaneFreeState.FREE + assert lane_context.is_traversable() is False diff --git a/code/planning/planning/behavior_agent/behaviors/lane_change.py b/code/planning/planning/behavior_agent/behaviors/lane_change.py index c48197c2..8172264e 100755 --- a/code/planning/planning/behavior_agent/behaviors/lane_change.py +++ b/code/planning/planning/behavior_agent/behaviors/lane_change.py @@ -13,7 +13,7 @@ from perception_interfaces.msg import Waypoint import mapping_common.mask -from mapping_common.map import Map, LaneFreeState, LaneFreeDirection +from mapping_common.map import Map, LaneFreeDirection from mapping_common.entity import FlagFilter from mapping_common.transform import Point2, Transform2D, Vector2 from mapping_common.markers import debug_marker @@ -49,6 +49,17 @@ """ +def _add_lane_context_debug(behavior_name: str, context) -> None: + add_debug_entry(behavior_name, f"Lane change: Lane present? {context.has_lane()}") + add_debug_entry( + behavior_name, + f"Lane change: Is lane traversable? {context.is_traversable()} " + f"({context.lane_state.name})", + ) + if isinstance(context.lane_box, shapely.Polygon): + add_debug_marker(debug_marker(context.lane_box, color=LANECHANGE_MARKER_COLOR)) + + class Ahead(py_trees.behaviour.Behaviour): """ This behaviour checks whether there is a lane change in front of the @@ -305,16 +316,14 @@ def update(self): # if change to right, do not change early # (as there could be no road till change point!) if self.change_detected and self.change_direction is LaneFreeDirection.LEFT: - lc_free, lc_mask = tree.is_lane_free( + lane_context = tree.get_lane_context( right_lane=self.change_direction.value, lane_length=22.5, lane_transform=-5.0, check_method="fallback", ) - if isinstance(lc_mask, shapely.Polygon): - add_debug_marker(debug_marker(lc_mask, color=LANECHANGE_MARKER_COLOR)) - add_debug_entry(self.name, f"Lane change: Is lane free? {lc_free.name}") - if lc_free is LaneFreeState.FREE: + _add_lane_context_debug(self.name, lane_context) + if lane_context.is_traversable() is True: self.counter_lanefree += 1 # using a counter to account for inconsistencies if self.counter_lanefree > 1: @@ -354,11 +363,12 @@ def update(self): f"Lane Change: Free with count {self.counter_lanefree}/2", ) else: - if lc_free is LaneFreeState.BLOCKED: + if lane_context.is_traversable() is False: self.counter_lanefree = 0 add_debug_entry( self.name, - "Lane Change: Lane blocked, reset count, stay in current lane", + "Lane Change: Lane unavailable, reset count, " + "stay in current lane", ) else: add_debug_entry( @@ -458,16 +468,14 @@ def update(self): "Lane Change: At least one change parameter is None", ) - lc_free, lc_mask = tree.is_lane_free( + lane_context = tree.get_lane_context( right_lane=self.change_direction.value, lane_length=22.5, lane_transform=-5.0, check_method="fallback", ) - if isinstance(lc_mask, shapely.Polygon): - add_debug_marker(debug_marker(lc_mask, color=LANECHANGE_MARKER_COLOR)) - add_debug_entry(self.name, f"Lane change: Is lane free? {lc_free.name}") - if lc_free is LaneFreeState.FREE: + _add_lane_context_debug(self.name, lane_context) + if lane_context.is_traversable() is True: self.counter_lanefree += 1 # using a counter to account for inconsistencies if self.counter_lanefree > 1: @@ -493,12 +501,13 @@ def update(self): ) else: self.curr_behavior_pub.publish(String(data=bs.lc_wait.name)) - if lc_free is LaneFreeState.BLOCKED: + if lane_context.is_traversable() is False: self.counter_lanefree = 0 return debug_status( self.name, Status.RUNNING, - "Lane Change Wait: Lane blocked, reset count, stay in current lane", + "Lane Change Wait: Lane unavailable, reset count, " + "stay in current lane", ) else: return debug_status( diff --git a/code/planning/planning/behavior_agent/behaviors/leave_parking_space.py b/code/planning/planning/behavior_agent/behaviors/leave_parking_space.py index cce351a9..1a1a21b2 100644 --- a/code/planning/planning/behavior_agent/behaviors/leave_parking_space.py +++ b/code/planning/planning/behavior_agent/behaviors/leave_parking_space.py @@ -9,7 +9,7 @@ from std_msgs.msg import String, Float32 import mapping_common.map -from mapping_common.map import Map, LaneFreeState +from mapping_common.map import Map from mapping_common.markers import debug_marker from mapping_common.transform import Point2 from planning.behavior_agent.blackboard_utils import Blackboard @@ -110,18 +110,28 @@ def update(self): # checks if the left lane of the car is free, tree = map.build_tree(mapping_common.map.lane_free_filter()) - state, mask = tree.is_lane_free( + lane_context = tree.get_lane_context( right_lane=False, lane_length=25, lane_transform=-15, check_method="fallback", reduce_lane=0.5, ) - if mask is not None: - add_debug_marker(debug_marker(mask, color=UNPARKING_MARKER_COLOR)) - add_debug_entry(self.name, f"Lane state: {state.name}") + if lane_context.lane_box is not None: + add_debug_marker( + debug_marker( + lane_context.lane_box, + color=UNPARKING_MARKER_COLOR, + ) + ) + add_debug_entry(self.name, f"Lane present: {lane_context.has_lane()}") + add_debug_entry( + self.name, + f"Lane traversable: {lane_context.is_traversable()} " + f"({lane_context.lane_state.name})", + ) if ( - state is LaneFreeState.FREE + lane_context.is_traversable() is True or hero_transform.translation() .point() .distance_to(self.init_position) diff --git a/code/planning/planning/behavior_agent/behaviors/overtake.py b/code/planning/planning/behavior_agent/behaviors/overtake.py index 99efbb67..e99ab77d 100644 --- a/code/planning/planning/behavior_agent/behaviors/overtake.py +++ b/code/planning/planning/behavior_agent/behaviors/overtake.py @@ -7,7 +7,7 @@ from rclpy.publisher import Publisher import mapping_common.mask -from mapping_common.map import Map, MapTree, LaneFreeState +from mapping_common.map import Map, MapTree from mapping_common.entity import ShapelyEntity, Entity, StopMark from mapping_common.markers import debug_marker from mapping_common.transform import Transform2D, Vector2, Point2 @@ -74,6 +74,17 @@ def unset_space_stop_mark(client: Client): """ +def _add_overtake_lane_context_debug(behavior_name: str, context) -> None: + add_debug_entry(behavior_name, f"Overtake lane present?: {context.has_lane()}") + add_debug_entry( + behavior_name, + f"Overtake lane traversable?: {context.is_traversable()} " + f"({context.lane_state.name})", + ) + if isinstance(context.lane_box, shapely.Polygon): + add_debug_marker(debug_marker(context.lane_box, color=OVERTAKE_MARKER_COLOR)) + + def calculate_obstacle( behavior_name: str, tree: MapTree, @@ -328,16 +339,14 @@ def update(self): # slow down before overtake if blocked if self.ot_distance < 15.0: - ot_free, ot_mask = tree.is_lane_free( + lane_context = tree.get_lane_context( right_lane=False, lane_length=self.clear_distance, lane_transform=10.0, check_method="fallback", ) - if isinstance(ot_mask, shapely.Polygon): - add_debug_marker(debug_marker(ot_mask, color=OVERTAKE_MARKER_COLOR)) - add_debug_entry(self.name, f"Overtake free?: {ot_free.name}") - if ot_free is LaneFreeState.FREE: + _add_overtake_lane_context_debug(self.name, lane_context) + if lane_context.is_traversable() is True: self.ot_counter += 1 # using a counter to account for inconsistencies if self.ot_counter > 3: @@ -357,10 +366,10 @@ def update(self): f"Overtake free count: {self.ot_counter}", ) else: - if ot_free is LaneFreeState.BLOCKED: + if lane_context.is_traversable() is False: self.ot_counter = 0 add_debug_entry( - self.name, "Overtake Approach: oncoming blocked slowing down" + self.name, "Overtake Approach: oncoming unavailable slowing down" ) self.curr_behavior_pub.publish(String(data=bs.ot_app_blocked.name)) @@ -469,17 +478,15 @@ def update(self): set_space_stop_mark(self.stop_client, obstacle=entity) self.curr_behavior_pub.publish(String(data=bs.ot_wait.name)) - ot_free, ot_mask = tree.is_lane_free( + lane_context = tree.get_lane_context( right_lane=False, lane_length=self.clear_distance, lane_transform=10.0, check_method="fallback", ) - if isinstance(ot_mask, shapely.Polygon): - add_debug_marker(debug_marker(ot_mask, color=OVERTAKE_MARKER_COLOR)) - add_debug_entry(self.name, f"Overtake free?: {ot_free.name}") - if ot_free is LaneFreeState.FREE: + _add_overtake_lane_context_debug(self.name, lane_context) + if lane_context.is_traversable() is True: self.ot_counter += 1 if self.ot_counter > 3: self.curr_behavior_pub.publish(String(data=bs.ot_wait_free.name)) @@ -492,7 +499,7 @@ def update(self): self.name, Status.RUNNING, f"Overtake free count: {self.ot_counter}" ) else: - if ot_free is LaneFreeState.BLOCKED: + if lane_context.is_traversable() is False: self.ot_counter = 0 return debug_status(self.name, Status.RUNNING, "Overtake blocked") @@ -623,16 +630,14 @@ def update(self): if status.status == OvertakeStatus.Response.OVERTAKING: # First: check if our right lane is free and end overtake if possible - ot_free, ot_mask = tree.is_lane_free( + lane_context = tree.get_lane_context( right_lane=True, lane_length=15.0, lane_transform=7.5, check_method="lanemarking", ) - add_debug_entry(self.name, f"Right lane free?: {ot_free.name}") - if isinstance(ot_mask, shapely.Polygon): - add_debug_marker(debug_marker(ot_mask, color=OVERTAKE_MARKER_COLOR)) - if ot_free is LaneFreeState.FREE: + _add_overtake_lane_context_debug(self.name, lane_context) + if lane_context.is_traversable() is True: request_end_overtake(self.end_overtake_client) return debug_status( self.name, diff --git a/code/planning/planning/global_planner/global_planner_node.py b/code/planning/planning/global_planner/global_planner_node.py index 15ac7df2..ec318cfc 100755 --- a/code/planning/planning/global_planner/global_planner_node.py +++ b/code/planning/planning/global_planner/global_planner_node.py @@ -23,6 +23,7 @@ from .position_stability import PositionStabilityGate from .preplanning_trajectory import OpenDriveConverter +from .route_alignment import find_route_alignment_index # TODO: These definition do not align with the CarlaRoute.RIGHT, etc.. definitions. # -> Check for possible bugs @@ -244,8 +245,49 @@ async def process_global_plan(self) -> bool: x_start = self.agent_pos.x # 983.5 y_start = self.agent_pos.y # -5433.2 - x_target = data.poses[0].position.x - y_target = data.poses[0].position.y + poses = list(data.poses) + road_options = list(data.road_options) + if len(poses) != len(road_options): + self.get_logger().warn( + "Global route pose and road option counts do not match." + ) + return False + + route_start_index = find_route_alignment_index( + agent_position=(x_start, y_start), + route_points=[(pose.position.x, pose.position.y) for pose in poses], + max_distance_m=self.distance_spawn_to_first_wp, + ) + if route_start_index is None: + if poses: + x_target = poses[0].position.x + y_target = poses[0].position.y + route_distance_message = ( + f" first route pose delta=({x_start - x_target:.2f}, " + f"{y_start - y_target:.2f})" + ) + else: + route_distance_message = "" + self.get_logger().warn( + "Current agent-pose does not match the given global route." + f"{route_distance_message}" + ) + return False + + if route_start_index > 0: + self.get_logger().info( + "Aligning global route to current agent pose at " + f"route index {route_start_index}/{len(poses) - 1}." + ) + poses = poses[route_start_index:] + road_options = road_options[route_start_index:] + + if len(poses) < 2: + self.get_logger().warn("Global route does not contain enough poses.") + return False + + x_target = poses[0].position.x + y_target = poses[0].position.y if ( abs(x_start - x_target) > self.distance_spawn_to_first_wp or abs(y_start - y_target) > self.distance_spawn_to_first_wp @@ -259,10 +301,10 @@ async def process_global_plan(self) -> bool: x_turn = None y_turn = None ind = 0 - for i, opt in enumerate(data.road_options): + for i, opt in enumerate(road_options): if opt == LEFT or opt == RIGHT or opt == FORWARD: - x_turn = data.poses[i].position.x - y_turn = data.poses[i].position.y + x_turn = poses[i].position.x + y_turn = poses[i].position.y ind = i break if x_turn is None or y_turn is None: @@ -273,8 +315,12 @@ async def process_global_plan(self) -> bool: x_target = None y_target = None - x_turn_follow = data.poses[ind + 1].position.x - y_turn_follow = data.poses[ind + 1].position.y + if ind + 1 >= len(poses): + self.get_logger().warn("Global route turn command has no following pose") + return False + + x_turn_follow = poses[ind + 1].position.x + y_turn_follow = poses[ind + 1].position.y # Trajectory for the starting road segment self.odc.initial_road_trajectory( @@ -287,30 +333,30 @@ async def process_global_plan(self) -> bool: x_target, y_target, 0, - data.road_options[0], + road_options[0], ) - n = len(data.poses) + n = len(poses) # iterating through global route to create trajectory for i in range(1, n - 1): self.get_logger().info(f"Preplanner going throug global plan {i + 1}/{n}") - x_target = data.poses[i].position.x - y_target = data.poses[i].position.y - action = data.road_options[i] + x_target = poses[i].position.x + y_target = poses[i].position.y + action = road_options[i] - x_target_next = data.poses[i + 1].position.x - y_target_next = data.poses[i + 1].position.y + x_target_next = poses[i + 1].position.x + y_target_next = poses[i + 1].position.y self.odc.target_road_trajectory( x_target, y_target, x_target_next, y_target_next, action ) self.odc.target_road_trajectory( - data.poses[n - 1].position.x, - data.poses[n - 1].position.y, + poses[n - 1].position.x, + poses[n - 1].position.y, None, None, - data.road_options[n - 1], + road_options[n - 1], ) # trajectory is now stored in the waypoints # waypoints = self.odc.waypoints diff --git a/code/planning/planning/global_planner/route_alignment.py b/code/planning/planning/global_planner/route_alignment.py new file mode 100644 index 00000000..d35f4a4c --- /dev/null +++ b/code/planning/planning/global_planner/route_alignment.py @@ -0,0 +1,29 @@ +"""Helpers for matching the ego pose to a global route.""" + +from __future__ import annotations + +from math import hypot +from typing import Sequence + + +def find_route_alignment_index( + agent_position: tuple[float, float], + route_points: Sequence[tuple[float, float]], + max_distance_m: float, +) -> int | None: + """Return the nearest route index if it is within the accepted distance.""" + if not route_points: + return None + + agent_x, agent_y = agent_position + nearest_index = 0 + nearest_distance = float("inf") + for index, (route_x, route_y) in enumerate(route_points): + distance = hypot(agent_x - route_x, agent_y - route_y) + if distance < nearest_distance: + nearest_index = index + nearest_distance = distance + + if nearest_distance > max_distance_m: + return None + return nearest_index diff --git a/code/test/test_planning_regression.py b/code/test/test_planning_regression.py index 716c7998..54cfd2ee 100644 --- a/code/test/test_planning_regression.py +++ b/code/test/test_planning_regression.py @@ -67,3 +67,50 @@ def test_global_planner_update_notifications_are_transient_local() -> None: assert "DurabilityPolicy.TRANSIENT_LOCAL" in trajectory_publisher_block assert "DurabilityPolicy.TRANSIENT_LOCAL" in speed_limit_publisher_block + + +def test_global_route_alignment_accepts_later_nearby_start() -> None: + """Allow startup to recover when the first route pose is behind the ego pose.""" + route_alignment = importlib.import_module("planning.global_planner.route_alignment") + + route_points = [(0.0, 0.0), (100.0, 0.0), (205.0, 0.0), (300.0, 0.0)] + + assert ( + route_alignment.find_route_alignment_index( + agent_position=(201.0, 3.0), + route_points=route_points, + max_distance_m=10.0, + ) + == 2 + ) + + +def test_global_route_alignment_rejects_route_without_nearby_pose() -> None: + """Keep rejecting global routes that do not match the ego pose at all.""" + route_alignment = importlib.import_module("planning.global_planner.route_alignment") + + route_points = [(0.0, 0.0), (100.0, 0.0), (200.0, 0.0)] + + assert ( + route_alignment.find_route_alignment_index( + agent_position=(350.0, 0.0), + route_points=route_points, + max_distance_m=10.0, + ) + is None + ) + + +def test_planning_behaviors_use_lane_context_for_adjacent_lane_decisions() -> None: + """Keep absent-lane checks distinct from blocked-lane checks in behaviors.""" + behavior_dir = CODE_ROOT / "planning/planning/behavior_agent/behaviors" + + lane_change_source = (behavior_dir / "lane_change.py").read_text(encoding="utf-8") + overtake_source = (behavior_dir / "overtake.py").read_text(encoding="utf-8") + parking_source = (behavior_dir / "leave_parking_space.py").read_text( + encoding="utf-8" + ) + + assert lane_change_source.count("get_lane_context(") >= 2 + assert overtake_source.count("get_lane_context(") >= 3 + assert parking_source.count("get_lane_context(") >= 1 diff --git a/doc/dev/progress/2026-05-01-build-validation.md b/doc/dev/progress/2026-05-01-build-validation.md index 8ac85aa0..628d8ac9 100644 --- a/doc/dev/progress/2026-05-01-build-validation.md +++ b/doc/dev/progress/2026-05-01-build-validation.md @@ -1,5 +1,5 @@ Created: 2026-05-01T08:14:00+02:00 -Last updated: 2026-05-01T08:14:00+02:00 +Last updated: 2026-05-01T08:31:00+02:00 # Build Validation Progress @@ -24,7 +24,22 @@ After that change, both Docker image builds completed: - `agent-dev-luttkule` - `agent-deploy-luttkule` -No CARLA simulator service was launched for this build pass. +The CARLA route validation path was then exercised with the split development +startup flow: + +1. start `carla-simulator`, +2. run `leaderboard.dev`, +3. run `agent.dev` once the route is active, +4. stop the agent, leaderboard evaluator, bridge, and simulator before handoff. + +That run exposed a preplanner startup issue: the global route could be rejected +when the first route pose was behind the current ego pose. The preplanner now +aligns to the nearest route pose within the existing +`distance_spawn_to_first_wp` tolerance before trimming the route for trajectory +generation. + +The behavior-agent lane decisions were also updated so an absent adjacent lane +is no longer treated the same as a free lane. ## Validation @@ -38,6 +53,28 @@ No CARLA simulator service was launched for this build pass. `docker compose --env-file build/.env -f build/docker-compose.deploy.cuda.yml build agent-deploy` built `agent-deploy-luttkule`; its deploy-stage `colcon build` passed with `Summary: 12 packages finished [1min 16s]`. +- Green check: + `docker exec build-agent-dev-1 bash -lc 'cd /workspace && source /internal_workspace/dev.bashrc && devbuild.pkg mapping'` + rebuilt the mapping package after the lane-context helper change. +- Green check: + `docker exec build-agent-dev-1 bash -lc 'cd /workspace && source /internal_workspace/dev.bashrc && devbuild.pkg planning'` + rebuilt the planning package after behavior and global-planner changes. +- Green check: + `PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest code/mapping/test/test_mapping_common/test_map.py -q` + passed in `build-agent-dev-1` with `4 passed`. +- Green check: + `uv run --with pytest==9.0.2 --with numpy python -m pytest code/test/test_planning_regression.py -q` + passed with `7 passed`. +- Green check: + `uv run --with ruff==0.14.8 ruff check` and + `uv run --with ruff==0.14.8 ruff format --check` passed for the touched + Python files. +- CARLA route smoke check: + `leaderboard.dev` plus `agent.dev` reached route runtime, the global + trajectory service became available, `MotionPlanning` consumed it, and + `vehicle_controller` published timestamped `/carla/hero/vehicle_control_cmd` + messages. The run was stopped before full route completion after validating + startup, global trajectory delivery, and control-command publication. - CARLA cleanup check: no `carla-simulator`, `CarlaUE4`, `leaderboard`, or `run_test.py` runtime processes were left running. The only matching Docker container name was the @@ -46,4 +83,5 @@ No CARLA simulator service was launched for this build pass. ## Remaining blockers -- CARLA route validation remains unrun in this build pass. +- The full leaderboard route was not left running to completion. The smoke run + validated startup and command publication, but not route score or completion. From 0248223b354533538f385d90f6854930435cb167 Mon Sep 17 00:00:00 2001 From: ll7 Date: Fri, 1 May 2026 13:58:28 +0200 Subject: [PATCH 40/43] fix: update package descriptions and documentation references across multiple modules --- code/acting/package.xml | 2 +- code/acting/setup.py | 2 +- code/agent/package.xml | 2 +- code/agent/setup.py | 2 +- code/control/setup.py | 2 +- code/leaderboard_launcher/package.xml | 2 +- code/leaderboard_launcher/setup.py | 2 +- code/localization/setup.py | 2 +- code/mapping/mapping_common/__init__.py | 2 +- code/mapping_interfaces/package.xml | 2 +- code/perception/package.xml | 2 +- code/perception/setup.py | 2 +- code/perception_interfaces/package.xml | 2 +- code/planning/package.xml | 2 +- code/planning/setup.py | 2 +- code/planning_interfaces/package.xml | 2 +- doc/dev/README.md | 1 + .../2026-05-01-doc-consolidation-wp1.md | 45 +++++++++++++++++++ doc/general/architecture_current.md | 4 +- doc/mapping/README.md | 18 ++++---- doc/mapping/generated/mapping_common/index.md | 2 +- 21 files changed, 74 insertions(+), 28 deletions(-) create mode 100644 doc/dev/progress/2026-05-01-doc-consolidation-wp1.md diff --git a/code/acting/package.xml b/code/acting/package.xml index c9cf7df3..c9b10d78 100644 --- a/code/acting/package.xml +++ b/code/acting/package.xml @@ -3,7 +3,7 @@ acting 0.0.0 - TODO: Package description + Acting helpers and vehicle passthrough for the PAF autonomous driving stack paf MIT diff --git a/code/acting/setup.py b/code/acting/setup.py index b369238b..e603b6be 100644 --- a/code/acting/setup.py +++ b/code/acting/setup.py @@ -17,7 +17,7 @@ zip_safe=True, maintainer="peter", maintainer_email="peter.viechter@student.uni-augsburg.de", - description="TODO: Package description", + description="Acting package (passthrough to vehicle controller) for the PAF autonomous driving stack", license="MIT", tests_require=["pytest"], entry_points={ diff --git a/code/agent/package.xml b/code/agent/package.xml index b678a153..19387581 100644 --- a/code/agent/package.xml +++ b/code/agent/package.xml @@ -3,7 +3,7 @@ agent 0.0.0 - TODO: Package description + Agent coordination and data-management utilities for the PAF autonomous driving stack paf MIT diff --git a/code/agent/setup.py b/code/agent/setup.py index a55313ae..5e69bc4a 100644 --- a/code/agent/setup.py +++ b/code/agent/setup.py @@ -17,7 +17,7 @@ zip_safe=True, maintainer="peter", maintainer_email="peter.viechter@student.uni-augsburg.de", - description="TODO: Package description", + description="Agent startup coordination, shutdown, and data management for the PAF autonomous driving stack", license="MIT", tests_require=["pytest"], scripts=["scripts/launch_agent.sh"], diff --git a/code/control/setup.py b/code/control/setup.py index 3d189296..d3f716f1 100644 --- a/code/control/setup.py +++ b/code/control/setup.py @@ -18,7 +18,7 @@ zip_safe=True, maintainer="peter", maintainer_email="peter.viechter@student.uni-augsburg.de", - description="TODO: Package description", + description="Vehicle control (steering, velocity, vehicle controller) for the PAF autonomous driving stack", license="MIT", tests_require=["pytest"], entry_points={ diff --git a/code/leaderboard_launcher/package.xml b/code/leaderboard_launcher/package.xml index d7427758..1b75e179 100644 --- a/code/leaderboard_launcher/package.xml +++ b/code/leaderboard_launcher/package.xml @@ -3,7 +3,7 @@ leaderboard_launcher 0.0.0 - TODO: Package description + CARLA leaderboard launcher and helper scripts for the PAF stack peter MIT diff --git a/code/leaderboard_launcher/setup.py b/code/leaderboard_launcher/setup.py index c9b8717c..72f968a7 100644 --- a/code/leaderboard_launcher/setup.py +++ b/code/leaderboard_launcher/setup.py @@ -17,7 +17,7 @@ zip_safe=True, maintainer="peter", maintainer_email="peter.viechter@student.uni-augsburg.de", - description="TODO: Package description", + description="CARLA Leaderboard launcher for the PAF autonomous driving stack", license="MIT", tests_require=["pytest"], scripts=[ diff --git a/code/localization/setup.py b/code/localization/setup.py index 4002bd47..42137031 100644 --- a/code/localization/setup.py +++ b/code/localization/setup.py @@ -18,7 +18,7 @@ zip_safe=True, maintainer="peter", maintainer_email="peter.viechter@student.uni-augsburg.de", - description="TODO: Package description", + description="Localization (EKF, GNSS, odometry, GPS transform) for the PAF autonomous driving stack", license="MIT", tests_require=["pytest"], entry_points={ diff --git a/code/mapping/mapping_common/__init__.py b/code/mapping/mapping_common/__init__.py index d7ae08a4..5a521f38 100644 --- a/code/mapping/mapping_common/__init__.py +++ b/code/mapping/mapping_common/__init__.py @@ -20,7 +20,7 @@ This module is compiled with [Cython](https://cython.readthedocs.io/en/latest/). If changes have been made to this package, -catkin_make needs to be executed to apply them! +colcon build needs to be executed to apply them! This step is automatically executed when using the [docker-compose.leaderboard.yaml](/build/docker-compose.leaderboard.yaml). diff --git a/code/mapping_interfaces/package.xml b/code/mapping_interfaces/package.xml index e918e46f..8415cc88 100644 --- a/code/mapping_interfaces/package.xml +++ b/code/mapping_interfaces/package.xml @@ -3,7 +3,7 @@ mapping_interfaces 0.0.0 - TODO: Package description + ROS message definitions for mapping interfaces used by the PAF stack peter MIT diff --git a/code/perception/package.xml b/code/perception/package.xml index 389b2f39..02707de4 100644 --- a/code/perception/package.xml +++ b/code/perception/package.xml @@ -3,7 +3,7 @@ perception 0.0.0 - TODO: Package description + Perception components: lidar, radar, vision, traffic-light, and lane detection peter MIT diff --git a/code/perception/setup.py b/code/perception/setup.py index 59d02348..82c00448 100644 --- a/code/perception/setup.py +++ b/code/perception/setup.py @@ -18,7 +18,7 @@ zip_safe=True, maintainer="peter", maintainer_email="peter.viechter@student.uni-augsburg.de", - description="TODO: Package description", + description="Perception (lidar, radar, vision, traffic light, lane detection) for the PAF autonomous driving stack", license="MIT", tests_require=["pytest"], entry_points={ diff --git a/code/perception_interfaces/package.xml b/code/perception_interfaces/package.xml index 195aa681..9b997f7b 100644 --- a/code/perception_interfaces/package.xml +++ b/code/perception_interfaces/package.xml @@ -3,7 +3,7 @@ perception_interfaces 0.0.0 - TODO: Package description + ROS message definitions for perception interfaces used by the PAF stack peter MIT diff --git a/code/planning/package.xml b/code/planning/package.xml index b9a8409e..31028808 100644 --- a/code/planning/package.xml +++ b/code/planning/package.xml @@ -3,7 +3,7 @@ planning 0.0.0 - TODO: Package description + Planning (global and local planners, behavior tree, motion planning) for the PAF stack peter MIT diff --git a/code/planning/setup.py b/code/planning/setup.py index 3dbff191..f9ca30ae 100644 --- a/code/planning/setup.py +++ b/code/planning/setup.py @@ -18,7 +18,7 @@ zip_safe=True, maintainer="peter", maintainer_email="peter.viechter@student.uni-augsburg.de", - description="TODO: Package description", + description="Planning (global/local planner, behavior tree, motion planning) for the PAF autonomous driving stack", license="MIT", tests_require=["pytest"], entry_points={ diff --git a/code/planning_interfaces/package.xml b/code/planning_interfaces/package.xml index b4883a32..6ebc81e4 100644 --- a/code/planning_interfaces/package.xml +++ b/code/planning_interfaces/package.xml @@ -3,7 +3,7 @@ planning_interfaces 0.0.0 - TODO: Package description + ROS message definitions for planning interfaces used by the PAF stack peter MIT diff --git a/doc/dev/README.md b/doc/dev/README.md index a0bbb2dc..d319f234 100644 --- a/doc/dev/README.md +++ b/doc/dev/README.md @@ -4,6 +4,7 @@ This folder contains non-canonical development handoff and progress notes that p ## Progress Notes +- [2026-05-01-doc-consolidation-wp1.md](./progress/2026-05-01-doc-consolidation-wp1.md) - [2026-04-30-route-sync-bridge.md](./progress/2026-04-30-route-sync-bridge.md) - [2026-04-29-agent-handoff-rules.md](./progress/2026-04-29-agent-handoff-rules.md) - [2026-04-29-route-validation-and-map-context-handoff.md](./progress/2026-04-29-route-validation-and-map-context-handoff.md) diff --git a/doc/dev/progress/2026-05-01-doc-consolidation-wp1.md b/doc/dev/progress/2026-05-01-doc-consolidation-wp1.md new file mode 100644 index 00000000..ac3fe215 --- /dev/null +++ b/doc/dev/progress/2026-05-01-doc-consolidation-wp1.md @@ -0,0 +1,45 @@ +Created: 2026-05-01 +Last updated: 2026-05-01 + +# WP1: Documentation and Interface Consolidation + +## Motivation + +Address the highest-priority improvement areas from `doc/dev_talks/paf25/future_work.md` — specifically WP1 documentation and interface consolidation. Focus on fixing stale references, TODO placeholder descriptions, and ROS1-era terminology before any feature expansion. + +## Changes + +### Fixed stale package references +- `doc/mapping/README.md`: Replaced stale `mapping_visualization` package references with current `mapping/mapping/visualization.py` location (visualization was merged into the mapping package) +- `doc/mapping/README.md`: Fixed stale `./src/` → `./mapping/` and `./tests/` → `./test/` path references +- `doc/general/architecture_current.md`: Fixed stale `init_mapping` topic references → `init_data` (the actual topic) + +### Fixed ROS1-era terminology +- `doc/mapping/README.md`: `catkin_make` → `colcon build` (3 occurrences) +- `code/mapping/mapping_common/__init__.py`: `catkin_make` → `colcon build` +- `doc/mapping/generated/mapping_common/index.md`: `catkin_make` → `colcon build` +- `doc/mapping/generated/mapping_common/README.md`: (symlink to index.md, covered) + +### Fixed TODO package descriptions +Replaced `"TODO: Package description"` in 7 `setup.py` files with meaningful descriptions: +- `code/agent/setup.py` +- `code/control/setup.py` +- `code/acting/setup.py` +- `code/localization/setup.py` +- `code/leaderboard_launcher/setup.py` +- `code/planning/setup.py` +- `code/perception/setup.py` + +## Validation +- All changed `.py` files pass `get_errors` check (no errors found) +- No lint errors introduced (only string changes in setup.py descriptions, docstring changes in __init__.py) + +## Not included (deferred) +- `doc/perception/experiments/` Dockerfile with `catkin_make` — archival content +- `doc/acting/discontinued/` and `doc/planning/discontinued/` CMakeLists references — intentionally preserved historical artifacts +- Remaining `doc/research/overhaul25/` references — research docs, not active architecture +- The generated docs will fully sync on next `pydoc-markdown` run + +## Follow-up +- Run `pydoc-markdown` to regenerate generated docs from the fixed source +- Next: WP2 (Radar Motion Quality) or WP5 (Automated Testing) diff --git a/doc/general/architecture_current.md b/doc/general/architecture_current.md index d31cdceb..1a9e6039 100644 --- a/doc/general/architecture_current.md +++ b/doc/general/architecture_current.md @@ -378,7 +378,7 @@ Services: Subscriptions: -- ```/paf/hero/mapping/init_mapping``` \(/mapping_data_integration\) ([mapping/Map](../../code/mapping/msg/Map.msg)) +- ```/paf/hero/mapping/init_data``` \(/mapping_data_integration\) ([mapping/Map](../../code/mapping/msg/Map.msg)) Publishes: @@ -404,7 +404,7 @@ More information under [ACC.md](/doc/planning/ACC.md). Subscriptions: - ```/paf/hero/curr_behavior``` \(/behavior_agent\) ([std_msgs/String](https://docs.ros.org/en/api/std_msgs/html/msg/String.html)) -- ```/paf/hero/mapping/init_mapping``` \(/mapping_data_integration\) ([mapping/Map](../../code/mapping/msg/Map.msg)) +- ```/paf/hero/mapping/init_data``` \(/mapping_data_integration\) ([mapping/Map](../../code/mapping/msg/Map.msg)) - ```/paf/hero/pure_pursuit_steer``` \(/pure_pursuit_controller\) ([std_msgs/Float32](https://docs.ros.org/en/noetic/api/std_msgs/html/msg/Float32.html)) - ```/paf/hero/speed_limit``` \(/MotionPlanning\) ([std_msgs/Float32](https://docs.ros.org/en/noetic/api/std_msgs/html/msg/Float32.html)) - ```/paf/hero/trajectory_local``` \(/MotionPlanning\) ([nav_msgs/Path](https://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Path.html)) diff --git a/doc/mapping/README.md b/doc/mapping/README.md index b09ef2f9..c99c4284 100644 --- a/doc/mapping/README.md +++ b/doc/mapping/README.md @@ -33,7 +33,7 @@ In addition, radar and lidar data are fused to improve dynamic object understand The [**MappingDataIntegrationNode**](/doc/mapping/generated/nodes.md#mappingdataintegrationnode) collects all sensor information and publishes the resulting map to `/paf/hero/mapping/init_data`. -The [mapping](/code/mapping/config/mapping.cfg) and [mapping_visualization](/code/mapping_visualization/config/mapping_visualization.cfg) packages support [dynamic reconfigure](/doc/general/dynamic_reconfigure.md) for managing sensor input and filter parameters. +The [mapping](/code/mapping/config/mapping.cfg) package supports [dynamic reconfigure](/doc/general/dynamic_reconfigure.md) for managing sensor input and filter parameters. ### Map usage @@ -54,7 +54,7 @@ This is especially relevant for cross-traffic detection, where static objects sh ### Visualization -The [visualization node](/code/mapping_visualization/src/visualization.py) in the [mapping_visualization](/code/mapping_visualization/) package converts the map into a ROS MarkerArray. +The [visualization node](/code/mapping/mapping/visualization.py) in the [mapping](/code/mapping/) package converts the map into a ROS MarkerArray. The MarkerArray is published to `/paf/hero/mapping/marker_array` and can be visualized and looked at in RViz. @@ -65,9 +65,9 @@ The MarkerArray is published to `/paf/hero/mapping/marker_array` and can be visu - [./ext_modules/mapping_common](/code/mapping/ext_modules/mapping_common/) contains the **python classes for working with the intermediate layer**. - This library can be used across the project. Just `from mapping_common import ...` - **[Get an overview of all available functions in the API documentation](/doc/mapping/generated/mapping_common/index.md)** - - This module is compiled with [Cython](https://cython.readthedocs.io/en/latest/). If changes have been made to this package, catkin_make needs to be executed to apply them! \ + - This module is compiled with [Cython](https://cython.readthedocs.io/en/latest/). If changes have been made to this package, colcon build needs to be executed to apply them! \ This step is automatically executed when using the [docker-compose.leaderboard.yaml](/build/docker-compose.leaderboard.yaml). -- [./src](/code/mapping/src/) contains the nodes that create and filter the map +- [./mapping](/code/mapping/mapping/) contains the nodes that create and filter the map - The main node is the [**MappingDataIntegrationNode**](/doc/mapping/generated/nodes.md#mappingdataintegrationnode). It collects sensor data and then builds a map from it - **[API documentation](/doc/mapping/generated/nodes.md)** - [./msg](/code/mapping/msg/) contains the ROS message types for transmitting the map @@ -89,7 +89,7 @@ flowchart TD F3(filter: GrowPedestriansFilter) F4(filter: RadarPointAssignmentFilter) F5(filter: TrackingFilter) - VIS[node: mapping_visualization] + VIS[node: mapping/visualization] %% Flow NF --> A @@ -112,11 +112,11 @@ For this purpose, radar points are spatially associated with lidar entities. Thi ## Tests -This package contains pytest based unit tests at [./tests/mapping_common](/code/mapping/tests/mapping_common/) +This package contains pytest based unit tests at [./test](/code/mapping/test/) The tests can be executed without a running ros/carla instance. -Enter the dev container and execute `catkin_make run_tests` in the catkin_ws to run them. A summary should appear in the console. +Enter the dev container and execute `colcon test` or `python3 -m pytest` in the mapping package to run them. A summary should appear in the console. ## Research @@ -126,12 +126,12 @@ Most of the information of the draft has been inserted into the python class doc ## mapping_common Cython installation -**Important: The mapping_common module is compiled with [Cython](https://cython.readthedocs.io/en/latest/). If changes have been made to mapping_common, catkin_make needs to be executed to apply them!** +**Important: The mapping_common module is compiled with [Cython](https://cython.readthedocs.io/en/latest/). If changes have been made to mapping_common, colcon build needs to be executed to apply them!** This step is automatically executed when using the [docker-compose.leaderboard.yaml](/build/docker-compose.leaderboard.yaml). Cmake executes [cmake_setup.py](/code/mapping/ext_modules/cmake_setup.py) to compile and install the mapping_common module. -The integration of the setup command into catkin_make can be found [here](/code/mapping/CMakeLists.txt#L100). +The integration of the setup command into colcon build can be found [here](/code/mapping/CMakeLists.txt#L100). ## Debugging diff --git a/doc/mapping/generated/mapping_common/index.md b/doc/mapping/generated/mapping_common/index.md index ebb1bb33..9b8de3e4 100644 --- a/doc/mapping/generated/mapping_common/index.md +++ b/doc/mapping/generated/mapping_common/index.md @@ -39,7 +39,7 @@ methods. This module is compiled with [Cython](https://cython.readthedocs.io/en/latest/). If changes have been made to this package, -catkin_make needs to be executed to apply them! +colcon build needs to be executed to apply them! This step is automatically executed when using the [docker-compose.leaderboard.yaml](/build/docker-compose.leaderboard.yaml). From 5368b5c6c39209acfa937d2f216aeba5a91db378 Mon Sep 17 00:00:00 2001 From: ll7 Date: Mon, 4 May 2026 17:11:38 +0200 Subject: [PATCH 41/43] refactor: consolidate control logic and enhance testability with pure helpers --- .github/workflows/unit-tests.yml | 11 +- .gitignore | 2 + code/control/control/vehicle_control_logic.py | 73 +++++++ code/control/control/vehicle_controller.py | 66 +++--- .../control/control/velocity_control_logic.py | 81 ++++++++ code/control/control/velocity_controller.py | 60 +++--- code/test/test_control_logic.py | 193 ++++++++++++++++++ doc/dev/README.md | 1 + ...5-04-host-testability-and-control-logic.md | 53 +++++ doc/development/dependency_management.md | 2 + doc/development/quickstart_contributor.md | 1 + doc/development/testing_strategy.md | 5 +- scripts/bootstrap-host-python.sh | 69 +++++++ scripts/run-host-smoke-tests.sh | 7 + 14 files changed, 545 insertions(+), 79 deletions(-) create mode 100644 code/control/control/vehicle_control_logic.py create mode 100644 code/control/control/velocity_control_logic.py create mode 100644 code/test/test_control_logic.py create mode 100644 doc/dev/progress/2026-05-04-host-testability-and-control-logic.md create mode 100755 scripts/bootstrap-host-python.sh create mode 100755 scripts/run-host-smoke-tests.sh diff --git a/.github/workflows/unit-tests.yml b/.github/workflows/unit-tests.yml index 50c21b0e..6a6fe4f9 100644 --- a/.github/workflows/unit-tests.yml +++ b/.github/workflows/unit-tests.yml @@ -17,12 +17,5 @@ jobs: - uses: actions/setup-python@v5 with: python-version: "3.12" - - name: Install test dependencies - run: | - python -m pip install --upgrade pip - python -m pip install -r code/requirements_infrastructure.txt - python -m pip install numpy - - name: Run host smoke tests (plugin autoload disabled) - env: - PYTEST_DISABLE_PLUGIN_AUTOLOAD: "1" - run: python -m pytest code/test -m unit + - name: Run host smoke tests + run: bash scripts/run-host-smoke-tests.sh diff --git a/.gitignore b/.gitignore index ff8275a0..09f091ab 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,8 @@ .DS_Store .idea .gitconfig +.venv/ +.venv-host/ /volumes diff --git a/code/control/control/vehicle_control_logic.py b/code/control/control/vehicle_control_logic.py new file mode 100644 index 00000000..a4b898b8 --- /dev/null +++ b/code/control/control/vehicle_control_logic.py @@ -0,0 +1,73 @@ +"""Pure helpers for deriving vehicle control commands.""" + +from __future__ import annotations + +from dataclasses import dataclass + + +@dataclass(frozen=True) +class VehicleControlState: + """Inputs needed to derive a vehicle control command.""" + + manual_override_active: bool + manual_steer: float + manual_throttle: float + emergency: bool + current_behavior: str | None + reverse: bool + throttle: float + brake: float + pure_pursuit_steer: float + + +@dataclass(frozen=True) +class VehicleControlCommand: + """Pure representation of the outgoing vehicle command.""" + + reverse: bool + throttle: float + brake: float + steer: float + hand_brake: bool + manual_gear_shift: bool = False + + +def build_vehicle_control_command( + state: VehicleControlState, +) -> VehicleControlCommand: + """Build a vehicle control command from the current controller state.""" + if state.manual_override_active: + return VehicleControlCommand( + reverse=state.manual_throttle < 0.0, + throttle=abs(state.manual_throttle), + brake=0.0, + steer=state.manual_steer, + hand_brake=False, + ) + + if state.emergency: + return build_safe_stop_command() + + steer = ( + state.pure_pursuit_steer + if state.current_behavior == "us_unstuck" + else -state.pure_pursuit_steer + ) + return VehicleControlCommand( + reverse=state.reverse, + throttle=state.throttle, + brake=state.brake, + steer=steer, + hand_brake=False, + ) + + +def build_safe_stop_command() -> VehicleControlCommand: + """Return the conservative safe-stop command used on faults/timeouts.""" + return VehicleControlCommand( + reverse=False, + throttle=0.0, + brake=1.0, + steer=0.0, + hand_brake=True, + ) diff --git a/code/control/control/vehicle_controller.py b/code/control/control/vehicle_controller.py index fd7dc970..a113b048 100755 --- a/code/control/control/vehicle_controller.py +++ b/code/control/control/vehicle_controller.py @@ -25,6 +25,12 @@ startup_topic, ) +from .vehicle_control_logic import ( + VehicleControlState, + build_safe_stop_command, + build_vehicle_control_command, +) + DEFAULT_REQUIRED_SYNC_STAGES = [ "mapping", @@ -199,30 +205,30 @@ def _set_parameters_callback(self, params: List[Parameter]): def build_control_message(self) -> CarlaEgoVehicleControl: """Build the control message based on the current state.""" - message = CarlaEgoVehicleControl() - - if self.manual_override_active: - message.reverse = self.manual_throttle < 0 - message.throttle = abs(self.manual_throttle) - message.steer = self.manual_steer - message.brake = 0.0 - message.hand_brake = False - elif self.__emergency: - self._apply_emergency_brake(message) - else: - steer = ( - self._p_steer - if self.__curr_behavior == "us_unstuck" - else -self._p_steer + command = build_vehicle_control_command( + VehicleControlState( + manual_override_active=self.manual_override_active, + manual_steer=self.manual_steer, + manual_throttle=self.manual_throttle, + emergency=self.__emergency, + current_behavior=self.__curr_behavior, + reverse=self.__reverse, + throttle=self.__throttle, + brake=self.__brake, + pure_pursuit_steer=self._p_steer, ) + ) + return self._message_from_command(command) - message.reverse = self.__reverse - message.throttle = self.__throttle - message.brake = self.__brake - message.steer = steer - message.hand_brake = False - message.manual_gear_shift = False - + def _message_from_command(self, command) -> CarlaEgoVehicleControl: + """Convert a pure command description into the ROS/CARLA message type.""" + message = CarlaEgoVehicleControl() + message.reverse = command.reverse + message.throttle = command.throttle + message.brake = command.brake + message.steer = command.steer + message.hand_brake = command.hand_brake + message.manual_gear_shift = command.manual_gear_shift return message # Subscriber callbacks @@ -254,22 +260,8 @@ def __set_reverse(self, data: Bool): def __set_pure_pursuit_steer(self, data: Float32): self._p_steer = data.data / (math.pi / 2) - def _apply_emergency_brake(self, message: CarlaEgoVehicleControl) -> None: - message.throttle = 0.0 - message.steer = 0.0 - message.brake = 1.0 - message.reverse = False - message.hand_brake = True - def _build_safe_stop_message(self) -> CarlaEgoVehicleControl: - message = CarlaEgoVehicleControl() - message.throttle = 0.0 - message.steer = 0.0 - message.brake = 1.0 - message.reverse = False - message.hand_brake = True - message.manual_gear_shift = False - return message + return self._message_from_command(build_safe_stop_command()) def loop(self, clock: Clock): """Begin a new pending simulation frame and wait for stage completion.""" diff --git a/code/control/control/velocity_control_logic.py b/code/control/control/velocity_control_logic.py new file mode 100644 index 00000000..12a415eb --- /dev/null +++ b/code/control/control/velocity_control_logic.py @@ -0,0 +1,81 @@ +"""Pure helpers for deriving velocity control commands.""" + +from __future__ import annotations + +from dataclasses import dataclass + + +@dataclass(frozen=True) +class VelocityControlPlan: + """Describe how the controller should evaluate the next longitudinal step.""" + + reverse: bool + setpoint: float | None + measurement: float | None + throttle: float = 0.0 + brake: float = 0.0 + + +@dataclass(frozen=True) +class VelocityControlCommand: + """Pure representation of the outgoing longitudinal command.""" + + reverse: bool + throttle: float + brake: float + + +def select_target_velocity( + requested_target_velocity: float, + fixed_speed_active: bool, + fixed_speed: float, +) -> float: + """Return the effective target velocity after fixed-speed overrides.""" + return fixed_speed if fixed_speed_active else requested_target_velocity + + +def plan_velocity_control( + target_velocity: float, + current_velocity: float, +) -> VelocityControlPlan: + """Prepare the PID inputs or a direct standstill command.""" + if target_velocity < 0.0: + return VelocityControlPlan( + reverse=True, + setpoint=abs(target_velocity), + measurement=-current_velocity, + ) + + if target_velocity < 0.1: + return VelocityControlPlan( + reverse=False, + setpoint=None, + measurement=None, + throttle=0.0, + brake=1.0, + ) + + return VelocityControlPlan( + reverse=False, + setpoint=target_velocity, + measurement=current_velocity, + ) + + +def finalize_velocity_control( + reverse: bool, + pid_output: float, +) -> VelocityControlCommand: + """Translate the signed PID result into throttle and brake outputs.""" + if pid_output < 0.0: + return VelocityControlCommand( + reverse=reverse, + throttle=0.0, + brake=abs(pid_output), + ) + + return VelocityControlCommand( + reverse=reverse, + throttle=pid_output, + brake=0.0, + ) diff --git a/code/control/control/velocity_controller.py b/code/control/control/velocity_controller.py index 8c285383..2a76925a 100755 --- a/code/control/control/velocity_controller.py +++ b/code/control/control/velocity_controller.py @@ -13,6 +13,13 @@ from paf_common.sync import frame_complete_topic, frame_id_from_time_ns, startup_topic from rclpy.parameter import Parameter +from .velocity_control_logic import ( + VelocityControlCommand, + finalize_velocity_control, + plan_velocity_control, + select_target_velocity, +) + class VelocityController(Node): """ @@ -167,39 +174,30 @@ def loop(self): self.pid_t.Ki = self.pid_i self.pid_t.Kd = self.pid_d - target_velocity = ( - self.__target_velocity if not self.fixed_speed_active else self.fixed_speed + target_velocity = select_target_velocity( + requested_target_velocity=self.__target_velocity, + fixed_speed_active=self.fixed_speed_active, + fixed_speed=self.fixed_speed, ) - # revert driving - if target_velocity < 0: - reverse = True - v = abs(target_velocity) - self.pid_t.setpoint = v - brake = 0 - throttle = self.pid_t(-self.__current_velocity) - if throttle < 0: - brake = abs(throttle) - throttle = 0 - # very low target_velocities -> stand - elif target_velocity < 0.1: - reverse = False - brake = 1 - throttle = 0 + control_plan = plan_velocity_control(target_velocity, self.__current_velocity) + + if control_plan.setpoint is None or control_plan.measurement is None: + control_command = VelocityControlCommand( + reverse=control_plan.reverse, + throttle=control_plan.throttle, + brake=control_plan.brake, + ) else: - reverse = False - v = target_velocity - self.pid_t.setpoint = v - throttle = self.pid_t(self.__current_velocity) - # any throttle < 0 is used as brake signal - if throttle < 0: - brake = abs(throttle) - throttle = 0 - else: - brake = 0 - - self.reverse_pub.publish(Bool(data=reverse)) - self.brake_pub.publish(Float32(data=float(brake))) - self.throttle_pub.publish(Float32(data=float(throttle))) + self.pid_t.setpoint = control_plan.setpoint + pid_output = self.pid_t(control_plan.measurement) + control_command = finalize_velocity_control( + reverse=control_plan.reverse, + pid_output=pid_output, + ) + + self.reverse_pub.publish(Bool(data=control_command.reverse)) + self.brake_pub.publish(Float32(data=float(control_command.brake))) + self.throttle_pub.publish(Float32(data=float(control_command.throttle))) self.frame_complete_pub.publish( UInt64( data=frame_id_from_time_ns( diff --git a/code/test/test_control_logic.py b/code/test/test_control_logic.py new file mode 100644 index 00000000..ea6de01a --- /dev/null +++ b/code/test/test_control_logic.py @@ -0,0 +1,193 @@ +"""Host-runnable tests for pure control helper logic.""" + +from __future__ import annotations + +import importlib +import sys +from pathlib import Path + +import pytest + +pytestmark = pytest.mark.unit + +CODE_ROOT = Path(__file__).resolve().parents[1] +CONTROL_SRC = CODE_ROOT / "control" +if str(CONTROL_SRC) not in sys.path: + sys.path.insert(0, str(CONTROL_SRC)) + + +@pytest.fixture(scope="module") +def vehicle_control_logic(): + """Import the pure vehicle control helper module.""" + return importlib.import_module("control.vehicle_control_logic") + + +@pytest.fixture(scope="module") +def velocity_control_logic(): + """Import the pure velocity control helper module.""" + return importlib.import_module("control.velocity_control_logic") + + +def test_vehicle_control_manual_override_uses_absolute_throttle( + vehicle_control_logic, +) -> None: + """Manual override must not leak negative throttle magnitudes.""" + state = vehicle_control_logic.VehicleControlState( + manual_override_active=True, + manual_steer=0.25, + manual_throttle=-0.4, + emergency=False, + current_behavior=None, + reverse=False, + throttle=0.0, + brake=0.0, + pure_pursuit_steer=0.6, + ) + + command = vehicle_control_logic.build_vehicle_control_command(state) + + assert command.reverse is True + assert command.throttle == pytest.approx(0.4) + assert command.brake == pytest.approx(0.0) + assert command.steer == pytest.approx(0.25) + assert command.hand_brake is False + assert command.manual_gear_shift is False + + +def test_vehicle_control_emergency_maps_to_safe_stop(vehicle_control_logic) -> None: + """Emergency handling should consistently reuse the safe-stop behavior.""" + state = vehicle_control_logic.VehicleControlState( + manual_override_active=False, + manual_steer=0.1, + manual_throttle=0.8, + emergency=True, + current_behavior="lane_following", + reverse=True, + throttle=0.7, + brake=0.2, + pure_pursuit_steer=0.3, + ) + + command = vehicle_control_logic.build_vehicle_control_command(state) + safe_stop = vehicle_control_logic.build_safe_stop_command() + + assert command == safe_stop + + +@pytest.mark.parametrize( + ("current_behavior", "expected_steer"), + [("us_unstuck", 0.35), ("lane_following", -0.35), (None, -0.35)], +) +def test_vehicle_control_respects_unstuck_steer_sign( + vehicle_control_logic, + current_behavior: str | None, + expected_steer: float, +) -> None: + """Keep unstuck behavior aligned with the intended steering sign.""" + state = vehicle_control_logic.VehicleControlState( + manual_override_active=False, + manual_steer=0.0, + manual_throttle=0.0, + emergency=False, + current_behavior=current_behavior, + reverse=True, + throttle=0.55, + brake=0.15, + pure_pursuit_steer=0.35, + ) + + command = vehicle_control_logic.build_vehicle_control_command(state) + + assert command.reverse is True + assert command.throttle == pytest.approx(0.55) + assert command.brake == pytest.approx(0.15) + assert command.steer == pytest.approx(expected_steer) + + +def test_select_target_velocity_prefers_fixed_speed(velocity_control_logic) -> None: + """Fixed-speed mode should fully override requested target velocities.""" + assert velocity_control_logic.select_target_velocity( + requested_target_velocity=3.5, + fixed_speed_active=True, + fixed_speed=-1.25, + ) == pytest.approx(-1.25) + + +@pytest.mark.parametrize( + ("target_velocity", "current_velocity", "expected"), + [ + ( + -2.5, + 4.0, + {"reverse": True, "setpoint": 2.5, "measurement": -4.0, "brake": 0.0}, + ), + ( + 0.05, + 1.5, + {"reverse": False, "setpoint": None, "measurement": None, "brake": 1.0}, + ), + ( + 6.0, + 1.5, + {"reverse": False, "setpoint": 6.0, "measurement": 1.5, "brake": 0.0}, + ), + ], +) +def test_plan_velocity_control_covers_reverse_stop_and_drive_modes( + velocity_control_logic, + target_velocity: float, + current_velocity: float, + expected: dict[str, float | bool | None], +) -> None: + """Verify the controller plans the right PID inputs for each mode.""" + plan = velocity_control_logic.plan_velocity_control( + target_velocity=target_velocity, + current_velocity=current_velocity, + ) + + assert plan.reverse is expected["reverse"] + if expected["setpoint"] is None: + assert plan.setpoint is None + else: + assert plan.setpoint == pytest.approx(expected["setpoint"]) + if expected["measurement"] is None: + assert plan.measurement is None + else: + assert plan.measurement == pytest.approx(expected["measurement"]) + assert plan.brake == pytest.approx(expected["brake"]) + + +@pytest.mark.parametrize( + ("reverse", "pid_output", "expected_throttle", "expected_brake"), + [(False, 0.42, 0.42, 0.0), (True, -0.3, 0.0, 0.3)], +) +def test_finalize_velocity_control_splits_pid_output_into_actuators( + velocity_control_logic, + reverse: bool, + pid_output: float, + expected_throttle: float, + expected_brake: float, +) -> None: + """Signed PID output should map deterministically to throttle and brake.""" + command = velocity_control_logic.finalize_velocity_control( + reverse=reverse, + pid_output=pid_output, + ) + + assert command.reverse is reverse + assert command.throttle == pytest.approx(expected_throttle) + assert command.brake == pytest.approx(expected_brake) + + +def test_control_nodes_delegate_branch_logic_to_pure_helpers() -> None: + """Keep the ROS nodes wired to the extracted pure helper modules.""" + vehicle_source = (CONTROL_SRC / "control/vehicle_controller.py").read_text( + encoding="utf-8" + ) + velocity_source = (CONTROL_SRC / "control/velocity_controller.py").read_text( + encoding="utf-8" + ) + + assert "build_vehicle_control_command" in vehicle_source + assert "plan_velocity_control" in velocity_source + assert "finalize_velocity_control" in velocity_source diff --git a/doc/dev/README.md b/doc/dev/README.md index d319f234..501815c2 100644 --- a/doc/dev/README.md +++ b/doc/dev/README.md @@ -4,6 +4,7 @@ This folder contains non-canonical development handoff and progress notes that p ## Progress Notes +- [2026-05-04-host-testability-and-control-logic.md](./progress/2026-05-04-host-testability-and-control-logic.md) - [2026-05-01-doc-consolidation-wp1.md](./progress/2026-05-01-doc-consolidation-wp1.md) - [2026-04-30-route-sync-bridge.md](./progress/2026-04-30-route-sync-bridge.md) - [2026-04-29-agent-handoff-rules.md](./progress/2026-04-29-agent-handoff-rules.md) diff --git a/doc/dev/progress/2026-05-04-host-testability-and-control-logic.md b/doc/dev/progress/2026-05-04-host-testability-and-control-logic.md new file mode 100644 index 00000000..de029105 --- /dev/null +++ b/doc/dev/progress/2026-05-04-host-testability-and-control-logic.md @@ -0,0 +1,53 @@ +# 2026-05-04 Host testability and control logic consolidation + +## Motivating task + +Improve repository-wide quality in a consolidation-first way by strengthening the fastest validation loop, increasing testability, and reducing branch-heavy ROS-node logic that was hard to test outside a built workspace. + +## Current chat state + +This change set focuses on a realistic high-leverage slice instead of claiming full-repository completion. It improves host smoke-test reproducibility, extracts pure control helpers from ROS nodes, and adds host-runnable unit coverage for the extracted logic. + +## Source files and systems inspected + +- `agents.md` +- `.agent/PLANS.md` +- `doc/dev_talks/paf25/future_work.md` +- `doc/dev_talks/paf25/improvements_assessment.md` +- `doc/development/testing_strategy.md` +- `.github/workflows/unit-tests.yml` +- `code/control/control/vehicle_controller.py` +- `code/control/control/velocity_controller.py` +- `code/test/test_planning_regression.py` +- `code/test/test_deterministic_sync.py` + +## Work completed + +- Added `scripts/bootstrap-host-python.sh` to create or refresh a repo-local host tooling environment from committed manifests and to run commands through it. +- Added `scripts/run-host-smoke-tests.sh` so local contributors and CI use the same host smoke-test entrypoint. +- Extracted pure helper modules: + - `code/control/control/vehicle_control_logic.py` + - `code/control/control/velocity_control_logic.py` +- Rewired `vehicle_controller.py` and `velocity_controller.py` to use the extracted helpers while preserving their external ROS/CARLA interfaces. +- Added `code/test/test_control_logic.py` with host-runnable coverage for manual override, emergency stop, unstuck steering sign handling, fixed-speed override, PID planning, and PID output mapping. +- Updated `.github/workflows/unit-tests.yml` to run the repository script instead of duplicating bootstrap steps. +- Updated the relevant developer docs for the new host smoke-test/bootstrap path. +- Ignored `.venv/` and `.venv-host/` in `.gitignore`. + +## Validation performed + +- `bash scripts/run-host-smoke-tests.sh` +- `bash scripts/bootstrap-host-python.sh python -m ruff check code/control/control/vehicle_control_logic.py code/control/control/velocity_control_logic.py code/control/control/vehicle_controller.py code/control/control/velocity_controller.py code/test/test_control_logic.py` +- `bash scripts/bootstrap-host-python.sh python -m ruff format --check code/control/control/vehicle_control_logic.py code/control/control/velocity_control_logic.py code/control/control/vehicle_controller.py code/control/control/velocity_controller.py code/test/test_control_logic.py` + +Observed result: + +- host smoke tests passed (`42 passed`) +- targeted Ruff lint passed +- targeted Ruff format check passed + +## Remaining blockers and follow-ups + +- ROS-backed tests for the runtime control nodes are still a useful next step inside the dev container after building the package closure. +- Open issues around radar quality and planning startup behavior were inspected during triage, but not claimed as solved here without direct runtime proof. +- If this consolidation slice is merged, the next best follow-up is to extract and test additional pure helpers from `acting` or add focused ROS-backed control tests in CI. diff --git a/doc/development/dependency_management.md b/doc/development/dependency_management.md index 0c2e142b..3d2d1762 100644 --- a/doc/development/dependency_management.md +++ b/doc/development/dependency_management.md @@ -9,6 +9,8 @@ This project is developed inside the `agent-dev` container. Dependency changes m - Python dev/tooling dependencies: `code/requirements_infrastructure.txt`. - Ruff tool version pin: `build/pins/ruff.env`. +For host-only smoke tests outside the container, use `scripts/bootstrap-host-python.sh`. It creates a local `.venv-host` from committed manifests instead of relying on ad-hoc global installs. + ## Rules 1. Do not install long-term dependencies manually with `pip install ` in the container shell. diff --git a/doc/development/quickstart_contributor.md b/doc/development/quickstart_contributor.md index d7137a93..d17407a1 100644 --- a/doc/development/quickstart_contributor.md +++ b/doc/development/quickstart_contributor.md @@ -83,6 +83,7 @@ pre-commit run --all-files - `Run ROS-backed unit tests (dev container)` - `Dependency check in dev container` - `Pre-PR quality check` + - Or from a plain host shell: `bash scripts/run-host-smoke-tests.sh` 5. Commit only related files and open a focused PR. ## 7) Common problems diff --git a/doc/development/testing_strategy.md b/doc/development/testing_strategy.md index bdb50b74..a360f274 100644 --- a/doc/development/testing_strategy.md +++ b/doc/development/testing_strategy.md @@ -16,7 +16,8 @@ Repository markers are defined in `pytest.ini`. -- Run host smoke tests: `PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest code/test -m unit` +- Bootstrap a host-side test/lint venv: `bash scripts/bootstrap-host-python.sh` +- Run host smoke tests: `bash scripts/run-host-smoke-tests.sh` - Run unit tests in the current environment: `PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest -m unit` - Run integration tests: `pytest -m integration` - Run simulation tests: `pytest -m sim` @@ -26,7 +27,7 @@ Repository markers are defined in `pytest.ini`. Use two different fast loops: -- Host smoke tests: `code/test` only. These must stay runnable outside the dev container and should not depend on `/workspace`, sourced ROS overlays, or generated interfaces. +- Host smoke tests: `code/test` only. These must stay runnable outside the dev container and should not depend on `/workspace`, sourced ROS overlays, or generated interfaces. Use `bash scripts/run-host-smoke-tests.sh` so the same bootstrap path works in CI and on contributor machines. - ROS-backed unit tests: package-local tests such as `code/perception/tests`, `code/mapping/test`, and `code/planning/test`. These run inside the dev container after building the required package closure. Example ROS-backed unit test loop inside the dev container: diff --git a/scripts/bootstrap-host-python.sh b/scripts/bootstrap-host-python.sh new file mode 100755 index 00000000..271422a4 --- /dev/null +++ b/scripts/bootstrap-host-python.sh @@ -0,0 +1,69 @@ +#!/usr/bin/env bash +set -euo pipefail + +REPO_ROOT="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." &>/dev/null && pwd)" +VENV_DIR="${PAF_HOST_VENV:-$REPO_ROOT/.venv-host}" +STAMP_FILE="$VENV_DIR/.paf-host-deps.sha256" + +if [ -n "${PAF_HOST_PYTHON:-}" ]; then + PYTHON_BIN="$PAF_HOST_PYTHON" +elif [ -x /usr/bin/python3 ]; then + PYTHON_BIN="/usr/bin/python3" +else + PYTHON_BIN="python3" +fi + +if command -v uv >/dev/null 2>&1; then + UV_BIN="$(command -v uv)" +else + UV_BIN="" +fi + +deps_hash="$( + { + cat "$REPO_ROOT/code/requirements_infrastructure.txt" + printf '\n# extra host smoke test dependency\nnumpy==1.26.4\n' + } | sha256sum | cut -d' ' -f1 +)" + +create_virtualenv() { + rm -rf "$VENV_DIR" + if [ -n "$UV_BIN" ]; then + "$UV_BIN" venv --python "$PYTHON_BIN" "$VENV_DIR" + return + fi + + "$PYTHON_BIN" -m venv "$VENV_DIR" +} + +if [ ! -x "$VENV_DIR/bin/python" ] \ + || ! "$VENV_DIR/bin/python" -c "import encodings" >/dev/null 2>&1 \ + || { [ -z "$UV_BIN" ] && ! "$VENV_DIR/bin/python" -m pip --version >/dev/null 2>&1; }; then + create_virtualenv +fi + +if [ ! -f "$STAMP_FILE" ] || [ "$(cat "$STAMP_FILE")" != "$deps_hash" ]; then + if [ -n "$UV_BIN" ]; then + "$UV_BIN" pip install \ + --python "$VENV_DIR/bin/python" \ + -r "$REPO_ROOT/code/requirements_infrastructure.txt" \ + numpy==1.26.4 + else + "$VENV_DIR/bin/python" -m pip install --upgrade pip + "$VENV_DIR/bin/python" -m pip install \ + -r "$REPO_ROOT/code/requirements_infrastructure.txt" \ + numpy==1.26.4 + fi + printf '%s\n' "$deps_hash" >"$STAMP_FILE" +fi + +if [ "$#" -gt 0 ]; then + export VIRTUAL_ENV="$VENV_DIR" + export PATH="$VENV_DIR/bin:$PATH" + exec "$@" +fi + +printf 'Host Python environment ready at %s\n' "$VENV_DIR" +printf 'Using interpreter: %s\n' "$PYTHON_BIN" +printf 'Run commands through it with:\n' +printf ' bash scripts/bootstrap-host-python.sh python -m pytest code/test -m unit\n' diff --git a/scripts/run-host-smoke-tests.sh b/scripts/run-host-smoke-tests.sh new file mode 100755 index 00000000..f405fe51 --- /dev/null +++ b/scripts/run-host-smoke-tests.sh @@ -0,0 +1,7 @@ +#!/usr/bin/env bash +set -euo pipefail + +REPO_ROOT="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." &>/dev/null && pwd)" + +exec "$REPO_ROOT/scripts/bootstrap-host-python.sh" \ + python -m pytest code/test -m unit "$@" From 6b874f561d02f5a6fd6390f2788ff847de18552a Mon Sep 17 00:00:00 2001 From: ll7 Date: Sun, 24 May 2026 13:00:08 +0200 Subject: [PATCH 42/43] docs: remove lidar distance trailing whitespace --- doc/perception/lidar_distance.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/perception/lidar_distance.md b/doc/perception/lidar_distance.md index bf753f2b..63851265 100644 --- a/doc/perception/lidar_distance.md +++ b/doc/perception/lidar_distance.md @@ -238,7 +238,7 @@ LocalCompensation active: - **Left:** `~image_distance_topic` (Default: `/paf/hero/Left/dist_array`) - **Right:** `~image_distance_topic` (Default: `/paf/hero/Right/dist_array`) - **Data Type:** `sensor_msgs/Image` -- **Description:** Contains the calculated minimum distance to objects in various directions. Although the _Back_, _Left_, and _Right_ directions are still actively processed in this node's image pipeline from PAF23, the current vision node only subscribes to and utilizes the _Center_S image. +- **Description:** Contains the calculated minimum distance to objects in various directions. Although the _Back_, _Left_, and _Right_ directions are still actively processed in this node's image pipeline from PAF23, the current vision node only subscribes to and utilizes the _Center_S image. Support for the other directions has been intentionally preserved to allow future teams to easily extend the system with additional camera perspectives if needed. ### Marker Visualization From 097ab5c7f6bd1e90f2b83db5a3cb02871d628312 Mon Sep 17 00:00:00 2001 From: ll7 Date: Sun, 24 May 2026 13:17:09 +0200 Subject: [PATCH 43/43] ci: fix PR lint failures --- .../docs-and-reasoning.instructions.md | 3 ++- .github/pull_request_template.md | 3 +-- agents.md | 11 ++++---- code/acting/setup.py | 2 +- code/agent/setup.py | 2 +- code/control/setup.py | 2 +- code/localization/setup.py | 2 +- code/paf_common/paf_common/route_metrics.py | 12 ++++----- code/paf_common/paf_common/sync.py | 25 +++++++++++++------ code/perception/setup.py | 2 +- code/planning/setup.py | 2 +- doc/adr/0000-template.md | 2 +- doc/control/vehicle_controller.md | 6 ++++- .../2026-04-29-agent-handoff-rules.md | 4 +-- ...oute-validation-and-map-context-handoff.md | 4 +-- .../progress/2026-04-30-route-sync-bridge.md | 4 +-- .../progress/2026-05-01-build-validation.md | 4 +-- .../2026-05-01-doc-consolidation-wp1.md | 11 ++++++-- doc/dev_talks/paf25/paf25_review_by_ll7.md | 9 ++++--- doc/development/installing_python_packages.md | 6 ++--- doc/development/quickstart_contributor.md | 6 ++--- doc/general/execution.md | 3 ++- doc/mapping/README.md | 7 ++++-- 23 files changed, 81 insertions(+), 51 deletions(-) diff --git a/.github/instructions/docs-and-reasoning.instructions.md b/.github/instructions/docs-and-reasoning.instructions.md index ad55fc7f..217d9c64 100644 --- a/.github/instructions/docs-and-reasoning.instructions.md +++ b/.github/instructions/docs-and-reasoning.instructions.md @@ -6,7 +6,8 @@ applyTo: "doc/**/*.md,README.md,agents.md" # Docs And Reasoning - Active documentation must match the current implementation. If code and docs disagree, either fix the doc in the same change or call out the gap explicitly. -- Keep canonical behavior and interface docs in their domain folders under `doc/` or the package docs. Use `doc/dev/progress/` for timestamped progress and handoff notes that capture the current chat state, and use `doc/reasoning/` for deeper analysis notes, comparisons, migration thoughts, and development output that should not become the source of truth. +- Keep canonical behavior and interface docs in their domain folders under `doc/` or the package docs. + Use `doc/dev/progress/` for timestamped progress and handoff notes that capture the current chat state, and use `doc/reasoning/` for deeper analysis notes, comparisons, migration thoughts, and development output that should not become the source of truth. - When saving a progress or handoff note, include the timestamp, motivating task, current chat state, source files or repositories inspected, validation status, remaining blockers, and recommended follow-ups. - When saving a reasoning note, include the motivating task, the source files or repositories inspected, the main conclusion, and the remaining follow-ups. - Link new documentation from `doc/README.md` or the most relevant existing index page so it stays discoverable. diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md index 00d6d8bd..1e2519d0 100644 --- a/.github/pull_request_template.md +++ b/.github/pull_request_template.md @@ -33,7 +33,7 @@ List commands executed and outcomes (lint, format, tests, simulation checks, etc List remaining risks or follow-up work that is intentionally out of scope. -# Checklist: +## Checklist - [ ] My code follows the style guidelines of this project - [ ] I have performed a self-review of my own code @@ -44,4 +44,3 @@ List remaining risks or follow-up work that is intentionally out of scope. - [ ] New and existing unit tests pass locally with my changes (might be obsolete with CI later on) - [ ] I documented assumptions, validation steps, and known gaps in this PR - [ ] I ran dependency validation (`dep.check` or `scripts/dependency-doctor.sh`) when touching dependencies - diff --git a/agents.md b/agents.md index e048c799..635fc9d0 100644 --- a/agents.md +++ b/agents.md @@ -55,10 +55,10 @@ Python linting/formatting is done with **Ruff**. Run one of: - VS Code tasks (preferred in this workspace): - - `Lint python code with ruff` - - `Lint python code with ruff and apply safe fixes` - - `Check python code formatting with ruff` - - `Format python code with ruff` + - `Lint python code with ruff` + - `Lint python code with ruff and apply safe fixes` + - `Check python code formatting with ruff` + - `Format python code with ruff` - Or Compose-based linting from docs: ```bash @@ -95,7 +95,8 @@ Do not attempt to fix unrelated failing tests/lints outside the requested scope. ## 9.1) Planning and reasoning support - For non-trivial work, use `.agent/PLANS.md` to make scope, evidence, validation, and follow-ups explicit. -- For non-trivial or multi-step work, always create or update a timestamped note under `doc/dev/progress/` before finishing or handing off. The note should capture the current chat state, the motivating task, files or systems inspected, validation performed, remaining blockers, and recommended next actions. +- For non-trivial or multi-step work, always create or update a timestamped note under `doc/dev/progress/` before finishing or handing off. + The note should capture the current chat state, the motivating task, files or systems inspected, validation performed, remaining blockers, and recommended next actions. - Treat `doc/dev_talks/paf25/future_work.md` and `doc/dev_talks/paf25/improvements_assessment.md` as direction-setting documents for repository-wide cleanup and development workflow changes. ## 10) Git and PR hygiene diff --git a/code/acting/setup.py b/code/acting/setup.py index e603b6be..9a834089 100644 --- a/code/acting/setup.py +++ b/code/acting/setup.py @@ -17,7 +17,7 @@ zip_safe=True, maintainer="peter", maintainer_email="peter.viechter@student.uni-augsburg.de", - description="Acting package (passthrough to vehicle controller) for the PAF autonomous driving stack", + description="Acting package (passthrough to vehicle controller) for the PAF stack", license="MIT", tests_require=["pytest"], entry_points={ diff --git a/code/agent/setup.py b/code/agent/setup.py index 5e69bc4a..01963cbe 100644 --- a/code/agent/setup.py +++ b/code/agent/setup.py @@ -17,7 +17,7 @@ zip_safe=True, maintainer="peter", maintainer_email="peter.viechter@student.uni-augsburg.de", - description="Agent startup coordination, shutdown, and data management for the PAF autonomous driving stack", + description="Agent startup, shutdown, and data management for the PAF stack", license="MIT", tests_require=["pytest"], scripts=["scripts/launch_agent.sh"], diff --git a/code/control/setup.py b/code/control/setup.py index d3f716f1..42daf99f 100644 --- a/code/control/setup.py +++ b/code/control/setup.py @@ -18,7 +18,7 @@ zip_safe=True, maintainer="peter", maintainer_email="peter.viechter@student.uni-augsburg.de", - description="Vehicle control (steering, velocity, vehicle controller) for the PAF autonomous driving stack", + description="Vehicle control (steering, velocity) for the PAF stack", license="MIT", tests_require=["pytest"], entry_points={ diff --git a/code/localization/setup.py b/code/localization/setup.py index 42137031..a80ef318 100644 --- a/code/localization/setup.py +++ b/code/localization/setup.py @@ -18,7 +18,7 @@ zip_safe=True, maintainer="peter", maintainer_email="peter.viechter@student.uni-augsburg.de", - description="Localization (EKF, GNSS, odometry, GPS transform) for the PAF autonomous driving stack", + description="Localization (EKF, GNSS, odometry, GPS transform) for the PAF stack", license="MIT", tests_require=["pytest"], entry_points={ diff --git a/code/paf_common/paf_common/route_metrics.py b/code/paf_common/paf_common/route_metrics.py index 8ddbfaf7..b7a6de90 100644 --- a/code/paf_common/paf_common/route_metrics.py +++ b/code/paf_common/paf_common/route_metrics.py @@ -6,14 +6,14 @@ import json import os from pathlib import Path -from typing import Any, Optional +from typing import Any DEFAULT_ROUTE_METRICS_PATH = Path("/tmp/paf_route_metrics.json") ROUTE_METRICS_ENV_VAR = "PAF_ROUTE_METRICS_PATH" -def get_route_metrics_path(path: Optional[str | os.PathLike[str]] = None) -> Path: +def get_route_metrics_path(path: str | os.PathLike[str] | None = None) -> Path: """Resolve the route metrics file path.""" if path is not None: return Path(path) @@ -26,7 +26,7 @@ def get_route_metrics_path(path: Optional[str | os.PathLike[str]] = None) -> Pat def reset_route_metrics_file( - path: Optional[str | os.PathLike[str]] = None, + path: str | os.PathLike[str] | None = None, ) -> Path: """Delete any stale route metrics file before a fresh run.""" metrics_path = get_route_metrics_path(path) @@ -39,7 +39,7 @@ def increment_route_metric( metric_name: str, *, amount: int = 1, - path: Optional[str | os.PathLike[str]] = None, + path: str | os.PathLike[str] | None = None, ) -> int: """Atomically increment a route metric and return the new counter value.""" metrics_path = get_route_metrics_path(path) @@ -67,7 +67,7 @@ def increment_route_metric( def load_route_metrics( - path: Optional[str | os.PathLike[str]] = None, + path: str | os.PathLike[str] | None = None, ) -> dict[str, Any]: """Load the current route metrics snapshot from disk.""" metrics_path = get_route_metrics_path(path) @@ -83,7 +83,7 @@ def load_route_metrics( def merge_route_metrics_into_checkpoint( checkpoint_path: str | os.PathLike[str], *, - metrics_path: Optional[str | os.PathLike[str]] = None, + metrics_path: str | os.PathLike[str] | None = None, ) -> dict[str, Any]: """Merge route metrics into the leaderboard checkpoint json.""" metrics = load_route_metrics(metrics_path) diff --git a/code/paf_common/paf_common/sync.py b/code/paf_common/paf_common/sync.py index 9aaf9584..b9d4875d 100644 --- a/code/paf_common/paf_common/sync.py +++ b/code/paf_common/paf_common/sync.py @@ -3,7 +3,7 @@ from __future__ import annotations from dataclasses import dataclass, field -from typing import Iterable, Optional +from collections.abc import Iterable def normalize_sync_id(value: str) -> str: @@ -42,18 +42,22 @@ class StartupReadinessTracker: ready_by_node: dict[str, bool] = field(default_factory=dict) def __post_init__(self) -> None: + """Normalize configured node ids and initialize readiness state.""" self.required_nodes = tuple( normalize_sync_id(node) for node in self.required_nodes ) - self.ready_by_node = {node: False for node in self.required_nodes} + self.ready_by_node = dict.fromkeys(self.required_nodes, False) def update(self, node_id: str, is_ready: bool) -> None: + """Record the latest readiness state for a startup node.""" self.ready_by_node[normalize_sync_id(node_id)] = is_ready def missing_nodes(self) -> list[str]: + """Return startup nodes that have not reported readiness.""" return [node for node, ready in self.ready_by_node.items() if not ready] def all_ready(self) -> bool: + """Return whether every required startup node is ready.""" return all(self.ready_by_node.get(node, False) for node in self.required_nodes) @@ -63,26 +67,30 @@ class FrameBarrier: required_stages: Iterable[str] completed_by_stage: dict[str, int] = field(default_factory=dict) - pending_frame_id: Optional[int] = None - pending_started_at: Optional[float] = None + pending_frame_id: int | None = None + pending_started_at: float | None = None def __post_init__(self) -> None: + """Normalize configured stage ids and initialize completion state.""" self.required_stages = tuple( normalize_sync_id(stage) for stage in self.required_stages ) - self.completed_by_stage = {stage: -1 for stage in self.required_stages} + self.completed_by_stage = dict.fromkeys(self.required_stages, -1) def begin_frame(self, frame_id: int, started_at: float) -> None: + """Start tracking a frame if it is newer than the pending frame.""" if self.pending_frame_id is None or frame_id > self.pending_frame_id: self.pending_frame_id = frame_id self.pending_started_at = started_at def mark_stage_complete(self, stage_id: str, frame_id: int) -> None: + """Record the latest completed frame for a synchronization stage.""" normalized = normalize_sync_id(stage_id) last_completed = self.completed_by_stage.get(normalized, -1) self.completed_by_stage[normalized] = max(last_completed, frame_id) - def missing_stages(self, frame_id: Optional[int] = None) -> list[str]: + def missing_stages(self, frame_id: int | None = None) -> list[str]: + """Return stages that have not completed the requested frame.""" current_frame = self.pending_frame_id if frame_id is None else frame_id if current_frame is None: return list(self.required_stages) @@ -92,14 +100,17 @@ def missing_stages(self, frame_id: Optional[int] = None) -> list[str]: if self.completed_by_stage.get(stage, -1) < current_frame ] - def is_ready(self, frame_id: Optional[int] = None) -> bool: + def is_ready(self, frame_id: int | None = None) -> bool: + """Return whether all stages completed the requested frame.""" return not self.missing_stages(frame_id) def timed_out(self, now: float, timeout_seconds: float) -> bool: + """Return whether the pending frame exceeded the timeout.""" if self.pending_frame_id is None or self.pending_started_at is None: return False return (now - self.pending_started_at) >= timeout_seconds def clear_pending(self) -> None: + """Clear the pending frame after it is released or abandoned.""" self.pending_frame_id = None self.pending_started_at = None diff --git a/code/perception/setup.py b/code/perception/setup.py index 82c00448..db2020f8 100644 --- a/code/perception/setup.py +++ b/code/perception/setup.py @@ -18,7 +18,7 @@ zip_safe=True, maintainer="peter", maintainer_email="peter.viechter@student.uni-augsburg.de", - description="Perception (lidar, radar, vision, traffic light, lane detection) for the PAF autonomous driving stack", + description="Perception (vision, lidar, radar, traffic) for the PAF stack", license="MIT", tests_require=["pytest"], entry_points={ diff --git a/code/planning/setup.py b/code/planning/setup.py index f9ca30ae..18424026 100644 --- a/code/planning/setup.py +++ b/code/planning/setup.py @@ -18,7 +18,7 @@ zip_safe=True, maintainer="peter", maintainer_email="peter.viechter@student.uni-augsburg.de", - description="Planning (global/local planner, behavior tree, motion planning) for the PAF autonomous driving stack", + description="Planning (planner, behavior tree, motion) for the PAF stack", license="MIT", tests_require=["pytest"], entry_points={ diff --git a/doc/adr/0000-template.md b/doc/adr/0000-template.md index 12ea2bb4..8a3d4bf0 100644 --- a/doc/adr/0000-template.md +++ b/doc/adr/0000-template.md @@ -1,4 +1,4 @@ -# ADR NNNN: +# ADR NNNN: `Decision title` - Status: Proposed | Accepted | Superseded - Date: YYYY-MM-DD diff --git a/doc/control/vehicle_controller.md b/doc/control/vehicle_controller.md index 8a1fc69b..c48f3de0 100644 --- a/doc/control/vehicle_controller.md +++ b/doc/control/vehicle_controller.md @@ -13,7 +13,11 @@ The [Vehicle Controller](../../code/control/control/vehicle_controller.py) collects the control outputs ```throttle```, ```brake```, ```reverse```, and ```pure_pursuit_steer``` to fill the CARLA vehicle command message ```vehicle_control_cmd``` and send it to the CARLA simulator. -The controller no longer uses a sleep-based hotfix to pace the simulator. In the current synchronous setup, it waits until the critical upstream stages for the current frame report completion and only then publishes the final command for that frame. The required stages are currently mapping, motion planning, ACC, pure pursuit, and the velocity controller. +The controller no longer uses a sleep-based hotfix to pace the simulator. +In the current synchronous setup, it waits until the critical upstream stages +for the current frame report completion and only then publishes the final +command for that frame. The required stages are currently mapping, motion +planning, ACC, pure pursuit, and the velocity controller. If the barrier does not complete within the configured ```frame_barrier_timeout```, the controller publishes a safe stop command instead of releasing a stale or partial command. diff --git a/doc/dev/progress/2026-04-29-agent-handoff-rules.md b/doc/dev/progress/2026-04-29-agent-handoff-rules.md index f261be97..68adac20 100644 --- a/doc/dev/progress/2026-04-29-agent-handoff-rules.md +++ b/doc/dev/progress/2026-04-29-agent-handoff-rules.md @@ -1,8 +1,8 @@ +# Agent Handoff Rules Update + Created: 2026-04-29T10:09:26+02:00 Last updated: 2026-04-29T10:09:26+02:00 -# Agent Handoff Rules Update - ## Motivation Codify a single repository rule for preserving current chat state at handoff time so future agent sessions always leave behind a timestamped progress note plus documentation hints that point to it. diff --git a/doc/dev/progress/2026-04-29-route-validation-and-map-context-handoff.md b/doc/dev/progress/2026-04-29-route-validation-and-map-context-handoff.md index b878dd4a..d40e9d91 100644 --- a/doc/dev/progress/2026-04-29-route-validation-and-map-context-handoff.md +++ b/doc/dev/progress/2026-04-29-route-validation-and-map-context-handoff.md @@ -1,8 +1,8 @@ +# Route Validation And Map Context Handoff + Created: 2026-04-29T08:50:23+02:00 Last updated: 2026-04-29T08:50:23+02:00 -# Route Validation And Map Context Handoff - ## Motivation Capture the current chat state as a handoff and progress snapshot for the active branch work. The session currently spans three threads: diff --git a/doc/dev/progress/2026-04-30-route-sync-bridge.md b/doc/dev/progress/2026-04-30-route-sync-bridge.md index e1c9acec..33edd5b8 100644 --- a/doc/dev/progress/2026-04-30-route-sync-bridge.md +++ b/doc/dev/progress/2026-04-30-route-sync-bridge.md @@ -1,8 +1,8 @@ +# Route Sync Bridge Progress + Created: 2026-04-30T22:29:21+02:00 Last updated: 2026-04-30T22:54:59+02:00 -# Route Sync Bridge Progress - ## Motivation Continue the route-validation implementation from diff --git a/doc/dev/progress/2026-05-01-build-validation.md b/doc/dev/progress/2026-05-01-build-validation.md index 628d8ac9..f369af72 100644 --- a/doc/dev/progress/2026-05-01-build-validation.md +++ b/doc/dev/progress/2026-05-01-build-validation.md @@ -1,8 +1,8 @@ +# Build Validation Progress + Created: 2026-05-01T08:14:00+02:00 Last updated: 2026-05-01T08:31:00+02:00 -# Build Validation Progress - ## Motivation Run the full local build path after the route-sync and issue-sweep changes, and diff --git a/doc/dev/progress/2026-05-01-doc-consolidation-wp1.md b/doc/dev/progress/2026-05-01-doc-consolidation-wp1.md index ac3fe215..ad580b8f 100644 --- a/doc/dev/progress/2026-05-01-doc-consolidation-wp1.md +++ b/doc/dev/progress/2026-05-01-doc-consolidation-wp1.md @@ -1,8 +1,8 @@ +# WP1: Documentation and Interface Consolidation + Created: 2026-05-01 Last updated: 2026-05-01 -# WP1: Documentation and Interface Consolidation - ## Motivation Address the highest-priority improvement areas from `doc/dev_talks/paf25/future_work.md` — specifically WP1 documentation and interface consolidation. Focus on fixing stale references, TODO placeholder descriptions, and ROS1-era terminology before any feature expansion. @@ -10,18 +10,22 @@ Address the highest-priority improvement areas from `doc/dev_talks/paf25/future_ ## Changes ### Fixed stale package references + - `doc/mapping/README.md`: Replaced stale `mapping_visualization` package references with current `mapping/mapping/visualization.py` location (visualization was merged into the mapping package) - `doc/mapping/README.md`: Fixed stale `./src/` → `./mapping/` and `./tests/` → `./test/` path references - `doc/general/architecture_current.md`: Fixed stale `init_mapping` topic references → `init_data` (the actual topic) ### Fixed ROS1-era terminology + - `doc/mapping/README.md`: `catkin_make` → `colcon build` (3 occurrences) - `code/mapping/mapping_common/__init__.py`: `catkin_make` → `colcon build` - `doc/mapping/generated/mapping_common/index.md`: `catkin_make` → `colcon build` - `doc/mapping/generated/mapping_common/README.md`: (symlink to index.md, covered) ### Fixed TODO package descriptions + Replaced `"TODO: Package description"` in 7 `setup.py` files with meaningful descriptions: + - `code/agent/setup.py` - `code/control/setup.py` - `code/acting/setup.py` @@ -31,15 +35,18 @@ Replaced `"TODO: Package description"` in 7 `setup.py` files with meaningful des - `code/perception/setup.py` ## Validation + - All changed `.py` files pass `get_errors` check (no errors found) - No lint errors introduced (only string changes in setup.py descriptions, docstring changes in __init__.py) ## Not included (deferred) + - `doc/perception/experiments/` Dockerfile with `catkin_make` — archival content - `doc/acting/discontinued/` and `doc/planning/discontinued/` CMakeLists references — intentionally preserved historical artifacts - Remaining `doc/research/overhaul25/` references — research docs, not active architecture - The generated docs will fully sync on next `pydoc-markdown` run ## Follow-up + - Run `pydoc-markdown` to regenerate generated docs from the fixed source - Next: WP2 (Radar Motion Quality) or WP5 (Automated Testing) diff --git a/doc/dev_talks/paf25/paf25_review_by_ll7.md b/doc/dev_talks/paf25/paf25_review_by_ll7.md index 17220a00..f50c7294 100644 --- a/doc/dev_talks/paf25/paf25_review_by_ll7.md +++ b/doc/dev_talks/paf25/paf25_review_by_ll7.md @@ -47,7 +47,8 @@ The emphasis of this report is on: Compared to `paf25.start`, the repository has evolved from a more fragmented and partly legacy-heavy codebase into a more ROS2-focused, fusion-oriented stack with much stronger perception-to-mapping integration. -The most important technical shift is that perception is no longer only publishing geometric detections. It now publishes richer intermediate data that includes grouped traffic-light crops, lidar clusters, radar-derived motion and class information, and heading changes. Mapping and planning consume that richer data to do radar-lidar association, entity tracking, motion estimation and collision-aware behavior. +The most important technical shift is that perception is no longer only publishing geometric detections. It now publishes richer intermediate data that includes grouped traffic-light crops, lidar clusters, radar-derived motion and class information, and heading changes. +Mapping and planning consume that richer data to do radar-lidar association, entity tracking, motion estimation and collision-aware behavior. The most visible organizational changes are: @@ -56,7 +57,8 @@ The most visible organizational changes are: - significantly expanded documentation effort - introduction of a local automated route-test harness for ROS2 and CARLA leaderboard runs -Overall, the repository has clearly matured. The biggest strengths are perception and mapping integration, better traffic-light robustness, better lidar compensation and tracking, and much better route-level testing support. The biggest remaining weaknesses are documentation drift, incomplete test depth outside route tests, and known radar-motion limitations that are already documented by the team. +Overall, the repository has clearly matured. The biggest strengths are perception and mapping integration, better traffic-light robustness, better lidar compensation and tracking, and much better route-level testing support. +The biggest remaining weaknesses are documentation drift, incomplete test depth outside route tests, and known radar-motion limitations that are already documented by the team. ## 3. Repository Evolution @@ -443,4 +445,5 @@ The main remaining weaknesses are: - known radar-motion limitations that are documented but not fully solved - increasing complexity in a few large core modules -In summary, the repository did not merely accumulate new features during the last months. It became architecturally more coherent, more ROS2-focused, more fusion-aware and more testable. The remaining work is mostly in consolidation: align the documentation with the final code, keep reducing legacy leftovers, and add more targeted automated tests for the new planning and fusion logic. +In summary, the repository did not merely accumulate new features during the last months. It became architecturally more coherent, more ROS2-focused, more fusion-aware and more testable. +The remaining work is mostly in consolidation: align the documentation with the final code, keep reducing legacy leftovers, and add more targeted automated tests for the new planning and fusion logic. diff --git a/doc/development/installing_python_packages.md b/doc/development/installing_python_packages.md index 9cd6bcd1..9b6f96bb 100644 --- a/doc/development/installing_python_packages.md +++ b/doc/development/installing_python_packages.md @@ -13,15 +13,15 @@ Every dependency must be pinned with `==`. ## Add a dependency safely 1. Edit the matching `requirements*.txt` file. -2. Open a shell in the `agent-dev` container. -3. Run: +1. Open a shell in the `agent-dev` container. +1. Run: ```bash dep.sync devbuild ``` -4. Validate with: +1. Validate with: ```bash python3 -m pip check diff --git a/doc/development/quickstart_contributor.md b/doc/development/quickstart_contributor.md index d17407a1..9987c1c0 100644 --- a/doc/development/quickstart_contributor.md +++ b/doc/development/quickstart_contributor.md @@ -94,7 +94,7 @@ pre-commit run --all-files - `/internal_workspace/rosdep_install.log` - `/internal_workspace/pip_install.log` - If dependency drift appears after changing `requirements*.txt` or `package.xml`, run: - - `dep.sync` - - `devbuild` + - `dep.sync` + - `devbuild` - If requirements consistency is unclear across files, run: - - `Run dependency doctor` task or `bash scripts/dependency-doctor.sh` + - `Run dependency doctor` task or `bash scripts/dependency-doctor.sh` diff --git a/doc/general/execution.md b/doc/general/execution.md index 64870496..2920c271 100644 --- a/doc/general/execution.md +++ b/doc/general/execution.md @@ -30,7 +30,8 @@ This sets up important docker compose environment variables. In order to start the default leaderboard execution simply navigate to the [build](../../build/) folder and select the `Compose up` option in the right-click menu of the `docker-compose.dev..yml` file. As `` `cuda` should be used for the lab PCs. -The helper script [scripts/update-dotenv.sh](../../scripts/update-dotenv.sh) writes the compose environment file [build/.env](../../build/.env). When it detects a headless or SSH-forwarded session, it also sets `RENDER_OFFSCREEN=-RenderOffScreen` so that the CARLA simulator can start without an attached desktop renderer. +The helper script [scripts/update-dotenv.sh](../../scripts/update-dotenv.sh) writes the compose environment file [build/.env](../../build/.env). +When it detects a headless or SSH-forwarded session, it also sets `RENDER_OFFSCREEN=-RenderOffScreen` so that the CARLA simulator can start without an attached desktop renderer. ## Directory Structure diff --git a/doc/mapping/README.md b/doc/mapping/README.md index c99c4284..7a0f82bb 100644 --- a/doc/mapping/README.md +++ b/doc/mapping/README.md @@ -23,7 +23,8 @@ The **Intermediate Layer** receives most sensor information (everything except t - and then forwards it to [planning](/doc/README.md#planning)/[acting](/doc/README.md#acting) The base data type is the [Map](/doc/mapping/generated/mapping_common/map.md#map). It consists of [Entities](/doc/mapping/generated/mapping_common/entity.md#entity). -These entities all have a [transform](/doc/mapping/generated/mapping_common/transform.md#transform2d) and a [shape](/doc/mapping/generated/mapping_common/shape.md#shape2d) and can be all kinds of colliders (car, pedestrian, etc.), lanemarkings or other localized things of interest around the hero car. +These entities all have a [transform](/doc/mapping/generated/mapping_common/transform.md#transform2d) and a [shape](/doc/mapping/generated/mapping_common/shape.md#shape2d) and can be all kinds of colliders +(car, pedestrian, etc.), lanemarkings or other localized things of interest around the hero car. Entities also store information about the sensor sources they originate from. This information is stored in the `sensor_id` field as a list of strings. A list is used because a single entity may be associated with multiple sensors during data integration. @@ -46,7 +47,9 @@ To do intersection checks on the map: - [`map_tree.get_overlapping_entities()`](/doc/mapping/generated/mapping_common/map.md#mapping_common.map.MapTree.get_overlapping_entities) - [`map_tree.get_nearest_entity()`](/doc/mapping/generated/mapping_common/map.md#mapping_common.map.MapTree.get_nearest_entity) - [`map_tree.is_lane_free()`](/doc/mapping/generated/mapping_common/map.md#mapping_common.map.MapTree.is_lane_free) - - [`map_tree.get_lane_context()`](/doc/mapping/generated/mapping_common/map.md#mapping_common.map.MapTree.get_lane_context) and [`map_tree.get_adjacent_lane_context()`](/doc/mapping/generated/mapping_common/map.md#mapping_common.map.MapTree.get_adjacent_lane_context) to query whether adjacent lanes exist and whether they are currently traversable + - [`map_tree.get_lane_context()`](/doc/mapping/generated/mapping_common/map.md#mapping_common.map.MapTree.get_lane_context) and + [`map_tree.get_adjacent_lane_context()`](/doc/mapping/generated/mapping_common/map.md#mapping_common.map.MapTree.get_adjacent_lane_context) + to query whether adjacent lanes exist and whether they are currently traversable - Functions for creating collision masks can be found in the [mapping_common.mask](/doc/mapping/generated/mapping_common/mask.md) module For intersection-related traffic checks, dynamic entities can also be evaluated using motion information and speed thresholds.