diff --git a/.agent/PLANS.md b/.agent/PLANS.md new file mode 100644 index 000000000..869a816b1 --- /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/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 000000000..b066abc8e --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,46 @@ +{ + "name": "PAF Agent Dev (CUDA)", + "dockerComposeFile": [ + "../build/docker-compose.dev.cuda.yml" + ], + "service": "agent-dev", + "runServices": [ + "agent-dev" + ], + "workspaceFolder": "/workspace", + "containerEnv": { + "PAF_LAUNCH_CONTAINER_VSCODE": "0" + }, + "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.copilot", + "github.copilot-chat", + "github.vscode-pull-request-github", + "ms-azuretools.vscode-docker", + "ms-vscode.cmake-tools", + "ms-vscode-remote.remote-containers", + "ms-python.python", + "ms-python.vscode-pylance", + "charliermarsh.ruff", + "editorconfig.editorconfig", + "augustocdias.tasks-shell-input", + "ranch-hand-robotics.rde-pack", + "ktnrg45.vscode-cython", + "timonwong.shellcheck", + "phil294.git-log--graph", + "richardkotze.git-mob", + "bierner.markdown-mermaid", + "valentjn.vscode-ltex", + "vscode-icons-team.vscode-icons", + "yzhang.markdown-all-in-one", + "njpwerner.autodocstring" + ] + } + } +} diff --git a/.github/instructions/docs-and-reasoning.instructions.md b/.github/instructions/docs-and-reasoning.instructions.md new file mode 100644 index 000000000..217d9c640 --- /dev/null +++ b/.github/instructions/docs-and-reasoning.instructions.md @@ -0,0 +1,15 @@ +--- +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/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/.github/instructions/repo-consolidation.instructions.md b/.github/instructions/repo-consolidation.instructions.md new file mode 100644 index 000000000..7b36453a9 --- /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/.github/pull_request_template.md b/.github/pull_request_template.md index 4c0d9a483..1e2519d07 100644 --- a/.github/pull_request_template.md +++ b/.github/pull_request_template.md @@ -21,7 +21,19 @@ 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? -# Checklist: +## 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 - [ ] I have performed a self-review of my own code @@ -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 +- [ ] 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 000000000..bbdd92dd8 --- /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/drive.yml b/.github/workflows/drive.yml index 9b4d40594..648ea9757 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 c330bf42b..bf4b6c856 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/.github/workflows/ros-unit-tests.yml b/.github/workflows/ros-unit-tests.yml new file mode 100644 index 000000000..bc180da4d --- /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/ruff-strict-rollout.yml b/.github/workflows/ruff-strict-rollout.yml new file mode 100644 index 000000000..cde799d20 --- /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/.github/workflows/unit-tests.yml b/.github/workflows/unit-tests.yml new file mode 100644 index 000000000..6a6fe4f9c --- /dev/null +++ b/.github/workflows/unit-tests.yml @@ -0,0 +1,21 @@ +name: Host smoke tests + +on: + pull_request: + branches: + - main + push: + branches: + - main + +jobs: + host-smoke-tests: + name: Run host smoke tests + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 + with: + python-version: "3.12" + - name: Run host smoke tests + run: bash scripts/run-host-smoke-tests.sh diff --git a/.gitignore b/.gitignore index a5e569483..09f091ab9 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,8 @@ .DS_Store .idea .gitconfig +.venv/ +.venv-host/ /volumes @@ -22,3 +24,9 @@ __pycache__/ # Auto downloaded models code/*.pt /*.pt + +# VS Code transient browse database files +.vscode/browse.vc.db* + +tmp/* +!tmp/.gitignore diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 000000000..5011911b7 --- /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/extensions.json b/.vscode/extensions.json index 23f827fbd..d3d3751d1 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/.vscode/settings.json b/.vscode/settings.json index b1875c791..d14ce26bb 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" + ] } diff --git a/.vscode/tasks.json b/.vscode/tasks.json index be7705e5e..466874792 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", @@ -57,6 +64,118 @@ "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." + }, + { + "label": "Run host smoke tests", + "type": "shell", + "command": "PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest code/test -m unit", + "problemMatcher": [], + "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_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_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_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." + }, + { + "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." + }, + { + "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", + "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 python3 -m pytest code/test -m unit'", + "problemMatcher": [], + "detail": "Runs dependency check, lint, format, and the host smoke test subset 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/CLAUDE.md b/CLAUDE.md new file mode 100644 index 000000000..81439c1d5 --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,13 @@ +# 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 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/README.md b/README.md index d30a92c1b..1a6a4dce7 100644 --- a/README.md +++ b/README.md @@ -43,6 +43,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 000000000..635fc9d08 --- /dev/null +++ b/agents.md @@ -0,0 +1,128 @@ +# 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. +- 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 + +- 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. +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. + +## 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. +- 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 + +- 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. + +## 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/build/.env.sample b/build/.env.sample index 119832c01..6f1d40a4a 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/build/docker-compose.carla.base.yaml b/build/docker-compose.carla.base.yaml index cfe0ecc6e..2d43d7879 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 diff --git a/build/docker-compose.carla.cuda.yaml b/build/docker-compose.carla.cuda.yaml index 23a263180..bf9a6fd7d 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/build/docker/agent-ros2/Dockerfile b/build/docker/agent-ros2/Dockerfile index d0fbbad86..52eb92491 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 @@ -368,6 +373,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/ @@ -383,7 +389,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 +398,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/dependency-sync.sh b/build/docker/agent-ros2/scripts/dependency-sync.sh new file mode 100644 index 000000000..99fd5fa80 --- /dev/null +++ b/build/docker/agent-ros2/scripts/dependency-sync.sh @@ -0,0 +1,53 @@ +#!/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 + +set +u +source "${INTERNAL_WORKSPACE_DIR}/env.bash" +set -u +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 <: 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 @@ -14,16 +15,25 @@ 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 - ruff.check-format: Manually trigger the ruff formatter to check if the formatting of the python files is correct - ruff.format: Apply the ruff formatting to the python files +- dep.check: Validates rosdep + pip dependency health for the current container environment +- dep.sync: Installs rosdep and pip dependencies from the repository manifests 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 @@ -48,6 +58,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 </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/ @@ -101,3 +168,17 @@ ruff.format() { ) } export -f ruff.format + +dep.check() { + ( + "${INTERNAL_WORKSPACE_DIR}/scripts/dependency-sync.sh" check + ) +} +export -f dep.check + +dep.sync() { + ( + "${INTERNAL_WORKSPACE_DIR}/scripts/dependency-sync.sh" sync + ) +} +export -f dep.sync diff --git a/build/docker/agent-ros2/scripts/entrypoint-dev.sh b/build/docker/agent-ros2/scripts/entrypoint-dev.sh index 1d72304f6..88b7dfc32 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 "$!" diff --git a/code/acting/acting/passthrough.py b/code/acting/acting/passthrough.py index add9c2898..1d3fa468c 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/acting/package.xml b/code/acting/package.xml index c9cf7df33..c9b10d78a 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 b369238b0..9a8340892 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 stack", license="MIT", tests_require=["pytest"], entry_points={ diff --git a/code/agent/agent/data_management_node.py b/code/agent/agent/data_management_node.py index 2bef35c04..721c6aa51 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): @@ -21,17 +21,23 @@ 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 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, @@ -61,41 +67,23 @@ 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 # 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() def get_open_drive_service( self, req: GetOpenDriveString.Request, res: GetOpenDriveString.Response @@ -112,17 +100,12 @@ 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)) + def _publish_startup_ready_if_available(self) -> 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( self, req: GetCarlaRoute.Request, res: GetCarlaRoute.Response ) -> GetCarlaRoute.Response: diff --git a/code/agent/agent/startup_coordinator.py b/code/agent/agent/startup_coordinator.py new file mode 100644 index 000000000..55b2d4ff1 --- /dev/null +++ b/code/agent/agent/startup_coordinator.py @@ -0,0 +1,84 @@ +"""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 = [ + # Route-dependent nodes become ready only after the leaderboard publishes + # the global plan, which happens after /carla//status is released. + "data_management", +] + + +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 749a8863d..b10b80cdd 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/package.xml b/code/agent/package.xml index b678a1534..19387581b 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 aa390b706..01963cbe8 100644 --- a/code/agent/setup.py +++ b/code/agent/setup.py @@ -17,13 +17,14 @@ zip_safe=True, maintainer="peter", maintainer_email="peter.viechter@student.uni-augsburg.de", - description="TODO: Package description", + description="Agent startup, shutdown, and data management for the PAF stack", license="MIT", tests_require=["pytest"], scripts=["scripts/launch_agent.sh"], 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 a769bece7..ec397a308 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 bcfe64523..de1366493 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 @@ -36,73 +38,52 @@ 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.sync_frame_delta_seconds = self.declare_parameter( + "sync_frame_delta_seconds", 0.05 + ).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 @@ -125,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]): @@ -164,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_control_logic.py b/code/control/control/vehicle_control_logic.py new file mode 100644 index 000000000..a4b898b80 --- /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 f0e4a3ead..a113b048c 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,46 @@ 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.route_metrics import increment_route_metric +from paf_common.sync import ( + FrameBarrier, + frame_complete_topic, + frame_id_from_time_ns, + startup_topic, +) + +from .vehicle_control_logic import ( + VehicleControlState, + build_safe_stop_command, + build_vehicle_control_command, +) + + +DEFAULT_REQUIRED_SYNC_STAGES = [ + "mapping", + "motion_planning", + "acc", + "pure_pursuit", + "velocity_controller", +] +FRAME_BARRIER_FALLBACK_METRIC = ( + "control.vehicle_controller.frame_barrier_fallback_frames" +) 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): @@ -38,69 +60,51 @@ 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.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="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.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", - 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 @@ -109,6 +113,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( @@ -123,6 +130,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", @@ -169,41 +183,53 @@ 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.""" - - 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 - elif self.__emergency: - self.__emergency_brake(True) - else: - steer = ( - self._p_steer - if self.__curr_behavior == "us_unstuck" - else -self._p_steer + def build_control_message(self) -> CarlaEgoVehicleControl: + """Build the control message based on the current state.""" + 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) - 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 + 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 def __set_curr_behavior(self, data: String): @@ -217,7 +243,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") @@ -234,27 +260,66 @@ 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 _build_safe_stop_message(self) -> CarlaEgoVehicleControl: + return self._message_from_command(build_safe_stop_command()) 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) + increment_route_metric(FRAME_BARRIER_FALLBACK_METRIC) + 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_control_logic.py b/code/control/control/velocity_control_logic.py new file mode 100644 index 000000000..12a415ebd --- /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 b219a28eb..2a76925aa 100755 --- a/code/control/control/velocity_controller.py +++ b/code/control/control/velocity_controller.py @@ -3,14 +3,23 @@ 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 +from .velocity_control_logic import ( + VelocityControlCommand, + finalize_velocity_control, + plan_velocity_control, + select_target_velocity, +) + class VelocityController(Node): """ @@ -22,88 +31,63 @@ 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.sync_frame_delta_seconds = self.declare_parameter( + "sync_frame_delta_seconds", 0.05 + ).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, @@ -130,6 +114,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 @@ -143,6 +139,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]): @@ -177,39 +174,38 @@ 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( + 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 fbdf8ba57..79b9092fc 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/control/setup.py b/code/control/setup.py index 3d189296a..42daf99f0 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) for the PAF stack", license="MIT", tests_require=["pytest"], entry_points={ diff --git a/code/leaderboard_launcher/launch/ros_bridge.dev.xml b/code/leaderboard_launcher/launch/ros_bridge.dev.xml index 73799a1c3..1f0a3ead1 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/leaderboard_launcher/leaderboard_launcher/paf_agent_base.py b/code/leaderboard_launcher/leaderboard_launcher/paf_agent_base.py index 0180759cd..61b3daa98 100755 --- a/code/leaderboard_launcher/leaderboard_launcher/paf_agent_base.py +++ b/code/leaderboard_launcher/leaderboard_launcher/paf_agent_base.py @@ -1,6 +1,15 @@ -from leaderboard.autoagents.ros2_agent import ROS2Agent -from leaderboard.autoagents.autonomous_agent import Track 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.ros_base_agent import ROSLauncher + +import rclpy +from rclpy.qos import DurabilityPolicy, QoSProfile def get_entry_point(): @@ -8,6 +17,80 @@ def get_entry_point(): 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, + "synchronous_mode_wait_for_vehicle_control_command": 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 +171,40 @@ 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/package.xml b/code/leaderboard_launcher/package.xml index d74277586..1b75e1794 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/scripts/launch_leaderboard.sh b/code/leaderboard_launcher/scripts/launch_leaderboard.sh index 4ab1b18ed..5379a3eae 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 - < 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 _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 + 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) + + +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: @@ -32,13 +113,45 @@ class CoordinateTransformer: la_ref = STD_LAT ln_ref = STD_LON h_ref = STD_H + geo_reference = None + _geodetic_to_local = None + _use_projected_transform = False 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._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): - return self.geodetic_to_enu(lat, lon, h) + if self._use_projected_transform and self._geodetic_to_local is not None: + return self.projected_geodetic_to_xyz(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. + + 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) + # 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/localization/ekf_state_publisher.py b/code/localization/localization/ekf_state_publisher.py index d0e705cba..cc30f5912 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/gps_debug_node.py b/code/localization/localization/gps_debug_node.py index da701d3b3..366d6c9b4 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): @@ -16,11 +19,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, @@ -56,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 b9b0c1178..8fd0e7fb6 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): @@ -27,16 +32,15 @@ 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 + 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( @@ -50,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) @@ -65,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 e0beef1d6..a22aca884 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 @@ -94,16 +96,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 @@ -430,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/odometry_fusion.py b/code/localization/localization/odometry_fusion.py index 901b81c08..d398ffe9b 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/position_heading_publisher_node.py b/code/localization/localization/position_heading_publisher_node.py index ea572c984..e54d9318f 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, @@ -58,44 +60,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}" ) @@ -435,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/localization/sensor_covariance_fusion.py b/code/localization/localization/sensor_covariance_fusion.py index 4ecfbd431..53d6b1f5e 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/localization/setup.py b/code/localization/setup.py index 4002bd47f..a80ef3186 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 stack", license="MIT", tests_require=["pytest"], entry_points={ diff --git a/code/localization/test/test_coordinate_transformation.py b/code/localization/test/test_coordinate_transformation.py new file mode 100644 index 000000000..f9bc4223d --- /dev/null +++ b/code/localization/test/test_coordinate_transformation.py @@ -0,0 +1,107 @@ +import importlib + +import pytest + +pyproj = pytest.importorskip("pyproj") + +coordinate_transformation = importlib.import_module( + "localization.coordinate_transformation" +) +CoordinateTransformer = coordinate_transformation.CoordinateTransformer +extract_geo_reference_from_opendrive = ( + coordinate_transformation.extract_geo_reference_from_opendrive +) + + +pytestmark = pytest.mark.unit + + +OPEN_DRIVE = """ + +
+ +
+ +""" + + +OPEN_DRIVE_NON_DEFAULT = """ + +
+ +
+ +""" + + +@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._use_projected_transform = False + 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_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=1 +lon_0=2 +k=0.9996 +x_0=500000 +y_0=1000000 +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(2.001, 1.0015) + + transformer = CoordinateTransformer() + 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) diff --git a/code/mapping/mapping/data_integration.py b/code/mapping/mapping/data_integration.py index c89257d8b..b212f8bd9 100755 --- a/code/mapping/mapping/data_integration.py +++ b/code/mapping/mapping/data_integration.py @@ -1,15 +1,17 @@ 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 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 @@ -39,6 +41,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. @@ -82,299 +125,209 @@ 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 + self.sync_frame_delta_seconds = self.declare_parameter( + "sync_frame_delta_seconds", 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 = {} @@ -464,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]): @@ -673,6 +639,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) @@ -687,6 +668,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: @@ -698,29 +687,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: @@ -879,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/mapping/mapping/visualization.py b/code/mapping/mapping/visualization.py index c057a68dd..d7cb7d723 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/mapping/mapping_common/__init__.py b/code/mapping/mapping_common/__init__.py index d7ae08a41..5a521f38d 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/mapping_common/map.py b/code/mapping/mapping_common/map.py index 1af2fc0d7..d8105da20 100644 --- a/code/mapping/mapping_common/map.py +++ b/code/mapping/mapping_common/map.py @@ -56,6 +56,68 @@ class LaneFreeDirection(Enum): RIGHT = True +class LanePresence(Enum): + PRESENT = 1 + ABSENT = 0 + UNKNOWN = -1 + + def as_optional_bool(self) -> 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 + + 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: + 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 +562,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 +905,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_data_integration_tracking_anchor.py b/code/mapping/test/test_data_integration_tracking_anchor.py new file mode 100644 index 000000000..d78210d95 --- /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/test/test_mapping_common/test_entity.py b/code/mapping/test/test_mapping_common/test_entity.py index 7bd8d5ff4..f01448905 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_general.py b/code/mapping/test/test_mapping_common/test_general.py index 036d0024c..7b8d5ed5c 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_map.py b/code/mapping/test/test_mapping_common/test_map.py new file mode 100644 index 000000000..339d08473 --- /dev/null +++ b/code/mapping/test/test_mapping_common/test_map.py @@ -0,0 +1,122 @@ +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 + + +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/mapping/test/test_mapping_common/test_shape.py b/code/mapping/test/test_mapping_common/test_shape.py index feafcc31c..228c6d46e 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/mapping/test/test_mapping_common/test_shapely.py b/code/mapping/test/test_mapping_common/test_shapely.py index 26f347c50..bb270208f 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 e88e14b08..90961269d 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/mapping_interfaces/msg/ClusteredPointsArray.msg b/code/mapping_interfaces/msg/ClusteredPointsArray.msg index aabc62ca7..ecb2a3397 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/mapping_interfaces/package.xml b/code/mapping_interfaces/package.xml index e918e46ff..8415cc88f 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/paf_common/paf_common/__init__.py b/code/paf_common/paf_common/__init__.py index e69de29bb..ff311d9d7 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/debugging.py b/code/paf_common/paf_common/debugging.py index 2c72560ca..266db9bc2 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 e1b058d35..0ab129283 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/logging_utils.py b/code/paf_common/paf_common/logging_utils.py new file mode 100644 index 000000000..61b7342a9 --- /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/code/paf_common/paf_common/parameters.py b/code/paf_common/paf_common/parameters.py index 8f2cde7a6..17d91ad27 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/paf_common/route_metrics.py b/code/paf_common/paf_common/route_metrics.py new file mode 100644 index 000000000..b7a6de90b --- /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 + + +DEFAULT_ROUTE_METRICS_PATH = Path("/tmp/paf_route_metrics.json") +ROUTE_METRICS_ENV_VAR = "PAF_ROUTE_METRICS_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) + + 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: str | os.PathLike[str] | None = 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: 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) + 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: str | os.PathLike[str] | None = 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: str | os.PathLike[str] | None = 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/paf_common/paf_common/sync.py b/code/paf_common/paf_common/sync.py new file mode 100644 index 000000000..b9d4875d7 --- /dev/null +++ b/code/paf_common/paf_common/sync.py @@ -0,0 +1,116 @@ +"""Helpers for deterministic startup and frame synchronization.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from collections.abc import Iterable + + +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: + """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 = 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) + + +@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: 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 = 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: 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) + return [ + stage + for stage in self.required_stages + if self.completed_by_stage.get(stage, -1) < current_frame + ] + + 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/paf_common/setup.py b/code/paf_common/setup.py index 9df790ea7..fa3c5d116 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/code/perception/launch/perception.xml b/code/perception/launch/perception.xml index 979af9fc5..15c6330db 100644 --- a/code/perception/launch/perception.xml +++ b/code/perception/launch/perception.xml @@ -35,6 +35,7 @@ + diff --git a/code/perception/package.xml b/code/perception/package.xml index 389b2f396..02707de49 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/perception/Lanedetection_node.py b/code/perception/perception/Lanedetection_node.py index adf874721..154b055d7 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 54ea2ed39..875be8fee 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 1b4ab12c2..ca477e51f 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, + ground_filter_mask, + remove_field_name, +) class LidarDistance(Node): @@ -46,55 +51,38 @@ 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.enable_pitch_ground_filter = self.declare_parameter( + "enable_pitch_ground_filter", + True, + ).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, @@ -106,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 @@ -169,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", @@ -244,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): """ @@ -284,20 +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 - ) - & ( - (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) - ] + 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( @@ -320,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))) @@ -346,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) @@ -788,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): """ @@ -821,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 @@ -834,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): @@ -874,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 @@ -896,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 @@ -954,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() @@ -964,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 @@ -1040,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() @@ -1064,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 b84fe86ec..60e0b2d06 100755 --- a/code/perception/perception/lidar_filter_utility.py +++ b/code/perception/perception/lidar_filter_utility.py @@ -46,6 +46,68 @@ 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. + """ + + 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 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) + + 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 def remove_field_name(a, name): """Removes a column from a structured numpy array diff --git a/code/perception/perception/perception_utils.py b/code/perception/perception/perception_utils.py index 8568041e4..50cb3a6f5 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/perception/radar_node.py b/code/perception/perception/radar_node.py index 5bed536c8..e0d1ed46f 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/perception/traffic_light_node.py b/code/perception/perception/traffic_light_node.py index d89c9a193..7a84c511f 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/perception/perception/vision_node.py b/code/perception/perception/vision_node.py index 3535dc3ce..d3af33b54 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/perception/setup.py b/code/perception/setup.py index 59d023487..db2020f86 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 (vision, lidar, radar, traffic) for the PAF stack", license="MIT", tests_require=["pytest"], entry_points={ 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 000000000..f5971d4b1 --- /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() + ) diff --git a/code/perception/tests/test_ego_motion_compensation.py b/code/perception/tests/test_ego_motion_compensation.py index 2cacd4991..46ff6d042 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/perception/tests/test_lidar_ground_filter.py b/code/perception/tests/test_lidar_ground_filter.py new file mode 100644 index 000000000..577a37542 --- /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/code/perception/tests/test_radar_velocity_compensation.py b/code/perception/tests/test_radar_velocity_compensation.py new file mode 100644 index 000000000..0ef721e4d --- /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/perception_interfaces/package.xml b/code/perception_interfaces/package.xml index 195aa6812..9b997f7bd 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 b9a8409e1..310288088 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/planning/behavior_agent/behavior_tree.py b/code/planning/planning/behavior_agent/behavior_tree.py index 86e35d1eb..ca4da1113 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, @@ -239,19 +242,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 @@ -272,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( @@ -315,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 f12f647ff..22c784d4c 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,8 +18,10 @@ 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 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 @@ -99,6 +101,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,23 +115,53 @@ 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 +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 SELF_EMERGENCY_THRESHOLD = 10 / 3.6 # m/s ≈ 2.78 +UNNECESSARY_INTERSECTION_STOP_CANDIDATE_METRIC = ( + "planning.intersection.unnecessary_stop_candidates" +) -def check_priority_cross_traffic(map: Map, tree: MapTree): - """ - Checks whether there are fast vehicles in the larger intersection area. +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) - 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 - # Larger rectangle for approaching emergency vehicles +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) ) @@ -135,24 +170,222 @@ def check_priority_cross_traffic(map: Map, tree: MapTree): width=PRIORITY_CHECK_WIDTH, offset=offset, ) - mask = rect.to_shapely(hero.transform) + 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 _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 + return entity.transform * entity.motion.linear_motion + + +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 None + + entity_position = Point2.from_vector(entity.transform.translation()) + to_target = entity_position.vector_to(target_point) + if to_target.length() == 0.0: + return target_point + + direction = velocity.normalized() + closing_speed = _dot(velocity, to_target.normalized()) + if closing_speed <= PRIORITY_CLOSING_SPEED_THRESHOLD: + return None + + along_path = _dot(direction, to_target) + if along_path <= 0.0: + return None + + conflict_radius = ( + PRIORITY_CONFLICT_MARGIN + (entity.get_width() * 0.5) + (hero_width * 0.5) + ) + 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( + 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 _find_priority_cross_traffic_threat( + map: Map, + tree: MapTree, + reference_pose: Optional[Transform2D] = None, + current_pose: Optional[Transform2D] = None, +): + hero = map.hero() + if hero is None: + return None, None, None + hero_width = hero.get_width() + + mask, target_point = _build_priority_check_mask( + hero, + 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 - motion = entity.motion - if motion is None: - continue + 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 None, mask, target_point - v = motion.linear_motion - speed = math.hypot(v.x(), v.y()) - # Only consider fast objects - if speed > PRIORITY_SPEED_THRESHOLD: - return False, mask +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 True, mask + return threat_entity is None, mask def set_line_stop(client: Client, distance: float): @@ -245,9 +478,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): """ @@ -511,6 +744,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): """ @@ -531,11 +767,23 @@ def update(self): tree = map.build_tree(FlagFilter(is_collider=True, is_hero=False)) if self.intersection_type != CarlaRoute.LEFT: - # Priority cross traffic check - 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 + + 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( @@ -715,11 +963,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 @@ -729,7 +979,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 +989,14 @@ 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 + ) + 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): """ @@ -766,40 +1024,82 @@ 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 + + speedometer = self.blackboard.try_get("/carla/hero/Speed") + ego_speed = speedometer.speed if speedometer is not None else 0.0 + 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 = ( + 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))) 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 + ): + self.priority_stop_metric_active = False + 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", + ) + 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) - 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, - ) + self.priority_stop_metric_active = False unset_line_stop(self.stop_client) self.emergency_pub.publish(Bool(data=False)) @@ -828,4 +1128,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/planning/behavior_agent/behaviors/lane_change.py b/code/planning/planning/behavior_agent/behaviors/lane_change.py index c48197c24..8172264ea 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 cce351a93..1a1a21b2c 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 99efbb67b..e99ab77d6 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/behavior_agent/bt_parameters.py b/code/planning/planning/behavior_agent/bt_parameters.py index 48b665520..9c3f78698 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 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 824086917..d281c3bdf 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 225a960bf..ec318cfcb 100755 --- a/code/planning/planning/global_planner/global_planner_node.py +++ b/code/planning/planning/global_planner/global_planner_node.py @@ -1,10 +1,10 @@ -from collections import deque -from typing import Deque, Optional +from typing import Optional from xml.etree import ElementTree as eTree 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 @@ -19,11 +19,11 @@ 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 +from .route_alignment import find_route_alignment_index # TODO: These definition do not align with the CarlaRoute.RIGHT, etc.. definitions. # -> Check for possible bugs @@ -54,9 +54,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 @@ -65,18 +62,27 @@ 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 + 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 @@ -109,13 +115,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 @@ -233,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 @@ -248,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: @@ -262,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( @@ -276,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 @@ -387,30 +444,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 000000000..7bfd55af7 --- /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/planning/planning/global_planner/route_alignment.py b/code/planning/planning/global_planner/route_alignment.py new file mode 100644 index 000000000..d35f4a4c6 --- /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/planning/planning/local_planner/ACC.py b/code/planning/planning/local_planner/ACC.py index 1ae801093..699ab904d 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) @@ -51,184 +52,135 @@ 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.sync_frame_delta_seconds = self.declare_parameter( + "sync_frame_delta_seconds", 0.05 + ).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( @@ -274,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( @@ -296,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]): @@ -340,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() @@ -348,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()) @@ -481,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 b8006500f..870779c33 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 @@ -72,35 +74,26 @@ 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.sync_frame_delta_seconds = self.declare_parameter( + "sync_frame_delta_seconds", 0.05 + ).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 @@ -197,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 = ( @@ -222,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]): @@ -401,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/planning/setup.py b/code/planning/setup.py index 3dbff1916..18424026e 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 (planner, behavior tree, motion) for the PAF stack", license="MIT", tests_require=["pytest"], entry_points={ 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 000000000..4bb3a8e4f --- /dev/null +++ b/code/planning/test/test_intersection_priority_cross_traffic.py @@ -0,0 +1,144 @@ +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_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() + 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) + + +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) + + +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/code/planning_interfaces/package.xml b/code/planning_interfaces/package.xml index b4883a325..6ebc81e41 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/code/requirements.txt b/code/requirements.txt index 1d048288d..bfdb9467a 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 diff --git a/code/requirements_infrastructure.txt b/code/requirements_infrastructure.txt index d0d8784a1..b5629a991 100644 --- a/code/requirements_infrastructure.txt +++ b/code/requirements_infrastructure.txt @@ -1,2 +1,4 @@ # Ruff version pinned via build/pins/ruff.env -ruff +ruff==0.14.8 +pytest==9.0.2 +pytest-cov==7.0.0 diff --git a/code/test/conftest.py b/code/test/conftest.py new file mode 100644 index 000000000..c5362444b --- /dev/null +++ b/code/test/conftest.py @@ -0,0 +1,38 @@ +"""Shared pytest fixtures for repository-wide tests.""" + +from __future__ import annotations + +import random +from types import SimpleNamespace + +import pytest + + +collect_ignore = ["run_test.py"] + + +@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/code/test/run_test.py b/code/test/run_test.py index 41adc403c..511040787 100644 --- a/code/test/run_test.py +++ b/code/test/run_test.py @@ -40,6 +40,14 @@ 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) + +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 = { "sensor.camera.rgb": "carla_camera", @@ -568,14 +576,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_control_logic.py b/code/test/test_control_logic.py new file mode 100644 index 000000000..ea6de01a1 --- /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/code/test/test_deterministic_sync.py b/code/test/test_deterministic_sync.py new file mode 100644 index 000000000..db9316721 --- /dev/null +++ b/code/test/test_deterministic_sync.py @@ -0,0 +1,133 @@ +"""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() + 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() + 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 + 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 '"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 + + 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_interface_contracts.py b/code/test/test_interface_contracts.py new file mode 100644 index 000000000..a69c59ea9 --- /dev/null +++ b/code/test/test_interface_contracts.py @@ -0,0 +1,44 @@ +"""Contract tests for ROS interface package manifests.""" + +from __future__ import annotations + +import xml.etree.ElementTree as ET +from pathlib import Path + +import pytest + +pytestmark = pytest.mark.unit + +CODE_ROOT = Path(__file__).resolve().parents[1] + +INTERFACE_PACKAGE_XML = [ + CODE_ROOT / "mapping_interfaces/package.xml", + CODE_ROOT / "perception_interfaces/package.xml", + CODE_ROOT / "planning_interfaces/package.xml", +] + +REQUIRED_TEXT_TAGS = { + "buildtool_depend": {"ament_cmake", "rosidl_default_generators"}, + "exec_depend": {"rosidl_default_runtime"}, + "member_of_group": {"rosidl_interface_packages"}, +} + + +@pytest.mark.parametrize("package_xml_path", INTERFACE_PACKAGE_XML) +def test_interface_package_contract(package_xml_path: Path) -> 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 000000000..b9796d21b --- /dev/null +++ b/code/test/test_launch_manifests.py @@ -0,0 +1,61 @@ +"""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 + +CODE_ROOT = Path(__file__).resolve().parents[1] + +LAUNCH_XML_FILES = [ + 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", +] + + +@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 = CODE_ROOT / "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 = 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/code/test/test_planning_regression.py b/code/test/test_planning_regression.py new file mode 100644 index 000000000..54cfd2ee1 --- /dev/null +++ b/code/test/test_planning_regression.py @@ -0,0 +1,116 @@ +"""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 + +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)) + + +@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) + + +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] + + +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 + + +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/code/test/test_route_metrics.py b/code/test/test_route_metrics.py new file mode 100644 index 000000000..4d2c2eeae --- /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 importlib +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)) + +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: + 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 bdfc66d97..961512633 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) @@ -13,6 +14,7 @@ This document provides an overview of the structure of the documentation. - [`dev_talks`](#dev_talks) - [`localization`](#localization) - [`control`](#control) +- [`reasoning`](#reasoning) ## `general` @@ -22,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. Use the [Development Notes index](./dev/README.md) to find the latest handoff and progress snapshots. + ## `research` The [`research`](./research/) folder contains the findings of each group during the initial phase of the project. @@ -60,3 +66,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/adr/0000-template.md b/doc/adr/0000-template.md new file mode 100644 index 000000000..8a3d4bf0f --- /dev/null +++ b/doc/adr/0000-template.md @@ -0,0 +1,33 @@ +# ADR NNNN: `Decision title` + +- 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 000000000..11046f58f --- /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/control/architecture_documentation.md b/doc/control/architecture_documentation.md index cc4a420f0..9946eb31a 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 585c10f1d..c48f3de06 100644 --- a/doc/control/vehicle_controller.md +++ b/doc/control/vehicle_controller.md @@ -10,12 +10,16 @@ ## 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 +27,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 +39,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 +60,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/dev/README.md b/doc/dev/README.md new file mode 100644 index 000000000..501815c2e --- /dev/null +++ b/doc/dev/README.md @@ -0,0 +1,11 @@ +# 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-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) +- [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 000000000..68adac204 --- /dev/null +++ b/doc/dev/progress/2026-04-29-agent-handoff-rules.md @@ -0,0 +1,40 @@ +# Agent Handoff Rules Update + +Created: 2026-04-29T10:09:26+02:00 +Last updated: 2026-04-29T10:09:26+02:00 + +## 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. 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 000000000..d40e9d917 --- /dev/null +++ b/doc/dev/progress/2026-04-29-route-validation-and-map-context-handoff.md @@ -0,0 +1,127 @@ +# Route Validation And Map Context Handoff + +Created: 2026-04-29T08:50:23+02:00 +Last updated: 2026-04-29T08:50:23+02:00 + +## 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/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 000000000..33edd5b8f --- /dev/null +++ b/doc/dev/progress/2026-04-30-route-sync-bridge.md @@ -0,0 +1,91 @@ +# Route Sync Bridge Progress + +Created: 2026-04-30T22:29:21+02:00 +Last updated: 2026-04-30T22:54:59+02:00 + +## 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. + +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 + 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`. +- 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 + +- `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. +- Several open GitHub issues remain intentionally unclaimed because they need + CARLA validation, perception/planning research, or larger architecture work. + +## 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/dev/progress/2026-05-01-build-validation.md b/doc/dev/progress/2026-05-01-build-validation.md new file mode 100644 index 000000000..f369af72e --- /dev/null +++ b/doc/dev/progress/2026-05-01-build-validation.md @@ -0,0 +1,87 @@ +# Build Validation Progress + +Created: 2026-05-01T08:14:00+02:00 +Last updated: 2026-05-01T08:31:00+02:00 + +## 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` + +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 + +- 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]`. +- 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 + pre-existing BuildKit helper `buildx_buildkit_carla-builder0`, not a CARLA + simulator. + +## Remaining blockers + +- The full leaderboard route was not left running to completion. The smoke run + validated startup and command publication, but not route score or completion. 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 000000000..ad580b8fd --- /dev/null +++ b/doc/dev/progress/2026-05-01-doc-consolidation-wp1.md @@ -0,0 +1,52 @@ +# WP1: Documentation and Interface Consolidation + +Created: 2026-05-01 +Last updated: 2026-05-01 + +## 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/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 000000000..de0291051 --- /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/dev_talks/paf25/future_work.md b/doc/dev_talks/paf25/future_work.md new file mode 100644 index 000000000..f35acd954 --- /dev/null +++ b/doc/dev_talks/paf25/future_work.md @@ -0,0 +1,333 @@ +# Future Work + +## 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) + - [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 + +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. diff --git a/doc/dev_talks/paf25/improvements_assessment.md b/doc/dev_talks/paf25/improvements_assessment.md new file mode 100644 index 000000000..5dafc2802 --- /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. 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 000000000..f50c72942 --- /dev/null +++ b/doc/dev_talks/paf25/paf25_review_by_ll7.md @@ -0,0 +1,449 @@ +# 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. diff --git a/doc/development/README.md b/doc/development/README.md index f522f8fd7..4edfada30 100644 --- a/doc/development/README.md +++ b/doc/development/README.md @@ -9,6 +9,8 @@ ## 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). +For the host-auth dev container workflow see also [Dev Container Workflow](./devcontainer.md). ## Development Guidelines @@ -25,6 +27,13 @@ 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) +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/build_action.md b/doc/development/build_action.md index ec5342530..3ae869285 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/context_retention.md b/doc/development/context_retention.md new file mode 100644 index 000000000..3ac8fceec --- /dev/null +++ b/doc/development/context_retention.md @@ -0,0 +1,30 @@ +# 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. +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. + +## 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/dependency_management.md b/doc/development/dependency_management.md new file mode 100644 index 000000000..3d2d1762c --- /dev/null +++ b/doc/development/dependency_management.md @@ -0,0 +1,89 @@ +# Dependency Management (Container-first) + +This project is developed inside the `agent-dev` container. Dependency changes must be reproducible from repository files and must not depend on ad-hoc `pip install` in a single shell. + +## Source of truth + +- ROS/apt dependencies: `code/**/package.xml` resolved via `rosdep`. +- Python runtime dependencies: `code/requirements.txt` and flavor-specific files (`requirements.cpu.txt`, `requirements.cuda.txt`, `requirements.rocm.txt`). +- 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. +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. + +### 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: + - `/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/devcontainer.md b/doc/development/devcontainer.md new file mode 100644 index 000000000..78899c032 --- /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. diff --git a/doc/development/developer_contract.md b/doc/development/developer_contract.md new file mode 100644 index 000000000..545897265 --- /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/distributed_simulation.md b/doc/development/distributed_simulation.md index a285e8a5c..3ab1977ef 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/documentation_requirements.md b/doc/development/documentation_requirements.md index 9946ac5f8..33ac48638 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/drive_action.md b/doc/development/drive_action.md index b734105bc..510e8a4c7 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 694a4a64a..933a409f2 100644 --- a/doc/development/first_steps.md +++ b/doc/development/first_steps.md @@ -16,11 +16,19 @@ 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` 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/development/installing_python_packages.md b/doc/development/installing_python_packages.md index fc908d9aa..9b6f96bb4 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. +1. Open a shell in the `agent-dev` container. +1. 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: +1. 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/linting.md b/doc/development/linting.md index dc37ea56b..5d3d63f56 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,17 @@ 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 +``` + +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: @@ -37,6 +53,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 000000000..32fc9bf7e --- /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/performance_iteration.md b/doc/development/performance_iteration.md new file mode 100644 index 000000000..5d5e7572c --- /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 new file mode 100644 index 000000000..9987c1c01 --- /dev/null +++ b/doc/development/quickstart_contributor.md @@ -0,0 +1,100 @@ +# 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`. +For host-auth details for GitHub, Copilot, `gh`, and GHCR see [Dev Container Workflow](./devcontainer.md). + +## 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` + - `Run host smoke tests` + - `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 + +- 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` +- 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/doc/development/testing_strategy.md b/doc/development/testing_strategy.md new file mode 100644 index 000000000..a360f2742 --- /dev/null +++ b/doc/development/testing_strategy.md @@ -0,0 +1,71 @@ +# 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`. + +- 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` +- 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. 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: + +```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 +``` + +## 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/`: + +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 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. diff --git a/doc/general/architecture_current.md b/doc/general/architecture_current.md index d31cdcebc..1a9e60396 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/general/execution.md b/doc/general/execution.md index a62ee23d0..2920c2713 100644 --- a/doc/general/execution.md +++ b/doc/general/execution.md @@ -30,6 +30,9 @@ 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 +57,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/doc/general/tests.md b/doc/general/tests.md index 701a34e6a..6dcb203e0 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 diff --git a/doc/mapping/README.md b/doc/mapping/README.md index b0df8bbbf..7a0f82bb1 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. @@ -33,7 +34,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 @@ -46,6 +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 - 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. @@ -53,7 +57,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. @@ -64,9 +68,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 @@ -88,7 +92,7 @@ flowchart TD F3(filter: GrowPedestriansFilter) F4(filter: RadarPointAssignmentFilter) F5(filter: TrackingFilter) - VIS[node: mapping_visualization] + VIS[node: mapping/visualization] %% Flow NF --> A @@ -111,11 +115,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 @@ -125,12 +129,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 ebb1bb330..9b8de3e43 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). diff --git a/doc/perception/lidar_distance.md b/doc/perception/lidar_distance.md index c7806a002..63851265d 100644 --- a/doc/perception/lidar_distance.md +++ b/doc/perception/lidar_distance.md @@ -1,259 +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 data used for calculating the heading. - -## 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`). -- 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. diff --git a/doc/perception/radar_node.md b/doc/perception/radar_node.md index bffa16da6..742f276e6 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/Global_Planner.md b/doc/planning/Global_Planner.md index 0928e7b3f..5186b0107 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 diff --git a/doc/planning/behaviors/Intersection.md b/doc/planning/behaviors/Intersection.md index 777eba1aa..59132eab9 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) 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 000000000..a8a0b7a2e --- /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." 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 000000000..fa05b8971 --- /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/2026-04-28-intersection-prediction-strategy.md b/doc/reasoning/2026-04-28-intersection-prediction-strategy.md new file mode 100644 index 000000000..60281cd71 --- /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. 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 000000000..bebdbf38f --- /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 new file mode 100644 index 000000000..23a0a043f --- /dev/null +++ b/doc/reasoning/README.md @@ -0,0 +1,30 @@ +# 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. + +## 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) diff --git a/pytest.ini b/pytest.ini new file mode 100644 index 000000000..105ad3e88 --- /dev/null +++ b/pytest.ini @@ -0,0 +1,12 @@ +[pytest] +testpaths = code +python_files = test_*.py *_test.py +addopts = -ra --strict-config --strict-markers --ignore=code/test/run_test.py +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 000000000..68d072324 --- /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"] diff --git a/scripts/bootstrap-host-python.sh b/scripts/bootstrap-host-python.sh new file mode 100755 index 000000000..271422a47 --- /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/dependency-doctor.sh b/scripts/dependency-doctor.sh new file mode 100755 index 000000000..e7773e097 --- /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" diff --git a/scripts/dev-up.sh b/scripts/dev-up.sh new file mode 100755 index 000000000..67fc145d0 --- /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'." diff --git a/scripts/run-host-smoke-tests.sh b/scripts/run-host-smoke-tests.sh new file mode 100755 index 000000000..f405fe514 --- /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 "$@" diff --git a/scripts/update-dotenv.sh b/scripts/update-dotenv.sh index e7148163c..6f2f29203 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" diff --git a/scripts/validate-gnss-projection.sh b/scripts/validate-gnss-projection.sh new file mode 100755 index 000000000..9ea0980f4 --- /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" diff --git a/tmp/.gitignore b/tmp/.gitignore new file mode 100644 index 000000000..58b913f5d --- /dev/null +++ b/tmp/.gitignore @@ -0,0 +1,3 @@ +# Keep this folder in the repository, but ignore local scratch data. +* +!.gitignore