diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index 5e1035de20c64..ab267ec3ac007 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -1,4 +1,5 @@ *:*/test/* +*:*/examples/* checkersReport missingInclude diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 7403da173eb13..cda3c8ceaca89 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -140,7 +140,7 @@ planning/autoware_obstacle_cruise_planner/** berkay@leodrive.ai kosuke.takeuchi@ planning/autoware_obstacle_stop_planner/** berkay@leodrive.ai bnk@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/autoware_path_optimizer/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/autoware_path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp -planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp +planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp planning/autoware_planning_topic_converter/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp planning/autoware_planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp planning/autoware_remaining_distance_time_calculator/** ahmed.ebrahim@leodrive.ai diff --git a/.github/actions/build-and-test-differential/action.yaml b/.github/actions/build-and-test-differential/action.yaml deleted file mode 100644 index 89893e9f55fb5..0000000000000 --- a/.github/actions/build-and-test-differential/action.yaml +++ /dev/null @@ -1,111 +0,0 @@ -name: build-and-test-differential -description: "" - -inputs: - rosdistro: - description: "" - required: true - container: - description: "" - required: true - container-suffix: - description: "" - required: true - runner: - description: "" - required: true - build-depends-repos: - description: "" - required: true - build-pre-command: - description: "" - required: true - codecov-token: - description: "" - required: true - -runs: - using: composite - steps: - - name: Show disk space before the tasks - run: df -h - shell: bash - - - name: Show machine specs - run: lscpu && free -h - shell: bash - - - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - - - name: Get modified packages - id: get-modified-packages - uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 - - - name: Create ccache directory - run: | - mkdir -p ${CCACHE_DIR} - du -sh ${CCACHE_DIR} && ccache -s - shell: bash - - - name: Attempt to restore ccache - uses: actions/cache/restore@v4 - with: - path: | - /root/.ccache - key: ccache-main-${{ runner.arch }}-${{ inputs.rosdistro }}-${{ github.event.pull_request.base.sha }} - restore-keys: | - ccache-main-${{ runner.arch }}-${{ inputs.rosdistro }}- - - - name: Show ccache stats before build and reset stats - run: | - du -sh ${CCACHE_DIR} && ccache -s - ccache --zero-stats - shell: bash - - - name: Export CUDA state as a variable for adding to cache key - run: | - build_type_cuda_state=nocuda - if [[ "${{ inputs.container-suffix }}" == "-cuda" ]]; then - build_type_cuda_state=cuda - fi - echo "BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" >> "${GITHUB_ENV}" - echo "::notice::BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" - shell: bash - - - name: Build - if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-build@v1 - with: - rosdistro: ${{ inputs.rosdistro }} - target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - build-depends-repos: ${{ inputs.build-depends-repos }} - cache-key-element: ${{ env.BUILD_TYPE_CUDA_STATE }} - build-pre-command: ${{ inputs.build-pre-command }} - - - name: Show ccache stats after build - run: du -sh ${CCACHE_DIR} && ccache -s - shell: bash - - - name: Test - id: test - if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-test@v1 - with: - rosdistro: ${{ inputs.rosdistro }} - target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - build-depends-repos: ${{ inputs.build-depends-repos }} - - - name: Upload coverage to CodeCov - if: ${{ steps.test.outputs.coverage-report-files != '' }} - uses: codecov/codecov-action@v4 - with: - files: ${{ steps.test.outputs.coverage-report-files }} - fail_ci_if_error: false - verbose: true - flags: differential - token: ${{ inputs.codecov-token }} - - - name: Show disk space after the tasks - run: df -h - shell: bash diff --git a/.github/actions/evaluate-job-result/action.yaml b/.github/actions/evaluate-job-result/action.yaml new file mode 100644 index 0000000000000..c5c013080fd27 --- /dev/null +++ b/.github/actions/evaluate-job-result/action.yaml @@ -0,0 +1,45 @@ +name: Evaluate Job Result +description: Evaluates the result of a job and updates the summary. +inputs: + job_result: + description: Result of the job to evaluate (e.g., success, failure, skipped) + required: true + type: string + job_name: + description: Name of the job to evaluate + required: true + type: string + expected_results: + description: Comma-separated list of acceptable results (e.g., success,skipped) + required: true + type: string +outputs: + failed: + description: Indicates if the job failed + value: ${{ steps.evaluate.outputs.failed }} + +runs: + using: composite + steps: + - name: Evaluate Job Result + id: evaluate + run: | + JOB_RESULT="${{ inputs.job_result }}" + IFS=',' read -ra EXPECTED <<< "${{ inputs.expected_results }}" + FAILED=false + + for RESULT in "${EXPECTED[@]}"; do + if [[ "$JOB_RESULT" == "$RESULT" ]]; then + echo "- **${{ inputs.job_name }}:** "$JOB_RESULT" ✅" >> "$GITHUB_STEP_SUMMARY" + echo "failed=false" >> "$GITHUB_OUTPUT" + exit 0 + fi + done + + # If no expected result matched + echo "::error::${{ inputs.job_name }} failed. ❌" + echo "- **${{ inputs.job_name }}:** failed ❌" >> "$GITHUB_STEP_SUMMARY" + echo "failed=true" >> "$GITHUB_OUTPUT" + + exit 1 + shell: bash diff --git a/.github/dependabot.yaml b/.github/dependabot.yaml index 8fd9b7f4ae0a5..8e2d7193aed5d 100644 --- a/.github/dependabot.yaml +++ b/.github/dependabot.yaml @@ -6,8 +6,9 @@ version: 2 updates: - package-ecosystem: github-actions directory: / + # https://docs.github.com/en/code-security/dependabot/dependabot-version-updates/configuration-options-for-the-dependabot.yml-file#scheduleinterval schedule: - interval: daily + interval: monthly open-pull-requests-limit: 1 labels: - tag:bot diff --git a/.github/labeler.yaml b/.github/labeler.yaml index 115f75197f41a..47f8737ebbf39 100644 --- a/.github/labeler.yaml +++ b/.github/labeler.yaml @@ -39,6 +39,3 @@ - tools/**/* "component:vehicle": - vehicle/**/* -"tag:require-cuda-build-and-test": - - perception/**/* - - sensing/**/* diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index f62904b03c6e4..0925cb0f53930 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -1,57 +1,35 @@ name: build-and-test-differential on: - pull_request: - types: - - opened - - synchronize - - reopened - - labeled - -concurrency: - group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.run_id }} - cancel-in-progress: true + workflow_call: + inputs: + container: + required: true + type: string + runner: + default: ubuntu-24.04 + required: false + type: string + rosdistro: + default: humble + required: false + type: string + container-suffix: + required: false + default: "" + type: string + secrets: + codecov-token: + required: true env: CC: /usr/lib/ccache/gcc CXX: /usr/lib/ccache/g++ jobs: - make-sure-label-is-present: - uses: autowarefoundation/autoware-github-actions/.github/workflows/make-sure-label-is-present.yaml@v1 - with: - label: tag:run-build-and-test-differential - - make-sure-require-cuda-label-is-present: - uses: autowarefoundation/autoware-github-actions/.github/workflows/make-sure-label-is-present.yaml@v1 - with: - label: tag:require-cuda-build-and-test - - prepare-build-and-test-differential: - runs-on: ubuntu-latest - needs: [make-sure-label-is-present, make-sure-require-cuda-label-is-present] - outputs: - cuda_build: ${{ steps.check-if-cuda-build-is-required.outputs.cuda_build }} - steps: - - name: Check if cuda-build is required - id: check-if-cuda-build-is-required - run: | - if ${{ needs.make-sure-require-cuda-label-is-present.outputs.result == 'true' }}; then - echo "cuda-build is required" - echo "cuda_build=true" >> $GITHUB_OUTPUT - else - echo "cuda-build is not required" - echo "cuda_build=false" >> $GITHUB_OUTPUT - fi - shell: bash - - name: Fail if the tag:run-build-and-test-differential is missing - if: ${{ needs.make-sure-label-is-present.outputs.result != 'true' }} - run: exit 1 - build-and-test-differential: - runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware:universe-devel - needs: prepare-build-and-test-differential + runs-on: ${{ inputs.runner }} + container: ${{ inputs.container }}${{ inputs.container-suffix }} steps: - name: Set PR fetch depth run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" @@ -63,61 +41,13 @@ jobs: ref: ${{ github.event.pull_request.head.sha }} fetch-depth: ${{ env.PR_FETCH_DEPTH }} - - name: Run build-and-test-differential action - uses: ./.github/actions/build-and-test-differential - with: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware:universe-devel - container-suffix: "" - runner: ubuntu-latest - build-depends-repos: build_depends.repos - build-pre-command: "" - codecov-token: ${{ secrets.CODECOV_TOKEN }} - - build-and-test-differential-cuda: - runs-on: codebuild-autoware-us-east-1-${{ github.run_id }}-${{ github.run_attempt }}-ubuntu-7.0-large - container: ghcr.io/autowarefoundation/autoware:universe-devel-cuda - needs: prepare-build-and-test-differential - if: ${{ needs.prepare-build-and-test-differential.outputs.cuda_build == 'true' }} - steps: - - name: Set PR fetch depth - run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" - shell: bash - - - name: Checkout PR branch and all PR commits - uses: actions/checkout@v4 - with: - ref: ${{ github.event.pull_request.head.sha }} - fetch-depth: ${{ env.PR_FETCH_DEPTH }} - - - name: Run build-and-test-differential action - uses: ./.github/actions/build-and-test-differential - with: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware:universe-devel - container-suffix: -cuda - runner: codebuild-autoware-us-east-1-${{ github.run_id }}-${{ github.run_attempt }}-ubuntu-7.0-large - build-depends-repos: build_depends.repos - build-pre-command: taskset --cpu-list 0-5 - codecov-token: ${{ secrets.CODECOV_TOKEN }} - - clang-tidy-differential: - needs: [build-and-test-differential, prepare-build-and-test-differential] - if: ${{ needs.prepare-build-and-test-differential.outputs.cuda_build == 'false' }} - runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware:universe-devel - steps: - - name: Set PR fetch depth - run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" - - - name: Checkout PR branch and all PR commits - uses: actions/checkout@v4 - with: - ref: ${{ github.event.pull_request.head.sha }} - fetch-depth: ${{ env.PR_FETCH_DEPTH }} - - name: Show disk space before the tasks run: df -h + shell: bash + + - name: Show machine specs + run: lscpu && free -h + shell: bash - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 @@ -126,66 +56,69 @@ jobs: id: get-modified-packages uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 - - name: Get changed files (existing files only) - id: get-changed-files + - name: Create ccache directory run: | - echo "changed-files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | while read -r file; do [ -e "$file" ] && echo -n "$file "; done)" >> $GITHUB_OUTPUT + mkdir -p ${CCACHE_DIR} + du -sh ${CCACHE_DIR} && ccache -s shell: bash - - name: Run clang-tidy - if: ${{ steps.get-changed-files.outputs.changed-files != '' }} - uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 + - name: Attempt to restore ccache + uses: actions/cache/restore@v4 with: - rosdistro: humble - target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy-ci - clang-tidy-ignore-path: .clang-tidy-ignore - build-depends-repos: build_depends.repos - cache-key-element: cuda + path: | + /root/.ccache + key: ccache-main-${{ runner.arch }}-${{ inputs.rosdistro }}-${{ github.event.pull_request.base.sha }} + restore-keys: | + ccache-main-${{ runner.arch }}-${{ inputs.rosdistro }}- - - name: Show disk space after the tasks - run: df -h + - name: Show ccache stats before build and reset stats + run: | + du -sh ${CCACHE_DIR} && ccache -s + ccache --zero-stats + shell: bash - clang-tidy-differential-cuda: - needs: build-and-test-differential-cuda - runs-on: codebuild-autoware-us-east-1-${{ github.run_id }}-${{ github.run_attempt }}-ubuntu-7.0-large - container: ghcr.io/autowarefoundation/autoware:universe-devel-cuda - steps: - - name: Set PR fetch depth - run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" + - name: Export CUDA state as a variable for adding to cache key + run: | + build_type_cuda_state=nocuda + if [[ "${{ inputs.container-suffix }}" == "-cuda" ]]; then + build_type_cuda_state=cuda + fi + echo "BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" >> "${GITHUB_ENV}" + echo "::notice::BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" + shell: bash - - name: Checkout PR branch and all PR commits - uses: actions/checkout@v4 + - name: Build + if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} + uses: autowarefoundation/autoware-github-actions/colcon-build@v1 with: - ref: ${{ github.event.pull_request.head.sha }} - fetch-depth: ${{ env.PR_FETCH_DEPTH }} - - - name: Show disk space before the tasks - run: df -h - - - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - - - name: Get modified packages - id: get-modified-packages - uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 + rosdistro: ${{ inputs.rosdistro }} + target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} + build-depends-repos: build_depends.repos + cache-key-element: ${{ env.BUILD_TYPE_CUDA_STATE }} - - name: Get changed files (existing files only) - id: get-changed-files - run: | - echo "changed-files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | while read -r file; do [ -e "$file" ] && echo -n "$file "; done)" >> $GITHUB_OUTPUT + - name: Show ccache stats after build + run: du -sh ${CCACHE_DIR} && ccache -s shell: bash - - name: Run clang-tidy - if: ${{ steps.get-changed-files.outputs.changed-files != '' }} - uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 + - name: Test + id: test + if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} + uses: autowarefoundation/autoware-github-actions/colcon-test@v1 with: - rosdistro: humble + rosdistro: ${{ inputs.rosdistro }} target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy-ci - clang-tidy-ignore-path: .clang-tidy-ignore build-depends-repos: build_depends.repos - cache-key-element: cuda + + - name: Upload coverage to CodeCov + if: ${{ steps.test.outputs.coverage-report-files != '' }} + uses: codecov/codecov-action@v4 + with: + files: ${{ steps.test.outputs.coverage-report-files }} + fail_ci_if_error: false + verbose: true + flags: differential${{ inputs.container-suffix }} + token: ${{ secrets.codecov-token }} - name: Show disk space after the tasks run: df -h + shell: bash diff --git a/.github/workflows/build-test-tidy-pr.yaml b/.github/workflows/build-test-tidy-pr.yaml new file mode 100644 index 0000000000000..d1ea9b2d0f79d --- /dev/null +++ b/.github/workflows/build-test-tidy-pr.yaml @@ -0,0 +1,137 @@ +name: build-test-tidy-pr + +on: + pull_request: + types: + - opened + - synchronize + - reopened + - labeled + +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.run_id }} + cancel-in-progress: true + +jobs: + require-label: + uses: autowarefoundation/autoware-github-actions/.github/workflows/require-label.yaml@v1 + with: + label: run:build-and-test-differential + + check-if-cuda-job-is-needed: + needs: require-label + runs-on: ubuntu-latest + outputs: + cuda_job_is_needed: ${{ steps.check.outputs.any_changed }} + steps: + - uses: actions/checkout@v4 + + - name: Check if relevant files changed + id: check + uses: tj-actions/changed-files@v45 + with: + files: | + perception/** + sensing/** + + - name: Output result + run: | + echo "CUDA job needed: ${{ steps.check.outputs.any_changed }}" + shell: bash + + build-and-test-differential: + needs: + - require-label + uses: ./.github/workflows/build-and-test-differential.yaml + with: + container: ghcr.io/autowarefoundation/autoware:universe-devel + secrets: + codecov-token: ${{ secrets.CODECOV_TOKEN }} + + build-and-test-differential-cuda: + needs: check-if-cuda-job-is-needed + if: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'true' }} + uses: ./.github/workflows/build-and-test-differential.yaml + with: + container: ghcr.io/autowarefoundation/autoware:universe-devel + container-suffix: -cuda + secrets: + codecov-token: ${{ secrets.CODECOV_TOKEN }} + + build-test-pr: + needs: + - build-and-test-differential + - build-and-test-differential-cuda + if: ${{ always() }} + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + + - name: Initialize Summary + run: echo "### Build Test PR Results" > $GITHUB_STEP_SUMMARY + shell: bash + + - name: Evaluate build-and-test-differential + uses: ./.github/actions/evaluate-job-result + with: + job_result: ${{ needs.build-and-test-differential.result }} + job_name: build-and-test-differential + expected_results: success + + - name: Evaluate build-and-test-differential-cuda + if: ${{ always() }} + uses: ./.github/actions/evaluate-job-result + with: + job_result: ${{ needs.build-and-test-differential-cuda.result }} + job_name: build-and-test-differential-cuda + expected_results: success,skipped + + clang-tidy-differential: + needs: + - check-if-cuda-job-is-needed + - build-and-test-differential + if: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'false' }} + uses: ./.github/workflows/clang-tidy-differential.yaml + with: + container: ghcr.io/autowarefoundation/autoware:universe-devel + + clang-tidy-differential-cuda: + needs: + - build-and-test-differential-cuda + uses: ./.github/workflows/clang-tidy-differential.yaml + with: + container: ghcr.io/autowarefoundation/autoware:universe-devel + container-suffix: -cuda + + clang-tidy-pr: + needs: + - clang-tidy-differential + - clang-tidy-differential-cuda + if: ${{ always() }} + runs-on: ubuntu-latest + steps: + - name: Initialize Summary + run: echo "### Clang Tidy PR Results" > $GITHUB_STEP_SUMMARY + shell: bash + + - name: Check clang-tidy success + if: ${{ needs.clang-tidy-differential.result == 'success' || needs.clang-tidy-differential-cuda.result == 'success' }} + run: | + echo "✅ Either one of the following has succeeded:" >> $GITHUB_STEP_SUMMARY + + - name: Fail if conditions not met + if: ${{ !(needs.clang-tidy-differential.result == 'success' || needs.clang-tidy-differential-cuda.result == 'success') }} + run: | + echo "::error::❌ Either one of the following should have succeeded:" + echo "::error::clang-tidy-differential: ${{ needs.clang-tidy-differential.result }}" + echo "::error::clang-tidy-differential-cuda: ${{ needs.clang-tidy-differential-cuda.result }}" + + echo "❌ Either one of the following should have succeeded:" >> $GITHUB_STEP_SUMMARY + + exit 1 + + - name: Print the results + if: ${{ always() }} + run: | + echo "- **clang-tidy-differential:** ${{ needs.clang-tidy-differential.result }}" >> $GITHUB_STEP_SUMMARY + echo "- **clang-tidy-differential-cuda:** ${{ needs.clang-tidy-differential-cuda.result }}" >> $GITHUB_STEP_SUMMARY diff --git a/.github/workflows/clang-tidy-differential.yaml b/.github/workflows/clang-tidy-differential.yaml new file mode 100644 index 0000000000000..51e0a8408468c --- /dev/null +++ b/.github/workflows/clang-tidy-differential.yaml @@ -0,0 +1,75 @@ +name: clang-tidy-differential + +on: + workflow_call: + inputs: + container: + required: true + type: string + container-suffix: + required: false + default: "" + type: string + runner: + default: ubuntu-24.04 + required: false + type: string + +jobs: + clang-tidy-differential: + runs-on: ${{ inputs.runner }} + container: ${{ inputs.container }}${{ inputs.container-suffix }} + steps: + - name: Set PR fetch depth + run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" + + - name: Checkout PR branch and all PR commits + uses: actions/checkout@v4 + with: + ref: ${{ github.event.pull_request.head.sha }} + fetch-depth: ${{ env.PR_FETCH_DEPTH }} + + - name: Show machine specs + run: lscpu && free -h + shell: bash + + - name: Show disk space before the tasks + run: df -h + shell: bash + + - name: Remove exec_depend + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 + + - name: Get modified packages + id: get-modified-packages + uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 + + - name: Get changed files (existing files only) + id: get-changed-files + run: | + echo "changed-files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | while read -r file; do [ -e "$file" ] && echo -n "$file "; done)" >> $GITHUB_OUTPUT + shell: bash + + - name: Export CUDA state as a variable for adding to cache key + run: | + build_type_cuda_state=nocuda + if [[ "${{ inputs.container-suffix }}" == "-cuda" ]]; then + build_type_cuda_state=cuda + fi + echo "BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" >> "${GITHUB_ENV}" + echo "::notice::BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" + shell: bash + + - name: Run clang-tidy + if: ${{ steps.get-changed-files.outputs.changed-files != '' }} + uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 + with: + rosdistro: humble + target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} + clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy-ci + clang-tidy-ignore-path: .clang-tidy-ignore + build-depends-repos: build_depends.repos + cache-key-element: ${{ env.BUILD_TYPE_CUDA_STATE }} + + - name: Show disk space after the tasks + run: df -h diff --git a/.github/workflows/deploy-docs.yaml b/.github/workflows/deploy-docs.yaml index d39e97e540794..47009a25e69fd 100644 --- a/.github/workflows/deploy-docs.yaml +++ b/.github/workflows/deploy-docs.yaml @@ -26,7 +26,7 @@ jobs: prevent-no-label-execution: uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 with: - label: tag:deploy-docs + label: run:deploy-docs deploy-docs: needs: prevent-no-label-execution diff --git a/.github/workflows/pre-commit-autoupdate.yaml b/.github/workflows/pre-commit-autoupdate.yaml index 489e32a1de166..60c17d9dabf72 100644 --- a/.github/workflows/pre-commit-autoupdate.yaml +++ b/.github/workflows/pre-commit-autoupdate.yaml @@ -6,7 +6,7 @@ name: pre-commit-autoupdate on: schedule: - - cron: 0 0 * * * + - cron: 0 0 1 1,4,7,10 * # quarterly workflow_dispatch: jobs: diff --git a/.github/workflows/pre-commit-optional-autoupdate.yaml b/.github/workflows/pre-commit-optional-autoupdate.yaml index be79ad481d16e..6639e5ca6c61a 100644 --- a/.github/workflows/pre-commit-optional-autoupdate.yaml +++ b/.github/workflows/pre-commit-optional-autoupdate.yaml @@ -6,7 +6,7 @@ name: pre-commit-optional-autoupdate on: schedule: - - cron: 0 0 * * * + - cron: 0 0 1 1,4,7,10 * # quarterly workflow_dispatch: jobs: diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 6e7c64fd982fc..48a97c13ef958 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,8 +2,12 @@ # https://github.com/autowarefoundation/sync-file-templates # To make changes, update the source repository and follow the guidelines in its README. +# https://pre-commit.ci/#configuration ci: autofix_commit_msg: "style(pre-commit): autofix" + # we already have our own daily update mechanism, we set this to quarterly + autoupdate_schedule: quarterly + autoupdate_commit_msg: "ci(pre-commit): quarterly autoupdate" repos: - repo: https://github.com/pre-commit/pre-commit-hooks @@ -70,7 +74,7 @@ repos: args: [--line-length=100] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.4 + rev: v19.1.5 hooks: - id: clang-format types_or: [c++, c, cuda] diff --git a/build_depends.repos b/build_depends.repos index 4db947b7c26a8..2313a9be487a6 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -32,7 +32,7 @@ repositories: core/autoware_internal_msgs: type: git url: https://github.com/autowarefoundation/autoware_internal_msgs.git - version: 1.1.0 + version: 1.3.0 # universe universe/external/tier4_autoware_msgs: type: git diff --git a/common/autoware_adapi_specs/include/autoware/adapi_specs/vehicle.hpp b/common/autoware_adapi_specs/include/autoware/adapi_specs/vehicle.hpp index a7568d54b5e1a..dc907b6610cde 100644 --- a/common/autoware_adapi_specs/include/autoware/adapi_specs/vehicle.hpp +++ b/common/autoware_adapi_specs/include/autoware/adapi_specs/vehicle.hpp @@ -41,7 +41,7 @@ struct VehicleStatus using Message = autoware_adapi_v1_msgs::msg::VehicleStatus; static constexpr char name[] = "/api/vehicle/status"; static constexpr size_t depth = 1; - static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; }; diff --git a/common/autoware_interpolation/test/src/test_interpolation_utils.cpp b/common/autoware_interpolation/test/src/test_interpolation_utils.cpp index 2aa41b6fdef00..9131b458dd68b 100644 --- a/common/autoware_interpolation/test/src/test_interpolation_utils.cpp +++ b/common/autoware_interpolation/test/src/test_interpolation_utils.cpp @@ -18,8 +18,6 @@ #include -constexpr double epsilon = 1e-6; - TEST(interpolation_utils, isIncreasing) { // empty diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/factor/planning_factor_interface.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/factor/planning_factor_interface.hpp new file mode 100644 index 0000000000000..7b720c69c6701 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/factor/planning_factor_interface.hpp @@ -0,0 +1,240 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__MOTION_UTILS__FACTOR__PLANNING_FACTOR_INTERFACE_HPP_ +#define AUTOWARE__MOTION_UTILS__FACTOR__PLANNING_FACTOR_INTERFACE_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace autoware::motion_utils +{ + +using geometry_msgs::msg::Pose; +using tier4_planning_msgs::msg::ControlPoint; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::PlanningFactorArray; +using tier4_planning_msgs::msg::SafetyFactorArray; + +class PlanningFactorInterface +{ +public: + PlanningFactorInterface(rclcpp::Node * node, const std::string & name) + : name_{name}, + pub_factors_{ + node->create_publisher("/planning/planning_factors/" + name, 1)}, + clock_{node->get_clock()} + { + } + + /** + * @brief factor setter for single control point. + * + * @param path points. + * @param ego current pose. + * @param control point pose. (e.g. stop or slow down point pose) + * @param behavior of this planning factor. + * @param safety factor. + * @param driving direction. + * @param target velocity of the control point. + * @param shift length of the control point. + * @param detail information. + */ + template + void add( + const std::vector & points, const Pose & ego_pose, const Pose & control_point_pose, + const uint16_t behavior, const SafetyFactorArray & safety_factors, + const bool is_driving_forward = true, const double velocity = 0.0, + const double shift_length = 0.0, const std::string & detail = "") + { + const auto distance = static_cast(autoware::motion_utils::calcSignedArcLength( + points, ego_pose.position, control_point_pose.position)); + add( + distance, control_point_pose, behavior, safety_factors, is_driving_forward, velocity, + shift_length, detail); + } + + /** + * @brief factor setter for two control points (section). + * + * @param path points. + * @param ego current pose. + * @param control section start pose. (e.g. lane change start point pose) + * @param control section end pose. (e.g. lane change end point pose) + * @param behavior of this planning factor. + * @param safety factor. + * @param driving direction. + * @param target velocity of the control point. + * @param shift length of the control point. + * @param detail information. + */ + template + void add( + const std::vector & points, const Pose & ego_pose, const Pose & start_pose, + const Pose & end_pose, const uint16_t behavior, const SafetyFactorArray & safety_factors, + const bool is_driving_forward = true, const double velocity = 0.0, + const double shift_length = 0.0, const std::string & detail = "") + { + const auto start_distance = static_cast( + autoware::motion_utils::calcSignedArcLength(points, ego_pose.position, start_pose.position)); + const auto end_distance = static_cast( + autoware::motion_utils::calcSignedArcLength(points, ego_pose.position, end_pose.position)); + add( + start_distance, end_distance, start_pose, end_pose, behavior, safety_factors, + is_driving_forward, velocity, shift_length, detail); + } + + /** + * @brief factor setter for single control point. + * + * @param distance to control point. + * @param control point pose. (e.g. stop point pose) + * @param behavior of this planning factor. + * @param safety factor. + * @param driving direction. + * @param target velocity of the control point. + * @param shift length of the control point. + * @param detail information. + */ + void add( + const double distance, const Pose & control_point_pose, const uint16_t behavior, + const SafetyFactorArray & safety_factors, const bool is_driving_forward = true, + const double velocity = 0.0, const double shift_length = 0.0, const std::string & detail = "") + { + const auto control_point = tier4_planning_msgs::build() + .pose(control_point_pose) + .velocity(velocity) + .shift_length(shift_length) + .distance(distance); + + const auto factor = tier4_planning_msgs::build() + .module(name_) + .is_driving_forward(is_driving_forward) + .control_points({control_point}) + .behavior(behavior) + .detail(detail) + .safety_factors(safety_factors); + + factors_.push_back(factor); + } + + /** + * @brief factor setter for two control points (section). + * + * @param distance to control section start point. + * @param distance to control section end point. + * @param control section start pose. (e.g. lane change start point pose) + * @param control section end pose. (e.g. lane change end point pose) + * @param behavior of this planning factor. + * @param safety factor. + * @param driving direction. + * @param target velocity of the control point. + * @param shift length of the control point. + * @param detail information. + */ + void add( + const double start_distance, const double end_distance, const Pose & start_pose, + const Pose & end_pose, const uint16_t behavior, const SafetyFactorArray & safety_factors, + const bool is_driving_forward = true, const double velocity = 0.0, + const double shift_length = 0.0, const std::string & detail = "") + { + const auto control_start_point = tier4_planning_msgs::build() + .pose(start_pose) + .velocity(velocity) + .shift_length(shift_length) + .distance(start_distance); + + const auto control_end_point = tier4_planning_msgs::build() + .pose(end_pose) + .velocity(velocity) + .shift_length(shift_length) + .distance(end_distance); + + const auto factor = tier4_planning_msgs::build() + .module(name_) + .is_driving_forward(is_driving_forward) + .control_points({control_start_point, control_end_point}) + .behavior(behavior) + .detail(detail) + .safety_factors(safety_factors); + + factors_.push_back(factor); + } + + /** + * @brief publish planning factors. + */ + void publish() + { + PlanningFactorArray msg; + msg.header.frame_id = "map"; + msg.header.stamp = clock_->now(); + msg.factors = factors_; + + pub_factors_->publish(msg); + + factors_.clear(); + } + +private: + std::string name_; + + rclcpp::Publisher::SharedPtr pub_factors_; + + rclcpp::Clock::SharedPtr clock_; + + std::vector factors_; +}; + +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); + +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); + +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__FACTOR__PLANNING_FACTOR_INTERFACE_HPP_ diff --git a/common/autoware_motion_utils/src/factor/planing_factor_interface.cpp b/common/autoware_motion_utils/src/factor/planing_factor_interface.cpp new file mode 100644 index 0000000000000..d09355b4003ae --- /dev/null +++ b/common/autoware_motion_utils/src/factor/planing_factor_interface.cpp @@ -0,0 +1,47 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +namespace autoware::motion_utils +{ +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); + +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp b/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp index dc828e885af64..d00053a7b4ff6 100644 --- a/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp @@ -26,8 +26,6 @@ using autoware::universe_utils::createPoint; using autoware::universe_utils::createQuaternionFromRPY; using autoware_planning_msgs::msg::Trajectory; -constexpr double epsilon = 1e-6; - geometry_msgs::msg::Pose createPose( double x, double y, double z, double roll, double pitch, double yaw) { diff --git a/common/autoware_pyplot/CMakeLists.txt b/common/autoware_pyplot/CMakeLists.txt index 6922d5d9306f7..5871929676a2d 100644 --- a/common/autoware_pyplot/CMakeLists.txt +++ b/common/autoware_pyplot/CMakeLists.txt @@ -15,8 +15,12 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} STATIC DIRECTORY src ) +target_compile_options(${PROJECT_NAME} PUBLIC "-fPIC") target_link_libraries(${PROJECT_NAME} ${Python3_LIBRARIES} pybind11::embed) +# NOTE(soblin): this is workaround for propagating the include of "Python.h" to user modules to avoid "'Python.h' not found" +ament_export_include_directories(${Python3_INCLUDE_DIRS}) + if(BUILD_TESTING) find_package(ament_cmake_ros REQUIRED) diff --git a/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp b/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp new file mode 100644 index 0000000000000..c2185e65422e8 --- /dev/null +++ b/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp @@ -0,0 +1,229 @@ +// Copyright 2024 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// NOTE(soblin): this file is intentionally inline to deal with link issue + +#ifndef AUTOWARE_TEST_UTILS__VISUALIZATION_HPP_ +#define AUTOWARE_TEST_UTILS__VISUALIZATION_HPP_ + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::test_utils +{ + +struct PointConfig +{ + std::optional color{}; + std::optional marker{}; + std::optional marker_size{}; +}; + +struct LineConfig +{ + static constexpr const char * default_color = "blue"; + static LineConfig defaults() + { + return LineConfig{std::string(default_color), 1.0, "solid", std::nullopt}; + } + std::optional color{}; + std::optional linewidth{}; + std::optional linestyle{}; + std::optional label{}; +}; + +struct LaneConfig +{ + static LaneConfig defaults() { return LaneConfig{std::nullopt, LineConfig::defaults(), true}; } + + std::optional label{}; + std::optional line_config{}; + bool plot_centerline = true; // alpha{}; + std::optional color{}; + bool fill{true}; + std::optional linewidth{}; + std::optional point_config{}; +}; + +/** + * @brief plot the linestring by `axes.plot()` + * @param [in] config_opt argument for plotting the linestring. if valid, each field is used as the + * kwargs + */ +inline void plot_lanelet2_object( + const lanelet::ConstLineString3d & line, autoware::pyplot::Axes & axes, + const std::optional & config_opt = std::nullopt) +{ + py::dict kwargs{}; + if (config_opt) { + const auto & config = config_opt.value(); + if (config.color) { + kwargs["color"] = config.color.value(); + } + if (config.linewidth) { + kwargs["linewidth"] = config.linewidth.value(); + } + if (config.linestyle) { + kwargs["linestyle"] = config.linestyle.value(); + } + if (config.label) { + kwargs["label"] = config.label.value(); + } + } + std::vector xs; + for (const auto & p : line) { + xs.emplace_back(p.x()); + } + std::vector ys; + for (const auto & p : line) { + ys.emplace_back(p.y()); + } + axes.plot(Args(xs, ys), kwargs); +} + +/** + * @brief plot the left/right bounds and optionally centerline + * @param [in] args used for plotting the left/right bounds as + */ +inline void plot_lanelet2_object( + const lanelet::ConstLanelet & lanelet, autoware::pyplot::Axes & axes, + const std::optional & config_opt = std::nullopt) +{ + const auto left = lanelet.leftBound(); + const auto right = lanelet.rightBound(); + + const auto line_config = [&]() -> std::optional { + if (!config_opt) { + return LineConfig{std::string(LineConfig::default_color)}; + } + return config_opt.value().line_config; + }(); + + if (config_opt) { + const auto & config = config_opt.value(); + + // plot lanelet centerline + if (config.plot_centerline) { + auto centerline_config = [&]() -> LineConfig { + if (!config.line_config) { + return LineConfig{"k", std::nullopt, "dashed"}; + } + auto cfg = config.line_config.value(); + cfg.color = "k"; + cfg.linestyle = "dashed"; + return cfg; + }(); + plot_lanelet2_object( + lanelet.centerline(), axes, std::make_optional(std::move(centerline_config))); + } + + // plot lanelet-id + const auto center = (left.front().basicPoint2d() + left.back().basicPoint2d() + + right.front().basicPoint2d() + right.back().basicPoint2d()) / + 4.0; + axes.text(Args(center.x(), center.y(), std::to_string(lanelet.id()))); + } + + if (config_opt && config_opt.value().label) { + auto left_line_config_for_legend = line_config ? line_config.value() : LineConfig::defaults(); + left_line_config_for_legend.label = config_opt.value().label.value(); + + // plot left + plot_lanelet2_object(lanelet.leftBound(), axes, left_line_config_for_legend); + + // plot right + plot_lanelet2_object(lanelet.rightBound(), axes, line_config); + } else { + // plot left + plot_lanelet2_object(lanelet.leftBound(), axes, line_config); + + // plot right + plot_lanelet2_object(lanelet.rightBound(), axes, line_config); + } + + // plot centerline direction + const auto centerline_size = lanelet.centerline().size(); + const auto mid_index = centerline_size / 2; + const auto before = static_cast(std::max(0, mid_index - 1)); + const auto after = static_cast(std::min(centerline_size - 1, mid_index + 1)); + const auto p_before = lanelet.centerline()[before]; + const auto p_after = lanelet.centerline()[after]; + const double azimuth = std::atan2(p_after.y() - p_before.y(), p_after.x() - p_before.x()); + const auto & mid = lanelet.centerline()[mid_index]; + axes.quiver( + Args(mid.x(), mid.y(), std::cos(azimuth), std::sin(azimuth)), Kwargs("units"_a = "width")); +} + +/** + * @brief plot the polygon as matplotlib.patches.Polygon + * @param [in] config_opt argument for plotting the polygon. if valid, each field is used as the + * kwargs + */ +inline void plot_lanelet2_object( + const lanelet::ConstPolygon3d & polygon, autoware::pyplot::Axes & axes, + const std::optional & config_opt = std::nullopt) +{ + std::vector> xy(polygon.size()); + for (unsigned i = 0; i < polygon.size(); ++i) { + xy.at(i) = std::vector({polygon[i].x(), polygon[i].y()}); + } + py::dict kwargs; + if (config_opt) { + const auto & config = config_opt.value(); + if (config.alpha) { + kwargs["alpha"] = config.alpha.value(); + } + if (config.color) { + kwargs["color"] = config.color.value(); + } + kwargs["fill"] = config.fill; + if (config.linewidth) { + kwargs["linewidth"] = config.linewidth.value(); + } + } + auto poly = autoware::pyplot::Polygon(Args(xy), kwargs); + axes.add_patch(Args(poly.unwrap())); +} + +/** + * @brief plot the point by `axes.plot()` + * @param [in] config_opt argument for plotting the point. if valid, each field is used as the + * kwargs + */ +/* +void plot_lanelet2_point( +const lanelet::ConstPoint3d & point, autoware::pyplot::Axes & axes, +const std::optional & config_opt = std::nullopt); +*/ +} // namespace autoware::test_utils + +#endif // AUTOWARE_TEST_UTILS__VISUALIZATION_HPP_ diff --git a/common/autoware_test_utils/package.xml b/common/autoware_test_utils/package.xml index 22572abb72b69..c328f37ba357d 100644 --- a/common/autoware_test_utils/package.xml +++ b/common/autoware_test_utils/package.xml @@ -23,6 +23,7 @@ autoware_map_msgs autoware_perception_msgs autoware_planning_msgs + autoware_pyplot autoware_universe_utils autoware_vehicle_msgs lanelet2_io diff --git a/common/autoware_test_utils/test_map/road_shoulder/lanelet2_map.osm b/common/autoware_test_utils/test_map/road_shoulder/lanelet2_map.osm index 09c6297f83752..2c86dad61875f 100644 --- a/common/autoware_test_utils/test_map/road_shoulder/lanelet2_map.osm +++ b/common/autoware_test_utils/test_map/road_shoulder/lanelet2_map.osm @@ -1,6 +1,6 @@ - + @@ -2336,31 +2336,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - @@ -2771,316 +2746,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -3831,41 +3496,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -47975,9 +47605,9 @@ - - - + + + @@ -48018,8 +47648,8 @@ - - + + @@ -48053,9 +47683,9 @@ - - - + + + @@ -48108,19 +47738,19 @@ - - - + + + - - - + + + - - - + + + @@ -48128,15 +47758,15 @@ - + - - + + - - - + + + @@ -48180,9 +47810,9 @@ - - - + + + @@ -48243,14 +47873,14 @@ - - - + + + - - - + + + @@ -48258,9 +47888,9 @@ - - - + + + @@ -48333,9 +47963,9 @@ - - - + + + @@ -48343,6 +47973,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -49132,99 +48827,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -49658,17 +49260,6 @@ - - - - - - - - - - - @@ -64187,15 +63778,6 @@ - - - - - - - - - @@ -64239,19 +63821,6 @@ - - - - - - - - - - - - - @@ -64330,6 +63899,22 @@ + + + + + + + + + + + + + + + + @@ -64478,7 +64063,7 @@ - + @@ -67744,15 +67329,14 @@ - - - - - - + + + + + diff --git a/common/autoware_universe_utils/CMakeLists.txt b/common/autoware_universe_utils/CMakeLists.txt index f295662e69091..72486b5de8818 100644 --- a/common/autoware_universe_utils/CMakeLists.txt +++ b/common/autoware_universe_utils/CMakeLists.txt @@ -21,6 +21,7 @@ ament_auto_add_library(autoware_universe_utils SHARED src/geometry/sat_2d.cpp src/math/sin_table.cpp src/math/trigonometry.cpp + src/ros/diagnostics_interface.cpp src/ros/msg_operation.cpp src/ros/marker_helper.cpp src/ros/logger_level_configure.cpp diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_traits.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_traits.hpp index 8420f930e0ce9..5878641676d9f 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_traits.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_traits.hpp @@ -15,6 +15,16 @@ #ifndef AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_TRAITS_HPP_ #define AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_TRAITS_HPP_ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -84,6 +94,58 @@ template <> struct is_debug_message : std::true_type { }; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message +: std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message +: std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; } // namespace autoware::universe_utils::debug_traits #endif // AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_TRAITS_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp similarity index 70% rename from localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp index 8c19c94127630..db370eb46ef43 100644 --- a/localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__DIAGNOSTICS_INTERFACE_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__DIAGNOSTICS_INTERFACE_HPP_ #include @@ -22,16 +22,18 @@ #include #include -namespace autoware::localization_util +namespace autoware::universe_utils { -class DiagnosticsModule +class DiagnosticsInterface { public: - DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name); + DiagnosticsInterface(rclcpp::Node * node, const std::string & diagnostic_name); void clear(); void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg); template void add_key_value(const std::string & key, const T & value); + void add_key_value(const std::string & key, const std::string & value); + void add_key_value(const std::string & key, bool value); void update_level_and_message(const int8_t level, const std::string & message); void publish(const rclcpp::Time & publish_time_stamp); @@ -46,7 +48,7 @@ class DiagnosticsModule }; template -void DiagnosticsModule::add_key_value(const std::string & key, const T & value) +void DiagnosticsInterface::add_key_value(const std::string & key, const T & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; @@ -54,11 +56,6 @@ void DiagnosticsModule::add_key_value(const std::string & key, const T & value) add_key_value(key_value); } -template <> -void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value); -template <> -void DiagnosticsModule::add_key_value(const std::string & key, const bool & value); +} // namespace autoware::universe_utils -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__DIAGNOSTICS_INTERFACE_HPP_ diff --git a/common/autoware_universe_utils/package.xml b/common/autoware_universe_utils/package.xml index d04e1a57e78ab..0e7b892a8c689 100644 --- a/common/autoware_universe_utils/package.xml +++ b/common/autoware_universe_utils/package.xml @@ -12,6 +12,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_internal_msgs autoware_perception_msgs autoware_planning_msgs diff --git a/localization/autoware_localization_util/src/diagnostics_module.cpp b/common/autoware_universe_utils/src/ros/diagnostics_interface.cpp similarity index 76% rename from localization/autoware_localization_util/src/diagnostics_module.cpp rename to common/autoware_universe_utils/src/ros/diagnostics_interface.cpp index 2b68dbf4f5890..e4d1ec8494113 100644 --- a/localization/autoware_localization_util/src/diagnostics_module.cpp +++ b/common/autoware_universe_utils/src/ros/diagnostics_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/localization_util/diagnostics_module.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include @@ -21,9 +21,9 @@ #include #include -namespace autoware::localization_util +namespace autoware::universe_utils { -DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name) +DiagnosticsInterface::DiagnosticsInterface(rclcpp::Node * node, const std::string & diagnostic_name) : clock_(node->get_clock()) { diagnostics_pub_ = @@ -34,7 +34,7 @@ DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & di diagnostics_status_msg_.hardware_id = node->get_name(); } -void DiagnosticsModule::clear() +void DiagnosticsInterface::clear() { diagnostics_status_msg_.values.clear(); diagnostics_status_msg_.values.shrink_to_fit(); @@ -43,7 +43,7 @@ void DiagnosticsModule::clear() diagnostics_status_msg_.message = ""; } -void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) +void DiagnosticsInterface::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) { auto it = std::find_if( std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values), @@ -56,8 +56,7 @@ void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key } } -template <> -void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value) +void DiagnosticsInterface::add_key_value(const std::string & key, const std::string & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; @@ -65,8 +64,7 @@ void DiagnosticsModule::add_key_value(const std::string & key, const std::string add_key_value(key_value); } -template <> -void DiagnosticsModule::add_key_value(const std::string & key, const bool & value) +void DiagnosticsInterface::add_key_value(const std::string & key, bool value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; @@ -74,7 +72,7 @@ void DiagnosticsModule::add_key_value(const std::string & key, const bool & valu add_key_value(key_value); } -void DiagnosticsModule::update_level_and_message(const int8_t level, const std::string & message) +void DiagnosticsInterface::update_level_and_message(const int8_t level, const std::string & message) { if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { if (!diagnostics_status_msg_.message.empty()) { @@ -87,12 +85,12 @@ void DiagnosticsModule::update_level_and_message(const int8_t level, const std:: } } -void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp) +void DiagnosticsInterface::publish(const rclcpp::Time & publish_time_stamp) { diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp)); } -diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_array( +diagnostic_msgs::msg::DiagnosticArray DiagnosticsInterface::create_diagnostics_array( const rclcpp::Time & publish_time_stamp) const { diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; @@ -105,4 +103,4 @@ diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_arra return diagnostics_msg; } -} // namespace autoware::localization_util +} // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp index df05b698693d6..5c8f96ddee67f 100644 --- a/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp +++ b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp @@ -18,6 +18,7 @@ #include #include +#include TEST(trigonometry, sin) { @@ -60,11 +61,15 @@ float normalize_angle(double angle) TEST(trigonometry, opencv_fast_atan2) { + std::random_device rd; + std::mt19937 gen(rd()); + + // Generate random x and y between -10.0 and 10.0 as float + std::uniform_real_distribution dis(-10.0f, 10.0f); + for (int i = 0; i < 100; ++i) { - // Generate random x and y between -10 and 10 - std::srand(0); - float x = static_cast(std::rand()) / RAND_MAX * 20.0 - 10.0; - float y = static_cast(std::rand()) / RAND_MAX * 20.0 - 10.0; + const float x = dis(gen); + const float y = dis(gen); float fast_atan = autoware::universe_utils::opencv_fast_atan2(y, x); float std_atan = normalize_angle(std::atan2(y, x)); diff --git a/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp b/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp new file mode 100644 index 0000000000000..6ec4fccf78b43 --- /dev/null +++ b/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp @@ -0,0 +1,179 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" + +#include + +#include +#include +#include + +#include + +#include +#include + +using autoware::universe_utils::DiagnosticsInterface; + +class TestDiagnosticsInterface : public ::testing::Test +{ +protected: + void SetUp() override + { + // Create a test node + node_ = std::make_shared("test_diagnostics_interface"); + } + + // Automatically destroyed at the end of each test + std::shared_ptr node_; +}; + +/* + * Test clear(): + * Verify that calling clear() resets DiagnosticStatus values, level, and message. + */ +TEST_F(TestDiagnosticsInterface, ClearTest) +{ + DiagnosticsInterface module(node_.get(), "test_diagnostic"); + + // Add some key-value pairs and update level/message + module.add_key_value("param1", 42); + module.update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Something is not OK"); + + // Call clear() + module.clear(); + + // After calling clear(), publish and check the contents of the message + diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr received_msg; + auto sub = node_->create_subscription( + "/diagnostics", 10, + [&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) { received_msg = msg; }); + + // Publish the message + module.publish(node_->now()); + + // Spin to allow the subscriber to receive messages + rclcpp::spin_some(node_); + + ASSERT_NE(nullptr, received_msg); + ASSERT_FALSE(received_msg->status.empty()); + const auto & status = received_msg->status.front(); + + // After clear(), key-value pairs should be empty + EXPECT_TRUE(status.values.empty()); + + // After clear(), level should be OK (=0) + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + // After clear(), message is empty internally, + // but "OK" is set during publishing when level == OK + EXPECT_EQ(status.message, "OK"); +} + +/* + * Test add_key_value(): + * Verify that adding the same key updates its value instead of adding a duplicate. + */ +TEST_F(TestDiagnosticsInterface, AddKeyValueTest) +{ + DiagnosticsInterface module(node_.get(), "test_diagnostic"); + + // Call the template version of add_key_value() with different types + module.add_key_value("string_key", std::string("initial_value")); + module.add_key_value("int_key", 123); + module.add_key_value("bool_key", true); + + // Overwrite the value for "string_key" + module.add_key_value("string_key", std::string("updated_value")); + + // Capture the published message to verify its contents + diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr received_msg; + auto sub = node_->create_subscription( + "/diagnostics", 10, + [&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) { received_msg = msg; }); + module.publish(node_->now()); + rclcpp::spin_some(node_); + + ASSERT_NE(nullptr, received_msg); + ASSERT_FALSE(received_msg->status.empty()); + const auto & status = received_msg->status.front(); + + // Expect 3 key-value pairs + ASSERT_EQ(status.values.size(), 3U); + + // Helper lambda to find a value by key + auto find_value = [&](const std::string & key) -> std::string { + for (const auto & kv : status.values) { + if (kv.key == key) { + return kv.value; + } + } + return ""; + }; + + EXPECT_EQ(find_value("string_key"), "updated_value"); + EXPECT_EQ(find_value("int_key"), "123"); + EXPECT_EQ(find_value("bool_key"), "True"); + + // Status level should still be OK + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + // Message should be "OK" + EXPECT_EQ(status.message, "OK"); +} + +/* + * Test update_level_and_message(): + * Verify that the level is updated to the highest severity and + * that messages are concatenated if level > OK. + */ +TEST_F(TestDiagnosticsInterface, UpdateLevelAndMessageTest) +{ + DiagnosticsInterface module(node_.get(), "test_diagnostic"); + + // Initial status is level=OK(0), message="" + module.update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::OK, "Initial OK"); + // Update to WARN (1) + module.update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::WARN, "Low battery"); + // Update to ERROR (2) + module.update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Sensor failure"); + // Another WARN (1) after ERROR should not downgrade the overall level + module.update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Should not override error"); + + diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr received_msg; + auto sub = node_->create_subscription( + "/diagnostics", 10, + [&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) { received_msg = msg; }); + + module.publish(node_->now()); + rclcpp::spin_some(node_); + + ASSERT_NE(nullptr, received_msg); + ASSERT_FALSE(received_msg->status.empty()); + const auto & status = received_msg->status.front(); + + // Final level should be ERROR (2) + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + + // The message should contain all parts that were added when level > OK. + // Thus, we expect something like: + // "Low battery; Sensor failure; Should not override error" + const std::string & final_message = status.message; + EXPECT_FALSE(final_message.find("Initial OK") != std::string::npos); + EXPECT_TRUE(final_message.find("Low battery") != std::string::npos); + EXPECT_TRUE(final_message.find("Sensor failure") != std::string::npos); + EXPECT_TRUE(final_message.find("Should not override error") != std::string::npos); +} diff --git a/control/autoware_mpc_lateral_controller/README.md b/control/autoware_mpc_lateral_controller/README.md index 7585e7db140b3..a4b2fa3045a74 100644 --- a/control/autoware_mpc_lateral_controller/README.md +++ b/control/autoware_mpc_lateral_controller/README.md @@ -95,13 +95,11 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. #### System -| Name | Type | Description | Default value | -| :------------------------ | :------ | :-------------------------------------------------------------------------- | :------------ | -| traj_resample_dist | double | distance of waypoints in resampling [m] | 0.1 | -| use_steer_prediction | boolean | flag for using steer prediction (do not use steer measurement) | false | -| admissible_position_error | double | stop vehicle when following position error is larger than this value [m] | 5.0 | -| admissible_yaw_error_rad | double | stop vehicle when following yaw angle error is larger than this value [rad] | 1.57 | -| use_delayed_initial_state | boolean | flag to use x0_delayed as initial state for predicted trajectory | true | +| Name | Type | Description | Default value | +| :------------------------ | :------ | :--------------------------------------------------------------- | :------------ | +| traj_resample_dist | double | distance of waypoints in resampling [m] | 0.1 | +| use_steer_prediction | boolean | flag for using steer prediction (do not use steer measurement) | false | +| use_delayed_initial_state | boolean | flag to use x0_delayed as initial state for predicted trajectory | true | #### Path Smoothing diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp index 81c8b71683092..4c8d5df2c22a7 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp @@ -421,10 +421,8 @@ class MPC double m_raw_steer_cmd_prev = 0.0; // Previous MPC raw output. /* Parameters for control */ - double m_admissible_position_error; // Threshold for lateral error to trigger stop command [m]. - double m_admissible_yaw_error_rad; // Threshold for yaw error to trigger stop command [rad]. - double m_steer_lim; // Steering command limit [rad]. - double m_ctrl_period; // Control frequency [s]. + double m_steer_lim; // Steering command limit [rad]. + double m_ctrl_period; // Control frequency [s]. //!< @brief steering rate limit list depending on curvature [/m], [rad/s] std::vector> m_steer_rate_lim_map_by_curvature{}; diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp index d638142daa844..015b4e6fad4d6 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -14,6 +14,7 @@ /* * Representation + * k : reference curvature (input) * e : lateral error * th : heading angle error * steer : steering angle @@ -32,10 +33,12 @@ * dx3/dt = -(x3 - u) / tau * * Linearized model around reference point (v = v_r, th = th_r, steer = steer_r) - * [0, vr, 0] [ 0] [ 0] - * dx/dt = [0, 0, vr/W/cos(steer_r)^2] * x + [ 0] * u + [-vr*steer_r/W/cos(steer_r)^2] - * [0, 0, 1/tau] [1/tau] [ 0] + * [0, vr, 0] [ 0] [ 0] + * dx/dt = [0, 0, B] * x + [ 0] * u + [-vr*k + A - B*steer_r] + * [0, 0, -1/tau] [1/tau] [ 0] * + * where A = vr*tan(steer_r)/W + * B = vr/(W*cos(steer_r)^2) (partial derivative of A with respect to steer_r) */ #ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ diff --git a/control/autoware_mpc_lateral_controller/model_predictive_control_algorithm.md b/control/autoware_mpc_lateral_controller/model_predictive_control_algorithm.md index a1116d8a11c74..f28869bf950fb 100644 --- a/control/autoware_mpc_lateral_controller/model_predictive_control_algorithm.md +++ b/control/autoware_mpc_lateral_controller/model_predictive_control_algorithm.md @@ -365,10 +365,10 @@ and aligning the inequality signs $$ \begin{align} -u_{1} - u_{0} &< \dot u_{max}\text{d}t \\\ + -u_{1} + u_{0} &< -\dot u_{min}\text{d}t \\\ -u_{2} - u_{1} &< \dot u_{max}\text{d}t \\\ + -u_{2} + u_{1} &< - \dot u_{min}\text{d}t +u_{1} - u_{0} &< \dot u_{max}\text{d}t \\\ +- u_{1} + u_{0} &< -\dot u_{min}\text{d}t \\\ +u_{2} - u_{1} &< \dot u_{max}\text{d}t \\\ +- u_{2} + u_{1} &< - \dot u_{min}\text{d}t \end{align} $$ diff --git a/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml index b358e95f86f99..1d30a28d05cb8 100644 --- a/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml +++ b/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml @@ -3,8 +3,6 @@ # -- system -- traj_resample_dist: 0.1 # path resampling interval [m] use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) - admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value - admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value use_delayed_initial_state: true # flag to use x0_delayed as initial state for predicted trajectory # -- path smoothing -- diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index 4c3c8bec9b12e..49bf90b89bc90 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -307,17 +307,6 @@ std::pair MPC::getData( // get predicted steer data.predicted_steer = m_steering_predictor->calcSteerPrediction(); - // check error limit - const double dist_err = calcDistance2d(current_pose, data.nearest_pose); - if (dist_err > m_admissible_position_error) { - return {ResultWithReason{false, "too large position error"}, MPCData{}}; - } - - // check yaw error limit - if (std::fabs(data.yaw_err) > m_admissible_yaw_error_rad) { - return {ResultWithReason{false, "too large yaw error"}, MPCData{}}; - } - // check trajectory time length const double max_prediction_time = m_param.min_prediction_length / static_cast(m_param.prediction_horizon - 1); diff --git a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp index 76e5b4737e418..86d0c0143ed34 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -57,8 +57,6 @@ MpcLateralController::MpcLateralController( p_filt.traj_resample_dist = dp_double("traj_resample_dist"); p_filt.extend_trajectory_for_end_yaw_control = dp_bool("extend_trajectory_for_end_yaw_control"); - m_mpc->m_admissible_position_error = dp_double("admissible_position_error"); - m_mpc->m_admissible_yaw_error_rad = dp_double("admissible_yaw_error_rad"); m_mpc->m_use_steer_prediction = dp_bool("use_steer_prediction"); m_mpc->m_param.steer_tau = dp_double("vehicle_model_steer_tau"); @@ -434,11 +432,18 @@ Lateral MpcLateralController::getInitialControlCommand() const bool MpcLateralController::isStoppedState() const { + const double current_vel = m_current_kinematic_state.twist.twist.linear.x; // If the nearest index is not found, return false - if (m_current_trajectory.points.empty()) { + if ( + m_current_trajectory.points.empty() || std::fabs(current_vel) > m_stop_state_entry_ego_speed) { return false; } + const auto latest_published_cmd = m_ctrl_cmd_prev; // use prev_cmd as a latest published command + if (m_keep_steer_control_until_converged && !isSteerConverged(latest_published_cmd)) { + return false; // not stopState: keep control + } + // Note: This function used to take into account the distance to the stop line // for the stop state judgement. However, it has been removed since the steering // control was turned off when approaching/exceeding the stop line on a curve or @@ -447,21 +452,23 @@ bool MpcLateralController::isStoppedState() const m_current_trajectory.points, m_current_kinematic_state.pose.pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); - const double current_vel = m_current_kinematic_state.twist.twist.linear.x; - const double target_vel = m_current_trajectory.points.at(nearest).longitudinal_velocity_mps; - - const auto latest_published_cmd = m_ctrl_cmd_prev; // use prev_cmd as a latest published command - if (m_keep_steer_control_until_converged && !isSteerConverged(latest_published_cmd)) { - return false; // not stopState: keep control - } + // It is possible that stop is executed earlier than stop point, and velocity controller + // will not start when the distance from ego to stop point is less than 0.5 meter. + // So we use a distance margin to ensure we can detect stopped state. + static constexpr double distance_margin = 1.0; + const double target_vel = std::invoke([&]() -> double { + auto min_vel = m_current_trajectory.points.at(nearest).longitudinal_velocity_mps; + auto covered_distance = 0.0; + for (auto i = nearest + 1; i < m_current_trajectory.points.size(); ++i) { + min_vel = std::min(min_vel, m_current_trajectory.points.at(i).longitudinal_velocity_mps); + covered_distance += autoware::universe_utils::calcDistance2d( + m_current_trajectory.points.at(i - 1).pose, m_current_trajectory.points.at(i).pose); + if (covered_distance > distance_margin) break; + } + return min_vel; + }); - if ( - std::fabs(current_vel) < m_stop_state_entry_ego_speed && - std::fabs(target_vel) < m_stop_state_entry_target_speed) { - return true; - } else { - return false; - } + return std::fabs(target_vel) < m_stop_state_entry_target_speed; } Lateral MpcLateralController::createCtrlCmdMsg(const Lateral & ctrl_cmd) diff --git a/control/autoware_mpc_lateral_controller/test/test_mpc.cpp b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp index ccc91d0e7751b..9c30369305f6e 100644 --- a/control/autoware_mpc_lateral_controller/test/test_mpc.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp @@ -96,8 +96,6 @@ class MPCTest : public ::testing::Test double error_deriv_lpf_cutoff_hz = 5.0; // Test Parameters - double admissible_position_error = 5.0; - double admissible_yaw_error_rad = M_PI_2; double steer_lim = 0.610865; // 35 degrees double steer_rate_lim = 2.61799; // 150 degrees double ctrl_period = 0.03; @@ -162,8 +160,6 @@ class MPCTest : public ::testing::Test void initializeMPC(mpc_lateral_controller::MPC & mpc) { mpc.m_param = param; - mpc.m_admissible_position_error = admissible_position_error; - mpc.m_admissible_yaw_error_rad = admissible_yaw_error_rad; mpc.m_steer_lim = steer_lim; mpc.m_steer_rate_lim_map_by_curvature.emplace_back(0.0, steer_rate_lim); mpc.m_steer_rate_lim_map_by_velocity.emplace_back(0.0, steer_rate_lim); @@ -480,37 +476,4 @@ TEST_F(MPCTest, MultiSolveWithBuffer) EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f); } - -TEST_F(MPCTest, FailureCases) -{ - auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); - auto mpc = std::make_unique(node); - std::shared_ptr vehicle_model_ptr = - std::make_shared(wheelbase, steer_limit, steer_tau); - mpc->setVehicleModel(vehicle_model_ptr); - std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc->setQPSolver(qpsolver_ptr); - - // Init parameters and reference trajectory - initializeMPC(*mpc); - - // Calculate MPC with a pose too far from the trajectory - Pose pose_far; - pose_far.position.x = pose_zero.position.x - admissible_position_error - 1.0; - pose_far.position.y = pose_zero.position.y - admissible_position_error - 1.0; - Lateral ctrl_cmd; - Trajectory pred_traj; - Float32MultiArrayStamped diag; - LateralHorizon ctrl_cmd_horizon; - const auto odom = makeOdometry(pose_far, default_velocity); - EXPECT_FALSE( - mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon).result); - - // Calculate MPC with a fast velocity to make the prediction go further than the reference path - EXPECT_FALSE(mpc - ->calculateMPC( - neutral_steer, makeOdometry(pose_far, default_velocity + 10.0), ctrl_cmd, - pred_traj, diag, ctrl_cmd_horizon) - .result); -} } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/autoware_pid_longitudinal_controller/README.md b/control/autoware_pid_longitudinal_controller/README.md index b147b3b795391..aba0f36f04d65 100644 --- a/control/autoware_pid_longitudinal_controller/README.md +++ b/control/autoware_pid_longitudinal_controller/README.md @@ -68,6 +68,8 @@ There are two sources of the slope information, which can be switched by a param - Cons: z-coordinates of high-precision map is needed. - Cons: Does not support free space planning (for now) +We also offer the options to switch between these, depending on driving conditions. + **Notation:** This function works correctly only in a vehicle system that does not have acceleration feedback in the low-level control system. This compensation adds gravity correction to the target acceleration, resulting in an output value that is no longer equal to the target acceleration that the autonomous driving system desires. Therefore, it conflicts with the role of the acceleration feedback in the low-level controller. diff --git a/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml b/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml index 6d9b93ab98870..ec6a6f11b437d 100644 --- a/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml +++ b/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml @@ -16,8 +16,6 @@ stopped_state_entry_vel: 0.01 stopped_state_entry_acc: 0.1 emergency_state_overshoot_stop_dist: 1.5 - emergency_state_traj_trans_dev: 3.0 - emergency_state_traj_rot_dev: 0.7854 # drive state kp: 1.0 @@ -76,7 +74,7 @@ # slope compensation lpf_pitch_gain: 0.95 - slope_source: "raw_pitch" # raw_pitch, trajectory_pitch or trajectory_adaptive + slope_source: "trajectory_goal_adaptive" # raw_pitch, trajectory_pitch, trajectory_adaptive or trajectory_goal_adaptive adaptive_trajectory_velocity_th: 1.0 max_pitch_rad: 0.1 min_pitch_rad: -0.1 diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp index a0bceda625b21..e1cc4d2fd1690 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp @@ -34,9 +34,9 @@ class DebugValues NEAREST_VEL = 4, NEAREST_ACC = 5, SHIFT = 6, - PITCH_LPF_RAD = 7, + PITCH_USING_RAD = 7, PITCH_RAW_RAD = 8, - PITCH_LPF_DEG = 9, + PITCH_USING_DEG = 9, PITCH_RAW_DEG = 10, ERROR_VEL = 11, ERROR_VEL_FILTERED = 12, @@ -61,6 +61,9 @@ class DebugValues ERROR_ACC = 31, ERROR_ACC_FILTERED = 32, ACC_CMD_ACC_FB_APPLIED = 33, + PITCH_LPF_RAD = 34, + PITCH_LPF_DEG = 35, + SMOOTH_STOP_MODE = 36, SIZE // this is the number of enum elements }; @@ -76,6 +79,9 @@ class DebugValues * @return array of all debug values */ std::array(TYPE::SIZE)> getValues() const { return m_values; } + double getValue(const size_t index) const { return m_values.at(index); } + double getValue(const TYPE type) const { return m_values.at(static_cast(type)); } + /** * @brief set the given type to the given value * @param [in] type TYPE of the value diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 7daf3013bab4a..1d03d1b339af6 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -50,11 +50,8 @@ namespace autoware::motion::control::pid_longitudinal_controller { -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerScale; using autoware_adapi_v1_msgs::msg::OperationModeState; -using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; @@ -87,7 +84,6 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro struct ControlData { - bool is_far_from_trajectory{false}; autoware_planning_msgs::msg::Trajectory interpolated_traj{}; size_t nearest_idx{0}; // nearest_idx = 0 when nearest_idx is not found with findNearestIdx size_t target_idx{0}; @@ -104,7 +100,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // ros variables rclcpp::Publisher::SharedPtr m_pub_slope; rclcpp::Publisher::SharedPtr m_pub_debug; - rclcpp::Publisher::SharedPtr m_pub_stop_reason_marker; + rclcpp::Publisher::SharedPtr m_pub_virtual_wall_marker; rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res; rcl_interfaces::msg::SetParametersResult paramCallback( @@ -162,8 +158,6 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro double stopped_state_entry_acc; // emergency double emergency_state_overshoot_stop_dist; - double emergency_state_traj_trans_dev; - double emergency_state_traj_rot_dev; }; StateTransitionParams m_state_transition_params; @@ -210,12 +204,18 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro double m_max_acc_cmd_diff; // slope compensation - enum class SlopeSource { RAW_PITCH = 0, TRAJECTORY_PITCH, TRAJECTORY_ADAPTIVE }; + enum class SlopeSource { + RAW_PITCH = 0, + TRAJECTORY_PITCH, + TRAJECTORY_ADAPTIVE, + TRAJECTORY_GOAL_ADAPTIVE + }; SlopeSource m_slope_source{SlopeSource::RAW_PITCH}; double m_adaptive_trajectory_velocity_th; std::shared_ptr m_lpf_pitch{nullptr}; double m_max_pitch_rad; double m_min_pitch_rad; + std::optional m_previous_slope_angle{std::nullopt}; // ego nearest index search double m_ego_nearest_dist_threshold; @@ -245,12 +245,6 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // Diagnostic std::shared_ptr diag_updater_{}; // Diagnostic updater for publishing diagnostic data. - struct DiagnosticData - { - double trans_deviation{0.0}; // translation deviation between nearest point and current_pose - double rot_deviation{0.0}; // rotation deviation between nearest point and current_pose - }; - DiagnosticData m_diagnostic_data; void setupDiagnosticUpdater(); void checkControlState(diagnostic_updater::DiagnosticStatusWrapper & stat); @@ -411,11 +405,14 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro /** * @brief update variables for debugging about pitch - * @param [in] pitch current pitch of the vehicle (filtered) - * @param [in] traj_pitch current trajectory pitch - * @param [in] raw_pitch current raw pitch of the vehicle (unfiltered) + * @param [in] pitch_using + * @param [in] traj_pitch + * @param [in] localization_pitch + * @param [in] localization_pitch_lpf */ - void updatePitchDebugValues(const double pitch, const double traj_pitch, const double raw_pitch); + void updatePitchDebugValues( + const double pitch_using, const double traj_pitch, const double localization_pitch, + const double localization_pitch_lpf); /** * @brief update variables for velocity and acceleration diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/smooth_stop.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/smooth_stop.hpp index e84b44d95c886..8f8fc57e278ef 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/smooth_stop.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/smooth_stop.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ #define AUTOWARE__PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ +#include "autoware/pid_longitudinal_controller/debug_values.hpp" #include "rclcpp/rclcpp.hpp" #include // NOLINT @@ -85,7 +86,8 @@ class SmoothStop */ double calculate( const double stop_dist, const double current_vel, const double current_acc, - const std::vector> & vel_hist, const double delay_time); + const std::vector> & vel_hist, const double delay_time, + DebugValues & debug_values); private: struct Params @@ -106,6 +108,8 @@ class SmoothStop }; Params m_params; + enum class Mode { STRONG = 0, WEAK, WEAK_STOP, STRONG_STOP }; + double m_strong_acc; rclcpp::Time m_weak_acc_time; bool m_is_set_params = false; diff --git a/control/autoware_pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg b/control/autoware_pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg index 88ac881f4f43c..f669b647dbf84 100644 --- a/control/autoware_pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg +++ b/control/autoware_pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg @@ -175,7 +175,7 @@ : For 500 [ms], self velocity is smaller than C1, and self acceleration is smaller than C2.
emergency condition - : Speed command is zero velocity, and distance to stop is smaller than D1, or self pose is not within D2, D3 from trajectory. + : Speed command is zero velocity, and distance to stop is smaller than D1.

@@ -315,10 +315,6 @@ C2 : stopped_state_entry_acc
D1 : emergency_state_overshoot_stop_dist -
- D2 : emergency_state_traj_trans_dev -
- D3 : emergency_state_traj_rot_dev

diff --git a/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json b/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json index ef1272582e52b..14a34f04458eb 100644 --- a/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json +++ b/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json @@ -71,16 +71,6 @@ "description": "If `enable_overshoot_emergency` is true and the ego is `emergency_state_overshoot_stop_dist`-meter ahead of the stop point, the state will transit to EMERGENCY. [m]", "default": "1.5" }, - "emergency_state_traj_trans_dev": { - "type": "number", - "description": "If the ego's position is `emergency_state_traj_trans_dev` meter away from the nearest trajectory point, the state will transit to EMERGENCY. [m]", - "default": "3.0" - }, - "emergency_state_traj_rot_dev": { - "type": "number", - "description": "If the ego's orientation is `emergency_state_traj_rot_dev` rad away from the nearest trajectory point orientation, the state will transit to EMERGENCY. [rad]", - "default": "0.7854" - }, "kp": { "type": "number", "description": "p gain for longitudinal control", @@ -326,8 +316,6 @@ "stopped_state_entry_vel", "stopped_state_entry_acc", "emergency_state_overshoot_stop_dist", - "emergency_state_traj_trans_dev", - "emergency_state_traj_rot_dev", "kp", "ki", "kd", diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 7f6da35527290..ec95ce656fa6f 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -14,6 +14,7 @@ #include "autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/normalization.hpp" @@ -79,10 +80,6 @@ PidLongitudinalController::PidLongitudinalController( // emergency p.emergency_state_overshoot_stop_dist = node.declare_parameter("emergency_state_overshoot_stop_dist"); // [m] - p.emergency_state_traj_trans_dev = - node.declare_parameter("emergency_state_traj_trans_dev"); // [m] - p.emergency_state_traj_rot_dev = - node.declare_parameter("emergency_state_traj_rot_dev"); // [m] } // parameters for drive state @@ -199,6 +196,8 @@ PidLongitudinalController::PidLongitudinalController( m_slope_source = SlopeSource::TRAJECTORY_PITCH; } else if (slope_source == "trajectory_adaptive") { m_slope_source = SlopeSource::TRAJECTORY_ADAPTIVE; + } else if (slope_source == "trajectory_goal_adaptive") { + m_slope_source = SlopeSource::TRAJECTORY_GOAL_ADAPTIVE; } else { RCLCPP_ERROR(logger_, "Slope source is not valid. Using raw_pitch option as default"); m_slope_source = SlopeSource::RAW_PITCH; @@ -219,7 +218,7 @@ PidLongitudinalController::PidLongitudinalController( "~/output/slope_angle", rclcpp::QoS{1}); m_pub_debug = node.create_publisher( "~/output/longitudinal_diagnostic", rclcpp::QoS{1}); - m_pub_stop_reason_marker = node.create_publisher("~/output/stop_reason", rclcpp::QoS{1}); + m_pub_virtual_wall_marker = node.create_publisher("~/virtual_wall", 1); // set parameter callback m_set_param_res = node.add_on_set_parameters_callback( @@ -286,8 +285,6 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac update_param("stopped_state_entry_vel", p.stopped_state_entry_vel); update_param("stopped_state_entry_acc", p.stopped_state_entry_acc); update_param("emergency_state_overshoot_stop_dist", p.emergency_state_overshoot_stop_dist); - update_param("emergency_state_traj_trans_dev", p.emergency_state_traj_trans_dev); - update_param("emergency_state_traj_rot_dev", p.emergency_state_traj_rot_dev); } // drive state @@ -423,24 +420,6 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run( const auto control_data = getControlData(current_pose); - // self pose is far from trajectory - if (control_data.is_far_from_trajectory) { - if (m_enable_large_tracking_error_emergency) { - // update control state - changeControlState(ControlState::EMERGENCY, "the tracking error is too large"); - } - const Motion raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); // calculate control command - m_prev_raw_ctrl_cmd = raw_ctrl_cmd; - const auto cmd_msg = - createCtrlCmdMsg(raw_ctrl_cmd, control_data.current_motion.vel); // create control command - publishDebugData(raw_ctrl_cmd, control_data); // publish debug data - trajectory_follower::LongitudinalOutput output; - output.control_cmd = cmd_msg; - output.control_cmd_horizon.controls.push_back(cmd_msg); - output.control_cmd_horizon.time_step_ms = 0.0; - return output; - } - // update control state updateControlState(control_data); @@ -491,23 +470,6 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData const auto nearest_point = current_interpolated_pose.first; auto target_point = current_interpolated_pose.first; - // check if the deviation is worth emergency - m_diagnostic_data.trans_deviation = - autoware::universe_utils::calcDistance2d(current_interpolated_pose.first, current_pose); - const bool is_dist_deviation_large = - m_state_transition_params.emergency_state_traj_trans_dev < m_diagnostic_data.trans_deviation; - m_diagnostic_data.rot_deviation = std::abs(autoware::universe_utils::normalizeRadian( - tf2::getYaw(current_interpolated_pose.first.pose.orientation) - - tf2::getYaw(current_pose.orientation))); - const bool is_yaw_deviation_large = - m_state_transition_params.emergency_state_traj_rot_dev < m_diagnostic_data.rot_deviation; - - if (is_dist_deviation_large || is_yaw_deviation_large) { - // return here if nearest index is not found - control_data.is_far_from_trajectory = true; - return control_data; - } - // Delay compensation - Calculate the distance we got, predicted velocity and predicted // acceleration after delay control_data.state_after_delay = @@ -562,35 +524,52 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData // distance to stopline control_data.stop_dist = longitudinal_utils::calcStopDistance( - control_data.interpolated_traj.points.at(control_data.nearest_idx).pose, - control_data.interpolated_traj, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); + current_pose, control_data.interpolated_traj, m_ego_nearest_dist_threshold, + m_ego_nearest_yaw_threshold); // pitch // NOTE: getPitchByTraj() calculates the pitch angle as defined in // ../media/slope_definition.drawio.svg while getPitchByPose() is not, so `raw_pitch` is reversed const double raw_pitch = (-1.0) * longitudinal_utils::getPitchByPose(current_pose.orientation); + m_lpf_pitch->filter(raw_pitch); const double traj_pitch = longitudinal_utils::getPitchByTraj( control_data.interpolated_traj, control_data.target_idx, m_wheel_base); if (m_slope_source == SlopeSource::RAW_PITCH) { - control_data.slope_angle = m_lpf_pitch->filter(raw_pitch); + control_data.slope_angle = m_lpf_pitch->getValue(); } else if (m_slope_source == SlopeSource::TRAJECTORY_PITCH) { control_data.slope_angle = traj_pitch; - } else if (m_slope_source == SlopeSource::TRAJECTORY_ADAPTIVE) { + } else if ( + m_slope_source == SlopeSource::TRAJECTORY_ADAPTIVE || + m_slope_source == SlopeSource::TRAJECTORY_GOAL_ADAPTIVE) { // if velocity is high, use target idx for slope, otherwise, use raw_pitch - if (control_data.current_motion.vel > m_adaptive_trajectory_velocity_th) { - control_data.slope_angle = traj_pitch; - m_lpf_pitch->filter(raw_pitch); - } else { - control_data.slope_angle = m_lpf_pitch->filter(raw_pitch); + const bool is_vel_slow = control_data.current_motion.vel < m_adaptive_trajectory_velocity_th && + m_slope_source == SlopeSource::TRAJECTORY_ADAPTIVE; + + const double goal_dist = autoware::motion_utils::calcSignedArcLength( + control_data.interpolated_traj.points, current_pose.position, + control_data.interpolated_traj.points.size() - 1); + const bool is_close_to_trajectory_end = + goal_dist < m_wheel_base && m_slope_source == SlopeSource::TRAJECTORY_GOAL_ADAPTIVE; + + control_data.slope_angle = + (is_close_to_trajectory_end || is_vel_slow) ? m_lpf_pitch->getValue() : traj_pitch; + + if (m_previous_slope_angle.has_value()) { + constexpr double gravity_const = 9.8; + control_data.slope_angle = std::clamp( + control_data.slope_angle, + m_previous_slope_angle.value() + m_min_jerk * control_data.dt / gravity_const, + m_previous_slope_angle.value() + m_max_jerk * control_data.dt / gravity_const); } + m_previous_slope_angle = control_data.slope_angle; } else { RCLCPP_ERROR_THROTTLE( logger_, *clock_, 3000, "Slope source is not valid. Using raw_pitch option as default"); - control_data.slope_angle = m_lpf_pitch->filter(raw_pitch); + control_data.slope_angle = m_lpf_pitch->getValue(); } - updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch); + updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch, m_lpf_pitch->getValue()); return control_data; } @@ -609,6 +588,11 @@ PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCm longitudinal_utils::applyDiffLimitFilter(raw_ctrl_cmd.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk); m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, raw_ctrl_cmd.acc); + const auto virtual_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( + m_current_kinematic_state.pose.pose, "velocity control\n (emergency)", clock_->now(), 0, + m_wheel_base + m_front_overhang); + m_pub_virtual_wall_marker->publish(virtual_wall_marker); + return raw_ctrl_cmd; } @@ -773,14 +757,12 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d } // publish debug marker - auto marker = createDefaultMarker( - "map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); - marker.pose = autoware::universe_utils::calcOffsetPose( - m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang, - m_vehicle_width / 2 + 2.0, 1.5); - marker.text = "steering not\nconverged"; - m_pub_stop_reason_marker->publish(marker); + if (is_under_control) { + const auto virtual_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( + m_current_kinematic_state.pose.pose, "velocity control\n(steering not converged)", + clock_->now(), 0, m_wheel_base + m_front_overhang); + m_pub_virtual_wall_marker->publish(virtual_wall_marker); + } // keep STOPPED return; @@ -863,7 +845,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( } else if (m_control_state == ControlState::STOPPING) { raw_ctrl_cmd.acc = m_smooth_stop.calculate( control_data.stop_dist, control_data.current_motion.vel, control_data.current_motion.acc, - m_vel_hist, m_delay_compensation_time); + m_vel_hist, m_delay_compensation_time, m_debug_values); raw_ctrl_cmd.vel = m_stopped_state_params.vel; RCLCPP_DEBUG( @@ -1189,13 +1171,16 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont } void PidLongitudinalController::updatePitchDebugValues( - const double pitch, const double traj_pitch, const double raw_pitch) + const double pitch_using, const double traj_pitch, const double localization_pitch, + const double localization_pitch_lpf) { const double to_degrees = (180.0 / static_cast(M_PI)); - m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_RAD, pitch); - m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_DEG, pitch * to_degrees); - m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_RAD, raw_pitch); - m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_DEG, raw_pitch * to_degrees); + m_debug_values.setValues(DebugValues::TYPE::PITCH_USING_RAD, pitch_using); + m_debug_values.setValues(DebugValues::TYPE::PITCH_USING_DEG, pitch_using * to_degrees); + m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_RAD, localization_pitch_lpf); + m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_DEG, localization_pitch_lpf * to_degrees); + m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_RAD, localization_pitch); + m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_DEG, localization_pitch * to_degrees); m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_RAD, traj_pitch); m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_DEG, traj_pitch * to_degrees); } @@ -1239,23 +1224,7 @@ void PidLongitudinalController::checkControlState( msg = "emergency occurred due to "; } - if ( - m_state_transition_params.emergency_state_traj_trans_dev < m_diagnostic_data.trans_deviation) { - msg += "translation deviation"; - } - - if (m_state_transition_params.emergency_state_traj_rot_dev < m_diagnostic_data.rot_deviation) { - msg += "rotation deviation"; - } - stat.add("control_state", static_cast(m_control_state)); - stat.addf( - "translation deviation threshold", "%lf", - m_state_transition_params.emergency_state_traj_trans_dev); - stat.addf("translation deviation", "%lf", m_diagnostic_data.trans_deviation); - stat.addf( - "rotation deviation threshold", "%lf", m_state_transition_params.emergency_state_traj_rot_dev); - stat.addf("rotation deviation", "%lf", m_diagnostic_data.rot_deviation); stat.summary(level, msg); } diff --git a/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp b/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp index 9ee4763c6c05f..2dd2950d4356e 100644 --- a/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp +++ b/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp @@ -116,7 +116,8 @@ std::experimental::optional SmoothStop::calcTimeToStop( double SmoothStop::calculate( const double stop_dist, const double current_vel, const double current_acc, - const std::vector> & vel_hist, const double delay_time) + const std::vector> & vel_hist, const double delay_time, + DebugValues & debug_values) { if (!m_is_set_params) { throw std::runtime_error("Trying to calculate uninitialized SmoothStop"); @@ -132,8 +133,11 @@ double SmoothStop::calculate( // when exceeding the stopline (stop_dist is negative in these cases.) if (stop_dist < m_params.strong_stop_dist) { // when exceeding the stopline much + debug_values.setValues( + DebugValues::TYPE::SMOOTH_STOP_MODE, static_cast(Mode::STRONG_STOP)); return m_params.strong_stop_acc; } else if (stop_dist < m_params.weak_stop_dist) { // when exceeding the stopline a bit + debug_values.setValues(DebugValues::TYPE::SMOOTH_STOP_MODE, static_cast(Mode::WEAK_STOP)); return m_params.weak_stop_acc; } @@ -143,19 +147,23 @@ double SmoothStop::calculate( if ( (time_to_stop && *time_to_stop > m_params.weak_stop_time + delay_time) || (!time_to_stop && is_fast_vel)) { + debug_values.setValues(DebugValues::TYPE::SMOOTH_STOP_MODE, static_cast(Mode::STRONG)); return m_strong_acc; } m_weak_acc_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + debug_values.setValues(DebugValues::TYPE::SMOOTH_STOP_MODE, static_cast(Mode::WEAK)); return m_params.weak_acc; } // for 0.5 seconds after the car stopped if ((rclcpp::Clock{RCL_ROS_TIME}.now() - m_weak_acc_time).seconds() < 0.5) { + debug_values.setValues(DebugValues::TYPE::SMOOTH_STOP_MODE, static_cast(Mode::WEAK)); return m_params.weak_acc; } // when the car is not running + debug_values.setValues(DebugValues::TYPE::SMOOTH_STOP_MODE, static_cast(Mode::STRONG_STOP)); return m_params.strong_stop_acc; } } // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp b/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp index a6d03b8032fe6..30d582bbbf1ef 100644 --- a/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp +++ b/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp @@ -21,6 +21,7 @@ TEST(TestSmoothStop, calculate_stopping_acceleration) { + using ::autoware::motion::control::pid_longitudinal_controller::DebugValues; using ::autoware::motion::control::pid_longitudinal_controller::SmoothStop; using rclcpp::Duration; using rclcpp::Time; @@ -40,9 +41,10 @@ TEST(TestSmoothStop, calculate_stopping_acceleration) const double delay_time = 0.17; SmoothStop ss; + DebugValues debug_values; // Cannot calculate before setting parameters - EXPECT_THROW(ss.calculate(0.0, 0.0, 0.0, {}, delay_time), std::runtime_error); + EXPECT_THROW(ss.calculate(0.0, 0.0, 0.0, {}, delay_time, debug_values), std::runtime_error); ss.setParams( max_strong_acc, min_strong_acc, weak_acc, weak_stop_acc, strong_stop_acc, max_fast_vel, @@ -62,34 +64,45 @@ TEST(TestSmoothStop, calculate_stopping_acceleration) stop_dist = strong_stop_dist - 0.1; current_vel = 2.0; ss.init(vel_in_target, stop_dist); - accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); EXPECT_EQ(accel, strong_stop_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 3); // weak stop when the stop distance is below the threshold (but not bellow the strong_stop_dist) stop_dist = weak_stop_dist - 0.1; current_vel = 2.0; ss.init(vel_in_target, stop_dist); - accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); EXPECT_EQ(accel, weak_stop_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 2); // if not running, weak accel for 0.5 seconds after the previous init or previous weak_acc rclcpp::Rate rate_quart(1.0 / 0.25); rclcpp::Rate rate_half(1.0 / 0.5); stop_dist = 0.0; current_vel = 0.0; - EXPECT_EQ( - ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); + EXPECT_EQ(accel, weak_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 1); rate_quart.sleep(); - EXPECT_EQ( - ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); + EXPECT_EQ(accel, weak_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 1); rate_half.sleep(); - EXPECT_NE( - ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); + EXPECT_NE(accel, weak_acc); + EXPECT_NE(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 1); // strong stop when the car is not running (and is at least 0.5seconds after initialization) - EXPECT_EQ( - ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), - strong_stop_acc); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); + EXPECT_EQ(accel, strong_stop_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 3); // accel between min/max_strong_acc when the car is running: // not predicted to exceed the stop line and is predicted to stop after weak_stop_time + delay @@ -97,19 +110,22 @@ TEST(TestSmoothStop, calculate_stopping_acceleration) current_vel = 1.0; vel_in_target = 1.0; ss.init(vel_in_target, stop_dist); - EXPECT_EQ( - ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), - max_strong_acc); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); + EXPECT_EQ(accel, max_strong_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 0); vel_in_target = std::sqrt(2.0); ss.init(vel_in_target, stop_dist); - EXPECT_EQ( - ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), - min_strong_acc); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); + EXPECT_EQ(accel, min_strong_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 0); for (double vel_in_target = 1.1; vel_in_target < std::sqrt(2.0); vel_in_target += 0.1) { ss.init(vel_in_target, stop_dist); - accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); EXPECT_GT(accel, min_strong_acc); EXPECT_LT(accel, max_strong_acc); } diff --git a/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml b/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml index 0de08076b8c06..fda7e1f3906e0 100644 --- a/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml +++ b/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml @@ -1,130 +1,130 @@ - - + + - - + + - - + + - + - + - + - + - + - + - + - - + + - + - + - - + + - + - + - + - - + + - + - + - + - + - - + + - + - + - - + + @@ -133,114 +133,150 @@ - + - + - - + + - + - + - + - + - - + + - - - - - - + - + + + + + + + + + + + - + - + - + - + - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - + - + - + - + - - + + - + + + + + + @@ -255,7 +291,7 @@ - + @@ -270,11 +306,11 @@ - + - - - + + + diff --git a/control/autoware_trajectory_follower_node/test/test_controller_node.cpp b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp index 0b1dac644a8ab..982af104538b5 100644 --- a/control/autoware_trajectory_follower_node/test/test_controller_node.cpp +++ b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp @@ -510,34 +510,6 @@ TEST_F(FakeNodeFixture, longitudinal_reverse) EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } -TEST_F(FakeNodeFixture, longitudinal_emergency) -{ - const auto node_options = makeNodeOptions(); - ControllerTester tester(this, node_options); - - tester.send_default_transform(); - tester.publish_default_odom(); - tester.publish_autonomous_operation_mode(); - tester.publish_default_steer(); - tester.publish_default_acc(); - - // Publish trajectory starting away from the current ego pose - Trajectory traj; - traj.header.stamp = tester.node->now(); - traj.header.frame_id = "map"; - traj.points.push_back(make_traj_point(10.0, 0.0, 1.0f)); - traj.points.push_back(make_traj_point(50.0, 0.0, 1.0f)); - traj.points.push_back(make_traj_point(100.0, 0.0, 1.0f)); - tester.traj_pub->publish(traj); - - test_utils::waitForMessage(tester.node, this, tester.received_control_command); - - ASSERT_TRUE(tester.received_control_command); - // Emergencies (e.g., far from trajectory) produces braking command (0 vel, negative accel) - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); - EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); -} - TEST_F(FakeNodeFixture, longitudinal_not_check_steer_converged) { const auto node_options = makeNodeOptions(); diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index 9c8579469f878..b0db41b0fdc73 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -52,45 +53,51 @@ ControlEvaluatorNode::~ControlEvaluatorNode() return; } - // generate json data - using json = nlohmann::json; - json j; - for (Metric metric : metrics_) { - const std::string base_name = metric_to_str.at(metric) + "/"; - j[base_name + "min"] = metric_accumulators_[static_cast(metric)].min(); - j[base_name + "max"] = metric_accumulators_[static_cast(metric)].max(); - j[base_name + "mean"] = metric_accumulators_[static_cast(metric)].mean(); - j[base_name + "count"] = metric_accumulators_[static_cast(metric)].count(); - j[base_name + "description"] = metric_descriptions.at(metric); - } + try { + // generate json data + using json = nlohmann::json; + json j; + for (Metric metric : metrics_) { + const std::string base_name = metric_to_str.at(metric) + "/"; + j[base_name + "min"] = metric_accumulators_[static_cast(metric)].min(); + j[base_name + "max"] = metric_accumulators_[static_cast(metric)].max(); + j[base_name + "mean"] = metric_accumulators_[static_cast(metric)].mean(); + j[base_name + "count"] = metric_accumulators_[static_cast(metric)].count(); + j[base_name + "description"] = metric_descriptions.at(metric); + } - // get output folder - const std::string output_folder_str = - rclcpp::get_logging_directory().string() + "/autoware_metrics"; - if (!std::filesystem::exists(output_folder_str)) { - if (!std::filesystem::create_directories(output_folder_str)) { - RCLCPP_ERROR( - this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); - return; + // get output folder + const std::string output_folder_str = + rclcpp::get_logging_directory().string() + "/autoware_metrics"; + if (!std::filesystem::exists(output_folder_str)) { + if (!std::filesystem::create_directories(output_folder_str)) { + RCLCPP_ERROR( + this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); + return; + } } - } - // get time stamp - std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); - std::tm * local_time = std::localtime(&now_time_t); - std::ostringstream oss; - oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); - std::string cur_time_str = oss.str(); - - // Write metrics .json to file - const std::string output_file_str = - output_folder_str + "/autoware_control_evaluator-" + cur_time_str + ".json"; - std::ofstream f(output_file_str); - if (f.is_open()) { - f << j.dump(4); - f.close(); - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + // get time stamp + std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + std::tm * local_time = std::localtime(&now_time_t); + std::ostringstream oss; + oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); + std::string cur_time_str = oss.str(); + + // Write metrics .json to file + const std::string output_file_str = + output_folder_str + "/autoware_control_evaluator-" + cur_time_str + ".json"; + std::ofstream f(output_file_str); + if (f.is_open()) { + f << j.dump(4); + f.close(); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + } + } catch (const std::exception & e) { + std::cerr << "Exception in ControlEvaluatorNode destructor: " << e.what() << std::endl; + } catch (...) { + std::cerr << "Unknown exception in ControlEvaluatorNode destructor" << std::endl; } } diff --git a/evaluator/autoware_planning_evaluator/README.md b/evaluator/autoware_planning_evaluator/README.md index 4fcdf4d7e55fd..b9dd3201a2541 100644 --- a/evaluator/autoware_planning_evaluator/README.md +++ b/evaluator/autoware_planning_evaluator/README.md @@ -13,6 +13,7 @@ Metrics are calculated using the following information: - the previous trajectory `T(-1)`. - the _reference_ trajectory assumed to be used as the reference to plan `T(0)`. - the current ego pose. +- the current ego odometry. - the set of objects in the environment. These information are maintained by an instance of class `MetricsCalculator` diff --git a/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml b/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml index 14c1bcc6beea4..7605ed2a5e859 100644 --- a/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml +++ b/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml @@ -14,6 +14,8 @@ - lateral_deviation - yaw_deviation - velocity_deviation + - lateral_trajectory_displacement_local + - lateral_trajectory_displacement_lookahead - stability - stability_frechet - obstacle_distance @@ -24,6 +26,7 @@ trajectory: min_point_dist_m: 0.1 # [m] minimum distance between two successive points to use for angle calculation + evaluation_time_s: 5.0 # [s] time duration for trajectory evaluation in seconds lookahead: max_dist_m: 5.0 # [m] maximum distance from ego along the trajectory to use for calculation max_time_s: 3.0 # [s] maximum time ahead of ego along the trajectory to use for calculation diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp index 59866c96ad702..2341ad2bb6ba3 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp @@ -38,6 +38,16 @@ using geometry_msgs::msg::Pose; */ Accumulator calcLateralDeviation(const Trajectory & ref, const Trajectory & traj); +/** + * @brief calculate lateral trajectory displacement from the previous trajectory and the trajectory + * @param [in] prev previous trajectory + * @param [in] traj input trajectory + * @param [in] base_pose base pose + * @return calculated statistics + */ +Accumulator calcLocalLateralTrajectoryDisplacement( + const Trajectory & prev, const Trajectory & traj, const Pose & base_pose); + /** * @brief calculate yaw deviation of the given trajectory from the reference trajectory * @param [in] ref reference trajectory diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp index c0313ed0727dd..7c207bf6c8f57 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp @@ -37,6 +37,8 @@ enum class Metric { lateral_deviation, yaw_deviation, velocity_deviation, + lateral_trajectory_displacement_local, + lateral_trajectory_displacement_lookahead, stability, stability_frechet, obstacle_distance, @@ -62,6 +64,8 @@ static const std::unordered_map str_to_metric = { {"lateral_deviation", Metric::lateral_deviation}, {"yaw_deviation", Metric::yaw_deviation}, {"velocity_deviation", Metric::velocity_deviation}, + {"lateral_trajectory_displacement_local", Metric::lateral_trajectory_displacement_local}, + {"lateral_trajectory_displacement_lookahead", Metric::lateral_trajectory_displacement_lookahead}, {"stability", Metric::stability}, {"stability_frechet", Metric::stability_frechet}, {"obstacle_distance", Metric::obstacle_distance}, @@ -82,6 +86,8 @@ static const std::unordered_map metric_to_str = { {Metric::lateral_deviation, "lateral_deviation"}, {Metric::yaw_deviation, "yaw_deviation"}, {Metric::velocity_deviation, "velocity_deviation"}, + {Metric::lateral_trajectory_displacement_local, "lateral_trajectory_displacement_local"}, + {Metric::lateral_trajectory_displacement_lookahead, "lateral_trajectory_displacement_lookahead"}, {Metric::stability, "stability"}, {Metric::stability_frechet, "stability_frechet"}, {Metric::obstacle_distance, "obstacle_distance"}, @@ -103,6 +109,8 @@ static const std::unordered_map metric_descriptions = { {Metric::lateral_deviation, "Lateral_deviation[m]"}, {Metric::yaw_deviation, "Yaw_deviation[rad]"}, {Metric::velocity_deviation, "Velocity_deviation[m/s]"}, + {Metric::lateral_trajectory_displacement_local, "Nearest Pose Lateral Deviation[m]"}, + {Metric::lateral_trajectory_displacement_lookahead, "Lateral_Offset_Over_Distance_Ahead[m]"}, {Metric::stability, "Stability[m]"}, {Metric::stability_frechet, "StabilityFrechet[m]"}, {Metric::obstacle_distance, "Obstacle_distance[m]"}, diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp index 0006e49c3338a..9f2b99007af7d 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp @@ -25,6 +25,7 @@ namespace metrics namespace utils { using autoware_planning_msgs::msg::Trajectory; +using geometry_msgs::msg::Pose; /** * @brief find the index in the trajectory at the given distance of the given index @@ -35,6 +36,29 @@ using autoware_planning_msgs::msg::Trajectory; */ size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance); +/** + * @brief trim a trajectory from the current ego pose to some fixed time or distance + * @param [in] traj input trajectory to trim + * @param [in] max_dist_m [m] maximum distance ahead of the ego pose + * @param [in] max_time_s [s] maximum time ahead of the ego pose + * @return sub-trajectory starting from the ego pose and of maximum length max_dist_m, maximum + * duration max_time_s + */ +Trajectory get_lookahead_trajectory( + const Trajectory & traj, const Pose & ego_pose, const double max_dist_m, const double max_time_s); + +/** + * @brief calculate the total distance from ego position to the end of trajectory + * @details finds the nearest point to ego position on the trajectory and calculates + * the cumulative distance by summing up the distances between consecutive points + * from that position to the end of the trajectory. + * + * @param [in] traj input trajectory to calculate distance along + * @param [in] ego_pose current ego vehicle pose + * @return total distance from ego position to trajectory end in meters + */ +double calc_lookahead_trajectory_distance(const Trajectory & traj, const Pose & ego_pose); + } // namespace utils } // namespace metrics } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp index 38f388feeadda..1b46fbddfb297 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp @@ -18,6 +18,7 @@ #include "autoware/universe_utils/math/accumulator.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" +#include namespace planning_diagnostics { @@ -42,6 +43,23 @@ Accumulator calcFrechetDistance(const Trajectory & traj1, const Trajecto */ Accumulator calcLateralDistance(const Trajectory & traj1, const Trajectory & traj2); +/** + * @brief calculate the total lateral displacement between two trajectories + * @details Evaluates the cumulative absolute lateral displacement by sampling points + * along the first trajectory and measuring their offset from the second trajectory. + * The evaluation section length is determined by the ego vehicle's velocity and + * the specified evaluation time. + * + * @param traj1 first trajectory to compare + * @param traj2 second trajectory to compare against + * @param [in] ego_odom current ego vehicle odometry containing pose and velocity + * @param [in] trajectory_eval_time_s time duration for trajectory evaluation in seconds + * @return statistical accumulator containing the total lateral displacement + */ +Accumulator calcLookaheadLateralTrajectoryDisplacement( + const Trajectory traj1, const Trajectory traj2, const nav_msgs::msg::Odometry & ego_odom, + const double trajectory_eval_time_s); + } // namespace metrics } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp index 97d23cad10af2..fe9d9eaf4278b 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp @@ -23,6 +23,7 @@ #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose.hpp" +#include #include @@ -74,7 +75,7 @@ class MetricsCalculator * @brief set the ego pose * @param [in] traj input previous trajectory */ - void setEgoPose(const geometry_msgs::msg::Pose & pose); + void setEgoPose(const nav_msgs::msg::Odometry & ego_odometry); /** * @brief get the ego pose @@ -83,23 +84,13 @@ class MetricsCalculator Pose getEgoPose(); private: - /** - * @brief trim a trajectory from the current ego pose to some fixed time or distance - * @param [in] traj input trajectory to trim - * @param [in] max_dist_m [m] maximum distance ahead of the ego pose - * @param [in] max_time_s [s] maximum time ahead of the ego pose - * @return sub-trajectory starting from the ego pose and of maximum length max_dist_m, maximum - * duration max_time_s - */ - Trajectory getLookaheadTrajectory( - const Trajectory & traj, const double max_dist_m, const double max_time_s) const; - Trajectory reference_trajectory_; Trajectory reference_trajectory_lookahead_; Trajectory previous_trajectory_; Trajectory previous_trajectory_lookahead_; PredictedObjects dynamic_objects_; geometry_msgs::msg::Pose ego_pose_; + nav_msgs::msg::Odometry ego_odometry_; PoseWithUuidStamped modified_goal_; }; // class MetricsCalculator diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/parameters.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/parameters.hpp index cd894fecc2331..94920195ee89c 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/parameters.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/parameters.hpp @@ -31,6 +31,7 @@ struct Parameters struct { double min_point_dist_m = 0.1; + double evaluation_time_s = 5.0; struct { double max_dist_m = 5.0; diff --git a/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json b/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json index 1ef3874a0dcbc..263bd374a7e45 100644 --- a/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json +++ b/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json @@ -34,6 +34,7 @@ "lateral_deviation", "yaw_deviation", "velocity_deviation", + "lateral_trajectory_displacement", "stability", "stability_frechet", "obstacle_distance", diff --git a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp index 7e2217a49ef87..82ba86c65d6af 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp @@ -45,6 +45,24 @@ Accumulator calcLateralDeviation(const Trajectory & ref, const Trajector return stat; } +Accumulator calcLocalLateralTrajectoryDisplacement( + const Trajectory & prev, const Trajectory & traj, const Pose & ego_pose) +{ + Accumulator stat; + + if (prev.points.empty() || traj.points.empty()) { + return stat; + } + + const auto prev_lateral_deviation = + autoware::motion_utils::calcLateralOffset(prev.points, ego_pose.position); + const auto traj_lateral_deviation = + autoware::motion_utils::calcLateralOffset(traj.points, ego_pose.position); + const auto lateral_trajectory_displacement = traj_lateral_deviation - prev_lateral_deviation; + stat.add(lateral_trajectory_displacement); + return stat; +} + Accumulator calcYawDeviation(const Trajectory & ref, const Trajectory & traj) { Accumulator stat; diff --git a/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp index 669afdd7229b0..7451264f037a6 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" + namespace planning_diagnostics { namespace metrics @@ -23,6 +25,7 @@ namespace utils using autoware::universe_utils::calcDistance2d; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Pose; size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance) { @@ -41,6 +44,52 @@ size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, cons return target_id; } +Trajectory get_lookahead_trajectory( + const Trajectory & traj, const Pose & ego_pose, const double max_dist_m, const double max_time_s) +{ + if (traj.points.empty()) { + return traj; + } + + const auto ego_index = + autoware::motion_utils::findNearestSegmentIndex(traj.points, ego_pose.position); + Trajectory lookahead_traj; + lookahead_traj.header = traj.header; + double dist = 0.0; + double time = 0.0; + auto curr_point_it = std::next(traj.points.begin(), ego_index); + auto prev_point_it = curr_point_it; + while (curr_point_it != traj.points.end() && dist <= max_dist_m && time <= max_time_s) { + lookahead_traj.points.push_back(*curr_point_it); + const auto d = autoware::universe_utils::calcDistance2d( + prev_point_it->pose.position, curr_point_it->pose.position); + dist += d; + if (prev_point_it->longitudinal_velocity_mps != 0.0) { + time += d / std::abs(prev_point_it->longitudinal_velocity_mps); + } + prev_point_it = curr_point_it; + ++curr_point_it; + } + return lookahead_traj; +} + +double calc_lookahead_trajectory_distance(const Trajectory & traj, const Pose & ego_pose) +{ + const auto ego_index = + autoware::motion_utils::findNearestSegmentIndex(traj.points, ego_pose.position); + double dist = 0.0; + auto curr_point_it = std::next(traj.points.begin(), ego_index); + auto prev_point_it = curr_point_it; + for (size_t i = 0; i < traj.points.size(); ++i) { + const auto d = autoware::universe_utils::calcDistance2d( + prev_point_it->pose.position, curr_point_it->pose.position); + dist += d; + prev_point_it = curr_point_it; + ++curr_point_it; + } + + return dist; +} } // namespace utils } // namespace metrics } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp index e6ede0d07b9b3..61e18a6ad0b63 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp @@ -14,7 +14,9 @@ #include "autoware/planning_evaluator/metrics/stability_metrics.hpp" +#include "autoware/motion_utils/resample/resample.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/planning_evaluator/metrics/metrics_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -96,5 +98,47 @@ Accumulator calcLateralDistance(const Trajectory & traj1, const Trajecto return stat; } +Accumulator calcLookaheadLateralTrajectoryDisplacement( + const Trajectory traj1, const Trajectory traj2, const nav_msgs::msg::Odometry & ego_odom, + const double trajectory_eval_time_s) +{ + Accumulator stat; + + if (traj1.points.empty() || traj2.points.empty()) { + return stat; + } + + const double ego_velocity = + std::hypot(ego_odom.twist.twist.linear.x, ego_odom.twist.twist.linear.y); + + const double evaluation_section_length = trajectory_eval_time_s * std::abs(ego_velocity); + + const double traj1_lookahead_distance = + utils::calc_lookahead_trajectory_distance(traj1, ego_odom.pose.pose); + const double traj2_lookahead_distance = + utils::calc_lookahead_trajectory_distance(traj2, ego_odom.pose.pose); + + if ( + traj1_lookahead_distance < evaluation_section_length || + traj2_lookahead_distance < evaluation_section_length) { + return stat; + } + + constexpr double num_evaluation_points = 10.0; + const double interval = evaluation_section_length / num_evaluation_points; + + const auto resampled_traj1 = autoware::motion_utils::resampleTrajectory( + utils::get_lookahead_trajectory( + traj1, ego_odom.pose.pose, evaluation_section_length, trajectory_eval_time_s), + interval); + + for (const auto & point : resampled_traj1.points) { + const auto p0 = autoware::universe_utils::getPoint(point); + const double dist = autoware::motion_utils::calcLateralOffset(traj2.points, p0); + stat.add(std::abs(dist)); + } + return stat; +} + } // namespace metrics } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp index 6e057bcc9fc3d..c30420a5632fa 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp @@ -16,6 +16,7 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/planning_evaluator/metrics/deviation_metrics.hpp" +#include "autoware/planning_evaluator/metrics/metrics_utils.hpp" #include "autoware/planning_evaluator/metrics/obstacle_metrics.hpp" #include "autoware/planning_evaluator/metrics/stability_metrics.hpp" #include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp" @@ -49,21 +50,26 @@ std::optional> MetricsCalculator::calculate( return metrics::calcYawDeviation(reference_trajectory_, traj); case Metric::velocity_deviation: return metrics::calcVelocityDeviation(reference_trajectory_, traj); + case Metric::lateral_trajectory_displacement_local: + return metrics::calcLocalLateralTrajectoryDisplacement(previous_trajectory_, traj, ego_pose_); + case Metric::lateral_trajectory_displacement_lookahead: + return metrics::calcLookaheadLateralTrajectoryDisplacement( + previous_trajectory_, traj, ego_odometry_, parameters.trajectory.evaluation_time_s); case Metric::stability_frechet: return metrics::calcFrechetDistance( - getLookaheadTrajectory( - previous_trajectory_, parameters.trajectory.lookahead.max_dist_m, + metrics::utils::get_lookahead_trajectory( + previous_trajectory_, ego_pose_, parameters.trajectory.lookahead.max_dist_m, parameters.trajectory.lookahead.max_time_s), - getLookaheadTrajectory( - traj, parameters.trajectory.lookahead.max_dist_m, + metrics::utils::get_lookahead_trajectory( + traj, ego_pose_, parameters.trajectory.lookahead.max_dist_m, parameters.trajectory.lookahead.max_time_s)); case Metric::stability: return metrics::calcLateralDistance( - getLookaheadTrajectory( - previous_trajectory_, parameters.trajectory.lookahead.max_dist_m, + metrics::utils::get_lookahead_trajectory( + previous_trajectory_, ego_pose_, parameters.trajectory.lookahead.max_dist_m, parameters.trajectory.lookahead.max_time_s), - getLookaheadTrajectory( - traj, parameters.trajectory.lookahead.max_dist_m, + metrics::utils::get_lookahead_trajectory( + traj, ego_pose_, parameters.trajectory.lookahead.max_dist_m, parameters.trajectory.lookahead.max_time_s)); case Metric::obstacle_distance: return metrics::calcDistanceToObstacle(dynamic_objects_, traj); @@ -105,9 +111,10 @@ void MetricsCalculator::setPredictedObjects(const PredictedObjects & objects) dynamic_objects_ = objects; } -void MetricsCalculator::setEgoPose(const geometry_msgs::msg::Pose & pose) +void MetricsCalculator::setEgoPose(const nav_msgs::msg::Odometry & ego_odometry) { - ego_pose_ = pose; + ego_pose_ = ego_odometry.pose.pose; + ego_odometry_ = ego_odometry; } Pose MetricsCalculator::getEgoPose() @@ -115,33 +122,4 @@ Pose MetricsCalculator::getEgoPose() return ego_pose_; } -Trajectory MetricsCalculator::getLookaheadTrajectory( - const Trajectory & traj, const double max_dist_m, const double max_time_s) const -{ - if (traj.points.empty()) { - return traj; - } - - const auto ego_index = - autoware::motion_utils::findNearestSegmentIndex(traj.points, ego_pose_.position); - Trajectory lookahead_traj; - lookahead_traj.header = traj.header; - double dist = 0.0; - double time = 0.0; - auto curr_point_it = std::next(traj.points.begin(), ego_index); - auto prev_point_it = curr_point_it; - while (curr_point_it != traj.points.end() && dist <= max_dist_m && time <= max_time_s) { - lookahead_traj.points.push_back(*curr_point_it); - const auto d = autoware::universe_utils::calcDistance2d( - prev_point_it->pose.position, curr_point_it->pose.position); - dist += d; - if (prev_point_it->longitudinal_velocity_mps != 0.0) { - time += d / std::abs(prev_point_it->longitudinal_velocity_mps); - } - prev_point_it = curr_point_it; - ++curr_point_it; - } - return lookahead_traj; -} - } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp index ac6f35179f771..d43aed1ec5687 100644 --- a/evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -50,47 +51,52 @@ MotionEvaluatorNode::~MotionEvaluatorNode() if (!output_metrics_) { return; } - - // generate json data - using json = nlohmann::json; - json j; - for (Metric metric : metrics_) { - const std::string base_name = metric_to_str.at(metric) + "/"; - const auto & stat = metrics_calculator_.calculate(metric, accumulated_trajectory_); - if (stat) { - j[base_name + "min"] = stat->min(); - j[base_name + "max"] = stat->max(); - j[base_name + "mean"] = stat->mean(); + try { + // generate json data + using json = nlohmann::json; + json j; + for (Metric metric : metrics_) { + const std::string base_name = metric_to_str.at(metric) + "/"; + const auto & stat = metrics_calculator_.calculate(metric, accumulated_trajectory_); + if (stat) { + j[base_name + "min"] = stat->min(); + j[base_name + "max"] = stat->max(); + j[base_name + "mean"] = stat->mean(); + } } - } - // get output folder - const std::string output_folder_str = - rclcpp::get_logging_directory().string() + "/autoware_metrics"; - if (!std::filesystem::exists(output_folder_str)) { - if (!std::filesystem::create_directories(output_folder_str)) { - RCLCPP_ERROR( - this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); - return; + // get output folder + const std::string output_folder_str = + rclcpp::get_logging_directory().string() + "/autoware_metrics"; + if (!std::filesystem::exists(output_folder_str)) { + if (!std::filesystem::create_directories(output_folder_str)) { + RCLCPP_ERROR( + this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); + return; + } } - } - // get time stamp - std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); - std::tm * local_time = std::localtime(&now_time_t); - std::ostringstream oss; - oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); - std::string cur_time_str = oss.str(); - - // Write metrics .json to file - const std::string output_file_str = - output_folder_str + "/autoware_motion_evaluator-" + cur_time_str + ".json"; - std::ofstream f(output_file_str); - if (f.is_open()) { - f << j.dump(4); - f.close(); - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + // get time stamp + std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + std::tm * local_time = std::localtime(&now_time_t); + std::ostringstream oss; + oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); + std::string cur_time_str = oss.str(); + + // Write metrics .json to file + const std::string output_file_str = + output_folder_str + "/autoware_motion_evaluator-" + cur_time_str + ".json"; + std::ofstream f(output_file_str); + if (f.is_open()) { + f << j.dump(4); + f.close(); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + } + } catch (const std::exception & e) { + std::cerr << "Exception in MotionEvaluatorNode destructor: " << e.what() << std::endl; + } catch (...) { + std::cerr << "Unknown exception in MotionEvaluatorNode destructor" << std::endl; } } diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index a61e56cd532ad..5889f15e48390 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -50,6 +51,8 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op declare_parameter("trajectory.lookahead.max_dist_m"); metrics_calculator_.parameters.trajectory.lookahead.max_time_s = declare_parameter("trajectory.lookahead.max_time_s"); + metrics_calculator_.parameters.trajectory.evaluation_time_s = + declare_parameter("trajectory.evaluation_time_s"); metrics_calculator_.parameters.obstacle.dist_thr_m = declare_parameter("obstacle.dist_thr_m"); @@ -74,45 +77,51 @@ PlanningEvaluatorNode::~PlanningEvaluatorNode() return; } - // generate json data - using json = nlohmann::json; - json j; - for (Metric metric : metrics_) { - const std::string base_name = metric_to_str.at(metric) + "/"; - j[base_name + "min"] = metric_accumulators_[static_cast(metric)][0].min(); - j[base_name + "max"] = metric_accumulators_[static_cast(metric)][1].max(); - j[base_name + "mean"] = metric_accumulators_[static_cast(metric)][2].mean(); - j[base_name + "count"] = metric_accumulators_[static_cast(metric)][2].count(); - j[base_name + "description"] = metric_descriptions.at(metric); - } + try { + // generate json data + using json = nlohmann::json; + json j; + for (Metric metric : metrics_) { + const std::string base_name = metric_to_str.at(metric) + "/"; + j[base_name + "min"] = metric_accumulators_[static_cast(metric)][0].min(); + j[base_name + "max"] = metric_accumulators_[static_cast(metric)][1].max(); + j[base_name + "mean"] = metric_accumulators_[static_cast(metric)][2].mean(); + j[base_name + "count"] = metric_accumulators_[static_cast(metric)][2].count(); + j[base_name + "description"] = metric_descriptions.at(metric); + } - // get output folder - const std::string output_folder_str = - rclcpp::get_logging_directory().string() + "/autoware_metrics"; - if (!std::filesystem::exists(output_folder_str)) { - if (!std::filesystem::create_directories(output_folder_str)) { - RCLCPP_ERROR( - this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); - return; + // get output folder + const std::string output_folder_str = + rclcpp::get_logging_directory().string() + "/autoware_metrics"; + if (!std::filesystem::exists(output_folder_str)) { + if (!std::filesystem::create_directories(output_folder_str)) { + RCLCPP_ERROR( + this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); + return; + } } - } - // get time stamp - std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); - std::tm * local_time = std::localtime(&now_time_t); - std::ostringstream oss; - oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); - std::string cur_time_str = oss.str(); - - // Write metrics .json to file - const std::string output_file_str = - output_folder_str + "/autoware_planning_evaluator-" + cur_time_str + ".json"; - std::ofstream f(output_file_str); - if (f.is_open()) { - f << j.dump(4); - f.close(); - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + // get time stamp + std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + std::tm * local_time = std::localtime(&now_time_t); + std::ostringstream oss; + oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); + std::string cur_time_str = oss.str(); + + // Write metrics .json to file + const std::string output_file_str = + output_folder_str + "/autoware_planning_evaluator-" + cur_time_str + ".json"; + std::ofstream f(output_file_str); + if (f.is_open()) { + f << j.dump(4); + f.close(); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + } + } catch (const std::exception & e) { + std::cerr << "Exception in MotionEvaluatorNode destructor: " << e.what() << std::endl; + } catch (...) { + std::cerr << "Unknown exception in MotionEvaluatorNode destructor" << std::endl; } } @@ -343,7 +352,7 @@ void PlanningEvaluatorNode::onModifiedGoal( void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_msg) { if (!odometry_msg) return; - metrics_calculator_.setEgoPose(odometry_msg->pose.pose); + metrics_calculator_.setEgoPose(*odometry_msg); { getRouteData(); if (route_handler_.isHandlerReady() && odometry_msg) { diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml index fcfa9baf4ae20..236fea11d9dda 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml @@ -120,6 +120,7 @@ + @@ -132,6 +133,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml index c289a81c906fe..5da942e8ff8f3 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml @@ -143,6 +143,7 @@ + @@ -155,6 +156,7 @@ + @@ -178,6 +180,7 @@ Control parameter 'use_radar_tracking_fusion' should defined in perception.launch.xml --> + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml index a492e7667c347..4242903c1082d 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml @@ -58,6 +58,7 @@ + @@ -70,6 +71,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index ce13a742c6202..1d34dd279b5fa 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -57,6 +57,7 @@ + diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp index 9511f168f346f..0a85f75b74ad7 100644 --- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp @@ -73,7 +73,7 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) "twist_with_covariance", rclcpp::QoS{10}); diagnostics_ = - std::make_unique(this, "gyro_odometer_status"); + std::make_unique(this, "gyro_odometer_status"); // TODO(YamatoAndo) createTimer } diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp index 036b3d7cab527..b59e6af341ec2 100644 --- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp @@ -15,7 +15,7 @@ #ifndef GYRO_ODOMETER_CORE_HPP_ #define GYRO_ODOMETER_CORE_HPP_ -#include "autoware/localization_util/diagnostics_module.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" #include "autoware/universe_utils/ros/transform_listener.hpp" @@ -80,7 +80,7 @@ class GyroOdometerNode : public rclcpp::Node std::deque vehicle_twist_queue_; std::deque gyro_queue_; - std::unique_ptr diagnostics_; + std::unique_ptr diagnostics_; }; } // namespace autoware::gyro_odometer diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp index 2faf2d19168a5..34fc61797231b 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp @@ -124,22 +124,22 @@ LidarMarkerLocalizer::LidarMarkerLocalizer(const rclcpp::NodeOptions & node_opti tf_buffer_ = std::make_shared(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_, this, false); - diagnostics_module_.reset( - new autoware::localization_util::DiagnosticsModule(this, "marker_detection_status")); + diagnostics_interface_.reset( + new autoware::universe_utils::DiagnosticsInterface(this, "marker_detection_status")); } void LidarMarkerLocalizer::initialize_diagnostics() { - diagnostics_module_->clear(); - diagnostics_module_->add_key_value("is_received_map", false); - diagnostics_module_->add_key_value("is_received_self_pose", false); - diagnostics_module_->add_key_value("detect_marker_num", 0); - diagnostics_module_->add_key_value("distance_self_pose_to_nearest_marker", 0.0); - diagnostics_module_->add_key_value( + diagnostics_interface_->clear(); + diagnostics_interface_->add_key_value("is_received_map", false); + diagnostics_interface_->add_key_value("is_received_self_pose", false); + diagnostics_interface_->add_key_value("detect_marker_num", 0); + diagnostics_interface_->add_key_value("distance_self_pose_to_nearest_marker", 0.0); + diagnostics_interface_->add_key_value( "limit_distance_from_self_pose_to_nearest_marker", param_.limit_distance_from_self_pose_to_nearest_marker); - diagnostics_module_->add_key_value("distance_lanelet2_marker_to_detected_marker", 0.0); - diagnostics_module_->add_key_value( + diagnostics_interface_->add_key_value("distance_lanelet2_marker_to_detected_marker", 0.0); + diagnostics_interface_->add_key_value( "limit_distance_from_lanelet2_marker_to_detected_marker", param_.limit_distance_from_self_pose_to_marker); } @@ -176,7 +176,7 @@ void LidarMarkerLocalizer::points_callback(const PointCloud2::ConstSharedPtr & p main_process(points_msg_ptr); - diagnostics_module_->publish(sensor_ros_time); + diagnostics_interface_->publish(sensor_ros_time); } void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & points_msg_ptr) @@ -186,13 +186,13 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin // (1) check if the map have be received const std::vector map_landmarks = landmark_manager_.get_landmarks(); const bool is_received_map = !map_landmarks.empty(); - diagnostics_module_->add_key_value("is_received_map", is_received_map); + diagnostics_interface_->add_key_value("is_received_map", is_received_map); if (!is_received_map) { std::stringstream message; message << "Not receive the landmark information. Please check if the vector map is being " << "published and if the landmark information is correctly specified."; RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_module_->update_level_and_message( + diagnostics_interface_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return; } @@ -202,13 +202,13 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin interpolate_result = ekf_pose_buffer_->interpolate(sensor_ros_time); const bool is_received_self_pose = interpolate_result != std::nullopt; - diagnostics_module_->add_key_value("is_received_self_pose", is_received_self_pose); + diagnostics_interface_->add_key_value("is_received_self_pose", is_received_self_pose); if (!is_received_self_pose) { std::stringstream message; message << "Could not get self_pose. Please check if the self pose is being published and if " << "the timestamp of the self pose is correctly specified"; RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_module_->update_level_and_message( + diagnostics_interface_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return; } @@ -221,7 +221,7 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin detect_landmarks(points_msg_ptr); const bool is_detected_marker = !detected_landmarks.empty(); - diagnostics_module_->add_key_value("detect_marker_num", detected_landmarks.size()); + diagnostics_interface_->add_key_value("detect_marker_num", detected_landmarks.size()); // (4) check distance to the nearest marker const landmark_manager::Landmark nearest_marker = get_nearest_landmark(self_pose, map_landmarks); @@ -230,7 +230,7 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin const double distance_from_self_pose_to_nearest_marker = std::abs(nearest_marker_pose_on_base_link.position.x); - diagnostics_module_->add_key_value( + diagnostics_interface_->add_key_value( "distance_self_pose_to_nearest_marker", distance_from_self_pose_to_nearest_marker); const bool is_exist_marker_within_self_pose = @@ -242,14 +242,14 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin std::stringstream message; message << "Could not detect marker, because the distance from self_pose to nearest_marker " << "is too far (" << distance_from_self_pose_to_nearest_marker << " [m])."; - diagnostics_module_->update_level_and_message( + diagnostics_interface_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::OK, message.str()); } else { std::stringstream message; message << "Could not detect marker, although the distance from self_pose to nearest_marker " << "is near (" << distance_from_self_pose_to_nearest_marker << " [m])."; RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_module_->update_level_and_message( + diagnostics_interface_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } return; @@ -279,13 +279,13 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin const bool is_exist_marker_within_lanelet2_map = diff_norm < param_.limit_distance_from_self_pose_to_marker; - diagnostics_module_->add_key_value("distance_lanelet2_marker_to_detected_marker", diff_norm); + diagnostics_interface_->add_key_value("distance_lanelet2_marker_to_detected_marker", diff_norm); if (!is_exist_marker_within_lanelet2_map) { std::stringstream message; message << "The distance from lanelet2 to the detect marker is too far(" << diff_norm << " [m]). The limit is " << param_.limit_distance_from_self_pose_to_marker << "."; RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_module_->update_level_and_message( + diagnostics_interface_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return; } diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp index d1482c6912592..22a0c3f642563 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp @@ -15,8 +15,8 @@ #ifndef LIDAR_MARKER_LOCALIZER_HPP_ #define LIDAR_MARKER_LOCALIZER_HPP_ -#include "autoware/localization_util/diagnostics_module.hpp" #include "autoware/localization_util/smart_pose_buffer.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include @@ -134,7 +134,7 @@ class LidarMarkerLocalizer : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_debug_pose_with_covariance_; rclcpp::Publisher::SharedPtr pub_marker_pointcloud_; - std::shared_ptr diagnostics_module_; + std::shared_ptr diagnostics_interface_; Param param_; bool is_activated_; diff --git a/localization/autoware_localization_error_monitor/CHANGELOG.rst b/localization/autoware_localization_error_monitor/CHANGELOG.rst index e9583916e2e8f..69833a04a8d3a 100644 --- a/localization/autoware_localization_error_monitor/CHANGELOG.rst +++ b/localization/autoware_localization_error_monitor/CHANGELOG.rst @@ -43,8 +43,8 @@ Changelog for package autoware_localization_error_monitor * unify package.xml version to 0.37.0 * refactor(localization_util)!: prefix package and namespace with autoware (`#8922 `_) add autoware prefix to localization_util -* fix(localization_error_monitor, system diag): fix to use diagnostics_module in localization_util (`#8543 `_) - * fix(localization_error_monitor): fix to use diagnostics_module in localization_util +* fix(localization_error_monitor, system diag): fix to use diagnostics_interface in localization_util (`#8543 `_) + * fix(localization_error_monitor): fix to use diagnostics_interface in localization_util * fix: update media * fix: update component name * fix: rename include file diff --git a/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp b/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp index 63edbe5f6a9c7..1f26f6b80aa17 100644 --- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp +++ b/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp @@ -59,7 +59,7 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o logger_configure_ = std::make_unique(this); diagnostics_error_monitor_ = - std::make_unique(this, "ellipse_error_status"); + std::make_unique(this, "ellipse_error_status"); } void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg) diff --git a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp index 7b26573964b38..b7d2454eb9f75 100644 --- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp +++ b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp @@ -16,7 +16,7 @@ #define LOCALIZATION_ERROR_MONITOR_HPP_ #include "autoware/localization_util/covariance_ellipse.hpp" -#include "autoware/localization_util/diagnostics_module.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include #include @@ -39,7 +39,7 @@ class LocalizationErrorMonitor : public rclcpp::Node std::unique_ptr logger_configure_; - std::unique_ptr diagnostics_error_monitor_; + std::unique_ptr diagnostics_error_monitor_; double scale_; double error_ellipse_size_; diff --git a/localization/autoware_localization_util/CMakeLists.txt b/localization/autoware_localization_util/CMakeLists.txt index dd18f3cbad932..de62633124f3d 100644 --- a/localization/autoware_localization_util/CMakeLists.txt +++ b/localization/autoware_localization_util/CMakeLists.txt @@ -6,7 +6,6 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/util_func.cpp - src/diagnostics_module.cpp src/smart_pose_buffer.cpp src/tree_structured_parzen_estimator.cpp src/covariance_ellipse.cpp diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp index d2dce42e3923e..12990259f88cd 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE__NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ #define AUTOWARE__NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ -#include "autoware/localization_util/diagnostics_module.hpp" #include "autoware/localization_util/util_func.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include "hyper_parameters.hpp" #include "ndt_omp/multigrid_ndt_omp.h" #include "particle.hpp" @@ -42,7 +42,7 @@ namespace autoware::ndt_scan_matcher { -using DiagnosticsModule = autoware::localization_util::DiagnosticsModule; +using DiagnosticsInterface = autoware::universe_utils::DiagnosticsInterface; class MapUpdateModule { @@ -63,19 +63,19 @@ class MapUpdateModule void callback_timer( const bool is_activated, const std::optional & position, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); [[nodiscard]] bool should_update_map( const geometry_msgs::msg::Point & position, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); void update_map( const geometry_msgs::msg::Point & position, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); // Update the specified NDT bool update_ndt( const geometry_msgs::msg::Point & position, NdtType & ndt, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); void publish_partial_pcd_map(); rclcpp::Publisher::SharedPtr loaded_pcd_pub_; diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 22b6bfb2ff740..e083e6751c3db 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -17,8 +17,8 @@ #define FMT_HEADER_ONLY -#include "autoware/localization_util/diagnostics_module.hpp" #include "autoware/localization_util/smart_pose_buffer.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include "hyper_parameters.hpp" #include "map_update_module.hpp" #include "ndt_omp/multigrid_ndt_omp.h" @@ -211,12 +211,12 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr regularization_pose_buffer_; std::atomic is_activated_; - std::unique_ptr diagnostics_scan_points_; - std::unique_ptr diagnostics_initial_pose_; - std::unique_ptr diagnostics_regularization_pose_; - std::unique_ptr diagnostics_map_update_; - std::unique_ptr diagnostics_ndt_align_; - std::unique_ptr diagnostics_trigger_node_; + std::unique_ptr diagnostics_scan_points_; + std::unique_ptr diagnostics_initial_pose_; + std::unique_ptr diagnostics_regularization_pose_; + std::unique_ptr diagnostics_map_update_; + std::unique_ptr diagnostics_ndt_align_; + std::unique_ptr diagnostics_trigger_node_; std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; diff --git a/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp index 25b51b55aebd7..299d596401b19 100644 --- a/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp @@ -56,7 +56,7 @@ MapUpdateModule::MapUpdateModule( void MapUpdateModule::callback_timer( const bool is_activated, const std::optional & position, - std::unique_ptr & diagnostics_ptr) + std::unique_ptr & diagnostics_ptr) { // check is_activated diagnostics_ptr->add_key_value("is_activated", is_activated); @@ -86,7 +86,8 @@ void MapUpdateModule::callback_timer( } bool MapUpdateModule::should_update_map( - const geometry_msgs::msg::Point & position, std::unique_ptr & diagnostics_ptr) + const geometry_msgs::msg::Point & position, + std::unique_ptr & diagnostics_ptr) { last_update_position_mtx_.lock(); @@ -141,7 +142,8 @@ bool MapUpdateModule::out_of_map_range(const geometry_msgs::msg::Point & positio } void MapUpdateModule::update_map( - const geometry_msgs::msg::Point & position, std::unique_ptr & diagnostics_ptr) + const geometry_msgs::msg::Point & position, + std::unique_ptr & diagnostics_ptr) { diagnostics_ptr->add_key_value("is_need_rebuild", need_rebuild_); @@ -229,7 +231,7 @@ void MapUpdateModule::update_map( bool MapUpdateModule::update_ndt( const geometry_msgs::msg::Point & position, NdtType & ndt, - std::unique_ptr & diagnostics_ptr) + std::unique_ptr & diagnostics_ptr) { diagnostics_ptr->add_key_value("maps_size_before", ndt.getCurrentMapIDs().size()); diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index a1871023d525b..d3e1fa5982867 100644 --- a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -49,9 +49,9 @@ using autoware::localization_util::matrix4f_to_pose; using autoware::localization_util::point_to_vector3d; using autoware::localization_util::pose_to_matrix4f; -using autoware::localization_util::DiagnosticsModule; using autoware::localization_util::SmartPoseBuffer; using autoware::localization_util::TreeStructuredParzenEstimator; +using autoware::universe_utils::DiagnosticsInterface; tier4_debug_msgs::msg::Float32Stamped make_float32_stamped( const builtin_interfaces::msg::Time & stamp, const float data) @@ -141,7 +141,7 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) std::make_unique(this->get_logger(), value_as_unlimited, value_as_unlimited); diagnostics_regularization_pose_ = - std::make_unique(this, "regularization_pose_subscriber_status"); + std::make_unique(this, "regularization_pose_subscriber_status"); } sensor_aligned_pose_pub_ = @@ -209,13 +209,13 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, param_.dynamic_map_loading); - diagnostics_scan_points_ = std::make_unique(this, "scan_matching_status"); + diagnostics_scan_points_ = std::make_unique(this, "scan_matching_status"); diagnostics_initial_pose_ = - std::make_unique(this, "initial_pose_subscriber_status"); - diagnostics_map_update_ = std::make_unique(this, "map_update_status"); - diagnostics_ndt_align_ = std::make_unique(this, "ndt_align_service_status"); + std::make_unique(this, "initial_pose_subscriber_status"); + diagnostics_map_update_ = std::make_unique(this, "map_update_status"); + diagnostics_ndt_align_ = std::make_unique(this, "ndt_align_service_status"); diagnostics_trigger_node_ = - std::make_unique(this, "trigger_node_service_status"); + std::make_unique(this, "trigger_node_service_status"); logger_configure_ = std::make_unique(this); } diff --git a/localization/autoware_pose_initializer/package.xml b/localization/autoware_pose_initializer/package.xml index eeeb46a8db3e5..de3ecd880ea45 100644 --- a/localization/autoware_pose_initializer/package.xml +++ b/localization/autoware_pose_initializer/package.xml @@ -21,7 +21,6 @@ autoware_component_interface_specs autoware_component_interface_utils - autoware_localization_util autoware_map_height_fitter autoware_motion_utils autoware_universe_utils diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp index 5e9c68d2acc80..5bde25a564f1d 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp @@ -40,7 +40,7 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) output_pose_covariance_ = get_covariance_parameter(this, "output_pose_covariance"); gnss_particle_covariance_ = get_covariance_parameter(this, "gnss_particle_covariance"); - diagnostics_pose_reliable_ = std::make_unique( + diagnostics_pose_reliable_ = std::make_unique( this, "pose_initializer_status"); if (declare_parameter("ekf_enabled")) { diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp index 28d2eae08c3f1..a0c1ed3a86576 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp @@ -15,10 +15,9 @@ #ifndef POSE_INITIALIZER_CORE_HPP_ #define POSE_INITIALIZER_CORE_HPP_ -#include "autoware/localization_util/diagnostics_module.hpp" - #include #include +#include #include #include @@ -61,7 +60,7 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; std::unique_ptr logger_configure_; - std::unique_ptr diagnostics_pose_reliable_; + std::unique_ptr diagnostics_pose_reliable_; double stop_check_duration_; void change_node_trigger(bool flag, bool need_spin = false); diff --git a/perception/autoware_bytetrack/lib/src/lapjv.cpp b/perception/autoware_bytetrack/lib/src/lapjv.cpp index 1b8b39ccbb9f7..8949c199f9b21 100644 --- a/perception/autoware_bytetrack/lib/src/lapjv.cpp +++ b/perception/autoware_bytetrack/lib/src/lapjv.cpp @@ -44,7 +44,7 @@ /** Column-reduction and reduction transfer for a dense cost matrix. */ -int_t _ccrrt_dense( +int_t ccrrt_dense( const uint_t n, cost_t * cost[], int_t * free_rows, int_t * x, int_t * y, cost_t * v) { int_t n_free_rows; @@ -108,7 +108,7 @@ int_t _ccrrt_dense( /** Augmenting row reduction for a dense cost matrix. */ -int_t _carr_dense( +int_t carr_dense( const uint_t n, cost_t * cost[], const uint_t n_free_rows, int_t * free_rows, int_t * x, int_t * y, cost_t * v) { @@ -181,7 +181,7 @@ int_t _carr_dense( /** Find columns with minimum d[j] and put them on the SCAN list. */ -uint_t _find_dense( +uint_t find_dense( const uint_t n, uint_t lo, const cost_t * d, int_t * cols, [[maybe_unused]] int_t * y) { uint_t hi = lo + 1; @@ -202,7 +202,7 @@ uint_t _find_dense( // Scan all columns in TODO starting from arbitrary column in SCAN // and try to decrease d of the TODO columns using the SCAN column. -int_t _scan_dense( +int_t scan_dense( const uint_t n, cost_t * cost[], uint_t * plo, uint_t * phi, cost_t * d, int_t * cols, int_t * pred, const int_t * y, const cost_t * v) { @@ -267,7 +267,7 @@ int_t find_path_dense( if (lo == hi) { PRINTF("%d..%d -> find\n", lo, hi); n_ready = lo; - hi = _find_dense(n, lo, d, cols, y); + hi = find_dense(n, lo, d, cols, y); PRINTF("check %d..%d\n", lo, hi); PRINT_INDEX_ARRAY(cols, n); for (uint_t k = lo; k < hi; k++) { @@ -279,7 +279,7 @@ int_t find_path_dense( } if (final_j == -1) { PRINTF("%d..%d -> scan\n", lo, hi); - final_j = _scan_dense(n, cost, &lo, &hi, d, cols, pred, y, v); + final_j = scan_dense(n, cost, &lo, &hi, d, cols, pred, y, v); PRINT_COST_ARRAY(d, n); PRINT_INDEX_ARRAY(cols, n); PRINT_INDEX_ARRAY(pred, n); @@ -304,7 +304,7 @@ int_t find_path_dense( /** Augment for a dense cost matrix. */ -int_t _ca_dense( +int_t ca_dense( const uint_t n, cost_t * cost[], const uint_t n_free_rows, int_t * free_rows, int_t * x, int_t * y, cost_t * v) { @@ -348,14 +348,14 @@ int lapjv_internal(const uint_t n, cost_t * cost[], int_t * x, int_t * y) NEW(free_rows, int_t, n); NEW(v, cost_t, n); - ret = _ccrrt_dense(n, cost, free_rows, x, y, v); + ret = ccrrt_dense(n, cost, free_rows, x, y, v); int i = 0; while (ret > 0 && i < 2) { - ret = _carr_dense(n, cost, ret, free_rows, x, y, v); + ret = carr_dense(n, cost, ret, free_rows, x, y, v); i++; } if (ret > 0) { - ret = _ca_dense(n, cost, ret, free_rows, x, y, v); + ret = ca_dense(n, cost, ret, free_rows, x, y, v); } FREE(v); FREE(free_rows); diff --git a/perception/autoware_detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml b/perception/autoware_detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml index cddee782e8af0..37b77eb436c7d 100644 --- a/perception/autoware_detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml +++ b/perception/autoware_detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml @@ -12,5 +12,7 @@ #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN [800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0] + validate_max_distance_m: 70.0 # [m] + using_2d_validator: false enable_debugger: false diff --git a/perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json b/perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json index d7aa970993ca1..95c83a90f075f 100644 --- a/perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json +++ b/perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json @@ -30,6 +30,11 @@ "default": [800, 800, 800, 800, 800, 800, 800, 800], "description": "Threshold value of the number of point clouds per object when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink." }, + "validate_max_distance_m": { + "type": "number", + "default": 70.0, + "description": "The maximum distance from the baselink to the object to be validated" + }, "using_2d_validator": { "type": "boolean", "default": false, diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp index 194f333acd87e..979cdd3909f14 100644 --- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp +++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp @@ -295,6 +295,8 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( declare_parameter>("max_points_num"); points_num_threshold_param_.min_points_and_distance_ratio = declare_parameter>("min_points_and_distance_ratio"); + const double validate_max_distance = declare_parameter("validate_max_distance_m"); + validate_max_distance_sq_ = validate_max_distance * validate_max_distance; using_2d_validator_ = declare_parameter("using_2d_validator"); @@ -346,6 +348,18 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { const auto & transformed_object = transformed_objects.objects.at(i); const auto & object = input_objects->objects.at(i); + // check object distance + const double distance_sq = + transformed_object.kinematics.pose_with_covariance.pose.position.x * + transformed_object.kinematics.pose_with_covariance.pose.position.x + + transformed_object.kinematics.pose_with_covariance.pose.position.y * + transformed_object.kinematics.pose_with_covariance.pose.position.y; + if (distance_sq > validate_max_distance_sq_) { + // pass to output + output.objects.push_back(object); + continue; + } + const auto validated = validation_is_ready ? validator_->validate_object(transformed_object) : false; if (debugger_) { diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp index 6d9a11634ea08..f10dbb7531d36 100644 --- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp +++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp @@ -152,6 +152,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node typedef message_filters::Synchronizer Sync; Sync sync_; PointsNumThresholdParam points_num_threshold_param_; + double validate_max_distance_sq_; // maximum object distance to validate, squared [m^2] std::shared_ptr debugger_; bool using_2d_validator_; diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp index 488de2da47f91..74b4edbe595ea 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp @@ -246,7 +246,7 @@ void ScanGroundFilterComponent::classifyPointCloud( float radius_distance_from_gnd = pd.radius - prev_gnd_radius; float height_from_gnd = point_curr.z - prev_gnd_point.z; float height_from_obj = point_curr.z - non_ground_cluster.getAverageHeight(); - bool calculate_slope = false; + bool calculate_slope = true; bool is_point_close_to_prev = (points_distance < (pd.radius * radial_divider_angle_rad_ + split_points_distance_tolerance_)); @@ -264,8 +264,6 @@ void ScanGroundFilterComponent::classifyPointCloud( // close to the previous point, set point follow label point_label_curr = PointLabel::POINT_FOLLOW; calculate_slope = false; - } else { - calculate_slope = true; } if (is_point_close_to_prev) { height_from_gnd = point_curr.z - ground_cluster.getAverageHeight(); diff --git a/perception/autoware_image_projection_based_fusion/CMakeLists.txt b/perception/autoware_image_projection_based_fusion/CMakeLists.txt index 73d305d2ab2a8..7c8160d6fe1b7 100644 --- a/perception/autoware_image_projection_based_fusion/CMakeLists.txt +++ b/perception/autoware_image_projection_based_fusion/CMakeLists.txt @@ -16,6 +16,7 @@ endif() # Build non-CUDA dependent nodes ament_auto_add_library(${PROJECT_NAME} SHARED + src/camera_projection.cpp src/fusion_node.cpp src/debugger.cpp src/utils/geometry.cpp diff --git a/perception/autoware_image_projection_based_fusion/README.md b/perception/autoware_image_projection_based_fusion/README.md index c976697b0396d..dcf35e45bbd9d 100644 --- a/perception/autoware_image_projection_based_fusion/README.md +++ b/perception/autoware_image_projection_based_fusion/README.md @@ -66,6 +66,12 @@ ros2 launch autoware_image_projection_based_fusion pointpainting_fusion.launch.x The rclcpp::TimerBase timer could not break a for loop, therefore even if time is out when fusing a roi msg at the middle, the program will run until all msgs are fused. +### Approximate camera projection + +For algorithms like `pointpainting_fusion`, the computation required to project points onto an unrectified (raw) image can be substantial. To address this, an option is provided to reduce the computational load. Set the [`approximate_camera_projection parameter`](config/fusion_common.param.yaml) to `true` for each camera (ROIs). If the corresponding `point_project_to_unrectified_image` parameter is also set to true, the projections will be pre-cached. + +The cached projections are calculated using a grid, with the grid size specified by the `approximation_grid_width_size` and `approximation_grid_height_size` parameters in the [configuration file](config/fusion_common.param.yaml). The centers of the grid are used as the projected points. + ### Detail description of each fusion's algorithm is in the following links | Fusion Name | Description | Detail | diff --git a/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml b/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml index 662362c4fd7c5..86db3bad4f8f8 100644 --- a/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml +++ b/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml @@ -4,7 +4,8 @@ timeout_ms: 70.0 match_threshold_ms: 50.0 image_buffer_size: 15 - point_project_to_unrectified_image: false + # projection setting for each ROI whether unrectify image + point_project_to_unrectified_image: [false, false, false, false, false, false] debug_mode: false filter_scope_min_x: -100.0 filter_scope_min_y: -100.0 @@ -12,3 +13,12 @@ filter_scope_max_x: 100.0 filter_scope_max_y: 100.0 filter_scope_max_z: 100.0 + + # camera cache setting for each ROI + approximate_camera_projection: [true, true, true, true, true, true] + # grid size in pixels + approximation_grid_cell_width: 1.0 + approximation_grid_cell_height: 1.0 + + # debug parameters + publish_processing_time_detail: false diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/camera_projection.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/camera_projection.hpp new file mode 100644 index 0000000000000..f9b3158b1672a --- /dev/null +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/camera_projection.hpp @@ -0,0 +1,81 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__CAMERA_PROJECTION_HPP_ +#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__CAMERA_PROJECTION_HPP_ + +#define EIGEN_MPL2_ONLY + +#include +#include + +#include + +#include + +#include + +namespace autoware::image_projection_based_fusion +{ +struct PixelPos +{ + float x; + float y; +}; + +class CameraProjection +{ +public: + explicit CameraProjection( + const sensor_msgs::msg::CameraInfo & camera_info, const float grid_cell_width, + const float grid_cell_height, const bool unrectify, const bool use_approximation); + CameraProjection() : cell_width_(1.0), cell_height_(1.0), unrectify_(false) {} + void initialize(); + std::function calcImageProjectedPoint; + sensor_msgs::msg::CameraInfo getCameraInfo(); + bool isOutsideHorizontalView(const float px, const float pz); + bool isOutsideVerticalView(const float py, const float pz); + bool isOutsideFOV(const float px, const float py, const float pz); + +protected: + bool calcRectifiedImageProjectedPoint( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point); + bool calcRawImageProjectedPoint(const cv::Point3d & point3d, Eigen::Vector2d & projected_point); + bool calcRawImageProjectedPointWithApproximation( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point); + void initializeCache(); + + sensor_msgs::msg::CameraInfo camera_info_; + uint32_t image_height_, image_width_; + double tan_h_x_, tan_h_y_; + double fov_left_, fov_right_, fov_top_, fov_bottom_; + + uint32_t cache_size_; + float cell_width_; + float cell_height_; + float inv_cell_width_; + float inv_cell_height_; + int grid_width_; + int grid_height_; + + bool unrectify_; + bool use_approximation_; + + std::unique_ptr projection_cache_; + image_geometry::PinholeCameraModel camera_model_; +}; + +} // namespace autoware::image_projection_based_fusion + +#endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__CAMERA_PROJECTION_HPP_ diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp index c9b4f1f7b903f..239d406650ce8 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp @@ -15,9 +15,11 @@ #ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ #define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ +#include #include #include #include +#include #include #include @@ -54,6 +56,7 @@ using sensor_msgs::msg::PointCloud2; using tier4_perception_msgs::msg::DetectedObjectsWithFeature; using tier4_perception_msgs::msg::DetectedObjectWithFeature; using PointCloud = pcl::PointCloud; +using autoware::image_projection_based_fusion::CameraProjection; using autoware_perception_msgs::msg::ObjectClassification; template @@ -85,7 +88,7 @@ class FusionNode : public rclcpp::Node virtual void fuseOnSingleImage( const TargetMsg3D & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, TargetMsg3D & output_msg) = 0; + TargetMsg3D & output_msg) = 0; // set args if you need virtual void postprocess(TargetMsg3D & output_msg); @@ -99,11 +102,16 @@ class FusionNode : public rclcpp::Node tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - bool point_project_to_unrectified_image_{false}; + std::vector point_project_to_unrectified_image_; // camera_info std::map camera_info_map_; std::vector::SharedPtr> camera_info_subs_; + // camera projection + std::vector camera_projectors_; + std::vector approx_camera_projection_; + float approx_grid_cell_w_size_; + float approx_grid_cell_h_size_; rclcpp::TimerBase::SharedPtr timer_; double timeout_ms_{}; @@ -142,6 +150,11 @@ class FusionNode : public rclcpp::Node /** \brief processing time publisher. **/ std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; + + // timekeeper + rclcpp::Publisher::SharedPtr + detailed_processing_time_publisher_; + std::shared_ptr time_keeper_; }; } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp index 8c0e2ed78fc6c..cd6cd87976522 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -53,15 +53,12 @@ class PointPaintingFusionNode void fuseOnSingleImage( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const std::size_t image_id, const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override; void postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override; rclcpp::Publisher::SharedPtr obj_pub_ptr_; - std::vector tan_h_; // horizontal field of view - int omp_num_threads_{1}; float score_threshold_{0.0}; std::vector class_names_; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp index 2c0283a190023..903b153af0681 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -38,7 +38,6 @@ class RoiClusterFusionNode void fuseOnSingleImage( const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id, const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg) override; std::string trust_object_iou_mode_{"iou"}; @@ -53,6 +52,7 @@ class RoiClusterFusionNode double fusion_distance_; double trust_object_distance_; std::string non_trust_object_iou_mode_{"iou_x"}; + bool is_far_enough(const DetectedObjectWithFeature & obj, const double distance_threshold); bool out_of_scope(const DetectedObjectWithFeature & obj) override; double cal_iou_by_mode( diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index 7103537ec852d..43ec134168ef9 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -42,13 +42,11 @@ class RoiDetectedObjectFusionNode void fuseOnSingleImage( const DetectedObjects & input_object_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjects & output_object_msg) override; + const DetectedObjectsWithFeature & input_roi_msg, DetectedObjects & output_object_msg) override; std::map generateDetectedObjectRoIs( - const DetectedObjects & input_object_msg, const double image_width, const double image_height, - const Eigen::Affine3d & object2camera_affine, - const image_geometry::PinholeCameraModel & pinhole_camera_model); + const DetectedObjects & input_object_msg, const std::size_t & image_id, + const Eigen::Affine3d & object2camera_affine); void fuseObjectsOnImage( const DetectedObjects & input_object_msg, diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp index 4de49df61a071..2f2c8222e196f 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -19,8 +19,10 @@ #include +#include #include #include + namespace autoware::image_projection_based_fusion { class RoiPointCloudFusionNode @@ -47,8 +49,7 @@ class RoiPointCloudFusionNode void fuseOnSingleImage( const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override; + const DetectedObjectsWithFeature & input_roi_msg, PointCloud2 & output_pointcloud_msg) override; bool out_of_scope(const DetectedObjectWithFeature & obj) override; }; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index b09e9521bdcdb..7454cb7bcd173 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -60,7 +61,7 @@ class SegmentPointCloudFusionNode : public FusionNode getTransformStamped( const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, const std::string & source_frame_id, const rclcpp::Time & time); diff --git a/perception/autoware_image_projection_based_fusion/schema/fusion_common.schema.json b/perception/autoware_image_projection_based_fusion/schema/fusion_common.schema.json index 73ee1661adaea..8077f3e2f3e30 100644 --- a/perception/autoware_image_projection_based_fusion/schema/fusion_common.schema.json +++ b/perception/autoware_image_projection_based_fusion/schema/fusion_common.schema.json @@ -31,6 +31,11 @@ "default": 15, "minimum": 1 }, + "point_project_to_unrectified_image": { + "type": "array", + "description": "An array of options indicating whether to project point to unrectified image or not.", + "default": [false, false, false, false, false, false] + }, "debug_mode": { "type": "boolean", "description": "Whether to debug or not.", @@ -65,6 +70,21 @@ "type": "number", "description": "Maximum z position [m].", "default": 100.0 + }, + "approximate_camera_projection": { + "type": "array", + "description": "An array of options indicating whether to use approximated projection for each camera.", + "default": [true, true, true, true, true, true] + }, + "approximation_grid_cell_width": { + "type": "number", + "description": "The width of grid cell used in approximated projection [pixel].", + "default": 1.0 + }, + "approximation_grid_cell_height": { + "type": "number", + "description": "The height of grid cell used in approximated projection [pixel].", + "default": 1.0 } } } diff --git a/perception/autoware_image_projection_based_fusion/src/camera_projection.cpp b/perception/autoware_image_projection_based_fusion/src/camera_projection.cpp new file mode 100644 index 0000000000000..c0a820609a0a7 --- /dev/null +++ b/perception/autoware_image_projection_based_fusion/src/camera_projection.cpp @@ -0,0 +1,252 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/image_projection_based_fusion/camera_projection.hpp" + +#include + +namespace autoware::image_projection_based_fusion +{ +CameraProjection::CameraProjection( + const sensor_msgs::msg::CameraInfo & camera_info, const float grid_cell_width, + const float grid_cell_height, const bool unrectify, const bool use_approximation = false) +: camera_info_(camera_info), + cell_width_(grid_cell_width), + cell_height_(grid_cell_height), + unrectify_(unrectify), + use_approximation_(use_approximation) +{ + if (grid_cell_width <= 0.0 || grid_cell_height <= 0.0) { + throw std::runtime_error("Both grid_cell_width and grid_cell_height must be > 0.0"); + } + + image_width_ = camera_info.width; + image_height_ = camera_info.height; + + // prepare camera model + camera_model_.fromCameraInfo(camera_info); + + // cache settings + inv_cell_width_ = 1 / cell_width_; + inv_cell_height_ = 1 / cell_height_; + grid_width_ = static_cast(std::ceil(image_width_ / cell_width_)); + grid_height_ = static_cast(std::ceil(image_height_ / cell_height_)); + cache_size_ = grid_width_ * grid_height_; + + // compute 3D rays for the image corners and pixels related to optical center + // to find the actual FOV size + // optical centers + const double ocx = static_cast(camera_info.k.at(2)); + const double ocy = static_cast(camera_info.k.at(5)); + // for checking pincushion shape case + const cv::Point3d ray_top_left = camera_model_.projectPixelTo3dRay(cv::Point(0, 0)); + const cv::Point3d ray_top_right = + camera_model_.projectPixelTo3dRay(cv::Point(image_width_ - 1, 0)); + const cv::Point3d ray_bottom_left = + camera_model_.projectPixelTo3dRay(cv::Point(0, image_height_ - 1)); + const cv::Point3d ray_bottom_right = + camera_model_.projectPixelTo3dRay(cv::Point(image_width_ - 1, image_height_ - 1)); + // for checking barrel shape case + const cv::Point3d ray_mid_top = camera_model_.projectPixelTo3dRay(cv::Point(ocx, 0)); + const cv::Point3d ray_mid_left = camera_model_.projectPixelTo3dRay(cv::Point(0, ocy)); + const cv::Point3d ray_mid_right = + camera_model_.projectPixelTo3dRay(cv::Point(image_width_ - 1, ocy)); + const cv::Point3d ray_mid_bottom = + camera_model_.projectPixelTo3dRay(cv::Point(ocx, image_height_ - 1)); + + cv::Point3d x_left = ray_top_left; + cv::Point3d x_right = ray_top_right; + cv::Point3d y_top = ray_top_left; + cv::Point3d y_bottom = ray_bottom_left; + + // check each side of the view + if (ray_bottom_left.x < x_left.x) x_left = ray_bottom_left; + if (ray_mid_left.x < x_left.x) x_left = ray_mid_left; + + if (x_right.x < ray_bottom_right.x) x_right = ray_bottom_right; + if (x_right.x < ray_mid_right.x) x_right = ray_mid_right; + + if (y_top.y < ray_top_right.y) y_top = ray_top_right; + if (y_top.y < ray_mid_top.y) y_top = ray_mid_top; + + if (ray_bottom_left.y < y_bottom.y) y_bottom = ray_bottom_left; + if (ray_mid_bottom.y < y_bottom.y) y_bottom = ray_mid_bottom; + + // set FOV at z = 1.0 + fov_left_ = x_left.x / x_left.z; + fov_right_ = x_right.x / x_right.z; + fov_top_ = y_top.y / y_top.z; + fov_bottom_ = y_bottom.y / y_bottom.z; +} + +void CameraProjection::initialize() +{ + if (unrectify_) { + if (use_approximation_) { + // create the cache with size of grid center + // store only xy position in float to reduce memory consumption + projection_cache_ = std::make_unique(cache_size_); + initializeCache(); + + calcImageProjectedPoint = [this]( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point) { + return this->calcRawImageProjectedPointWithApproximation(point3d, projected_point); + }; + } else { + calcImageProjectedPoint = [this]( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point) { + return this->calcRawImageProjectedPoint(point3d, projected_point); + }; + } + } else { + calcImageProjectedPoint = [this]( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point) { + return this->calcRectifiedImageProjectedPoint(point3d, projected_point); + }; + } +} + +void CameraProjection::initializeCache() +{ + // sample grid cell centers till the camera height, width to precompute the projections + // + // grid cell size + // / + // v + // |---| w + // 0-----------------> + // 0 | . | . | . | + // |___|___|___| + // | . | . | . | + // | ^ + // h | | + // v grid cell center + // + // each pixel will be rounded in these grid cell centers + // edge pixels in right and bottom side in the image will be assign to these centers + // that is the outside of the image + + for (int idx_y = 0; idx_y < grid_height_; idx_y++) { + for (int idx_x = 0; idx_x < grid_width_; idx_x++) { + const uint32_t grid_index = idx_y * grid_width_ + idx_x; + const float px = (idx_x + 0.5f) * cell_width_; + const float py = (idx_y + 0.5f) * cell_height_; + + // precompute projected point + cv::Point2d raw_image_point = camera_model_.unrectifyPoint(cv::Point2d(px, py)); + projection_cache_[grid_index] = + PixelPos{static_cast(raw_image_point.x), static_cast(raw_image_point.y)}; + } + } +} + +/** + * @brief Calculate a projection of 3D point to rectified image plane 2D point. + * @return Return a boolean indicating whether the projected point is on the image plane. + */ +bool CameraProjection::calcRectifiedImageProjectedPoint( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point) +{ + const cv::Point2d rectified_image_point = camera_model_.project3dToPixel(point3d); + + if ( + rectified_image_point.x < 0.0 || rectified_image_point.x >= image_width_ || + rectified_image_point.y < 0.0 || rectified_image_point.y >= image_height_) { + return false; + } else { + projected_point << rectified_image_point.x, rectified_image_point.y; + return true; + } +} + +/** + * @brief Calculate a projection of 3D point to raw image plane 2D point. + * @return Return a boolean indicating whether the projected point is on the image plane. + */ +bool CameraProjection::calcRawImageProjectedPoint( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point) +{ + const cv::Point2d rectified_image_point = camera_model_.project3dToPixel(point3d); + const cv::Point2d raw_image_point = camera_model_.unrectifyPoint(rectified_image_point); + + if ( + rectified_image_point.x < 0.0 || rectified_image_point.x >= image_width_ || + rectified_image_point.y < 0.0 || rectified_image_point.y >= image_height_) { + return false; + } else { + projected_point << raw_image_point.x, raw_image_point.y; + return true; + } +} + +/** + * @brief Calculate a projection of 3D point to raw image plane 2D point with approximation. + * @return Return a boolean indicating whether the projected point is on the image plane. + */ +bool CameraProjection::calcRawImageProjectedPointWithApproximation( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point) +{ + const cv::Point2d rectified_image_point = camera_model_.project3dToPixel(point3d); + + // round to a near grid cell + const int grid_idx_x = static_cast(std::floor(rectified_image_point.x * inv_cell_width_)); + const int grid_idx_y = static_cast(std::floor(rectified_image_point.y * inv_cell_height_)); + + if (grid_idx_x < 0.0 || grid_idx_x >= grid_width_) return false; + if (grid_idx_y < 0.0 || grid_idx_y >= grid_height_) return false; + + const uint32_t grid_index = grid_idx_y * grid_width_ + grid_idx_x; + projected_point << projection_cache_[grid_index].x, projection_cache_[grid_index].y; + + return true; +} + +sensor_msgs::msg::CameraInfo CameraProjection::getCameraInfo() +{ + return camera_info_; +} + +bool CameraProjection::isOutsideHorizontalView(const float px, const float pz) +{ + // assuming the points' origin is centered on the camera + if (pz <= 0.0) return true; + if (px < fov_left_ * pz) return true; + if (px > fov_right_ * pz) return true; + + // inside of the horizontal view + return false; +} + +bool CameraProjection::isOutsideVerticalView(const float py, const float pz) +{ + // assuming the points' origin is centered on the camera + if (pz <= 0.0) return true; + // up in the camera coordinate is negative + if (py < fov_top_ * pz) return true; + if (py > fov_bottom_ * pz) return true; + + // inside of the vertical view + return false; +} + +bool CameraProjection::isOutsideFOV(const float px, const float py, const float pz) +{ + if (isOutsideHorizontalView(px, pz)) return true; + if (isOutsideVerticalView(py, pz)) return true; + + // inside of the FOV + return false; +} + +} // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index 6c575752b52e5..ea0904215cb86 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -18,6 +18,7 @@ #include #include +#include #include @@ -42,6 +43,7 @@ static double processing_time_ms = 0; namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; template FusionNode::FusionNode( @@ -123,6 +125,34 @@ FusionNode::FusionNode( timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&FusionNode::timer_callback, this)); + // camera projection settings + camera_projectors_.resize(rois_number_); + point_project_to_unrectified_image_ = + declare_parameter>("point_project_to_unrectified_image"); + + if (rois_number_ > point_project_to_unrectified_image_.size()) { + throw std::runtime_error( + "The number of point_project_to_unrectified_image does not match the number of rois topics."); + } + approx_camera_projection_ = declare_parameter>("approximate_camera_projection"); + if (rois_number_ != approx_camera_projection_.size()) { + const std::size_t current_size = approx_camera_projection_.size(); + RCLCPP_WARN( + this->get_logger(), + "The number of elements in approximate_camera_projection should be the same as in " + "rois_number. " + "It has %zu elements.", + current_size); + if (current_size < rois_number_) { + approx_camera_projection_.resize(rois_number_); + for (std::size_t i = current_size; i < rois_number_; i++) { + approx_camera_projection_.at(i) = true; + } + } + } + approx_grid_cell_w_size_ = declare_parameter("approximation_grid_cell_width"); + approx_grid_cell_h_size_ = declare_parameter("approximation_grid_cell_height"); + // debugger if (declare_parameter("debug_mode", false)) { std::size_t image_buffer_size = @@ -130,8 +160,16 @@ FusionNode::FusionNode( debugger_ = std::make_shared(this, rois_number_, image_buffer_size, input_camera_topics_); } - point_project_to_unrectified_image_ = - declare_parameter("point_project_to_unrectified_image"); + + // time keeper + bool use_time_keeper = declare_parameter("publish_processing_time_detail"); + if (use_time_keeper) { + detailed_processing_time_publisher_ = + this->create_publisher( + "~/debug/processing_time_detail_ms", 1); + auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); + time_keeper_ = std::make_shared(time_keeper); + } // initialize debug tool { @@ -156,7 +194,18 @@ void FusionNode::cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id) { - camera_info_map_[camera_id] = *input_camera_info_msg; + // create the CameraProjection when the camera info arrives for the first time + // assuming the camera info does not change while the node is running + if ( + camera_info_map_.find(camera_id) == camera_info_map_.end() && + checkCameraInfo(*input_camera_info_msg)) { + camera_projectors_.at(camera_id) = CameraProjection( + *input_camera_info_msg, approx_grid_cell_w_size_, approx_grid_cell_h_size_, + point_project_to_unrectified_image_.at(camera_id), approx_camera_projection_.at(camera_id)); + camera_projectors_.at(camera_id).initialize(); + + camera_info_map_[camera_id] = *input_camera_info_msg; + } } template @@ -170,6 +219,9 @@ template void FusionNode::subCallback( const typename TargetMsg3D::ConstSharedPtr input_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (cached_msg_.second != nullptr) { stop_watch_ptr_->toc("processing_time", true); timer_->cancel(); @@ -258,8 +310,7 @@ void FusionNode::subCallback( } fuseOnSingleImage( - *input_msg, roi_i, *((cached_roi_msgs_.at(roi_i))[matched_stamp]), - camera_info_map_.at(roi_i), *output_msg); + *input_msg, roi_i, *((cached_roi_msgs_.at(roi_i))[matched_stamp]), *output_msg); (cached_roi_msgs_.at(roi_i)).erase(matched_stamp); is_fused_.at(roi_i) = true; @@ -306,6 +357,9 @@ template void FusionNode::roiCallback( const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + stop_watch_ptr_->toc("processing_time", true); int64_t timestamp_nsec = (*input_roi_msg).header.stamp.sec * static_cast(1e9) + @@ -328,9 +382,7 @@ void FusionNode::roiCallback( debugger_->clear(); } - fuseOnSingleImage( - *(cached_msg_.second), roi_i, *input_roi_msg, camera_info_map_.at(roi_i), - *(cached_msg_.second)); + fuseOnSingleImage(*(cached_msg_.second), roi_i, *input_roi_msg, *(cached_msg_.second)); is_fused_.at(roi_i) = true; if (debug_publisher_) { @@ -378,6 +430,9 @@ void FusionNode::postprocess(TargetMsg3D & output_msg template void FusionNode::timer_callback() { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + using std::chrono_literals::operator""ms; timer_->cancel(); if (mutex_cached_msgs_.try_lock()) { diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp index d4601e26dda46..2837bac458541 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -35,6 +36,7 @@ namespace { +using autoware::universe_utils::ScopedTimeTrack; Eigen::Affine3f _transformToEigen(const geometry_msgs::msg::Transform & t) { @@ -159,13 +161,6 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS().keep_last(3), sub_callback); - tan_h_.resize(rois_number_); - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - auto fx = camera_info_map_[roi_i].k.at(0); - auto x0 = camera_info_map_[roi_i].k.at(2); - tan_h_[roi_i] = x0 / fx; - } - detection_class_remapper_.setParameters( allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); @@ -202,6 +197,9 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); @@ -255,9 +253,7 @@ void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted void PointPaintingFusionNode::fuseOnSingleImage( __attribute__((unused)) const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, - __attribute__((unused)) const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, + const std::size_t image_id, const DetectedObjectsWithFeature & input_roi_msg, sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { @@ -271,7 +267,8 @@ void PointPaintingFusionNode::fuseOnSingleImage( return; } - if (!checkCameraInfo(camera_info)) return; + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); std::vector debug_image_rois; std::vector debug_image_points; @@ -304,9 +301,6 @@ void PointPaintingFusionNode::fuseOnSingleImage( .offset; const auto class_offset = painted_pointcloud_msg.fields.at(4).offset; const auto p_step = painted_pointcloud_msg.point_step; - // projection matrix - image_geometry::PinholeCameraModel pinhole_camera_model; - pinhole_camera_model.fromCameraInfo(camera_info); Eigen::Vector3f point_lidar, point_camera; /** dc : don't care @@ -338,24 +332,27 @@ dc | dc dc dc dc ||zc| p_y = point_camera.y(); p_z = point_camera.z(); - if (p_z <= 0.0 || p_x > (tan_h_.at(image_id) * p_z) || p_x < (-tan_h_.at(image_id) * p_z)) { + if (camera_projectors_[image_id].isOutsideHorizontalView(p_x, p_z)) { continue; } + // project - Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(p_x, p_y, p_z), point_project_to_unrectified_image_); - - // iterate 2d bbox - for (const auto & feature_object : objects) { - sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; - // paint current point if it is inside bbox - int label2d = feature_object.object.classification.front().label; - if (!isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) { - // cppcheck-suppress invalidPointerCast - auto p_class = reinterpret_cast(&output[stride + class_offset]); - for (const auto & cls : isClassTable_) { - // add up the class values if the point belongs to multiple classes - *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; + Eigen::Vector2d projected_point; + if (camera_projectors_[image_id].calcImageProjectedPoint( + cv::Point3d(p_x, p_y, p_z), projected_point)) { + // iterate 2d bbox + for (const auto & feature_object : objects) { + sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; + // paint current point if it is inside bbox + int label2d = feature_object.object.classification.front().label; + if ( + !isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) { + // cppcheck-suppress invalidPointerCast + auto p_class = reinterpret_cast(&output[stride + class_offset]); + for (const auto & cls : isClassTable_) { + // add up the class values if the point belongs to multiple classes + *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; + } } } #if 0 @@ -367,11 +364,15 @@ dc | dc dc dc dc ||zc| } } - for (const auto & feature_object : input_roi_msg.feature_objects) { - debug_image_rois.push_back(feature_object.feature.roi); - } - if (debugger_) { + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = std::make_unique("publish debug message", *time_keeper_); + + for (const auto & feature_object : input_roi_msg.feature_objects) { + debug_image_rois.push_back(feature_object.feature.roi); + } + debugger_->image_rois_ = debug_image_rois; debugger_->obstacle_points_ = debug_image_points; debugger_->publishImage(image_id, input_roi_msg.header.stamp); @@ -380,6 +381,9 @@ dc | dc dc dc dc ||zc| void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + const auto objects_sub_count = obj_pub_ptr_->get_subscription_count() + obj_pub_ptr_->get_intra_process_subscription_count(); if (objects_sub_count < 1) { diff --git a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 9a94613a2b788..32db5319bb487 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -16,12 +16,14 @@ #include #include +#include #include #include #include #include +#include #include #include #include @@ -36,6 +38,7 @@ namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) : FusionNode( @@ -55,6 +58,9 @@ RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + // reset cluster semantic type if (!use_cluster_semantic_type_) { for (auto & feature_object : output_cluster_msg.feature_objects) { @@ -67,6 +73,9 @@ void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluste void RoiClusterFusionNode::postprocess(DetectedObjectsWithFeature & output_cluster_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (!remove_unknown_) { return; } @@ -86,13 +95,12 @@ void RoiClusterFusionNode::postprocess(DetectedObjectsWithFeature & output_clust void RoiClusterFusionNode::fuseOnSingleImage( const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg) + const DetectedObjectsWithFeature & input_roi_msg, DetectedObjectsWithFeature & output_cluster_msg) { - if (!checkCameraInfo(camera_info)) return; + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - image_geometry::PinholeCameraModel pinhole_camera_model; - pinhole_camera_model.fromCameraInfo(camera_info); + const sensor_msgs::msg::CameraInfo & camera_info = camera_projectors_[image_id].getCameraInfo(); // get transform from cluster frame id to camera optical frame id geometry_msgs::msg::TransformStamped transform_stamped; @@ -110,6 +118,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( } std::map m_cluster_roi; + for (std::size_t i = 0; i < input_cluster_msg.feature_objects.size(); ++i) { if (input_cluster_msg.feature_objects.at(i).feature.cluster.data.empty()) { continue; @@ -139,18 +148,17 @@ void RoiClusterFusionNode::fuseOnSingleImage( continue; } - Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(*iter_x, *iter_y, *iter_z), - point_project_to_unrectified_image_); - if ( - 0 <= static_cast(projected_point.x()) && - static_cast(projected_point.x()) <= static_cast(camera_info.width) - 1 && - 0 <= static_cast(projected_point.y()) && - static_cast(projected_point.y()) <= static_cast(camera_info.height) - 1) { - min_x = std::min(static_cast(projected_point.x()), min_x); - min_y = std::min(static_cast(projected_point.y()), min_y); - max_x = std::max(static_cast(projected_point.x()), max_x); - max_y = std::max(static_cast(projected_point.y()), max_y); + Eigen::Vector2d projected_point; + if (camera_projectors_[image_id].calcImageProjectedPoint( + cv::Point3d(*iter_x, *iter_y, *iter_z), projected_point)) { + const int px = static_cast(projected_point.x()); + const int py = static_cast(projected_point.y()); + + min_x = std::min(px, min_x); + min_y = std::min(py, min_y); + max_x = std::max(px, max_x); + max_y = std::max(py, max_y); + projected_points.push_back(projected_point); if (debugger_) debugger_->obstacle_points_.push_back(projected_point); } diff --git a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 3ef3af8168f53..7be18d59fdbf1 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -18,14 +18,17 @@ #include #include +#include #include #include +#include #include #include namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) : FusionNode( @@ -49,6 +52,9 @@ RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptio void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + std::vector passthrough_object_flags, fused_object_flags, ignored_object_flags; passthrough_object_flags.resize(output_msg.objects.size()); fused_object_flags.resize(output_msg.objects.size()); @@ -78,10 +84,10 @@ void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) void RoiDetectedObjectFusionNode::fuseOnSingleImage( const DetectedObjects & input_object_msg, const std::size_t image_id, const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjects & output_object_msg __attribute__((unused))) { - if (!checkCameraInfo(camera_info)) return; + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); Eigen::Affine3d object2camera_affine; { @@ -94,12 +100,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( object2camera_affine = transformToEigen(transform_stamped_optional.value().transform); } - image_geometry::PinholeCameraModel pinhole_camera_model; - pinhole_camera_model.fromCameraInfo(camera_info); - - const auto object_roi_map = generateDetectedObjectRoIs( - input_object_msg, static_cast(camera_info.width), - static_cast(camera_info.height), object2camera_affine, pinhole_camera_model); + const auto object_roi_map = + generateDetectedObjectRoIs(input_object_msg, image_id, object2camera_affine); fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map); if (debugger_) { @@ -113,10 +115,12 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( std::map RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( - const DetectedObjects & input_object_msg, const double image_width, const double image_height, - const Eigen::Affine3d & object2camera_affine, - const image_geometry::PinholeCameraModel & pinhole_camera_model) + const DetectedObjects & input_object_msg, const std::size_t & image_id, + const Eigen::Affine3d & object2camera_affine) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + std::map object_roi_map; int64_t timestamp_nsec = input_object_msg.header.stamp.sec * static_cast(1e9) + input_object_msg.header.stamp.nanosec; @@ -127,6 +131,10 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( return object_roi_map; } const auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); + const sensor_msgs::msg::CameraInfo & camera_info = camera_projectors_[image_id].getCameraInfo(); + const double image_width = static_cast(camera_info.width); + const double image_height = static_cast(camera_info.height); + for (std::size_t obj_i = 0; obj_i < input_object_msg.objects.size(); ++obj_i) { std::vector vertices_camera_coord; const auto & object = input_object_msg.objects.at(obj_i); @@ -153,18 +161,17 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( continue; } - Eigen::Vector2d proj_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(point.x(), point.y(), point.z()), - point_project_to_unrectified_image_); + Eigen::Vector2d proj_point; + if (camera_projectors_[image_id].calcImageProjectedPoint( + cv::Point3d(point.x(), point.y(), point.z()), proj_point)) { + const double px = proj_point.x(); + const double py = proj_point.y(); - min_x = std::min(proj_point.x(), min_x); - min_y = std::min(proj_point.y(), min_y); - max_x = std::max(proj_point.x(), max_x); - max_y = std::max(proj_point.y(), max_y); + min_x = std::min(px, min_x); + min_y = std::min(py, min_y); + max_x = std::max(px, max_x); + max_y = std::max(py, max_y); - if ( - proj_point.x() >= 0 && proj_point.x() < image_width && proj_point.y() >= 0 && - proj_point.y() < image_height) { point_on_image_cnt++; if (debugger_) { @@ -202,6 +209,9 @@ void RoiDetectedObjectFusionNode::fuseObjectsOnImage( const std::vector & image_rois, const std::map & object_roi_map) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + int64_t timestamp_nsec = input_object_msg.header.stamp.sec * static_cast(1e9) + input_object_msg.header.stamp.nanosec; if (fused_object_flags_map_.size() == 0 || ignored_object_flags_map_.size() == 0) { diff --git a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index f9eb4ef909282..998072d87774e 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -17,6 +17,9 @@ #include "autoware/image_projection_based_fusion/utils/geometry.hpp" #include "autoware/image_projection_based_fusion/utils/utils.hpp" +#include + +#include #include #ifdef ROS_DISTRO_GALACTIC @@ -31,6 +34,8 @@ namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; + RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) : FusionNode( "roi_pointcloud_fusion", options) @@ -47,12 +52,18 @@ RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & opt void RoiPointCloudFusionNode::preprocess( __attribute__((unused)) sensor_msgs::msg::PointCloud2 & pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + return; } void RoiPointCloudFusionNode::postprocess( __attribute__((unused)) sensor_msgs::msg::PointCloud2 & pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + const auto objects_sub_count = pub_objects_ptr_->get_subscription_count() + pub_objects_ptr_->get_intra_process_subscription_count(); if (objects_sub_count < 1) { @@ -75,16 +86,16 @@ void RoiPointCloudFusionNode::postprocess( } } void RoiPointCloudFusionNode::fuseOnSingleImage( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, - __attribute__((unused)) const std::size_t image_id, + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const std::size_t image_id, const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, __attribute__((unused)) sensor_msgs::msg::PointCloud2 & output_pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (input_pointcloud_msg.data.empty()) { return; } - if (!checkCameraInfo(camera_info)) return; std::vector output_objs; std::vector debug_image_rois; @@ -108,10 +119,6 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( return; } - // transform pointcloud to camera optical frame id - image_geometry::PinholeCameraModel pinhole_camera_model; - pinhole_camera_model.fromCameraInfo(camera_info); - geometry_msgs::msg::TransformStamped transform_stamped; { const auto transform_stamped_optional = getTransformStamped( @@ -122,10 +129,13 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( } transform_stamped = transform_stamped_optional.value(); } - int point_step = input_pointcloud_msg.point_step; - int x_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "x")].offset; - int y_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "y")].offset; - int z_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "z")].offset; + const int point_step = input_pointcloud_msg.point_step; + const int x_offset = + input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "x")].offset; + const int y_offset = + input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "y")].offset; + const int z_offset = + input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "z")].offset; sensor_msgs::msg::PointCloud2 transformed_cloud; tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); @@ -150,33 +160,36 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( if (transformed_z <= 0.0) { continue; } - Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z), - point_project_to_unrectified_image_); - for (std::size_t i = 0; i < output_objs.size(); ++i) { - auto & feature_obj = output_objs.at(i); - const auto & check_roi = feature_obj.feature.roi; - auto & cluster = clusters.at(i); - - if ( - clusters_data_size.at(i) >= - static_cast(max_cluster_size_) * static_cast(point_step)) { - continue; - } - if ( - check_roi.x_offset <= projected_point.x() && check_roi.y_offset <= projected_point.y() && - check_roi.x_offset + check_roi.width >= projected_point.x() && - check_roi.y_offset + check_roi.height >= projected_point.y()) { - std::memcpy( - &cluster.data[clusters_data_size.at(i)], &input_pointcloud_msg.data[offset], point_step); - clusters_data_size.at(i) += point_step; + + Eigen::Vector2d projected_point; + if (camera_projectors_[image_id].calcImageProjectedPoint( + cv::Point3d(transformed_x, transformed_y, transformed_z), projected_point)) { + for (std::size_t i = 0; i < output_objs.size(); ++i) { + auto & feature_obj = output_objs.at(i); + const auto & check_roi = feature_obj.feature.roi; + auto & cluster = clusters.at(i); + + const double px = projected_point.x(); + const double py = projected_point.y(); + + if ( + clusters_data_size.at(i) >= + static_cast(max_cluster_size_) * static_cast(point_step)) { + continue; + } + if ( + check_roi.x_offset <= px && check_roi.y_offset <= py && + check_roi.x_offset + check_roi.width >= px && + check_roi.y_offset + check_roi.height >= py) { + std::memcpy( + &cluster.data[clusters_data_size.at(i)], &input_pointcloud_msg.data[offset], + point_step); + clusters_data_size.at(i) += point_step; + } } - } - if (debugger_) { - // add all points inside image to debug - if ( - projected_point.x() > 0 && projected_point.x() < camera_info.width && - projected_point.y() > 0 && projected_point.y() < camera_info.height) { + + if (debugger_) { + // add all points inside image to debug debug_image_points.push_back(projected_point); } } diff --git a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index e678a956bc64e..486ae291abc6a 100644 --- a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -17,6 +17,7 @@ #include "autoware/image_projection_based_fusion/utils/geometry.hpp" #include "autoware/image_projection_based_fusion/utils/utils.hpp" +#include #include #include @@ -32,6 +33,8 @@ namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; + SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options) : FusionNode("segmentation_pointcloud_fusion", options) { @@ -49,11 +52,17 @@ SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptio void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + return; } void SegmentPointCloudFusionNode::postprocess(PointCloud2 & pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + auto original_cloud = std::make_shared(pointcloud_msg); int point_step = original_cloud->point_step; @@ -78,17 +87,20 @@ void SegmentPointCloudFusionNode::postprocess(PointCloud2 & pointcloud_msg) } void SegmentPointCloudFusionNode::fuseOnSingleImage( - const PointCloud2 & input_pointcloud_msg, __attribute__((unused)) const std::size_t image_id, - [[maybe_unused]] const Image & input_mask, __attribute__((unused)) const CameraInfo & camera_info, - __attribute__((unused)) PointCloud2 & output_cloud) + const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, + [[maybe_unused]] const Image & input_mask, __attribute__((unused)) PointCloud2 & output_cloud) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (input_pointcloud_msg.data.empty()) { return; } - if (!checkCameraInfo(camera_info)) return; if (input_mask.height == 0 || input_mask.width == 0) { return; } + + const sensor_msgs::msg::CameraInfo & camera_info = camera_projectors_[image_id].getCameraInfo(); std::vector mask_data(input_mask.data.begin(), input_mask.data.end()); cv::Mat mask = perception_utils::runLengthDecoder(mask_data, input_mask.height, input_mask.width); @@ -103,8 +115,6 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( const int orig_height = camera_info.height; // resize mask to the same size as the camera image cv::resize(mask, mask, cv::Size(orig_width, orig_height), 0, 0, cv::INTER_NEAREST); - image_geometry::PinholeCameraModel pinhole_camera_model; - pinhole_camera_model.fromCameraInfo(camera_info); geometry_msgs::msg::TransformStamped transform_stamped; // transform pointcloud from frame id to camera optical frame id @@ -139,13 +149,9 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( continue; } - Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z), - point_project_to_unrectified_image_); - - bool is_inside_image = projected_point.x() > 0 && projected_point.x() < camera_info.width && - projected_point.y() > 0 && projected_point.y() < camera_info.height; - if (!is_inside_image) { + Eigen::Vector2d projected_point; + if (!camera_projectors_[image_id].calcImageProjectedPoint( + cv::Point3d(transformed_x, transformed_y, transformed_z), projected_point)) { continue; } diff --git a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp index 338a5605e5a79..4456d25b18167 100644 --- a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp @@ -44,20 +44,6 @@ bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info) return true; } -Eigen::Vector2d calcRawImageProjectedPoint( - const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d, - const bool & unrectify) -{ - const cv::Point2d rectified_image_point = pinhole_camera_model.project3dToPixel(point3d); - - if (!unrectify) { - return Eigen::Vector2d(rectified_image_point.x, rectified_image_point.y); - } - const cv::Point2d raw_image_point = pinhole_camera_model.unrectifyPoint(rectified_image_point); - - return Eigen::Vector2d(raw_image_point.x, raw_image_point.y); -} - std::optional getTransformStamped( const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, const std::string & source_frame_id, const rclcpp::Time & time) diff --git a/perception/autoware_multi_object_tracker/CMakeLists.txt b/perception/autoware_multi_object_tracker/CMakeLists.txt index 298b6605192a5..fcea50235bf0d 100644 --- a/perception/autoware_multi_object_tracker/CMakeLists.txt +++ b/perception/autoware_multi_object_tracker/CMakeLists.txt @@ -30,6 +30,8 @@ set(${PROJECT_NAME}_src set(${PROJECT_NAME}_lib lib/association/association.cpp lib/association/mu_successive_shortest_path/mu_ssp.cpp + lib/object_model/types.cpp + lib/object_model/shapes.cpp lib/tracker/motion_model/motion_model_base.cpp lib/tracker/motion_model/bicycle_motion_model.cpp # cspell: ignore ctrv diff --git a/perception/autoware_multi_object_tracker/README.md b/perception/autoware_multi_object_tracker/README.md index afb1598645733..3f83817f6509f 100644 --- a/perception/autoware_multi_object_tracker/README.md +++ b/perception/autoware_multi_object_tracker/README.md @@ -69,7 +69,12 @@ Multiple inputs are pre-defined in the input channel parameters (described below ### Core Parameters +- Node + {{ json_to_markdown("perception/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json") }} + +- Association + {{ json_to_markdown("perception/autoware_multi_object_tracker/schema/data_association_matrix.schema.json") }} #### Simulation parameters diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp index 2c12341d0aa67..b92e17987eb5f 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp @@ -27,7 +27,7 @@ #include #include -#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include #include #include @@ -58,7 +58,7 @@ class DataAssociation const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, std::unordered_map & reverse_assignment); Eigen::MatrixXd calcScoreMatrix( - const autoware_perception_msgs::msg::DetectedObjects & measurements, + const types::DynamicObjectList & measurements, const std::list> & trackers); virtual ~DataAssociation() {} }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp new file mode 100644 index 0000000000000..d399f356136fc --- /dev/null +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp @@ -0,0 +1,57 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee + +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_ + +#include "autoware/multi_object_tracker/object_model/types.hpp" + +#include + +#include + +#include + +namespace autoware::multi_object_tracker +{ +namespace shapes +{ +bool transformObjects( + const types::DynamicObjectList & input_msg, const std::string & target_frame_id, + const tf2_ros::Buffer & tf_buffer, types::DynamicObjectList & output_msg); + +double get2dIoU( + const types::DynamicObject & source_object, const types::DynamicObject & target_object, + const double min_union_area = 0.01); + +bool convertConvexHullToBoundingBox( + const types::DynamicObject & input_object, types::DynamicObject & output_object); + +bool getMeasurementYaw( + const types::DynamicObject & object, const double & predicted_yaw, double & measurement_yaw); + +int getNearestCornerOrSurface( + const double x, const double y, const double yaw, const double width, const double length, + const geometry_msgs::msg::Transform & self_transform); + +void calcAnchorPointOffset( + const double w, const double l, const int indx, const types::DynamicObject & input_object, + const double & yaw, types::DynamicObject & offset_object, Eigen::Vector2d & tracking_offset); +} // namespace shapes +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_ diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp new file mode 100644 index 0000000000000..7dab840aac481 --- /dev/null +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp @@ -0,0 +1,88 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee + +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace autoware::multi_object_tracker +{ +namespace types +{ +enum OrientationAvailability : uint8_t { + UNAVAILABLE = 0, + SIGN_UNKNOWN = 1, + AVAILABLE = 2, +}; + +struct ObjectKinematics +{ + geometry_msgs::msg::PoseWithCovariance pose_with_covariance; + geometry_msgs::msg::TwistWithCovariance twist_with_covariance; + bool has_position_covariance = false; + OrientationAvailability orientation_availability; + bool has_twist = false; + bool has_twist_covariance = false; +}; + +struct DynamicObject +{ + unique_identifier_msgs::msg::UUID object_id = unique_identifier_msgs::msg::UUID(); + uint channel_index; + float existence_probability; + std::vector classification; + ObjectKinematics kinematics; + autoware_perception_msgs::msg::Shape shape; +}; + +struct DynamicObjectList +{ + std_msgs::msg::Header header; + uint channel_index; + std::vector objects; +}; + +DynamicObject toDynamicObject( + const autoware_perception_msgs::msg::DetectedObject & det_object, const uint channel_index = 0); + +DynamicObjectList toDynamicObjectList( + const autoware_perception_msgs::msg::DetectedObjects & det_objects, const uint channel_index = 0); + +autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObject & dyn_object); + +} // namespace types + +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_ diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 8501c68b0cf23..ad3667eb240c0 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -19,18 +19,20 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include + namespace autoware::multi_object_tracker { class BicycleTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; + types::DynamicObject object_; rclcpp::Logger logger_; object_model::ObjectModel object_model_ = object_model::bicycle; @@ -50,23 +52,19 @@ class BicycleTracker : public Tracker public: BicycleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool measureWithPose(const types::DynamicObject & object); + bool measureWithShape(const types::DynamicObject & object); + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; private: - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, + types::DynamicObject getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform) const; }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index 8b4fa1babf652..b9e026bf3c009 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -19,10 +19,11 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp" +#include #include namespace autoware::multi_object_tracker @@ -36,17 +37,13 @@ class MultipleVehicleTracker : public Tracker public: MultipleVehicleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; virtual ~MultipleVehicleTracker() {} }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp index 45cd0f31a4e85..6230ba6e3b0f4 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -19,32 +19,30 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "tracker_base.hpp" +#include + namespace autoware::multi_object_tracker { class PassThroughTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; - autoware_perception_msgs::msg::DetectedObject prev_observed_object_; + types::DynamicObject object_; + types::DynamicObject prev_observed_object_; rclcpp::Logger logger_; rclcpp::Time last_update_time_; public: PassThroughTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index 4287e0f99d5ee..8a4bfc59d37ac 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -19,11 +19,13 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" #include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include + namespace autoware::multi_object_tracker { @@ -35,17 +37,13 @@ class PedestrianAndBicycleTracker : public Tracker public: PedestrianAndBicycleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; virtual ~PedestrianAndBicycleTracker() {} }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index f20b38f73e95f..5a2acc50a8249 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -19,18 +19,20 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" +#include + namespace autoware::multi_object_tracker { class PedestrianTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; + types::DynamicObject object_; rclcpp::Logger logger_; object_model::ObjectModel object_model_ = object_model::pedestrian; @@ -56,23 +58,19 @@ class PedestrianTracker : public Tracker public: PedestrianTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool measureWithPose(const types::DynamicObject & object); + bool measureWithShape(const types::DynamicObject & object); + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; private: - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, + types::DynamicObject getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform) const; }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index 0b9ea9c44a6cf..ac5527fca6400 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp @@ -20,14 +20,14 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ #define EIGEN_MPL2_ONLY -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include +#include #include -#include "autoware_perception_msgs/msg/detected_object.hpp" -#include "autoware_perception_msgs/msg/tracked_object.hpp" +#include +#include #include #include @@ -67,9 +67,8 @@ class Tracker return existence_vector.size() > 0; } bool updateWithMeasurement( - const autoware_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform, - const uint & channel_index); + const types::DynamicObject & object, const rclcpp::Time & measurement_time, + const geometry_msgs::msg::Transform & self_transform); bool updateWithoutMeasurement(const rclcpp::Time & now); // classification @@ -108,12 +107,11 @@ class Tracker protected: virtual bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) = 0; public: - virtual bool getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const = 0; + virtual bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const = 0; virtual bool predict(const rclcpp::Time & time) = 0; }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp index 9f128c864ad6c..075db6b8a9d69 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -19,17 +19,19 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" +#include + namespace autoware::multi_object_tracker { class UnknownTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; + types::DynamicObject object_; rclcpp::Logger logger_; struct EkfParams @@ -47,22 +49,17 @@ class UnknownTracker : public Tracker public: UnknownTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + types::DynamicObject getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform); + bool measureWithPose(const types::DynamicObject & object); + bool measureWithShape(const types::DynamicObject & object); + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp index f3708fd1ff905..f50d117acc081 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp @@ -19,11 +19,13 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__VEHICLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__VEHICLE_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include + namespace autoware::multi_object_tracker { @@ -35,7 +37,7 @@ class VehicleTracker : public Tracker double velocity_deviation_threshold_; - autoware_perception_msgs::msg::DetectedObject object_; + types::DynamicObject object_; double z_; struct BoundingBox @@ -53,24 +55,19 @@ class VehicleTracker : public Tracker public: VehicleTracker( const object_model::ObjectModel & object_model, const rclcpp::Time & time, - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool measureWithPose(const types::DynamicObject & object); + bool measureWithShape(const types::DynamicObject & object); + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; private: - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + types::DynamicObject getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform); }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp index 12bd865795b85..44ad012a5f428 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp @@ -19,6 +19,7 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__UNCERTAINTY__UNCERTAINTY_PROCESSOR_HPP_ #include "autoware/multi_object_tracker/object_model/object_model.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include @@ -32,21 +33,19 @@ namespace uncertainty { using autoware::multi_object_tracker::object_model::ObjectModel; -using autoware_perception_msgs::msg::DetectedObject; -using autoware_perception_msgs::msg::DetectedObjects; using autoware_perception_msgs::msg::ObjectClassification; using nav_msgs::msg::Odometry; ObjectModel decodeObjectModel(const ObjectClassification & object_class); -DetectedObjects modelUncertainty(const DetectedObjects & detected_objects); +types::DynamicObjectList modelUncertainty(const types::DynamicObjectList & detected_objects); object_model::StateCovariance covarianceFromObjectClass( - const DetectedObject & detected_object, const ObjectClassification & object_class); + const types::DynamicObject & detected_object, const ObjectClassification & object_class); -void normalizeUncertainty(DetectedObjects & detected_objects); +void normalizeUncertainty(types::DynamicObjectList & detected_objects); -void addOdometryUncertainty(const Odometry & odometry, DetectedObjects & detected_objects); +void addOdometryUncertainty(const Odometry & odometry, types::DynamicObjectList & detected_objects); } // namespace uncertainty } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/association/association.cpp b/perception/autoware_multi_object_tracker/lib/association/association.cpp index 0c809ee5f086d..d74d87489f8db 100644 --- a/perception/autoware_multi_object_tracker/lib/association/association.cpp +++ b/perception/autoware_multi_object_tracker/lib/association/association.cpp @@ -15,8 +15,10 @@ #include "autoware/multi_object_tracker/association/association.hpp" #include "autoware/multi_object_tracker/association/solver/gnn_solver.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/multi_object_tracker/object_model/shapes.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" + +#include #include #include @@ -150,7 +152,7 @@ void DataAssociation::assign( } Eigen::MatrixXd DataAssociation::calcScoreMatrix( - const autoware_perception_msgs::msg::DetectedObjects & measurements, + const types::DynamicObjectList & measurements, const std::list> & trackers) { Eigen::MatrixXd score_matrix = @@ -162,14 +164,13 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( for (size_t measurement_idx = 0; measurement_idx < measurements.objects.size(); ++measurement_idx) { - const autoware_perception_msgs::msg::DetectedObject & measurement_object = - measurements.objects.at(measurement_idx); + const types::DynamicObject & measurement_object = measurements.objects.at(measurement_idx); const std::uint8_t measurement_label = autoware::object_recognition_utils::getHighestProbLabel(measurement_object.classification); double score = 0.0; if (can_assign_matrix_(tracker_label, measurement_label)) { - autoware_perception_msgs::msg::TrackedObject tracked_object; + types::DynamicObject tracked_object; (*tracker_itr)->getTrackedObject(measurements.header.stamp, tracked_object); const double max_dist = max_dist_matrix_(tracker_label, measurement_label); @@ -210,8 +211,8 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double min_iou = min_iou_matrix_(tracker_label, measurement_label); const double min_union_iou_area = 1e-2; - const double iou = autoware::object_recognition_utils::get2dIoU( - measurement_object, tracked_object, min_union_iou_area); + const double iou = + shapes::get2dIoU(measurement_object, tracked_object, min_union_iou_area); if (iou < min_iou) passed_gate = false; } diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp similarity index 59% rename from perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp rename to perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp index 833f768e171f4..2ce23e5df814e 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,31 +13,198 @@ // limitations under the License. // // -// Author: v1.0 Yukihiro Saito -// +// Author: v1.0 Yukihiro Saito, Taekjin Lee -#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ -#define AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#include "autoware/multi_object_tracker/object_model/shapes.hpp" -#include #include +#include +#include + +#include +#include -#include "autoware_perception_msgs/msg/detected_object.hpp" -#include "autoware_perception_msgs/msg/shape.hpp" -#include "autoware_perception_msgs/msg/tracked_object.hpp" -#include -#include -#include +#include #include +#include #include -#include -#include +#include #include -namespace utils +namespace autoware::multi_object_tracker +{ +namespace shapes +{ +inline boost::optional getTransform( + const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, + const std::string & target_frame_id, const rclcpp::Time & time) +{ + try { + geometry_msgs::msg::TransformStamped self_transform_stamped; + self_transform_stamped = tf_buffer.lookupTransform( + target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + return self_transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("multi_object_tracker"), ex.what()); + return boost::none; + } +} + +bool transformObjects( + const types::DynamicObjectList & input_msg, const std::string & target_frame_id, + const tf2_ros::Buffer & tf_buffer, types::DynamicObjectList & output_msg) +{ + output_msg = input_msg; + + // transform to world coordinate + if (input_msg.header.frame_id != target_frame_id) { + output_msg.header.frame_id = target_frame_id; + tf2::Transform tf_target2objects_world; + tf2::Transform tf_target2objects; + tf2::Transform tf_objects_world2objects; + { + const auto ros_target2objects_world = + getTransform(tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + if (!ros_target2objects_world) { + return false; + } + tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); + } + for (auto & object : output_msg.objects) { + auto & pose_with_cov = object.kinematics.pose_with_covariance; + tf2::fromMsg(pose_with_cov.pose, tf_objects_world2objects); + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; + // transform pose, frame difference and object pose + tf2::toMsg(tf_target2objects, pose_with_cov.pose); + // transform covariance, only the frame difference + pose_with_cov.covariance = + tf2::transformCovariance(pose_with_cov.covariance, tf_target2objects_world); + } + } + return true; +} + +inline double getSumArea(const std::vector & polygons) +{ + return std::accumulate( + polygons.begin(), polygons.end(), 0.0, [](double acc, autoware::universe_utils::Polygon2d p) { + return acc + boost::geometry::area(p); + }); +} + +inline double getIntersectionArea( + const autoware::universe_utils::Polygon2d & source_polygon, + const autoware::universe_utils::Polygon2d & target_polygon) +{ + std::vector intersection_polygons; + boost::geometry::intersection(source_polygon, target_polygon, intersection_polygons); + return getSumArea(intersection_polygons); +} + +inline double getUnionArea( + const autoware::universe_utils::Polygon2d & source_polygon, + const autoware::universe_utils::Polygon2d & target_polygon) +{ + std::vector union_polygons; + boost::geometry::union_(source_polygon, target_polygon, union_polygons); + return getSumArea(union_polygons); +} + +double get2dIoU( + const types::DynamicObject & source_object, const types::DynamicObject & target_object, + const double min_union_area) { + static const double MIN_AREA = 1e-6; + + const auto source_polygon = autoware::universe_utils::toPolygon2d( + source_object.kinematics.pose_with_covariance.pose, source_object.shape); + if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; + const auto target_polygon = autoware::universe_utils::toPolygon2d( + target_object.kinematics.pose_with_covariance.pose, target_object.shape); + if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; + + const double intersection_area = getIntersectionArea(source_polygon, target_polygon); + if (intersection_area < MIN_AREA) return 0.0; + const double union_area = getUnionArea(source_polygon, target_polygon); + + const double iou = + union_area < min_union_area ? 0.0 : std::min(1.0, intersection_area / union_area); + return iou; +} + +/** + * @brief convert convex hull shape object to bounding box object + * @param input_object: input convex hull objects + * @param output_object: output bounding box objects + */ +bool convertConvexHullToBoundingBox( + const types::DynamicObject & input_object, types::DynamicObject & output_object) +{ + // check footprint size + if (input_object.shape.footprint.points.size() < 3) { + return false; + } + + // look for bounding box boundary + float max_x = 0; + float max_y = 0; + float min_x = 0; + float min_y = 0; + float max_z = 0; + for (const auto & point : input_object.shape.footprint.points) { + max_x = std::max(max_x, point.x); + max_y = std::max(max_y, point.y); + min_x = std::min(min_x, point.x); + min_y = std::min(min_y, point.y); + max_z = std::max(max_z, point.z); + } + + // calc new center + const Eigen::Vector2d center{ + input_object.kinematics.pose_with_covariance.pose.position.x, + input_object.kinematics.pose_with_covariance.pose.position.y}; + const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation); + const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); + const Eigen::Vector2d new_local_center{(max_x + min_x) / 2.0, (max_y + min_y) / 2.0}; + const Eigen::Vector2d new_center = center + R_inv.transpose() * new_local_center; + + // set output parameters + output_object = input_object; + output_object.kinematics.pose_with_covariance.pose.position.x = new_center.x(); + output_object.kinematics.pose_with_covariance.pose.position.y = new_center.y(); + + output_object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + output_object.shape.dimensions.x = max_x - min_x; + output_object.shape.dimensions.y = max_y - min_y; + output_object.shape.dimensions.z = max_z; + + return true; +} + +bool getMeasurementYaw( + const types::DynamicObject & object, const double & predicted_yaw, double & measurement_yaw) +{ + measurement_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + + // check orientation sign is known or not, and fix the limiting delta yaw + double limiting_delta_yaw = M_PI_2; + if (object.kinematics.orientation_availability == types::OrientationAvailability::AVAILABLE) { + limiting_delta_yaw = M_PI; + } + // limiting delta yaw, even the availability is unknown + while (std::fabs(predicted_yaw - measurement_yaw) > limiting_delta_yaw) { + if (measurement_yaw < predicted_yaw) { + measurement_yaw += 2 * limiting_delta_yaw; + } else { + measurement_yaw -= 2 * limiting_delta_yaw; + } + } + // return false if the orientation is unknown + return object.kinematics.orientation_availability != types::OrientationAvailability::UNAVAILABLE; +} + enum BBOX_IDX { FRONT_SURFACE = 0, RIGHT_SURFACE = 1, @@ -51,17 +218,6 @@ enum BBOX_IDX { INVALID = -1 }; -/** - * @brief check if object label belongs to "large vehicle" - * @param label: input object label - * @return True if object label means large vehicle - */ -inline bool isLargeVehicleLabel(const uint8_t label) -{ - using Label = autoware_perception_msgs::msg::ObjectClassification; - return label == Label::BUS || label == Label::TRUCK || label == Label::TRAILER; -} - /** * @brief Determine the Nearest Corner or Surface of detected object observed from ego vehicle * @@ -73,7 +229,7 @@ inline bool isLargeVehicleLabel(const uint8_t label) * @param self_transform: Ego vehicle position in map frame * @return int index */ -inline int getNearestCornerOrSurface( +int getNearestCornerOrSurface( const double x, const double y, const double yaw, const double width, const double length, const geometry_msgs::msg::Transform & self_transform) { @@ -171,10 +327,9 @@ inline Eigen::Vector2d calcOffsetVectorFromShapeChange( * @param offset_object: output tracking measurement to feed ekf * @return nearest corner index(int) */ -inline void calcAnchorPointOffset( - const double w, const double l, const int indx, - const autoware_perception_msgs::msg::DetectedObject & input_object, const double & yaw, - autoware_perception_msgs::msg::DetectedObject & offset_object, Eigen::Vector2d & tracking_offset) +void calcAnchorPointOffset( + const double w, const double l, const int indx, const types::DynamicObject & input_object, + const double & yaw, types::DynamicObject & offset_object, Eigen::Vector2d & tracking_offset) { // copy value offset_object = input_object; @@ -198,82 +353,6 @@ inline void calcAnchorPointOffset( offset_object.kinematics.pose_with_covariance.pose.position.y += rotated_offset.y(); } -/** - * @brief convert convex hull shape object to bounding box object - * @param input_object: input convex hull objects - * @param output_object: output bounding box objects - */ -inline bool convertConvexHullToBoundingBox( - const autoware_perception_msgs::msg::DetectedObject & input_object, - autoware_perception_msgs::msg::DetectedObject & output_object) -{ - // check footprint size - if (input_object.shape.footprint.points.size() < 3) { - return false; - } - - // look for bounding box boundary - float max_x = 0; - float max_y = 0; - float min_x = 0; - float min_y = 0; - float max_z = 0; - for (const auto & point : input_object.shape.footprint.points) { - max_x = std::max(max_x, point.x); - max_y = std::max(max_y, point.y); - min_x = std::min(min_x, point.x); - min_y = std::min(min_y, point.y); - max_z = std::max(max_z, point.z); - } - - // calc new center - const Eigen::Vector2d center{ - input_object.kinematics.pose_with_covariance.pose.position.x, - input_object.kinematics.pose_with_covariance.pose.position.y}; - const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation); - const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); - const Eigen::Vector2d new_local_center{(max_x + min_x) / 2.0, (max_y + min_y) / 2.0}; - const Eigen::Vector2d new_center = center + R_inv.transpose() * new_local_center; - - // set output parameters - output_object = input_object; - output_object.kinematics.pose_with_covariance.pose.position.x = new_center.x(); - output_object.kinematics.pose_with_covariance.pose.position.y = new_center.y(); - - output_object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; - output_object.shape.dimensions.x = max_x - min_x; - output_object.shape.dimensions.y = max_y - min_y; - output_object.shape.dimensions.z = max_z; - - return true; -} - -inline bool getMeasurementYaw( - const autoware_perception_msgs::msg::DetectedObject & object, const double & predicted_yaw, - double & measurement_yaw) -{ - measurement_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - - // check orientation sign is known or not, and fix the limiting delta yaw - double limiting_delta_yaw = M_PI_2; - if ( - object.kinematics.orientation_availability == - autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { - limiting_delta_yaw = M_PI; - } - // limiting delta yaw, even the availability is unknown - while (std::fabs(predicted_yaw - measurement_yaw) > limiting_delta_yaw) { - if (measurement_yaw < predicted_yaw) { - measurement_yaw += 2 * limiting_delta_yaw; - } else { - measurement_yaw -= 2 * limiting_delta_yaw; - } - } - // return false if the orientation is unknown - return object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; -} - -} // namespace utils +} // namespace shapes -#endif // AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +} // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/object_model/types.cpp b/perception/autoware_multi_object_tracker/lib/object_model/types.cpp new file mode 100644 index 0000000000000..671d5313d0ff8 --- /dev/null +++ b/perception/autoware_multi_object_tracker/lib/object_model/types.cpp @@ -0,0 +1,101 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee + +#include "autoware/multi_object_tracker/object_model/types.hpp" + +#include + +namespace autoware::multi_object_tracker +{ + +namespace types +{ + +DynamicObject toDynamicObject( + const autoware_perception_msgs::msg::DetectedObject & det_object, const uint channel_index) +{ + DynamicObject dynamic_object; + dynamic_object.channel_index = channel_index; + dynamic_object.existence_probability = det_object.existence_probability; + dynamic_object.classification = det_object.classification; + + dynamic_object.kinematics.pose_with_covariance = det_object.kinematics.pose_with_covariance; + dynamic_object.kinematics.twist_with_covariance = det_object.kinematics.twist_with_covariance; + dynamic_object.kinematics.has_position_covariance = det_object.kinematics.has_position_covariance; + if ( + det_object.kinematics.orientation_availability == + autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE) { + dynamic_object.kinematics.orientation_availability = OrientationAvailability::UNAVAILABLE; + } else if ( + det_object.kinematics.orientation_availability == + autoware_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) { + dynamic_object.kinematics.orientation_availability = OrientationAvailability::SIGN_UNKNOWN; + } else if ( + det_object.kinematics.orientation_availability == + autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { + dynamic_object.kinematics.orientation_availability = OrientationAvailability::AVAILABLE; + } + dynamic_object.kinematics.has_twist = det_object.kinematics.has_twist; + dynamic_object.kinematics.has_twist_covariance = det_object.kinematics.has_twist_covariance; + + dynamic_object.shape = det_object.shape; + + return dynamic_object; +} + +DynamicObjectList toDynamicObjectList( + const autoware_perception_msgs::msg::DetectedObjects & det_objects, const uint channel_index) +{ + DynamicObjectList dynamic_objects; + dynamic_objects.header = det_objects.header; + dynamic_objects.channel_index = channel_index; + dynamic_objects.objects.reserve(det_objects.objects.size()); + for (const auto & det_object : det_objects.objects) { + dynamic_objects.objects.emplace_back(toDynamicObject(det_object, channel_index)); + } + return dynamic_objects; +} + +autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObject & dyn_object) +{ + autoware_perception_msgs::msg::TrackedObject tracked_object; + tracked_object.object_id = dyn_object.object_id; + tracked_object.existence_probability = dyn_object.existence_probability; + tracked_object.classification = dyn_object.classification; + + tracked_object.kinematics.pose_with_covariance = dyn_object.kinematics.pose_with_covariance; + tracked_object.kinematics.twist_with_covariance = dyn_object.kinematics.twist_with_covariance; + if (dyn_object.kinematics.orientation_availability == OrientationAvailability::UNAVAILABLE) { + tracked_object.kinematics.orientation_availability = + autoware_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE; + } else if ( + dyn_object.kinematics.orientation_availability == OrientationAvailability::SIGN_UNKNOWN) { + tracked_object.kinematics.orientation_availability = + autoware_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; + } else if (dyn_object.kinematics.orientation_availability == OrientationAvailability::AVAILABLE) { + tracked_object.kinematics.orientation_availability = + autoware_perception_msgs::msg::TrackedObjectKinematics::AVAILABLE; + } + tracked_object.kinematics.is_stationary = false; + + tracked_object.shape = dyn_object.shape; + + return tracked_object; +} +} // namespace types + +} // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index 098ad39dd3df9..e0380a7c33e77 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -19,15 +19,15 @@ #include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "autoware/multi_object_tracker/object_model/shapes.hpp" #include #include +#include +#include +#include +#include +#include #include #include @@ -46,9 +46,7 @@ namespace autoware::multi_object_tracker using Label = autoware_perception_msgs::msg::ObjectClassification; BicycleTracker::BicycleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, - const uint & channel_index) + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("BicycleTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) @@ -56,7 +54,7 @@ BicycleTracker::BicycleTracker( object_ = object; // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); // OBJECT SHAPE MODEL if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -148,16 +146,16 @@ bool BicycleTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, +types::DynamicObject BicycleTracker::getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) const { - autoware_perception_msgs::msg::DetectedObject updating_object = object; + types::DynamicObject updating_object = object; // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - if (!utils::convertConvexHullToBoundingBox(object, updating_object)) { + if (!shapes::convertConvexHullToBoundingBox(object, updating_object)) { updating_object = object; } } @@ -165,12 +163,12 @@ autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( return updating_object; } -bool BicycleTracker::measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object) +bool BicycleTracker::measureWithPose(const types::DynamicObject & object) { // get measurement yaw angle to update const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); double measurement_yaw = 0.0; - bool is_yaw_available = utils::getMeasurementYaw(object, tracked_yaw, measurement_yaw); + bool is_yaw_available = shapes::getMeasurementYaw(object, tracked_yaw, measurement_yaw); // update bool is_updated = false; @@ -196,7 +194,7 @@ bool BicycleTracker::measureWithPose(const autoware_perception_msgs::msg::Detect return is_updated; } -bool BicycleTracker::measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object) +bool BicycleTracker::measureWithShape(const types::DynamicObject & object) { if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { // do not update shape if the input is not a bounding box @@ -208,11 +206,8 @@ bool BicycleTracker::measureWithShape(const autoware_perception_msgs::msg::Detec constexpr double size_min = 0.1; // [m] if ( object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max || - object.shape.dimensions.z > size_max) { - return false; - } else if ( - object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min || - object.shape.dimensions.z < size_min) { + object.shape.dimensions.z > size_max || object.shape.dimensions.x < size_min || + object.shape.dimensions.y < size_min || object.shape.dimensions.z < size_min) { return false; } @@ -238,7 +233,7 @@ bool BicycleTracker::measureWithShape(const autoware_perception_msgs::msg::Detec } bool BicycleTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { // keep the latest input object @@ -263,8 +258,7 @@ bool BicycleTracker::measure( } // update object - const autoware_perception_msgs::msg::DetectedObject updating_object = - getUpdatingObject(object, self_transform); + const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); measureWithPose(updating_object); measureWithShape(updating_object); @@ -272,9 +266,9 @@ bool BicycleTracker::measure( } bool BicycleTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { - object = autoware::object_recognition_utils::toTrackedObject(object_); + object = object_; object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp index ff06544bd509c..9f249ab3bc7bc 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp @@ -26,17 +26,13 @@ namespace autoware::multi_object_tracker using Label = autoware_perception_msgs::msg::ObjectClassification; MultipleVehicleTracker::MultipleVehicleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index) + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), - normal_vehicle_tracker_( - object_model::normal_vehicle, time, object, self_transform, channel_size, channel_index), - big_vehicle_tracker_( - object_model::big_vehicle, time, object, self_transform, channel_size, channel_index) + normal_vehicle_tracker_(object_model::normal_vehicle, time, object, channel_size), + big_vehicle_tracker_(object_model::big_vehicle, time, object, channel_size) { // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); } bool MultipleVehicleTracker::predict(const rclcpp::Time & time) @@ -47,7 +43,7 @@ bool MultipleVehicleTracker::predict(const rclcpp::Time & time) } bool MultipleVehicleTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { big_vehicle_tracker_.measure(object, time, self_transform); @@ -60,14 +56,14 @@ bool MultipleVehicleTracker::measure( } bool MultipleVehicleTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { using Label = autoware_perception_msgs::msg::ObjectClassification; const uint8_t label = getHighestProbLabel(); if (label == Label::CAR) { normal_vehicle_tracker_.getTrackedObject(time, object); - } else if (utils::isLargeVehicleLabel(label)) { + } else if (label == Label::BUS || label == Label::TRUCK || label == Label::TRAILER) { big_vehicle_tracker_.getTrackedObject(time, object); } object.object_id = getUUID(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp index f66e616241ae0..45b3b067e2848 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp @@ -18,12 +18,10 @@ #define EIGEN_MPL2_ONLY #include "autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" - #include #include +#include +#include #include #include @@ -40,9 +38,7 @@ namespace autoware::multi_object_tracker { PassThroughTracker::PassThroughTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, - const uint & channel_index) + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("PassThroughTracker")), last_update_time_(time) @@ -51,7 +47,7 @@ PassThroughTracker::PassThroughTracker( prev_observed_object_ = object; // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); } bool PassThroughTracker::predict(const rclcpp::Time & time) @@ -66,7 +62,7 @@ bool PassThroughTracker::predict(const rclcpp::Time & time) } bool PassThroughTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { prev_observed_object_ = object_; @@ -88,10 +84,10 @@ bool PassThroughTracker::measure( } bool PassThroughTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - object = autoware::object_recognition_utils::toTrackedObject(object_); + object = object_; object.object_id = getUUID(); object.classification = getClassification(); object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X] = 0.0; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp index 1b8018351f5a5..21ce949231062 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -24,15 +24,13 @@ namespace autoware::multi_object_tracker using Label = autoware_perception_msgs::msg::ObjectClassification; PedestrianAndBicycleTracker::PedestrianAndBicycleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index) + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), - pedestrian_tracker_(time, object, self_transform, channel_size, channel_index), - bicycle_tracker_(time, object, self_transform, channel_size, channel_index) + pedestrian_tracker_(time, object, channel_size), + bicycle_tracker_(time, object, channel_size) { // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); } bool PedestrianAndBicycleTracker::predict(const rclcpp::Time & time) @@ -43,7 +41,7 @@ bool PedestrianAndBicycleTracker::predict(const rclcpp::Time & time) } bool PedestrianAndBicycleTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { pedestrian_tracker_.measure(object, time, self_transform); @@ -56,7 +54,7 @@ bool PedestrianAndBicycleTracker::measure( } bool PedestrianAndBicycleTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { using Label = autoware_perception_msgs::msg::ObjectClassification; const uint8_t label = getHighestProbLabel(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index 2135514df8485..bc53689739439 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -19,15 +19,13 @@ #include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" - #include #include +#include +#include +#include +#include +#include #include #include @@ -46,9 +44,7 @@ namespace autoware::multi_object_tracker using Label = autoware_perception_msgs::msg::ObjectClassification; PedestrianTracker::PedestrianTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, - const uint & channel_index) + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("PedestrianTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) @@ -56,7 +52,7 @@ PedestrianTracker::PedestrianTracker( object_ = object; // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); // OBJECT SHAPE MODEL bounding_box_ = { @@ -148,17 +144,16 @@ bool PedestrianTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, +types::DynamicObject PedestrianTracker::getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) const { - autoware_perception_msgs::msg::DetectedObject updating_object = object; + types::DynamicObject updating_object = object; return updating_object; } -bool PedestrianTracker::measureWithPose( - const autoware_perception_msgs::msg::DetectedObject & object) +bool PedestrianTracker::measureWithPose(const types::DynamicObject & object) { // update motion model bool is_updated = false; @@ -178,8 +173,7 @@ bool PedestrianTracker::measureWithPose( return is_updated; } -bool PedestrianTracker::measureWithShape( - const autoware_perception_msgs::msg::DetectedObject & object) +bool PedestrianTracker::measureWithShape(const types::DynamicObject & object) { constexpr double gain = 0.1; constexpr double gain_inv = 1.0 - gain; @@ -235,7 +229,7 @@ bool PedestrianTracker::measureWithShape( } bool PedestrianTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { // keep the latest input object @@ -259,8 +253,7 @@ bool PedestrianTracker::measure( } // update object - const autoware_perception_msgs::msg::DetectedObject updating_object = - getUpdatingObject(object, self_transform); + const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); measureWithPose(updating_object); measureWithShape(updating_object); @@ -269,9 +262,9 @@ bool PedestrianTracker::measure( } bool PedestrianTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { - object = autoware::object_recognition_utils::toTrackedObject(object_); + object = object_; object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp index 24e2b0c9f5acf..31ad1bf94cadd 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp @@ -16,8 +16,6 @@ #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" - #include #include #include @@ -81,9 +79,8 @@ void Tracker::initializeExistenceProbabilities( } bool Tracker::updateWithMeasurement( - const autoware_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform, - const uint & channel_index) + const types::DynamicObject & object, const rclcpp::Time & measurement_time, + const geometry_msgs::msg::Transform & self_transform) { // Update existence probability { @@ -97,6 +94,7 @@ bool Tracker::updateWithMeasurement( constexpr float probability_false_detection = 0.2; // update measured channel probability without decay + const uint channel_index = object.channel_index; existence_probabilities_[channel_index] = updateProbability( existence_probabilities_[channel_index], probability_true_detection, probability_false_detection); @@ -202,7 +200,7 @@ void Tracker::updateClassification( geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance( const rclcpp::Time & time) const { - autoware_perception_msgs::msg::TrackedObject object; + types::DynamicObject object; getTrackedObject(time, object); return object.kinematics.pose_with_covariance; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index 2805e43b88323..ed01165ed8176 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -15,14 +15,13 @@ #include "autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" - #include #include +#include +#include +#include +#include +#include #include #include @@ -34,15 +33,12 @@ #else #include #endif -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" namespace autoware::multi_object_tracker { UnknownTracker::UnknownTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, - const uint & channel_index) + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("UnknownTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) @@ -50,7 +46,7 @@ UnknownTracker::UnknownTracker( object_ = object; // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); // initialize params // measurement noise covariance @@ -142,11 +138,10 @@ bool UnknownTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -autoware_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) +types::DynamicObject UnknownTracker::getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) { - autoware_perception_msgs::msg::DetectedObject updating_object = object; + types::DynamicObject updating_object = object; // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { @@ -169,7 +164,7 @@ autoware_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( return updating_object; } -bool UnknownTracker::measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object) +bool UnknownTracker::measureWithPose(const types::DynamicObject & object) { // update motion model bool is_updated = false; @@ -190,7 +185,7 @@ bool UnknownTracker::measureWithPose(const autoware_perception_msgs::msg::Detect } bool UnknownTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { // keep the latest input object @@ -207,17 +202,16 @@ bool UnknownTracker::measure( } // update object - const autoware_perception_msgs::msg::DetectedObject updating_object = - getUpdatingObject(object, self_transform); + const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); measureWithPose(updating_object); return true; } bool UnknownTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { - object = autoware::object_recognition_utils::toTrackedObject(object_); + object = object_; object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index 2d1f48a3ad5ee..749640ce4324a 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -19,15 +19,15 @@ #include "autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "autoware/multi_object_tracker/object_model/shapes.hpp" #include #include +#include +#include +#include +#include +#include #include #include @@ -47,9 +47,7 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; VehicleTracker::VehicleTracker( const object_model::ObjectModel & object_model, const rclcpp::Time & time, - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, - const uint & channel_index) + const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), object_model_(object_model), logger_(rclcpp::get_logger("VehicleTracker")), @@ -59,7 +57,7 @@ VehicleTracker::VehicleTracker( object_ = object; // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); // velocity deviation threshold // if the predicted velocity is close to the observed velocity, @@ -71,8 +69,8 @@ VehicleTracker::VehicleTracker( bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; } else { - autoware_perception_msgs::msg::DetectedObject bbox_object; - if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { + types::DynamicObject bbox_object; + if (!shapes::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( logger_, "VehicleTracker::VehicleTracker: Failed to convert convex hull to bounding " @@ -167,17 +165,16 @@ bool VehicleTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -autoware_perception_msgs::msg::DetectedObject VehicleTracker::getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) +types::DynamicObject VehicleTracker::getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform) { - autoware_perception_msgs::msg::DetectedObject updating_object = object; + types::DynamicObject updating_object = object; // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape - autoware_perception_msgs::msg::DetectedObject bbox_object = object; + types::DynamicObject bbox_object = object; if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { + if (!shapes::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( logger_, "VehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box."); @@ -191,16 +188,16 @@ autoware_perception_msgs::msg::DetectedObject VehicleTracker::getUpdatingObject( const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); // get offset measurement - const int nearest_corner_index = utils::getNearestCornerOrSurface( + const int nearest_corner_index = shapes::getNearestCornerOrSurface( tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); - utils::calcAnchorPointOffset( + shapes::calcAnchorPointOffset( bounding_box_.width, bounding_box_.length, nearest_corner_index, bbox_object, tracked_yaw, updating_object, tracking_offset_); return updating_object; } -bool VehicleTracker::measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object) +bool VehicleTracker::measureWithPose(const types::DynamicObject & object) { // current (predicted) state const double tracked_vel = motion_model_.getStateElement(IDX::VEL); @@ -242,7 +239,7 @@ bool VehicleTracker::measureWithPose(const autoware_perception_msgs::msg::Detect return is_updated; } -bool VehicleTracker::measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object) +bool VehicleTracker::measureWithShape(const types::DynamicObject & object) { if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { // do not update shape if the input is not a bounding box @@ -295,7 +292,7 @@ bool VehicleTracker::measureWithShape(const autoware_perception_msgs::msg::Detec } bool VehicleTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { // keep the latest input object @@ -320,8 +317,7 @@ bool VehicleTracker::measure( } // update object - const autoware_perception_msgs::msg::DetectedObject updating_object = - getUpdatingObject(object, self_transform); + const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); measureWithPose(updating_object); measureWithShape(updating_object); @@ -329,9 +325,9 @@ bool VehicleTracker::measure( } bool VehicleTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { - object = autoware::object_recognition_utils::toTrackedObject(object_); + object = object_; object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index 89258835f920b..1e71bf8548b33 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -20,13 +20,12 @@ #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include +#include +#include +#include #include @@ -315,7 +314,7 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c double w = vel * sin_slip / lr_; const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); const double w_dtdt = w * dt * dt; - const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + const double vv_dtdt_lr = vel * vel * dt * dt / lr_; // Predict state vector X t+1 Eigen::MatrixXd X_next_t(DIM, 1); // predicted state @@ -332,11 +331,11 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; A(IDX::X, IDX::SLIP) = - -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt_lr; A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; A(IDX::Y, IDX::SLIP) = - vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt_lr; A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp index 700800e94ecd5..e10fbca073047 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp @@ -20,13 +20,12 @@ #include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include +#include +#include +#include namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp index e139419197d79..fd3b03da398e1 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp @@ -20,13 +20,12 @@ #include "autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include +#include +#include +#include #include diff --git a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp index ddfdc7ef7cb10..36d48d35f74b7 100644 --- a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp +++ b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp @@ -55,10 +55,10 @@ object_model::StateCovariance covarianceFromObjectClass(const ObjectClassificati return obj_class_model.measurement_covariance; } -DetectedObject modelUncertaintyByClass( - const DetectedObject & object, const ObjectClassification & object_class) +types::DynamicObject modelUncertaintyByClass( + const types::DynamicObject & object, const ObjectClassification & object_class) { - DetectedObject updating_object = object; + types::DynamicObject updating_object = object; // measurement noise covariance const object_model::StateCovariance measurement_covariance = @@ -87,8 +87,7 @@ DetectedObject modelUncertaintyByClass( pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y pose_cov[XYZRPY_COV_IDX::YAW_YAW] = measurement_covariance.yaw; // yaw - yaw const bool is_yaw_available = - object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + object.kinematics.orientation_availability != types::OrientationAvailability::UNAVAILABLE; if (!is_yaw_available) { pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value } @@ -103,10 +102,11 @@ DetectedObject modelUncertaintyByClass( return updating_object; } -DetectedObjects modelUncertainty(const DetectedObjects & detected_objects) +types::DynamicObjectList modelUncertainty(const types::DynamicObjectList & detected_objects) { - DetectedObjects updating_objects; + types::DynamicObjectList updating_objects; updating_objects.header = detected_objects.header; + updating_objects.channel_index = detected_objects.channel_index; for (const auto & object : detected_objects.objects) { if (object.kinematics.has_position_covariance) { updating_objects.objects.push_back(object); @@ -119,7 +119,7 @@ DetectedObjects modelUncertainty(const DetectedObjects & detected_objects) return updating_objects; } -void normalizeUncertainty(DetectedObjects & detected_objects) +void normalizeUncertainty(types::DynamicObjectList & detected_objects) { constexpr double min_cov_dist = 1e-4; constexpr double min_cov_rad = 1e-6; @@ -140,7 +140,7 @@ void normalizeUncertainty(DetectedObjects & detected_objects) } } -void addOdometryUncertainty(const Odometry & odometry, DetectedObjects & detected_objects) +void addOdometryUncertainty(const Odometry & odometry, types::DynamicObjectList & detected_objects) { const auto & odom_pose = odometry.pose.pose; const auto & odom_pose_cov = odometry.pose.covariance; diff --git a/perception/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json b/perception/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json index f32db284c1738..d3bca273da803 100644 --- a/perception/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json +++ b/perception/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json @@ -86,38 +86,45 @@ "properties": { "UNKNOWN": { "type": "number", + "description": "Number of measurements to consider tracker as confident for unknown object classes.", "default": 3 }, "CAR": { "type": "number", + "description": "Number of measurements to consider tracker as confident for car object classes.", "default": 3 }, "TRUCK": { "type": "number", + "description": "Number of measurements to consider tracker as confident for truck object classes.", "default": 3 }, "BUS": { "type": "number", + "description": "Number of measurements to consider tracker as confident for bus object classes.", "default": 3 }, "TRAILER": { "type": "number", + "description": "Number of measurements to consider tracker as confident for trailer object classes.", "default": 3 }, "MOTORBIKE": { "type": "number", + "description": "Number of measurements to consider tracker as confident for motorbike object classes.", "default": 3 }, "BICYCLE": { "type": "number", + "description": "Number of measurements to consider tracker as confident for bicycle object classes.", "default": 3 }, "PEDESTRIAN": { "type": "number", + "description": "Number of measurements to consider tracker as confident for pedestrian object classes.", "default": 3 } - }, - "description": "Number of measurements to consider tracker as confident for different object classes." + } }, "publish_processing_time": { "type": "boolean", diff --git a/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp index bc27525d85f55..6a1703e029920 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp @@ -80,8 +80,7 @@ void TrackerObjectDebugger::reset() void TrackerObjectDebugger::collect( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const types::DynamicObjectList & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & /*reverse_assignment*/) { @@ -94,13 +93,13 @@ void TrackerObjectDebugger::collect( ++tracker_itr, ++tracker_idx) { ObjectData object_data; object_data.time = message_time; - object_data.channel_id = channel_index; - autoware_perception_msgs::msg::TrackedObject tracked_object; + types::DynamicObject tracked_object; (*(tracker_itr))->getTrackedObject(message_time, tracked_object); object_data.uuid = uuidToBoostUuid(tracked_object.object_id); object_data.uuid_int = uuidToInt(object_data.uuid); object_data.uuid_str = uuidToString(tracked_object.object_id); + object_data.channel_id = tracked_object.channel_index; // tracker bool is_associated = false; diff --git a/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp index e564afc9fd9d0..d507a0350bbb8 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp @@ -88,8 +88,7 @@ class TrackerObjectDebugger } void collect( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const types::DynamicObjectList & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment); diff --git a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp index 9d830bb9e5b81..3eebb3c11ee3e 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp @@ -185,15 +185,13 @@ void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Tim void TrackerDebugger::collectObjectInfo( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const types::DynamicObjectList & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment) { if (!debug_settings_.publish_debug_markers) return; object_debugger_.collect( - message_time, list_tracker, channel_index, detected_objects, direct_assignment, - reverse_assignment); + message_time, list_tracker, detected_objects, direct_assignment, reverse_assignment); } // ObjectDebugger diff --git a/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp index 2c05da0c999e2..3df901a63505c 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp @@ -101,8 +101,7 @@ class TrackerDebugger } void collectObjectInfo( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const types::DynamicObjectList & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment); void publishObjectsMarkers(); diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index d2d2d3e087c4e..333b301ce9fcf 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -17,8 +17,8 @@ #include "multi_object_tracker_node.hpp" +#include "autoware/multi_object_tracker/object_model/shapes.hpp" #include "autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" #include #include @@ -95,7 +95,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) get_parameter("selected_input_channels").as_string_array(); // ROS interface - Publisher - tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); + tracked_objects_pub_ = + create_publisher("output", rclcpp::QoS{1}); // ROS interface - Input channels // Get input channels @@ -239,18 +240,18 @@ void MultiObjectTracker::onTrigger() // process start last_updated_time_ = current_time; - const rclcpp::Time latest_time(objects_list.back().second.header.stamp); + const rclcpp::Time latest_time(objects_list.back().header.stamp); debugger_->startMeasurementTime(this->now(), latest_time); - // run process for each DetectedObjects + // run process for each DynamicObject for (const auto & objects_data : objects_list) { - runProcess(objects_data.second, objects_data.first); + runProcess(objects_data); } // process end debugger_->endMeasurementTime(this->now()); // Publish without delay compensation if (!publish_timer_) { - const auto latest_object_time = rclcpp::Time(objects_list.back().second.header.stamp); + const auto latest_object_time = rclcpp::Time(objects_list.back().header.stamp); checkAndPublish(latest_object_time); } } @@ -278,8 +279,7 @@ void MultiObjectTracker::onTimer() if (should_publish) checkAndPublish(current_time); } -void MultiObjectTracker::runProcess( - const DetectedObjects & input_objects, const uint & channel_index) +void MultiObjectTracker::runProcess(const types::DynamicObjectList & input_objects) { // Get the time of the measurement const rclcpp::Time measurement_time = @@ -293,9 +293,8 @@ void MultiObjectTracker::runProcess( } // Transform the objects to the world frame - DetectedObjects transformed_objects; - if (!autoware::object_recognition_utils::transformObjects( - input_objects, world_frame_id_, tf_buffer_, transformed_objects)) { + types::DynamicObjectList transformed_objects; + if (!shapes::transformObjects(input_objects, world_frame_id_, tf_buffer_, transformed_objects)) { return; } @@ -350,19 +349,19 @@ void MultiObjectTracker::runProcess( // Collect debug information - tracker list, existence probabilities, association results debugger_->collectObjectInfo( - measurement_time, processor_->getListTracker(), channel_index, transformed_objects, - direct_assignment, reverse_assignment); + measurement_time, processor_->getListTracker(), transformed_objects, direct_assignment, + reverse_assignment); } /* tracker update */ - processor_->update(transformed_objects, *self_transform, direct_assignment, channel_index); + processor_->update(transformed_objects, *self_transform, direct_assignment); /* tracker pruning */ processor_->prune(measurement_time); /* spawn new tracker */ - if (input_manager_->isChannelSpawnEnabled(channel_index)) { - processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index); + if (input_manager_->isChannelSpawnEnabled(input_objects.channel_index)) { + processor_->spawn(transformed_objects, reverse_assignment); } } @@ -387,7 +386,7 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const return; } // Create output msg - TrackedObjects output_msg, tentative_objects_msg; + autoware_perception_msgs::msg::TrackedObjects output_msg; output_msg.header.frame_id = world_frame_id_; processor_->getTrackedObjects(time, output_msg); @@ -399,7 +398,7 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const debugger_->endPublishTime(this->now(), time); if (debugger_->shouldPublishTentativeObjects()) { - TrackedObjects tentative_output_msg; + autoware_perception_msgs::msg::TrackedObjects tentative_output_msg; tentative_output_msg.header.frame_id = world_frame_id_; processor_->getTentativeObjects(time, tentative_output_msg); debugger_->publishTentativeObjects(tentative_output_msg); diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp index 79a8c1b98dcca..0472d67617f7f 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp @@ -20,6 +20,7 @@ #define MULTI_OBJECT_TRACKER_NODE_HPP_ #include "autoware/multi_object_tracker/association/association.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "debugger/debugger.hpp" #include "processor/input_manager.hpp" @@ -55,10 +56,6 @@ namespace autoware::multi_object_tracker { -using DetectedObject = autoware_perception_msgs::msg::DetectedObject; -using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects; -using TrackedObjects = autoware_perception_msgs::msg::TrackedObjects; - class MultiObjectTracker : public rclcpp::Node { public: @@ -66,8 +63,9 @@ class MultiObjectTracker : public rclcpp::Node private: // ROS interface - rclcpp::Publisher::SharedPtr tracked_objects_pub_; - rclcpp::Subscription::SharedPtr detected_object_sub_; + rclcpp::Publisher::SharedPtr tracked_objects_pub_; + rclcpp::Subscription::SharedPtr + detected_object_sub_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -100,7 +98,7 @@ class MultiObjectTracker : public rclcpp::Node void onTrigger(); // publish processes - void runProcess(const DetectedObjects & input_objects, const uint & channel_index); + void runProcess(const types::DynamicObjectList & input_objects); void checkAndPublish(const rclcpp::Time & time); void publish(const rclcpp::Time & time) const; inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp index fa333ea06a4b9..bc461191af271 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp @@ -14,6 +14,8 @@ #include "input_manager.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" + #include #include @@ -53,10 +55,13 @@ void InputStream::init(const InputChannel & input_channel) void InputStream::onMessage( const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) { - const DetectedObjects & objects = *msg; + const autoware_perception_msgs::msg::DetectedObjects & objects = *msg; + + types::DynamicObjectList dynamic_objects = types::toDynamicObjectList(objects, index_); // Model the object uncertainty only if it is not available - DetectedObjects objects_with_uncertainty = uncertainty::modelUncertainty(objects); + types::DynamicObjectList objects_with_uncertainty = + uncertainty::modelUncertainty(dynamic_objects); // Move the objects_with_uncertainty to the objects queue objects_que_.push_back(std::move(objects_with_uncertainty)); @@ -167,8 +172,7 @@ void InputStream::getObjectsOlderThan( // Add the object if the object is older than the specified latest time if (object_time <= object_latest_time) { - std::pair objects_pair(index_, objects); - objects_list.push_back(objects_pair); + objects_list.push_back(objects); } } @@ -216,10 +220,11 @@ void InputManager::init(const std::vector & input_channels) RCLCPP_INFO( node_.get_logger(), "InputManager::init Initializing %s input stream from %s", input_channels[i].long_name.c_str(), input_channels[i].input_topic.c_str()); - std::function func = - std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1); - sub_objects_array_.at(i) = node_.create_subscription( - input_channels[i].input_topic, rclcpp::QoS{1}, func); + std::function + func = std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1); + sub_objects_array_.at(i) = + node_.create_subscription( + input_channels[i].input_topic, rclcpp::QoS{1}, func); } // Check if any spawn enabled input streams @@ -263,11 +268,11 @@ void InputManager::getObjectTimeInterval( // The default object_earliest_time is to have a 1-second time interval const rclcpp::Time object_earliest_time_default = object_latest_time - rclcpp::Duration::from_seconds(1.0); - if (latest_exported_object_time_ < object_earliest_time_default) { - // if the latest exported object time is too old, set to the default - object_earliest_time = object_earliest_time_default; - } else if (latest_exported_object_time_ > object_latest_time) { - // if the latest exported object time is newer than the object_latest_time, set to the default + if ( + latest_exported_object_time_ < object_earliest_time_default || + latest_exported_object_time_ > object_latest_time) { + // if the latest exported object time is too old or newer than the object_latest_time, + // set to the default object_earliest_time = object_earliest_time_default; } else { // The object_earliest_time is the latest exported object time @@ -339,15 +344,14 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li // Sort objects by timestamp std::sort( objects_list.begin(), objects_list.end(), - [](const std::pair & a, const std::pair & b) { - return (rclcpp::Time(a.second.header.stamp) - rclcpp::Time(b.second.header.stamp)).seconds() < - 0; + [](const types::DynamicObjectList & a, const types::DynamicObjectList & b) { + return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0; }); // Update the latest exported object time bool is_any_object = !objects_list.empty(); if (is_any_object) { - latest_exported_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp); + latest_exported_object_time_ = rclcpp::Time(objects_list.back().header.stamp); } else { // check time jump back if (now < latest_exported_object_time_) { diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp index 01b3148251743..189d7b7dc48fe 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp @@ -15,9 +15,10 @@ #ifndef PROCESSOR__INPUT_MANAGER_HPP_ #define PROCESSOR__INPUT_MANAGER_HPP_ +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "rclcpp/rclcpp.hpp" -#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include #include #include @@ -28,8 +29,7 @@ namespace autoware::multi_object_tracker { -using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects; -using ObjectsList = std::vector>; +using ObjectsList = std::vector; struct InputChannel { @@ -82,11 +82,10 @@ class InputStream bool is_spawn_enabled_{}; size_t que_size_{30}; - std::deque objects_que_; + std::deque objects_que_; std::function func_trigger_; - // bool is_time_initialized_{false}; int initial_count_{0}; double latency_mean_{}; double latency_var_{}; @@ -115,7 +114,8 @@ class InputManager private: rclcpp::Node & node_; - std::vector::SharedPtr> sub_objects_array_{}; + std::vector::SharedPtr> + sub_objects_array_{}; bool is_initialized_{false}; rclcpp::Time latest_exported_object_time_; diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index b3bcd018faf9d..02ad0767dc815 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -15,10 +15,13 @@ #include "processor.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" +#include "autoware/multi_object_tracker/object_model/shapes.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/tracker.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware_perception_msgs/msg/tracked_objects.hpp" +#include + +#include #include #include @@ -44,9 +47,9 @@ void TrackerProcessor::predict(const rclcpp::Time & time) } void TrackerProcessor::update( - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const types::DynamicObjectList & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & direct_assignment, const uint & channel_index) + const std::unordered_map & direct_assignment) { int tracker_idx = 0; const auto & time = detected_objects.header.stamp; @@ -55,8 +58,7 @@ void TrackerProcessor::update( if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { // found const auto & associated_object = detected_objects.objects.at(direct_assignment.find(tracker_idx)->second); - (*(tracker_itr)) - ->updateWithMeasurement(associated_object, time, self_transform, channel_index); + (*(tracker_itr))->updateWithMeasurement(associated_object, time, self_transform); } else { // not found (*(tracker_itr))->updateWithoutMeasurement(time); } @@ -64,9 +66,8 @@ void TrackerProcessor::update( } void TrackerProcessor::spawn( - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, - const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & reverse_assignment, const uint & channel_index) + const types::DynamicObjectList & detected_objects, + const std::unordered_map & reverse_assignment) { const auto & time = detected_objects.header.stamp; for (size_t i = 0; i < detected_objects.objects.size(); ++i) { @@ -74,46 +75,36 @@ void TrackerProcessor::spawn( continue; } const auto & new_object = detected_objects.objects.at(i); - std::shared_ptr tracker = - createNewTracker(new_object, time, self_transform, channel_index); + std::shared_ptr tracker = createNewTracker(new_object, time); if (tracker) list_tracker_.push_back(tracker); } } std::shared_ptr TrackerProcessor::createNewTracker( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const + const types::DynamicObject & object, const rclcpp::Time & time) const { const LabelType label = autoware::object_recognition_utils::getHighestProbLabel(object.classification); if (config_.tracker_map.count(label) != 0) { const auto tracker = config_.tracker_map.at(label); if (tracker == "bicycle_tracker") - return std::make_shared( - time, object, self_transform, config_.channel_size, channel_index); + return std::make_shared(time, object, config_.channel_size); if (tracker == "big_vehicle_tracker") return std::make_shared( - object_model::big_vehicle, time, object, self_transform, config_.channel_size, - channel_index); + object_model::big_vehicle, time, object, config_.channel_size); if (tracker == "multi_vehicle_tracker") - return std::make_shared( - time, object, self_transform, config_.channel_size, channel_index); + return std::make_shared(time, object, config_.channel_size); if (tracker == "normal_vehicle_tracker") return std::make_shared( - object_model::normal_vehicle, time, object, self_transform, config_.channel_size, - channel_index); + object_model::normal_vehicle, time, object, config_.channel_size); if (tracker == "pass_through_tracker") - return std::make_shared( - time, object, self_transform, config_.channel_size, channel_index); + return std::make_shared(time, object, config_.channel_size); if (tracker == "pedestrian_and_bicycle_tracker") - return std::make_shared( - time, object, self_transform, config_.channel_size, channel_index); + return std::make_shared(time, object, config_.channel_size); if (tracker == "pedestrian_tracker") - return std::make_shared( - time, object, self_transform, config_.channel_size, channel_index); + return std::make_shared(time, object, config_.channel_size); } - return std::make_shared( - time, object, self_transform, config_.channel_size, channel_index); + return std::make_shared(time, object, config_.channel_size); } void TrackerProcessor::prune(const rclcpp::Time & time) @@ -143,12 +134,12 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time) { // Iterate through the list of trackers for (auto itr1 = list_tracker_.begin(); itr1 != list_tracker_.end(); ++itr1) { - autoware_perception_msgs::msg::TrackedObject object1; + types::DynamicObject object1; if (!(*itr1)->getTrackedObject(time, object1)) continue; // Compare the current tracker with the remaining trackers for (auto itr2 = std::next(itr1); itr2 != list_tracker_.end(); ++itr2) { - autoware_perception_msgs::msg::TrackedObject object2; + types::DynamicObject object2; if (!(*itr2)->getTrackedObject(time, object2)) continue; // Calculate the distance between the two objects @@ -164,9 +155,8 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time) } // Check the Intersection over Union (IoU) between the two objects - const double min_union_iou_area = 1e-2; - const auto iou = - autoware::object_recognition_utils::get2dIoU(object1, object2, min_union_iou_area); + constexpr double min_union_iou_area = 1e-2; + const auto iou = shapes::get2dIoU(object1, object2, min_union_iou_area); const auto & label1 = (*itr1)->getHighestProbLabel(); const auto & label2 = (*itr2)->getHighestProbLabel(); bool should_delete_tracker1 = false; @@ -225,13 +215,13 @@ void TrackerProcessor::getTrackedObjects( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & tracked_objects) const { tracked_objects.header.stamp = time; + types::DynamicObject tracked_object; for (const auto & tracker : list_tracker_) { // Skip if the tracker is not confident if (!isConfidentTracker(tracker)) continue; // Get the tracked object, extrapolated to the given time - autoware_perception_msgs::msg::TrackedObject tracked_object; if (tracker->getTrackedObject(time, tracked_object)) { - tracked_objects.objects.push_back(tracked_object); + tracked_objects.objects.push_back(toTrackedObjectMsg(tracked_object)); } } } @@ -241,11 +231,11 @@ void TrackerProcessor::getTentativeObjects( autoware_perception_msgs::msg::TrackedObjects & tentative_objects) const { tentative_objects.header.stamp = time; + types::DynamicObject tracked_object; for (const auto & tracker : list_tracker_) { if (!isConfidentTracker(tracker)) { - autoware_perception_msgs::msg::TrackedObject tracked_object; if (tracker->getTrackedObject(time, tracked_object)) { - tentative_objects.objects.push_back(tracked_object); + tentative_objects.objects.push_back(toTrackedObjectMsg(tracked_object)); } } } diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.hpp b/perception/autoware_multi_object_tracker/src/processor/processor.hpp index 80ca92bc43671..ad296b1c07d8d 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.hpp @@ -15,6 +15,7 @@ #ifndef PROCESSOR__PROCESSOR_HPP_ #define PROCESSOR__PROCESSOR_HPP_ +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include @@ -53,13 +54,12 @@ class TrackerProcessor // tracker processes void predict(const rclcpp::Time & time); void update( - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const types::DynamicObjectList & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & direct_assignment, const uint & channel_index); + const std::unordered_map & direct_assignment); void spawn( - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, - const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & reverse_assignment, const uint & channel_index); + const types::DynamicObjectList & detected_objects, + const std::unordered_map & reverse_assignment); void prune(const rclcpp::Time & time); // output @@ -79,8 +79,7 @@ class TrackerProcessor void removeOldTracker(const rclcpp::Time & time); void removeOverlappedTracker(const rclcpp::Time & time); std::shared_ptr createNewTracker( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const; + const types::DynamicObject & object, const rclcpp::Time & time) const; }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_object_merger/README.md b/perception/autoware_object_merger/README.md index c65353efa3a91..a78cd70052707 100644 --- a/perception/autoware_object_merger/README.md +++ b/perception/autoware_object_merger/README.md @@ -25,8 +25,16 @@ The successive shortest path algorithm is used to solve the data association pro ## Parameters +- object association merger + {{ json_to_markdown("perception/autoware_object_merger/schema/object_association_merger.schema.json") }} + +- data association matrix + {{ json_to_markdown("perception/autoware_object_merger/schema/data_association_matrix.schema.json") }} + +- overlapped judge + {{ json_to_markdown("perception/autoware_object_merger/schema/overlapped_judge.schema.json") }} ## Tips diff --git a/perception/autoware_object_merger/launch/object_association_merger.launch.xml b/perception/autoware_object_merger/launch/object_association_merger.launch.xml index f3c0e8bd5a829..4747b2af284fc 100644 --- a/perception/autoware_object_merger/launch/object_association_merger.launch.xml +++ b/perception/autoware_object_merger/launch/object_association_merger.launch.xml @@ -1,5 +1,6 @@ + @@ -8,7 +9,7 @@ - + diff --git a/perception/autoware_object_merger/schema/data_association_matrix.schema.json b/perception/autoware_object_merger/schema/data_association_matrix.schema.json index 68dc977224ba5..52f6aa3d8c37c 100644 --- a/perception/autoware_object_merger/schema/data_association_matrix.schema.json +++ b/perception/autoware_object_merger/schema/data_association_matrix.schema.json @@ -2,8 +2,8 @@ "$schema": "http://json-schema.org/draft-07/schema#", "title": "Data Association Matrix Parameters", "type": "object", - "properties": { - "ros__parameters": { + "definitions": { + "data_association_matrix": { "type": "object", "properties": { "can_assign_matrix": { @@ -11,31 +11,72 @@ "items": { "type": "number" }, - "description": "Assignment table for data association" + "description": "Assignment table for data association.", + "default": [ + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, + 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, + 0, 0, 0, 1, 1, 1 + ] }, "max_dist_matrix": { "type": "array", "items": { "type": "number" }, - "description": "Maximum distance table for data association" + "description": "Maximum distance table for data association.", + "default": [ + 4.0, 4.0, 5.0, 5.0, 5.0, 2.0, 2.0, 2.0, 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, 2.0, 1.0, 1.0, + 1.0, 1.0, 3.0, 3.0, 3.0, 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0 + ] }, "max_rad_matrix": { "type": "array", "items": { - "type": "number" + "type": "number", + "minimum": 0.0 }, - "description": "Maximum angle table for data association. If value is greater than pi, it will be ignored." + "description": "Maximum angle table for data association. If value is greater than pi, it will be ignored.", + "default": [ + 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 1.047, 1.047, 1.047, 1.047, 3.15, + 3.15, 3.15, 3.15, 1.047, 1.047, 1.047, 1.047, 3.15, 3.15, 3.15, 3.15, 1.047, 1.047, + 1.047, 1.047, 3.15, 3.15, 3.15, 3.15, 1.047, 1.047, 1.047, 1.047, 3.15, 3.15, 3.15, + 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, + 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15 + ] }, "min_iou_matrix": { "type": "array", "items": { - "type": "number" + "type": "number", + "minimum": 0.0 }, - "description": "Minimum IoU threshold matrix for data association. If value is negative, it will be ignored." + "description": "Minimum IoU threshold matrix for data association. If value is negative, it will be ignored.", + "default": [ + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.3, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, 0.1, + 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, 0.1, 0.2, + 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1 + ] + } + }, + "required": ["can_assign_matrix", "max_dist_matrix", "max_rad_matrix", "min_iou_matrix"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/data_association_matrix" } }, - "required": ["can_assign_matrix", "max_dist_matrix", "max_rad_matrix", "min_iou_matrix"] + "required": ["ros__parameters"], + "additionalProperties": false } - } + }, + "required": ["/**"], + "additionalProperties": false } diff --git a/perception/autoware_object_merger/schema/object_association_merger.schema.json b/perception/autoware_object_merger/schema/object_association_merger.schema.json index 11090fab9c7b3..c31cb8866b8cc 100644 --- a/perception/autoware_object_merger/schema/object_association_merger.schema.json +++ b/perception/autoware_object_merger/schema/object_association_merger.schema.json @@ -45,7 +45,8 @@ "remove_overlapped_unknown_objects", "base_link_frame_id", "priority_mode" - ] + ], + "additionalProperties": false } }, "properties": { @@ -56,8 +57,10 @@ "$ref": "#/definitions/object_association_merger" } }, - "required": ["ros__parameters"] + "required": ["ros__parameters"], + "additionalProperties": false } }, - "required": ["/**"] + "required": ["/**"], + "additionalProperties": false } diff --git a/perception/autoware_object_merger/schema/overlapped_judge.schema.json b/perception/autoware_object_merger/schema/overlapped_judge.schema.json index b65464c6201d1..b8ed9f313eb3a 100644 --- a/perception/autoware_object_merger/schema/overlapped_judge.schema.json +++ b/perception/autoware_object_merger/schema/overlapped_judge.schema.json @@ -2,8 +2,8 @@ "$schema": "http://json-schema.org/draft-07/schema#", "title": "Overlapped Judge Parameters", "type": "object", - "properties": { - "ros__parameters": { + "definitions": { + "overlapped_judge": { "type": "object", "properties": { "distance_threshold_list": { @@ -11,17 +11,36 @@ "items": { "type": "number" }, - "description": "Distance threshold for each class used in judging overlap." + "description": "Distance threshold for each class used in judging overlap.", + "default": [9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0] }, "generalized_iou_threshold": { "type": "array", "items": { - "type": "number" + "type": "number", + "minimum": -1.0, + "maximum": 1.0 }, - "description": "Generalized IoU threshold for each class." + "description": "Generalized IoU threshold for each class.", + "default": [-0.1, -0.1, -0.1, -0.6, -0.6, -0.1, -0.1, -0.1] + } + }, + "required": ["distance_threshold_list", "generalized_iou_threshold"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/overlapped_judge" } }, - "required": ["distance_threshold_list", "generalized_iou_threshold"] + "required": ["ros__parameters"], + "additionalProperties": false } - } + }, + "required": ["/**"], + "additionalProperties": false } diff --git a/perception/autoware_occupancy_grid_map_outlier_filter/README.md b/perception/autoware_occupancy_grid_map_outlier_filter/README.md index 7de2cc1dce92c..b920aa6908946 100644 --- a/perception/autoware_occupancy_grid_map_outlier_filter/README.md +++ b/perception/autoware_occupancy_grid_map_outlier_filter/README.md @@ -40,7 +40,7 @@ The following video is a sample. Yellow points are high occupancy probability, g ## Parameters -{{ json_to_markdown("perception/occupancy_grid_map_outlier_filter/schema/occupancy_grid_map_outlier_filter.schema.json") }} +{{ json_to_markdown("perception/autoware_occupancy_grid_map_outlier_filter/schema/occupancy_grid_map_outlier_filter.schema.json") }} ## Assumptions / Known limits diff --git a/perception/autoware_probabilistic_occupancy_grid_map/README.md b/perception/autoware_probabilistic_occupancy_grid_map/README.md index 575411bcbd220..3637dcb10daeb 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/README.md +++ b/perception/autoware_probabilistic_occupancy_grid_map/README.md @@ -21,12 +21,29 @@ You may need to choose `scan_origin_frame` and `gridmap_origin_frame` which mean ### Parameters -{{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/binary_bayes_filter_updater.schema.json") }} -{{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/grid_map.schema.json") }} -{{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/laserscan_based_occupancy_grid_map.schema.json") }} -{{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/multi_lidar_pointcloud_based_occupancy_grid_map.schema.json") }} -{{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/pointcloud_based_occupancy_grid_map.schema.json") }} -{{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json") }} +- binary bayes filter updater + + {{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/binary_bayes_filter_updater.schema.json") }} + +- grid map + + {{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/grid_map.schema.json") }} + +- laserscan based occupancy grid map + + {{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/laserscan_based_occupancy_grid_map.schema.json") }} + +- multi lidar pointcloud based occupancy grid map + + {{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/multi_lidar_pointcloud_based_occupancy_grid_map.schema.json") }} + +- pointcloud based occupancy grid map + + {{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/pointcloud_based_occupancy_grid_map.schema.json") }} + +- synchronized grid map fusion + + {{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json") }} ### Downsample input pointcloud(Optional) diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp index 518052a1a4b8c..da22ef125fb19 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp @@ -144,10 +144,11 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( transformPointAndCalculate(pt, pt_map, angle_bin_index, range); // Ignore obstacle points exceed the range of the raw points - if (raw_pointcloud_angle_bins.at(angle_bin_index).empty()) { - continue; // No raw point in this angle bin - } else if (range > raw_pointcloud_angle_bins.at(angle_bin_index).back().range) { - continue; // Obstacle point exceeds the range of the raw points + // No raw point in this angle bin, or obstacle point exceeds the range of the raw points + if ( + raw_pointcloud_angle_bins.at(angle_bin_index).empty() || + range > raw_pointcloud_angle_bins.at(angle_bin_index).back().range) { + continue; } obstacle_pointcloud_angle_bins.at(angle_bin_index).emplace_back(range, pt_map[0], pt_map[1]); } diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp index dd934dea26c55..4f5b8b8eeabed 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp @@ -153,10 +153,11 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const double dz = scan_z - obstacle_z; // Ignore obstacle points exceed the range of the raw points - if (raw_pointcloud_angle_bins.at(angle_bin_index).empty()) { - continue; // No raw point in this angle bin - } else if (range > raw_pointcloud_angle_bins.at(angle_bin_index).back().range) { - continue; // Obstacle point exceeds the range of the raw points + // No raw point in this angle bin, or obstacle point exceeds the range of the raw points + if ( + raw_pointcloud_angle_bins.at(angle_bin_index).empty() || + range > raw_pointcloud_angle_bins.at(angle_bin_index).back().range) { + continue; } if (dz > projection_dz_threshold_) { diff --git a/perception/autoware_probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json b/perception/autoware_probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json index 14f4305f55de8..de7c1e194ed42 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json +++ b/perception/autoware_probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json @@ -2,104 +2,118 @@ "$schema": "http://json-schema.org/draft-07/schema#", "title": "Parameters for Synchronized Grid Map Fusion Node", "type": "object", + "definitions": { + "synchronized_grid_map_fusion": { + "type": "object", + "properties": { + "fusion_input_ogm_topics": { + "type": "array", + "description": "List of fusion input occupancy grid map topics.", + "items": { + "type": "string" + }, + "default": ["topic1", "topic2"] + }, + "input_ogm_reliabilities": { + "type": "array", + "description": "Reliability of each sensor for fusion.", + "items": { + "type": "number", + "minimum": 0.0, + "maximum": 1.0 + }, + "default": [0.8, 0.2] + }, + "fusion_method": { + "type": "string", + "description": "Method for occupancy grid map fusion.", + "enum": ["overwrite", "log-odds", "dempster-shafer"], + "default": "overwrite" + }, + "match_threshold_sec": { + "type": "number", + "description": "Time threshold for matching in seconds.", + "default": 0.01 + }, + "timeout_sec": { + "type": "number", + "description": "Timeout for synchronization in seconds.", + "default": 0.1 + }, + "input_offset_sec": { + "type": "array", + "description": "Offset for each input in seconds.", + "items": { + "type": "number" + }, + "default": [0.0, 0.0] + }, + "map_frame_": { + "type": "string", + "description": "The frame ID of the map.", + "default": "map" + }, + "base_link_frame_": { + "type": "string", + "description": "The frame ID of the base link.", + "default": "base_link" + }, + "grid_map_origin_frame_": { + "type": "string", + "description": "The frame ID of the grid map origin.", + "default": "base_link" + }, + "fusion_map_length_x": { + "type": "number", + "description": "The length of the fusion map in the x direction.", + "default": 100.0 + }, + "fusion_map_length_y": { + "type": "number", + "description": "The length of the fusion map in the y direction.", + "default": 100.0 + }, + "fusion_map_resolution": { + "type": "number", + "description": "The resolution of the fusion map.", + "default": 0.5 + }, + "publish_processing_time_detail": { + "type": "boolean", + "description": "True for showing detail of publish processing time.", + "default": false + } + }, + "required": [ + "fusion_input_ogm_topics", + "input_ogm_reliabilities", + "fusion_method", + "match_threshold_sec", + "timeout_sec", + "input_offset_sec", + "map_frame_", + "base_link_frame_", + "grid_map_origin_frame_", + "fusion_map_length_x", + "fusion_map_length_y", + "fusion_map_resolution", + "publish_processing_time_detail" + ], + "additionalProperties": false + } + }, "properties": { "/**": { "type": "object", "properties": { "ros__parameters": { - "type": "object", - "properties": { - "fusion_input_ogm_topics": { - "type": "array", - "description": "List of fusion input occupancy grid map topics.", - "items": { - "type": "string" - }, - "default": ["topic1", "topic2"] - }, - "input_ogm_reliabilities": { - "type": "array", - "description": "Reliability of each sensor for fusion.", - "items": { - "type": "number", - "minimum": 0.0, - "maximum": 1.0 - }, - "default": [0.8, 0.2] - }, - "fusion_method": { - "type": "string", - "description": "Method for occupancy grid map fusion.", - "enum": ["overwrite", "log-odds", "dempster-shafer"], - "default": "overwrite" - }, - "match_threshold_sec": { - "type": "number", - "description": "Time threshold for matching in seconds.", - "default": 0.01 - }, - "timeout_sec": { - "type": "number", - "description": "Timeout for synchronization in seconds.", - "default": 0.1 - }, - "input_offset_sec": { - "type": "array", - "description": "Offset for each input in seconds.", - "items": { - "type": "number" - }, - "default": [0.0, 0.0] - }, - "map_frame_": { - "type": "string", - "description": "The frame ID of the map.", - "default": "map" - }, - "base_link_frame_": { - "type": "string", - "description": "The frame ID of the base link.", - "default": "base_link" - }, - "grid_map_origin_frame_": { - "type": "string", - "description": "The frame ID of the grid map origin.", - "default": "base_link" - }, - "fusion_map_length_x": { - "type": "number", - "description": "The length of the fusion map in the x direction.", - "default": 100.0 - }, - "fusion_map_length_y": { - "type": "number", - "description": "The length of the fusion map in the y direction.", - "default": 100.0 - }, - "fusion_map_resolution": { - "type": "number", - "description": "The resolution of the fusion map.", - "default": 0.5 - } - }, - "required": [ - "fusion_input_ogm_topics", - "input_ogm_reliabilities", - "fusion_method", - "match_threshold_sec", - "timeout_sec", - "input_offset_sec", - "map_frame_", - "base_link_frame_", - "grid_map_origin_frame_", - "fusion_map_length_x", - "fusion_map_length_y", - "fusion_map_resolution" - ] + "$ref": "#/definitions/synchronized_grid_map_fusion" } }, - "required": ["ros__parameters"] + "required": ["ros__parameters"], + "additionalProperties": false } }, - "required": ["/**"] + "required": ["/**"], + "additionalProperties": false } diff --git a/perception/autoware_radar_fusion_to_detected_object/src/node.cpp b/perception/autoware_radar_fusion_to_detected_object/src/node.cpp index 58e893032340c..fcebbbc905425 100644 --- a/perception/autoware_radar_fusion_to_detected_object/src/node.cpp +++ b/perception/autoware_radar_fusion_to_detected_object/src/node.cpp @@ -25,7 +25,6 @@ using namespace std::literals; using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::nanoseconds; -using std::placeholders::_1; namespace { @@ -58,7 +57,7 @@ RadarObjectFusionToDetectedObjectNode::RadarObjectFusionToDetectedObjectNode( { // Parameter Server set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&RadarObjectFusionToDetectedObjectNode::onSetParam, this, _1)); + std::bind(&RadarObjectFusionToDetectedObjectNode::onSetParam, this, std::placeholders::_1)); // Node Parameter node_param_.update_rate_hz = declare_parameter("node_params.update_rate_hz"); @@ -93,11 +92,10 @@ RadarObjectFusionToDetectedObjectNode::RadarObjectFusionToDetectedObjectNode( sub_object_.subscribe(this, "~/input/objects", rclcpp::QoS{1}.get_rmw_qos_profile()); sub_radar_.subscribe(this, "~/input/radars", rclcpp::QoS{1}.get_rmw_qos_profile()); - using std::placeholders::_1; - using std::placeholders::_2; sync_ptr_ = std::make_shared(SyncPolicy(20), sub_object_, sub_radar_); - sync_ptr_->registerCallback( - std::bind(&RadarObjectFusionToDetectedObjectNode::onData, this, _1, _2)); + sync_ptr_->registerCallback(std::bind( + &RadarObjectFusionToDetectedObjectNode::onData, this, std::placeholders::_1, + std::placeholders::_2)); // Publisher pub_objects_ = create_publisher("~/output/objects", 1); diff --git a/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp index 98f429292ec73..6d19f299973bd 100644 --- a/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp +++ b/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -158,9 +158,7 @@ bool RadarFusionToDetectedObject::isYawCorrect( const double object_yaw = autoware::universe_utils::normalizeRadian( tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); const double diff_yaw = autoware::universe_utils::normalizeRadian(twist_yaw - object_yaw); - if (std::abs(diff_yaw) < yaw_threshold) { - return true; - } else if (M_PI - yaw_threshold < std::abs(diff_yaw)) { + if (std::abs(diff_yaw) < yaw_threshold || M_PI - yaw_threshold < std::abs(diff_yaw)) { return true; } else { return false; diff --git a/perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp b/perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp index 005138623427e..0e06541f90af1 100644 --- a/perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp +++ b/perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp @@ -34,7 +34,6 @@ using namespace std::literals; using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::nanoseconds; -using std::placeholders::_1; namespace { @@ -73,6 +72,7 @@ enum class RadarTrackObjectID { RadarTracksMsgsConverterNode::RadarTracksMsgsConverterNode(const rclcpp::NodeOptions & node_options) : Node("radar_tracks_msgs_converter", node_options) { + using std::placeholders::_1; // Parameter Server set_param_res_ = this->add_on_set_parameters_callback( std::bind(&RadarTracksMsgsConverterNode::onSetParam, this, _1)); diff --git a/perception/autoware_shape_estimation/lib/corrector/utils.cpp b/perception/autoware_shape_estimation/lib/corrector/utils.cpp index 5e90c9d54f78a..678eb41c56831 100644 --- a/perception/autoware_shape_estimation/lib/corrector/utils.cpp +++ b/perception/autoware_shape_estimation/lib/corrector/utils.cpp @@ -163,7 +163,7 @@ bool correctWithDefaultValue( (param.min_width < (v_point.at(second_most_distant_index) * 2.0).norm() && (v_point.at(second_most_distant_index) * 2.0).norm() < param.max_width)) // both of edge is within width threshold - { + { // NOLINT correction_vector = v_point.at(first_most_distant_index); if (correction_vector.x() == 0.0) { correction_vector.y() = diff --git a/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp b/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp index b24e4fe56e8b8..4aa008e42c966 100644 --- a/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp +++ b/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp @@ -193,9 +193,15 @@ TrtClassifier::TrtClassifier( TrtClassifier::~TrtClassifier() { - if (m_cuda) { - if (h_img_) CHECK_CUDA_ERROR(cudaFreeHost(h_img_)); - if (d_img_) CHECK_CUDA_ERROR(cudaFree(d_img_)); + try { + if (m_cuda) { + if (h_img_) CHECK_CUDA_ERROR(cudaFreeHost(h_img_)); + if (d_img_) CHECK_CUDA_ERROR(cudaFree(d_img_)); + } + } catch (const std::exception & e) { + std::cerr << "Exception in TrtClassifier destructor: " << e.what() << std::endl; + } catch (...) { + std::cerr << "Unknown exception in TrtClassifier destructor" << std::endl; } } diff --git a/perception/autoware_tensorrt_common/src/tensorrt_common.cpp b/perception/autoware_tensorrt_common/src/tensorrt_common.cpp index 897010d22bb4b..990433ee277a0 100644 --- a/perception/autoware_tensorrt_common/src/tensorrt_common.cpp +++ b/perception/autoware_tensorrt_common/src/tensorrt_common.cpp @@ -311,12 +311,13 @@ void TrtCommon::printNetworkInfo(const std::string & onnx_file_path) int groups = conv->getNbGroups(); int stride = s_dims.d[0]; int num_weights = (dim_in.d[1] / groups) * dim_out.d[1] * k_dims.d[0] * k_dims.d[1]; - float gflops = (2 * num_weights) * (dim_in.d[3] / stride * dim_in.d[2] / stride / 1e9); - ; + float gflops = (2.0 * num_weights) * (static_cast(dim_in.d[3]) / stride * + static_cast(dim_in.d[2]) / stride / 1e9); total_gflops += gflops; total_params += num_weights; std::cout << "L" << i << " [conv " << k_dims.d[0] << "x" << k_dims.d[1] << " (" << groups - << ") " << "/" << s_dims.d[0] << "] " << dim_in.d[3] << "x" << dim_in.d[2] << "x" + << ") " + << "/" << s_dims.d[0] << "] " << dim_in.d[3] << "x" << dim_in.d[2] << "x" << dim_in.d[1] << " -> " << dim_out.d[3] << "x" << dim_out.d[2] << "x" << dim_out.d[1]; std::cout << " weights:" << num_weights; @@ -336,8 +337,10 @@ void TrtCommon::printNetworkInfo(const std::string & onnx_file_path) } else if (p_type == nvinfer1::PoolingType::kMAX_AVERAGE_BLEND) { std::cout << "max avg blend "; } - float gflops = dim_in.d[1] * dim_window.d[0] / dim_stride.d[0] * dim_window.d[1] / - dim_stride.d[1] * dim_in.d[2] * dim_in.d[3] / 1e9; + float gflops = static_cast(dim_in.d[1]) * + (static_cast(dim_window.d[0]) / static_cast(dim_stride.d[0])) * + (static_cast(dim_window.d[1]) / static_cast(dim_stride.d[1])) * + static_cast(dim_in.d[2]) * static_cast(dim_in.d[3]) / 1e9; total_gflops += gflops; std::cout << "pool " << dim_window.d[0] << "x" << dim_window.d[1] << "]"; std::cout << " GFLOPs:" << gflops; @@ -381,7 +384,8 @@ bool TrtCommon::buildEngineFromOnnx( if (num_available_dla > 0) { std::cout << "###" << num_available_dla << " DLAs are supported! ###" << std::endl; } else { - std::cout << "###Warning : " << "No DLA is supported! ###" << std::endl; + std::cout << "###Warning : " + << "No DLA is supported! ###" << std::endl; } config->setDefaultDeviceType(nvinfer1::DeviceType::kDLA); config->setDLACore(build_config_->dla_core_id); diff --git a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp index 06540f2b7cd19..7e2e327bf6f5e 100644 --- a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp +++ b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp @@ -341,16 +341,22 @@ TrtYoloX::TrtYoloX( TrtYoloX::~TrtYoloX() { - if (use_gpu_preprocess_) { - if (image_buf_h_) { - image_buf_h_.reset(); - } - if (image_buf_d_) { - image_buf_d_.reset(); - } - if (argmax_buf_d_) { - argmax_buf_d_.reset(); + try { + if (use_gpu_preprocess_) { + if (image_buf_h_) { + image_buf_h_.reset(); + } + if (image_buf_d_) { + image_buf_d_.reset(); + } + if (argmax_buf_d_) { + argmax_buf_d_.reset(); + } } + } catch (const std::exception & e) { + std::cerr << "Exception in TrtYoloX destructor: " << e.what() << std::endl; + } catch (...) { + std::cerr << "Unknown exception in TrtYoloX destructor" << std::endl; } } diff --git a/perception/autoware_tracking_object_merger/launch/decorative_tracker_merger.launch.xml b/perception/autoware_tracking_object_merger/launch/decorative_tracker_merger.launch.xml index cd609a0fa612a..e2892bc75dfe0 100644 --- a/perception/autoware_tracking_object_merger/launch/decorative_tracker_merger.launch.xml +++ b/perception/autoware_tracking_object_merger/launch/decorative_tracker_merger.launch.xml @@ -1,5 +1,6 @@ + @@ -7,7 +8,7 @@ - + diff --git a/perception/autoware_tracking_object_merger/src/utils/utils.cpp b/perception/autoware_tracking_object_merger/src/utils/utils.cpp index 15d130b4fcd50..2d7b930da9e66 100644 --- a/perception/autoware_tracking_object_merger/src/utils/utils.cpp +++ b/perception/autoware_tracking_object_merger/src/utils/utils.cpp @@ -98,10 +98,10 @@ TrackedObject linearInterpolationForTrackedObject( output_shape.dimensions.x = shape1.dimensions.x * (1 - weight) + shape2.dimensions.x * weight; output_shape.dimensions.y = shape1.dimensions.y * (1 - weight) + shape2.dimensions.y * weight; output_shape.dimensions.z = shape1.dimensions.z * (1 - weight) + shape2.dimensions.z * weight; - } else if (shape1.type == autoware_perception_msgs::msg::Shape::CYLINDER) { - // (TODO) implement - } else if (shape1.type == autoware_perception_msgs::msg::Shape::POLYGON) { - // (TODO) implement + } else if (shape1.type == autoware_perception_msgs::msg::Shape::CYLINDER) { // NOLINT + // (TODO) implement and remove NOLINT + } else if (shape1.type == autoware_perception_msgs::msg::Shape::POLYGON) { // NOLINT + // (TODO) implement and remove NOLINT } else { // when type is unknown, print warning and do nothing std::cerr << "unknown shape type in linearInterpolationForTrackedObject function." diff --git a/perception/autoware_traffic_light_arbiter/README.md b/perception/autoware_traffic_light_arbiter/README.md index 619154e1e183b..f27afab62818e 100644 --- a/perception/autoware_traffic_light_arbiter/README.md +++ b/perception/autoware_traffic_light_arbiter/README.md @@ -43,7 +43,8 @@ The table below outlines how the matching process determines the output based on | Name | Type | Default Value | Description | | --------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -| `external_time_tolerance` | double | 5.0 | The duration in seconds an external message is considered valid for merging | -| `perception_time_tolerance` | double | 1.0 | The duration in seconds a perception message is considered valid for merging | +| `external_delay_tolerance` | double | 5.0 | The duration in seconds an external message is considered valid for merging in comparison with current time | +| `external_time_tolerance` | double | 5.0 | The duration in seconds an external message is considered valid for merging in comparison with a timestamp of perception message | +| `perception_time_tolerance` | double | 1.0 | The duration in seconds a perception message is considered valid for merging in comparison with a timestamp of external message | | `external_priority` | bool | false | Whether or not externals signals take precedence over perception-based ones. If false, the merging uses confidence as a criteria | | `enable_signal_matching` | bool | false | Decide whether to validate the match between perception signals and external signals. If set to true, verify that the colors match and only publish them if they are identical | diff --git a/perception/autoware_traffic_light_arbiter/config/traffic_light_arbiter.param.yaml b/perception/autoware_traffic_light_arbiter/config/traffic_light_arbiter.param.yaml index dfe12ff352f16..36e1b8593bebc 100644 --- a/perception/autoware_traffic_light_arbiter/config/traffic_light_arbiter.param.yaml +++ b/perception/autoware_traffic_light_arbiter/config/traffic_light_arbiter.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + external_delay_tolerance: 5.0 external_time_tolerance: 5.0 perception_time_tolerance: 1.0 external_priority: false diff --git a/perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp b/perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp index ccd928d52b367..916bd04a6bdd0 100644 --- a/perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp +++ b/perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp @@ -51,6 +51,7 @@ class TrafficLightArbiter : public rclcpp::Node std::unique_ptr> map_regulatory_elements_set_; + double external_delay_tolerance_; double external_time_tolerance_; double perception_time_tolerance_; bool external_priority_; diff --git a/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp b/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp index e71629fa5dd28..8ce002780813f 100644 --- a/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp +++ b/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -70,6 +71,7 @@ namespace autoware TrafficLightArbiter::TrafficLightArbiter(const rclcpp::NodeOptions & options) : Node("traffic_light_arbiter", options) { + external_delay_tolerance_ = this->declare_parameter("external_delay_tolerance", 5.0); external_time_tolerance_ = this->declare_parameter("external_time_tolerance", 5.0); perception_time_tolerance_ = this->declare_parameter("perception_time_tolerance", 1.0); external_priority_ = this->declare_parameter("external_priority", false); @@ -119,7 +121,7 @@ void TrafficLightArbiter::onPerceptionMsg(const TrafficSignalArray::ConstSharedP latest_perception_msg_ = *msg; if ( - (rclcpp::Time(msg->stamp) - rclcpp::Time(latest_external_msg_.stamp)).seconds() > + std::abs((rclcpp::Time(msg->stamp) - rclcpp::Time(latest_external_msg_.stamp)).seconds()) > external_time_tolerance_) { latest_external_msg_.traffic_light_groups.clear(); } @@ -129,10 +131,16 @@ void TrafficLightArbiter::onPerceptionMsg(const TrafficSignalArray::ConstSharedP void TrafficLightArbiter::onExternalMsg(const TrafficSignalArray::ConstSharedPtr msg) { + if (std::abs((this->now() - rclcpp::Time(msg->stamp)).seconds()) > external_delay_tolerance_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, "Received outdated V2X traffic signal messages"); + return; + } + latest_external_msg_ = *msg; if ( - (rclcpp::Time(msg->stamp) - rclcpp::Time(latest_perception_msg_.stamp)).seconds() > + std::abs((rclcpp::Time(msg->stamp) - rclcpp::Time(latest_perception_msg_.stamp)).seconds()) > perception_time_tolerance_) { latest_perception_msg_.traffic_light_groups.clear(); } @@ -229,6 +237,13 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim } pub_->publish(output_signals_msg); + + const auto latest_time = + std::max(rclcpp::Time(latest_perception_msg_.stamp), rclcpp::Time(latest_external_msg_.stamp)); + if (rclcpp::Time(output_signals_msg.stamp) < latest_time) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, "Published traffic signal messages are not latest"); + } } } // namespace autoware diff --git a/perception/autoware_traffic_light_arbiter/test/test_node.cpp b/perception/autoware_traffic_light_arbiter/test/test_node.cpp index f993ad7cec84d..44b3ca7925fd0 100644 --- a/perception/autoware_traffic_light_arbiter/test/test_node.cpp +++ b/perception/autoware_traffic_light_arbiter/test/test_node.cpp @@ -196,6 +196,9 @@ TEST(TrafficLightArbiterTest, testTrafficSignalOnlyPerceptionMsg) }; test_manager->set_subscriber(output_topic, callback); + // stamp preparation + perception_msg.stamp = test_target_node->now(); + test_manager->test_pub_msg( test_target_node, input_map_topic, vector_map_msg, rclcpp::QoS(1).transient_local()); test_manager->test_pub_msg( @@ -229,6 +232,9 @@ TEST(TrafficLightArbiterTest, testTrafficSignalOnlyExternalMsg) }; test_manager->set_subscriber(output_topic, callback); + // stamp preparation + external_msg.stamp = test_target_node->now(); + test_manager->test_pub_msg( test_target_node, input_map_topic, vector_map_msg, rclcpp::QoS(1).transient_local()); test_manager->test_pub_msg( @@ -268,6 +274,10 @@ TEST(TrafficLightArbiterTest, testTrafficSignalBothMsg) }; test_manager->set_subscriber(output_topic, callback); + // stamp preparation + external_msg.stamp = test_target_node->now(); + perception_msg.stamp = test_target_node->now(); + test_manager->test_pub_msg( test_target_node, input_map_topic, vector_map_msg, rclcpp::QoS(1).transient_local()); test_manager->test_pub_msg( diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp b/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp index ccfaf37fd7c6d..f12eb656033b6 100644 --- a/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp @@ -169,7 +169,7 @@ void TrafficLightMapVisualizerNode::trafficSignalsCallback( visualization_msgs::msg::Marker marker; if ( isAttributeValue(pt, "color", "red") && - elem.color == autoware_perception_msgs::msg::TrafficLightElement::RED) { + elem.color == autoware_perception_msgs::msg::TrafficLightElement::RED) { // NOLINT lightAsMarker( get_node_logging_interface(), pt, &marker, "traffic_light", current_time); } else if ( // NOLINT @@ -177,7 +177,6 @@ void TrafficLightMapVisualizerNode::trafficSignalsCallback( elem.color == autoware_perception_msgs::msg::TrafficLightElement::GREEN) { lightAsMarker( get_node_logging_interface(), pt, &marker, "traffic_light", current_time); - } else if ( // NOLINT isAttributeValue(pt, "color", "yellow") && elem.color == autoware_perception_msgs::msg::TrafficLightElement::AMBER) { diff --git a/planning/autoware_costmap_generator/src/costmap_generator.cpp b/planning/autoware_costmap_generator/src/costmap_generator.cpp index 052bb2728a3b1..081a6ead91cda 100644 --- a/planning/autoware_costmap_generator/src/costmap_generator.cpp +++ b/planning/autoware_costmap_generator/src/costmap_generator.cpp @@ -51,6 +51,7 @@ #include #include #include +#include #include #include @@ -68,14 +69,15 @@ namespace // Copied from scenario selector geometry_msgs::msg::PoseStamped::ConstSharedPtr getCurrentPose( - const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger) + const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr clock) { geometry_msgs::msg::TransformStamped tf_current_pose; try { tf_current_pose = tf_buffer.lookupTransform("map", "base_link", tf2::TimePointZero); } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(logger, "%s", ex.what()); + RCLCPP_ERROR_THROTTLE(logger, *clock, 5000, "%s", ex.what()); return nullptr; } @@ -253,7 +255,7 @@ void CostmapGenerator::update_data() void CostmapGenerator::set_current_pose() { - current_pose_ = getCurrentPose(tf_buffer_, this->get_logger()); + current_pose_ = getCurrentPose(tf_buffer_, this->get_logger(), this->get_clock()); } void CostmapGenerator::onTimer() diff --git a/planning/autoware_costmap_generator/src/utils/objects_to_costmap.cpp b/planning/autoware_costmap_generator/src/utils/objects_to_costmap.cpp index 64f558b4f5604..6b1f4331f55ad 100644 --- a/planning/autoware_costmap_generator/src/utils/objects_to_costmap.cpp +++ b/planning/autoware_costmap_generator/src/utils/objects_to_costmap.cpp @@ -200,10 +200,10 @@ grid_map::Matrix ObjectsToCostmap::makeCostmapFromObjects( grid_map::Polygon polygon; if (object.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { polygon = makePolygonFromObjectConvexHull(in_objects->header, object, expand_polygon_size); - } else if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { // NOLINT polygon = makePolygonFromObjectBox(in_objects->header, object, expand_polygon_size); } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { - // TODO(Kenji Miyake): Add makePolygonFromObjectCylinder + // TODO(Kenji Miyake): Add makePolygonFromObjectCylinder and remove NOLINT polygon = makePolygonFromObjectBox(in_objects->header, object, expand_polygon_size); } const auto highest_probability_label = *std::max_element( diff --git a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp index 56d6526e26272..255288603016b 100644 --- a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -115,7 +115,7 @@ void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & cost std::vector is_obstacle_table; is_obstacle_table.resize(nb_of_cells); for (uint32_t i = 0; i < nb_of_cells; ++i) { - const int cost = costmap_.data[i]; + const int cost = costmap_.data[i]; // NOLINT if (cost < 0 || planner_common_param_.obstacle_threshold <= cost) { is_obstacle_table[i] = true; } diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp index 0a17b112eed6f..c9f4b46dab737 100644 --- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp @@ -388,7 +388,7 @@ double AstarSearch::getExpansionDistance(const AstarNode & current_node) const double AstarSearch::getSteeringCost(const int steering_index) const { return planner_common_param_.curve_weight * - (abs(steering_index) / planner_common_param_.turning_steps); + (static_cast(abs(steering_index)) / planner_common_param_.turning_steps); } double AstarSearch::getSteeringChangeCost( diff --git a/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp b/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp index cbdb2542b97e7..e18190a8bf9e7 100644 --- a/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp +++ b/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp @@ -76,14 +76,14 @@ ColorRGBA ObjectsOfInterestMarkerInterface::getColor( const ColorName & color_name, const float alpha) { switch (color_name) { + case ColorName::GRAY: + return coloring::getGray(alpha); case ColorName::GREEN: return coloring::getGreen(alpha); case ColorName::AMBER: return coloring::getAmber(alpha); case ColorName::RED: return coloring::getRed(alpha); - case ColorName::GRAY: - return coloring::getGray(alpha); default: return coloring::getGray(alpha); } diff --git a/planning/autoware_obstacle_cruise_planner/README.md b/planning/autoware_obstacle_cruise_planner/README.md index 42a569c07ddb2..273363489d56f 100644 --- a/planning/autoware_obstacle_cruise_planner/README.md +++ b/planning/autoware_obstacle_cruise_planner/README.md @@ -75,6 +75,10 @@ The obstacles not in front of the ego will be ignored. ![determine_cruise_stop_slow_down](./media/determine_cruise_stop_slow_down.drawio.svg) +The behavior determination flowchart is shown below. + +![behavior_determination_flowchart](./media/behavior_determination_flowchart.drawio.svg) + #### Determine cruise vehicles The obstacles meeting the following condition are determined as obstacles for cruising. @@ -241,6 +245,10 @@ $$ | `lpf(val)` | apply low-pass filter to `val` | | `pid(val)` | apply pid to `val` | +#### Block diagram + +![cruise_planning_block_diagram](./media/cruise_planning_block_diagram.drawio.svg) + ### Slow down planning | Parameter | Type | Description | diff --git a/planning/autoware_obstacle_cruise_planner/media/behavior_determination_flowchart.drawio.svg b/planning/autoware_obstacle_cruise_planner/media/behavior_determination_flowchart.drawio.svg new file mode 100644 index 0000000000000..e37e2250ed3f8 --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/media/behavior_determination_flowchart.drawio.svg @@ -0,0 +1,1961 @@ + + + + + + + + + + + + + + + + + +
+
+
+ + Stop obstacle + +
+
+
+ + + + + + + + + + + + + + +
+
+
+ + Slow down + +
+ + obstacle + +
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ + Object + +
+ (PredictedObject / PointCloud) +
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ + Cruise obstacle + +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Type + for +
+ cruising? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Type + for +
+ stop? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Should +
maintain stop?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Previously +
stop obstacle?
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ Previously +
slow down obstacle?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Lateral +
distance > C1?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Ego + driving +
+ forward? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Moving in +
+ same + direction? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Lateral +
+ distance + < A1? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Not + crossing +
ego's trajectory?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Inside ego's +
trajectory?
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Ego moving? +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Lateral +
+ distance + < B1? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Type for +
outside stop?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Time to approach +
+ ego's + trajectory < B2? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Type for +
outside cruising?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Type for +
inside cruising?
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+
Collision
+
predicted?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Type for +
inside stop?
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Longitudinal +
velocity > A3?
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Longitudinal +
velocity > A4?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Previously +
cruise obstacle?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Longitudinal +
velocity > A5?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
Collision
+
predicted?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
Collision
+
predicted?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
+ Not + transient +
+
obstacle?
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Type + for +
+ slow down? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Lateral +
distance < C2?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ decrement +
count
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ increment +
count
+
+
+
+ + + + + + + + + + + + + + +
+
+
count > -C3?
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
count > C4?
+
+
+ + + + + + + + + + + + + + +
+
+
+
Collision
+
predicted?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
reset
+
count
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + A1: + + + behavior_determination.cruise.max_lat_margin +
+ +
+ A2: + + behavior_determination.cruise.outside_obstacle.max_lateral_time_margin +
+ +
+
+ A3: + + behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop +
+ +
+
+ A4: + + behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise +
+ +
+
+ A5: + + behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold +
+ +
+
+ +
+ +
+
+ B1: + + behavior_determination.stop.max_lat_margin +
+ +
+
+ B2: + + behavior_determination.stop.outside_obstacle.max_lateral_time_margin +
+ +
+
+ B3: + + behavior_determination.stop.crossing_obstacle.collision_time_margin +
+ +
+
+ +
+ +
+
+ C1: + behavior_determination.slow_down.max_lat_margin +
+
+ + + behavior_determination.slow_down.lat_hysteresis_margin + +
+ +
+
+ C2: + behavior_determination.slow_down.max_lat_margin +
+
+ - + behavior_determination.slow_down.lat_hysteresis_margin + +
+ +
+
+ C3: + + behavior_determination.slow_down.successive_num_to_exit_slow_down_condition +
+ +
+
+ C4: + + behavior_determination.slow_down.successive_num_to_entry_slow_down_condition +
+ +
+
+ +
+ +
+
+ D1: + + behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise +
+ +
+
+ D2: + + behavior_determination.stop_obstacle_hold_time_threshold +
+ +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Time to approach +
+ ego's + trajectory < A2? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Should maintain stop? +
+
+
+ Transient obstacle? +
+
+
+
Collision predicted?
+
+
+
+ + + + + + + + + + + + + + +
+
+
Obstacle velocity < D1 AND elapsed time since last stop decision < D2
+
+
Time to reach collision point > B3 OR collision not predicted at the time of reaching collision point
+
+
+
+
At least 1 vehicle footprint polygon at each point on trajectory intersects object footprint polygon
+
+
+
+ + + + + + + + + + + + + + + + + diff --git a/planning/autoware_obstacle_cruise_planner/media/cruise_planning_block_diagram.drawio.svg b/planning/autoware_obstacle_cruise_planner/media/cruise_planning_block_diagram.drawio.svg new file mode 100644 index 0000000000000..c6b29be2b1096 --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/media/cruise_planning_block_diagram.drawio.svg @@ -0,0 +1,989 @@ + + + + + + + + + + + + + + + + + + + +
+
+
+ Obstacle velocity +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Obstacle acceleration +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Ego velocity / acceleration +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Actual distance to obstacle +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+
Normalize
+
&
+ LPF +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ PID +
&
+
suppress output
+
when accelerating
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ Ideal distance to obstacle +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Distance error +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Normalized distance error +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Target acceleration +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+
RSS
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Actual distance to obstacle +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + +
+
+
+ + + + + + + + + + + + + + +
+
+
+ - +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ LPF +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Square +
(preserve sign)
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ Squared distance error +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Trajectory +
+
+
+ + + + + + + + + + diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/type_alias.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/type_alias.hpp index 5b16d4d5ba5c6..cf545d1a967e7 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/type_alias.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/type_alias.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__PATH_OPTIMIZER__TYPE_ALIAS_HPP_ #define AUTOWARE__PATH_OPTIMIZER__TYPE_ALIAS_HPP_ +#include "autoware_internal_debug_msgs/msg/string_stamped.hpp" #include "autoware_planning_msgs/msg/path.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -25,7 +26,6 @@ #include "nav_msgs/msg/odometry.hpp" #include "std_msgs/msg/header.hpp" #include "tier4_debug_msgs/msg/float64_stamped.hpp" -#include "tier4_debug_msgs/msg/string_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" namespace autoware::path_optimizer @@ -43,8 +43,8 @@ using nav_msgs::msg::Odometry; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; // debug +using autoware_internal_debug_msgs::msg::StringStamped; using tier4_debug_msgs::msg::Float64Stamped; -using tier4_debug_msgs::msg::StringStamped; } // namespace autoware::path_optimizer #endif // AUTOWARE__PATH_OPTIMIZER__TYPE_ALIAS_HPP_ diff --git a/planning/autoware_path_optimizer/package.xml b/planning/autoware_path_optimizer/package.xml index eb02db8ea9586..21ce487af6f4c 100644 --- a/planning/autoware_path_optimizer/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -14,6 +14,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_interpolation autoware_motion_utils autoware_osqp_interface diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/type_alias.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/type_alias.hpp index 5ec4b21955892..5f79689d20875 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/type_alias.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/type_alias.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__PATH_SMOOTHER__TYPE_ALIAS_HPP_ #define AUTOWARE__PATH_SMOOTHER__TYPE_ALIAS_HPP_ +#include "autoware_internal_debug_msgs/msg/string_stamped.hpp" #include "autoware_planning_msgs/msg/path.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -23,7 +24,6 @@ #include "nav_msgs/msg/odometry.hpp" #include "std_msgs/msg/header.hpp" #include "tier4_debug_msgs/msg/float64_stamped.hpp" -#include "tier4_debug_msgs/msg/string_stamped.hpp" namespace autoware::path_smoother { @@ -37,8 +37,8 @@ using autoware_planning_msgs::msg::TrajectoryPoint; // navigation using nav_msgs::msg::Odometry; // debug +using autoware_internal_debug_msgs::msg::StringStamped; using tier4_debug_msgs::msg::Float64Stamped; -using tier4_debug_msgs::msg::StringStamped; } // namespace autoware::path_smoother #endif // AUTOWARE__PATH_SMOOTHER__TYPE_ALIAS_HPP_ diff --git a/planning/autoware_path_smoother/package.xml b/planning/autoware_path_smoother/package.xml index 3fa7b9fa482ed..58bd970db541e 100644 --- a/planning/autoware_path_smoother/package.xml +++ b/planning/autoware_path_smoother/package.xml @@ -14,6 +14,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_interpolation autoware_motion_utils autoware_osqp_interface diff --git a/planning/autoware_path_smoother/src/elastic_band.cpp b/planning/autoware_path_smoother/src/elastic_band.cpp index 20786b7b3601e..8c751e698c8fb 100644 --- a/planning/autoware_path_smoother/src/elastic_band.cpp +++ b/planning/autoware_path_smoother/src/elastic_band.cpp @@ -53,9 +53,7 @@ Eigen::SparseMatrix makePMatrix(const int num_points) assign_value_to_triplet_vec(r, c, 6.0); } } else if (std::abs(c - r) == 1) { - if (r == 0 || r == num_points - 1) { - assign_value_to_triplet_vec(r, c, -2.0); - } else if (c == 0 || c == num_points - 1) { + if (r == 0 || r == num_points - 1 || c == 0 || c == num_points - 1) { assign_value_to_triplet_vec(r, c, -2.0); } else { assign_value_to_triplet_vec(r, c, -4.0); diff --git a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp index 5081b14dda63b..8a075594ebd3a 100644 --- a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp +++ b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp @@ -35,7 +35,7 @@ using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; using RouteSections = std::vector; -Pose createPoseFromLaneID( +inline Pose createPoseFromLaneID( const lanelet::Id & lane_id, const std::string & package_name = "autoware_test_utils", const std::string & map_filename = "lanelet2_map.osm") { @@ -70,7 +70,7 @@ Pose createPoseFromLaneID( // Function to create a route from given start and goal lanelet ids // start pose and goal pose are set to the middle of the lanelet -LaneletRoute makeBehaviorRouteFromLaneId( +inline LaneletRoute makeBehaviorRouteFromLaneId( const int & start_lane_id, const int & goal_lane_id, const std::string & package_name = "autoware_test_utils", const std::string & map_filename = "lanelet2_map.osm") @@ -119,7 +119,7 @@ LaneletRoute makeBehaviorRouteFromLaneId( return route; } -Odometry makeInitialPoseFromLaneId(const lanelet::Id & lane_id) +inline Odometry makeInitialPoseFromLaneId(const lanelet::Id & lane_id) { Odometry current_odometry; current_odometry.pose.pose = createPoseFromLaneID(lane_id); diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index 01e52d85f01c3..89930fd4a23a9 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -1163,32 +1164,40 @@ lanelet::ConstLanelets RouteHandler::getAllLeftSharedLinestringLanelets( const bool & invert_opposite) const noexcept { lanelet::ConstLanelets linestring_shared; - auto lanelet_at_left = getLeftLanelet(lane); - auto lanelet_at_left_opposite = getLeftOppositeLanelets(lane); - while (lanelet_at_left) { - linestring_shared.push_back(lanelet_at_left.value()); - lanelet_at_left = getLeftLanelet(lanelet_at_left.value()); - if (!lanelet_at_left) { - break; + try { + auto lanelet_at_left = getLeftLanelet(lane); + auto lanelet_at_left_opposite = getLeftOppositeLanelets(lane); + while (lanelet_at_left) { + linestring_shared.push_back(lanelet_at_left.value()); + lanelet_at_left = getLeftLanelet(lanelet_at_left.value()); + if (!lanelet_at_left) { + break; + } + lanelet_at_left_opposite = getLeftOppositeLanelets(lanelet_at_left.value()); } - lanelet_at_left_opposite = getLeftOppositeLanelets(lanelet_at_left.value()); - } - if (!lanelet_at_left_opposite.empty() && include_opposite) { - if (invert_opposite) { - linestring_shared.push_back(lanelet_at_left_opposite.front().invert()); - } else { - linestring_shared.push_back(lanelet_at_left_opposite.front()); - } - auto lanelet_at_right = getRightLanelet(lanelet_at_left_opposite.front()); - while (lanelet_at_right) { + if (!lanelet_at_left_opposite.empty() && include_opposite) { if (invert_opposite) { - linestring_shared.push_back(lanelet_at_right.value().invert()); + linestring_shared.push_back(lanelet_at_left_opposite.front().invert()); } else { - linestring_shared.push_back(lanelet_at_right.value()); + linestring_shared.push_back(lanelet_at_left_opposite.front()); + } + auto lanelet_at_right = getRightLanelet(lanelet_at_left_opposite.front()); + while (lanelet_at_right) { + if (invert_opposite) { + linestring_shared.push_back(lanelet_at_right.value().invert()); + } else { + linestring_shared.push_back(lanelet_at_right.value()); + } + lanelet_at_right = getRightLanelet(lanelet_at_right.value()); } - lanelet_at_right = getRightLanelet(lanelet_at_right.value()); } + } catch (const std::exception & e) { + std::cerr << "Exception in getAllLeftSharedLinestringLanelets: " << e.what() << std::endl; + return {}; + } catch (...) { + std::cerr << "Unknown exception in getAllLeftSharedLinestringLanelets" << std::endl; + return {}; } return linestring_shared; } @@ -1198,32 +1207,40 @@ lanelet::ConstLanelets RouteHandler::getAllRightSharedLinestringLanelets( const bool & invert_opposite) const noexcept { lanelet::ConstLanelets linestring_shared; - auto lanelet_at_right = getRightLanelet(lane); - auto lanelet_at_right_opposite = getRightOppositeLanelets(lane); - while (lanelet_at_right) { - linestring_shared.push_back(lanelet_at_right.value()); - lanelet_at_right = getRightLanelet(lanelet_at_right.value()); - if (!lanelet_at_right) { - break; + try { + auto lanelet_at_right = getRightLanelet(lane); + auto lanelet_at_right_opposite = getRightOppositeLanelets(lane); + while (lanelet_at_right) { + linestring_shared.push_back(lanelet_at_right.value()); + lanelet_at_right = getRightLanelet(lanelet_at_right.value()); + if (!lanelet_at_right) { + break; + } + lanelet_at_right_opposite = getRightOppositeLanelets(lanelet_at_right.value()); } - lanelet_at_right_opposite = getRightOppositeLanelets(lanelet_at_right.value()); - } - if (!lanelet_at_right_opposite.empty() && include_opposite) { - if (invert_opposite) { - linestring_shared.push_back(lanelet_at_right_opposite.front().invert()); - } else { - linestring_shared.push_back(lanelet_at_right_opposite.front()); - } - auto lanelet_at_left = getLeftLanelet(lanelet_at_right_opposite.front()); - while (lanelet_at_left) { + if (!lanelet_at_right_opposite.empty() && include_opposite) { if (invert_opposite) { - linestring_shared.push_back(lanelet_at_left.value().invert()); + linestring_shared.push_back(lanelet_at_right_opposite.front().invert()); } else { - linestring_shared.push_back(lanelet_at_left.value()); + linestring_shared.push_back(lanelet_at_right_opposite.front()); + } + auto lanelet_at_left = getLeftLanelet(lanelet_at_right_opposite.front()); + while (lanelet_at_left) { + if (invert_opposite) { + linestring_shared.push_back(lanelet_at_left.value().invert()); + } else { + linestring_shared.push_back(lanelet_at_left.value()); + } + lanelet_at_left = getLeftLanelet(lanelet_at_left.value()); } - lanelet_at_left = getLeftLanelet(lanelet_at_left.value()); } + } catch (const std::exception & e) { + std::cerr << "Exception in getAllRightSharedLinestringLanelets: " << e.what() << std::endl; + return {}; + } catch (...) { + std::cerr << "Unknown exception in getAllRightSharedLinestringLanelets" << std::endl; + return {}; } return linestring_shared; } diff --git a/planning/autoware_static_centerline_generator/src/main.cpp b/planning/autoware_static_centerline_generator/src/main.cpp index 811d57c6036ef..71a076ee86fe0 100644 --- a/planning/autoware_static_centerline_generator/src/main.cpp +++ b/planning/autoware_static_centerline_generator/src/main.cpp @@ -14,38 +14,46 @@ #include "static_centerline_generator_node.hpp" +#include #include #include int main(int argc, char * argv[]) { - rclcpp::init(argc, argv); - - // initialize node - rclcpp::NodeOptions node_options; - auto node = - std::make_shared( - node_options); - - // get ros parameter - const auto mode = node->declare_parameter("mode"); - - // process - if (mode == "AUTO") { - node->generate_centerline(); - node->validate(); - node->save_map(); - } else if (mode == "GUI") { - node->generate_centerline(); - } else if (mode == "VMB") { - // Do nothing - } else { - throw std::invalid_argument("The `mode` is invalid."); + try { + rclcpp::init(argc, argv); + + // initialize node + rclcpp::NodeOptions node_options; + auto node = + std::make_shared( + node_options); + + // get ros parameter + const auto mode = node->declare_parameter("mode"); + + // process + if (mode == "AUTO") { + node->generate_centerline(); + node->validate(); + node->save_map(); + } else if (mode == "GUI") { + node->generate_centerline(); + } else if (mode == "VMB") { + // Do nothing + } else { + throw std::invalid_argument("The `mode` is invalid."); + } + + // NOTE: spin node to keep showing debug path/trajectory in rviz with transient local + rclcpp::spin(node); + rclcpp::shutdown(); + } catch (const std::exception & e) { + std::cerr << "Exception in main(): " << e.what() << std::endl; + return {}; + } catch (...) { + std::cerr << "Unknown exception in main()" << std::endl; + return {}; } - - // NOTE: spin node to keep showing debug path/trajectory in rviz with transient local - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; } diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index 069363d2f65e0..fc0066b1a84f3 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -39,12 +39,12 @@ #include #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" +#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "tier4_debug_msgs/msg/float32_stamped.hpp" // temporary -#include "tier4_debug_msgs/msg/float64_stamped.hpp" // temporary #include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" // temporary #include "tier4_planning_msgs/msg/velocity_limit.hpp" // temporary #include "visualization_msgs/msg/marker_array.hpp" @@ -62,12 +62,12 @@ using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_internal_debug_msgs::msg::Float32Stamped; +using autoware_internal_debug_msgs::msg::Float64Stamped; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using nav_msgs::msg::Odometry; -using tier4_debug_msgs::msg::Float32Stamped; // temporary -using tier4_debug_msgs::msg::Float64Stamped; // temporary using tier4_planning_msgs::msg::StopSpeedExceeded; // temporary using tier4_planning_msgs::msg::VelocityLimit; // temporary using visualization_msgs::msg::MarkerArray; diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index 96462602d8da0..3d1252ff0b7a6 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -21,6 +21,7 @@ autoware_cmake eigen3_cmake_module + autoware_internal_debug_msgs autoware_interpolation autoware_motion_utils autoware_osqp_interface @@ -34,7 +35,6 @@ rclcpp tf2 tf2_ros - tier4_debug_msgs tier4_planning_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 9dad3681f7570..3a1773324fe24 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -18,6 +18,7 @@ #include "autoware/behavior_path_goal_planner_module/decision_state.hpp" #include "autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp" #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp" #include "autoware/behavior_path_goal_planner_module/thread_data.hpp" #include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" @@ -151,13 +152,19 @@ bool checkOccupancyGridCollision( // freespace parking std::optional planFreespacePath( const FreespaceParkingRequest & req, const PredictedObjects & static_target_objects, - std::shared_ptr freespace_planner); + std::shared_ptr freespace_planner); bool isStopped( std::deque & odometry_buffer, const nav_msgs::msg::Odometry::ConstSharedPtr self_odometry, const double duration_lower, const double velocity_upper); +void sortPullOverPaths( + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, + const std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates, const PredictedObjects & static_target_objects, + rclcpp::Logger logger, std::vector & sorted_path_indices); + // Flag class for managing whether a certain callback is running in multi-threading class ScopedFlag { @@ -198,7 +205,7 @@ class FreespaceParkingPlanner std::mutex & freespace_parking_mutex, const std::optional & request, FreespaceParkingResponse & response, std::atomic & is_freespace_parking_cb_running, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr freespace_planner) + const std::shared_ptr freespace_planner) : mutex_(freespace_parking_mutex), request_(request), response_(response), @@ -218,7 +225,7 @@ class FreespaceParkingPlanner rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; - std::shared_ptr freespace_planner_; + std::shared_ptr freespace_planner_; bool isStuck( const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp index ee370fd96b9a0..e32488965f69a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp @@ -36,6 +36,8 @@ class FreespacePullOver : public PullOverPlannerBase PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } + void setMap(const nav_msgs::msg::OccupancyGrid & costmap) { planner_->setMap(costmap); } + std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml index 232c548b28da3..78c4ea7e4c609 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml @@ -23,6 +23,7 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common autoware_bezier_sampler + autoware_freespace_planning_algorithms autoware_motion_utils autoware_rtc_interface autoware_test_utils diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 8e935c73c6ce9..60c056280e4db 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -16,7 +16,6 @@ #include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" #include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" -#include "autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp" #include "autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp" #include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" #include "autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp" @@ -225,7 +224,7 @@ bool checkOccupancyGridCollision( std::optional planFreespacePath( const FreespaceParkingRequest & req, const PredictedObjects & static_target_objects, - std::shared_ptr freespace_planner) + std::shared_ptr freespace_planner) { auto goal_candidates = req.goal_candidates_; auto goal_searcher = std::make_shared(req.parameters_, req.vehicle_footprint_); @@ -238,8 +237,8 @@ std::optional planFreespacePath( if (!goal_candidate.is_safe) { continue; } - // TODO(soblin): this calls setMap() in freespace_planner in the for-loop, which is very - // inefficient + + freespace_planner->setMap(*(req.get_planner_data()->costmap)); const auto freespace_path = freespace_planner->plan( goal_candidate, 0, req.get_planner_data(), BehaviorModuleOutput{} // NOTE: not used so passing {} is OK @@ -982,63 +981,14 @@ BehaviorModuleOutput GoalPlannerModule::plan() return fixed_goal_planner_->plan(planner_data_); } -std::optional GoalPlannerModule::selectPullOverPath( - const PullOverContextData & context_data, +void sortPullOverPaths( + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates) const + const GoalCandidates & goal_candidates, const PredictedObjects & static_target_objects, + rclcpp::Logger logger, std::vector & sorted_path_indices) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - - const auto & goal_pose = planner_data_->route_handler->getOriginalGoalPose(); - const double backward_length = - parameters_->backward_goal_search_length + parameters_->decide_path_distance; - const auto & prev_module_output_path = getPreviousModuleOutput().path; - const auto & soft_margins = parameters_->object_recognition_collision_check_soft_margins; - const auto & hard_margins = parameters_->object_recognition_collision_check_hard_margins; - - // STEP1: Filter valid paths before sorting - // NOTE: Since copying pull over path takes time, it is handled by indices - std::vector sorted_path_indices; - sorted_path_indices.reserve(pull_over_path_candidates.size()); - - // STEP1-1: Extract paths which have safe goal - // Create a map of goal_id to GoalCandidate for quick access - std::unordered_map goal_candidate_map; - for (const auto & goal_candidate : goal_candidates) { - goal_candidate_map[goal_candidate.id] = goal_candidate; - } - for (size_t i = 0; i < pull_over_path_candidates.size(); ++i) { - const auto & path = pull_over_path_candidates[i]; - const auto goal_candidate_it = goal_candidate_map.find(path.goal_id()); - if (goal_candidate_it != goal_candidate_map.end() && goal_candidate_it->second.is_safe) { - sorted_path_indices.push_back(i); - } - } - - // STEP1-2: Remove paths which do not have enough distance - const double prev_path_front_to_goal_dist = calcSignedArcLength( - prev_module_output_path.points, prev_module_output_path.points.front().point.pose.position, - goal_pose.position); - const auto & long_tail_reference_path = [&]() { - if (prev_path_front_to_goal_dist > backward_length) { - return prev_module_output_path; - } - // get road lanes which is at least backward_length[m] behind the goal - const auto road_lanes = utils::getExtendedCurrentLanesFromPath( - prev_module_output_path, planner_data_, backward_length, 0.0, false); - const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; - return planner_data_->route_handler->getCenterLinePath( - road_lanes, std::max(0.0, goal_pose_length - backward_length), - goal_pose_length + parameters_->forward_goal_search_length); - }(); - - sorted_path_indices.erase( - std::remove_if( - sorted_path_indices.begin(), sorted_path_indices.end(), - [&](const size_t i) { - return !hasEnoughDistance(pull_over_path_candidates[i], long_tail_reference_path); - }), - sorted_path_indices.end()); + const auto & soft_margins = parameters.object_recognition_collision_check_soft_margins; + const auto & hard_margins = parameters.object_recognition_collision_check_hard_margins; // STEP2: Sort pull over path candidates // STEP2-1: Sort pull_over_path_candidates based on the order in goal_candidates @@ -1066,14 +1016,14 @@ std::optional GoalPlannerModule::selectPullOverPath( // compare to sort pull_over_path_candidates based on the order in efficient_path_order const auto comparePathTypePriority = [&](const PullOverPath & a, const PullOverPath & b) -> bool { - const auto & order = parameters_->efficient_path_order; + const auto & order = parameters.efficient_path_order; const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type())); const auto b_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(b.type())); return a_pos < b_pos; }; // if object recognition is enabled, sort by collision check margin - if (parameters_->use_object_recognition) { + if (parameters.use_object_recognition) { // STEP2-2: Sort by collision check margins const auto [margins, margins_with_zero] = std::invoke([&]() -> std::tuple, std::vector> { @@ -1086,11 +1036,11 @@ std::optional GoalPlannerModule::selectPullOverPath( // Create a map of PullOverPath pointer to largest collision check margin std::map path_id_to_rough_margin_map; - const auto & target_objects = context_data.static_target_objects; + const auto & target_objects = static_target_objects; for (const size_t i : sorted_path_indices) { const auto & path = pull_over_path_candidates[i]; const double distance = utils::path_safety_checker::calculateRoughDistanceToObjects( - path.parking_path(), target_objects, planner_data_->parameters, false, "max"); + path.parking_path(), target_objects, planner_data->parameters, false, "max"); auto it = std::lower_bound( margins_with_zero.begin(), margins_with_zero.end(), distance, std::greater()); if (it == margins_with_zero.end()) { @@ -1120,7 +1070,7 @@ std::optional GoalPlannerModule::selectPullOverPath( // STEP2-3: Sort by curvature // If the curvature is less than the threshold, prioritize the path. const auto isHighCurvature = [&](const PullOverPath & path) -> bool { - return path.parking_path_max_curvature() >= parameters_->high_curvature_threshold; + return path.parking_path_max_curvature() >= parameters.high_curvature_threshold; }; const auto isSoftMargin = [&](const PullOverPath & path) -> bool { @@ -1156,7 +1106,7 @@ std::optional GoalPlannerModule::selectPullOverPath( // STEP2-4: Sort pull_over_path_candidates based on the order in efficient_path_order keeping // the collision check margin and curvature priority. - if (parameters_->path_priority == "efficient_path") { + if (parameters.path_priority == "efficient_path") { std::stable_sort( sorted_path_indices.begin(), sorted_path_indices.end(), [&](const size_t a_i, const size_t b_i) { @@ -1183,14 +1133,14 @@ std::optional GoalPlannerModule::selectPullOverPath( const std::string path_priority_info_str = goal_planner_utils::makePathPriorityDebugMessage( sorted_path_indices, pull_over_path_candidates, goal_id_to_index, goal_candidates, path_id_to_rough_margin_map, isSoftMargin, isHighCurvature); - RCLCPP_DEBUG_STREAM(getLogger(), path_priority_info_str); + RCLCPP_DEBUG_STREAM(logger, path_priority_info_str); } else { /** * NOTE: use_object_recognition=false is not recommended. This option will be deprecated in the * future. sort by curvature is not implemented yet. * Sort pull_over_path_candidates based on the order in efficient_path_order */ - if (parameters_->path_priority == "efficient_path") { + if (parameters.path_priority == "efficient_path") { std::stable_sort( sorted_path_indices.begin(), sorted_path_indices.end(), [&](const size_t a_i, const size_t b_i) { @@ -1203,6 +1153,67 @@ std::optional GoalPlannerModule::selectPullOverPath( }); } } +} + +std::optional GoalPlannerModule::selectPullOverPath( + const PullOverContextData & context_data, + const std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates) const +{ + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto & goal_pose = planner_data_->route_handler->getOriginalGoalPose(); + const double backward_length = + parameters_->backward_goal_search_length + parameters_->decide_path_distance; + const auto & prev_module_output_path = getPreviousModuleOutput().path; + + // STEP1: Filter valid paths before sorting + // NOTE: Since copying pull over path takes time, it is handled by indices + std::vector sorted_path_indices; + sorted_path_indices.reserve(pull_over_path_candidates.size()); + + // STEP1-1: Extract paths which have safe goal + // Create a map of goal_id to GoalCandidate for quick access + std::unordered_map goal_candidate_map; + for (const auto & goal_candidate : goal_candidates) { + goal_candidate_map[goal_candidate.id] = goal_candidate; + } + for (size_t i = 0; i < pull_over_path_candidates.size(); ++i) { + const auto & path = pull_over_path_candidates[i]; + const auto goal_candidate_it = goal_candidate_map.find(path.goal_id()); + if (goal_candidate_it != goal_candidate_map.end() && goal_candidate_it->second.is_safe) { + sorted_path_indices.push_back(i); + } + } + + // STEP1-2: Remove paths which do not have enough distance + const double prev_path_front_to_goal_dist = calcSignedArcLength( + prev_module_output_path.points, prev_module_output_path.points.front().point.pose.position, + goal_pose.position); + const auto & long_tail_reference_path = [&]() { + if (prev_path_front_to_goal_dist > backward_length) { + return prev_module_output_path; + } + // get road lanes which is at least backward_length[m] behind the goal + const auto road_lanes = utils::getExtendedCurrentLanesFromPath( + prev_module_output_path, planner_data_, backward_length, 0.0, false); + const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; + return planner_data_->route_handler->getCenterLinePath( + road_lanes, std::max(0.0, goal_pose_length - backward_length), + goal_pose_length + parameters_->forward_goal_search_length); + }(); + + sorted_path_indices.erase( + std::remove_if( + sorted_path_indices.begin(), sorted_path_indices.end(), + [&](const size_t i) { + return !hasEnoughDistance(pull_over_path_candidates[i], long_tail_reference_path); + }), + sorted_path_indices.end()); + + sortPullOverPaths( + planner_data_, *parameters_, pull_over_path_candidates, goal_candidates, + context_data.static_target_objects, getLogger(), sorted_path_indices); // STEP3: Select the final pull over path by checking collision to make it as high priority as // possible diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp index b51d5df42c75b..5b11d1b170ce1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp @@ -64,8 +64,6 @@ std::optional FreespacePullOver::plan( { const Pose & current_pose = planner_data->self_odometry->pose.pose; - planner_->setMap(*planner_data->costmap); - // offset goal pose to make straight path near goal for improving parking precision // todo: support straight path when using back constexpr double straight_distance = 1.0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp index de231b78041b4..472adb2d57944 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp @@ -92,7 +92,7 @@ TEST_F(TestUtilWithMap, getBusStopAreaPolygons) const auto shoulder_lanes = lanelet::utils::query::shoulderLanelets(lanes); const auto bus_stop_area_polygons = autoware::behavior_path_planner::goal_planner_utils::getBusStopAreaPolygons(shoulder_lanes); - EXPECT_EQ(bus_stop_area_polygons.size(), 1); + EXPECT_EQ(bus_stop_area_polygons.size(), 2); } TEST_F(DISABLED_TestUtilWithMap, isWithinAreas) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt index d1b6bfed868f0..0d56a28aaf450 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt @@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils/calculation.cpp src/utils/markers.cpp src/utils/utils.cpp + src/utils/path.cpp ) if(BUILD_TESTING) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 86d8d1c0c1413..5a6c22a731bf9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -886,6 +886,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | | `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | | `backward_length_from_intersection` | [m] | double | Distance threshold from the last intersection to invalidate or cancel the lane change path | 5.0 | +| `enable_stopped_vehicle_buffer` | [-] | bool | If true, will keep enough distance from current lane front stopped object to perform lane change when possible | true | | `trajectory.max_prepare_duration` | [s] | double | The maximum preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | | `trajectory.min_prepare_duration` | [s] | double | The minimum preparation time for the ego vehicle to be ready to perform lane change. | 2.0 | | `trajectory.lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index bf892b3058a16..61e41b4ea3ea4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -5,6 +5,7 @@ backward_length_buffer_for_end_of_lane: 3.0 # [m] backward_length_buffer_for_blocking_object: 3.0 # [m] backward_length_from_intersection: 5.0 # [m] + enable_stopped_vehicle_buffer: true # turn signal min_length_for_turn_signal_activation: 10.0 # [m] diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp index 5317b94db2d6b..1061e07e516a2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp @@ -18,6 +18,7 @@ #include "autoware/behavior_path_lane_change_module/structs/debug.hpp" #include "autoware/behavior_path_lane_change_module/structs/path.hpp" #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -38,6 +39,7 @@ class TestNormalLaneChange; namespace autoware::behavior_path_planner { +using autoware::behavior_path_planner::PoseWithDetailOpt; using autoware::route_handler::Direction; using autoware::universe_utils::StopWatch; using geometry_msgs::msg::Point; @@ -102,8 +104,6 @@ class LaneChangeBase virtual LaneChangePath getLaneChangePath() const = 0; - virtual BehaviorModuleOutput getTerminalLaneChangePath() const = 0; - virtual bool isEgoOnPreparePhase() const = 0; virtual bool isRequiredStop(const bool is_trailing_object) = 0; @@ -224,7 +224,7 @@ class LaneChangeBase return direction_; } - std::optional getStopPose() const { return lane_change_stop_pose_; } + PoseWithDetailOpt getStopPose() const { return lane_change_stop_pose_; } void resetStopPose() { lane_change_stop_pose_ = std::nullopt; } @@ -235,9 +235,6 @@ class LaneChangeBase protected: virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; - virtual bool get_prepare_segment( - PathWithLaneId & prepare_segment, const double prepare_length) const = 0; - virtual bool isValidPath(const PathWithLaneId & path) const = 0; virtual bool isAbleToStopSafely() const = 0; @@ -287,7 +284,7 @@ class LaneChangeBase lane_change::CommonDataPtr common_data_ptr_; FilteredLanesObjects filtered_objects_{}; BehaviorModuleOutput prev_module_output_{}; - std::optional lane_change_stop_pose_{std::nullopt}; + PoseWithDetailOpt lane_change_stop_pose_{std::nullopt}; PathWithLaneId prev_approved_path_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 9bf66722dcec2..5f1da79bc7ea0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -18,6 +18,7 @@ #include "autoware/behavior_path_lane_change_module/structs/data.hpp" #include +#include #include #include @@ -64,8 +65,6 @@ class NormalLaneChange : public LaneChangeBase LaneChangePath getLaneChangePath() const override; - BehaviorModuleOutput getTerminalLaneChangePath() const override; - BehaviorModuleOutput generateOutput() override; void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; @@ -128,14 +127,6 @@ class NormalLaneChange : public LaneChangeBase void filterOncomingObjects(PredictedObjects & objects) const; - bool get_prepare_segment( - PathWithLaneId & prepare_segment, const double prepare_length) const override; - - PathWithLaneId getTargetSegment( - const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, - const double target_lane_length, const double lane_changing_length, - const double lane_changing_velocity, const double buffer_for_next_lane_change) const; - std::vector get_prepare_metrics() const; std::vector get_lane_changing_metrics( const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metrics, @@ -143,32 +134,19 @@ class NormalLaneChange : public LaneChangeBase bool get_lane_change_paths(LaneChangePaths & candidate_paths) const; - LaneChangePath get_candidate_path( - const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, - const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, - const Pose & lc_start_pose, const double shift_length) const; - bool check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const; - std::optional calcTerminalLaneChangePath( - const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) const; - bool isValidPath(const PathWithLaneId & path) const override; PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, + const std::vector> & ego_predicted_paths, const lane_change::TargetObjects & collision_check_objects, const utils::path_safety_checker::RSSparams & rss_params, - const size_t deceleration_sampling_num, CollisionCheckDebugMap & debug_data) const; - - bool has_collision_with_decel_patterns( - const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects, - const size_t deceleration_sampling_num, const RSSparams & rss_param, - const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const; + CollisionCheckDebugMap & debug_data) const; - bool is_collided( + bool is_colliding( const LaneChangePath & lane_change_path, const ExtendedPredictedObject & obj, const std::vector & ego_predicted_path, const RSSparams & selected_rss_param, const bool check_prepare_phase, @@ -178,27 +156,10 @@ class NormalLaneChange : public LaneChangeBase bool is_ego_stuck() const; - /** - * @brief Checks if the given pose is a valid starting point for a lane change. - * - * This function determines whether the given pose (position) of the vehicle is within - * the area of either the target neighbor lane or the target lane itself. It uses geometric - * checks to see if the start point of the lane change is covered by the polygons representing - * these lanes. - * - * @param common_data_ptr Shared pointer to a CommonData structure, which should include: - * - Non-null `lanes_polygon_ptr` that contains the polygon data for lanes. - * @param pose The current pose of the vehicle - * - * @return `true` if the pose is within either the target neighbor lane or the target lane; - * `false` otherwise. - */ - bool is_valid_start_point( - const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) const; - bool check_prepare_phase() const; - void set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path); + void set_stop_pose( + const double arc_length_to_stop_pose, PathWithLaneId & path, const std::string & reason = ""); void updateStopTime(); @@ -213,7 +174,6 @@ class NormalLaneChange : public LaneChangeBase std::vector path_after_intersection_; double stop_time_{0.0}; - static constexpr double floating_err_th{1e-3}; }; } // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp index 4dcb1c7b685d0..5f36be806bba4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp @@ -160,6 +160,8 @@ struct TargetObjects : leading(std::move(leading)), trailing(std::move(trailing)) { } + + [[nodiscard]] bool empty() const { return leading.empty() && trailing.empty(); } }; enum class ModuleType { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp index ea9807ad1616f..90b13f86976b2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp @@ -25,10 +25,20 @@ #include #include +#include namespace autoware::behavior_path_planner::lane_change { using utils::path_safety_checker::CollisionCheckDebugMap; + +struct MetricsDebug +{ + LaneChangePhaseMetrics prep_metric; + std::vector lc_metrics; + double max_prepare_length; + double max_lane_changing_length; +}; + struct Debug { std::string module_type; @@ -41,11 +51,12 @@ struct Debug lanelet::ConstLanelets current_lanes; lanelet::ConstLanelets target_lanes; lanelet::ConstLanelets target_backward_lanes; + std::vector lane_change_metrics; double collision_check_object_debug_lifetime{0.0}; double distance_to_end_of_current_lane{std::numeric_limits::max()}; double distance_to_lane_change_finished{std::numeric_limits::max()}; double distance_to_abort_finished{std::numeric_limits::max()}; - bool is_able_to_return_to_current_lane{false}; + bool is_able_to_return_to_current_lane{true}; bool is_stuck{false}; bool is_abort{false}; @@ -64,12 +75,13 @@ struct Debug current_lanes.clear(); target_lanes.clear(); target_backward_lanes.clear(); + lane_change_metrics.clear(); collision_check_object_debug_lifetime = 0.0; distance_to_end_of_current_lane = std::numeric_limits::max(); distance_to_lane_change_finished = std::numeric_limits::max(); distance_to_abort_finished = std::numeric_limits::max(); - is_able_to_return_to_current_lane = false; + is_able_to_return_to_current_lane = true; is_stuck = false; is_abort = false; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp index 719bbbbf3057a..94020e8a08279 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp @@ -149,6 +149,7 @@ struct Parameters double backward_length_buffer_for_end_of_lane{0.0}; double backward_length_buffer_for_blocking_object{0.0}; double backward_length_from_intersection{5.0}; + bool enable_stopped_vehicle_buffer{false}; // parked vehicle double object_check_min_road_shoulder_width{0.5}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp index 4c2712f053b13..059db8e38300f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp @@ -35,7 +35,7 @@ using autoware::behavior_path_planner::lane_change::Debug; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using visualization_msgs::msg::MarkerArray; MarkerArray showAllValidLaneChangePath( - const std::vector & lanes, std::string && ns); + const std::vector & lane_change_paths, std::string && ns); MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp new file mode 100644 index 0000000000000..dcc327b4793e1 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp @@ -0,0 +1,102 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ + +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.hpp" + +#include + +#include + +namespace autoware::behavior_path_planner::utils::lane_change +{ +using behavior_path_planner::LaneChangePath; +using behavior_path_planner::lane_change::CommonDataPtr; + +/** + * @brief Generates a prepare segment for a lane change maneuver. + * + * This function generates the "prepare segment" of the path by trimming it to the specified length, + * adjusting longitudinal velocity for acceleration or deceleration, and ensuring the starting point + * meets necessary constraints for a lane change. + * + * @param common_data_ptr Shared pointer to CommonData containing current and target lane + * information, vehicle parameters, and ego state. + * @param prev_module_path The input path from the previous module, which will be used + * as the base for the prepare segment. + * @param prep_metric Preparation metrics containing desired length and velocity for the segment. + * @param prepare_segment Output parameter where the resulting prepare segment path will be stored. + * + * @throws std::logic_error If the lane change start point is behind the target lanelet or + * if lane information is invalid. + * + * @return true if the prepare segment is successfully generated, false otherwise. + */ +bool get_prepare_segment( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, + const LaneChangePhaseMetrics prep_metric, PathWithLaneId & prepare_segment); + +/** + * @brief Generates the candidate path for a lane change maneuver. + * + * This function calculates the candidate lane change path based on the provided preparation + * and lane change metrics. It resamples the target lane reference path, determines the start + * and end poses for the lane change, and constructs the shift line required for the maneuver. + * The function ensures that the resulting path satisfies length and distance constraints. + * + * @param common_data_ptr Shared pointer to CommonData containing route, lane, and transient data. + * @param prep_metric Metrics for the preparation phase, including path length and velocity. + * @param lc_metric Metrics for the lane change phase, including path length and velocity. + * @param prep_segment The path segment prepared before initiating the lane change. + * @param sorted_lane_ids A vector of sorted lane IDs used for updating lane information in the + * path. + * @param lc_start_pose The pose where the lane change begins. + * @param shift_length The lateral distance to shift during the lane change maneuver. + * + * @throws std::logic_error If the target lane reference path is empty, candidate path generation + * fails, or the resulting path length exceeds terminal constraints. + * + * @return LaneChangePath The constructed candidate lane change path. + */ +LaneChangePath get_candidate_path( + const CommonDataPtr & common_data_ptr, const LaneChangePhaseMetrics & prep_metric, + const LaneChangePhaseMetrics & lc_metric, const PathWithLaneId & prep_segment, + const std::vector> & sorted_lane_ids, const double shift_length); + +/** + * @brief Constructs a candidate path for a lane change maneuver. + * + * This function generates a candidate lane change path by shifting the target lane's reference path + * and combining it with the prepare segment. It verifies the path's validity by checking the yaw + * difference, adjusting longitudinal velocity, and updating lane IDs based on the provided lane + * sorting information. + * + * @param lane_change_info Information about the lane change, including shift line and target + * velocity. + * @param prepare_segment The path segment leading up to the lane change. + * @param target_lane_reference_path The reference path of the target lane. + * @param sorted_lane_ids A vector of sorted lane IDs used to update the candidate path's lane IDs. + * + * @return std::optional The constructed candidate path if valid, or std::nullopt + * if the path fails any constraints. + */ +LaneChangePath construct_candidate_path( + const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, + const PathWithLaneId & target_lane_reference_path, + const std::vector> & sorted_lane_ids); +} // namespace autoware::behavior_path_planner::utils::lane_change +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 0adb9409fb4b0..69362994dbbac 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -52,6 +52,7 @@ using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LanesPolygon; +using behavior_path_planner::lane_change::LCParamPtr; using behavior_path_planner::lane_change::ModuleType; using behavior_path_planner::lane_change::PathSafetyStatus; using behavior_path_planner::lane_change::TargetLaneLeadingObjects; @@ -61,12 +62,11 @@ using geometry_msgs::msg::Twist; using path_safety_checker::CollisionCheckDebugMap; using tier4_planning_msgs::msg::PathWithLaneId; -bool is_mandatory_lane_change(const ModuleType lc_type); +rclcpp::Logger get_logger(); -double calcLaneChangeResampleInterval( - const double lane_changing_length, const double lane_changing_velocity); +bool is_mandatory_lane_change(const ModuleType lc_type); -void setPrepareVelocity( +void set_prepare_velocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity); std::vector replaceWithSortedIds( @@ -79,27 +79,10 @@ lanelet::ConstLanelets get_target_neighbor_lanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const LaneChangeModuleType & type); -bool isPathInLanelets( - const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes); - bool path_footprint_exceeds_target_lane_bound( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, const double margin = 0.1); -std::optional construct_candidate_path( - const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path, - const std::vector> & sorted_lane_ids); - -ShiftLine get_lane_changing_shift_line( - const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, - const PathWithLaneId & reference_path, const double shift_length); - -PathWithLaneId get_reference_path_from_target_Lane( - const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, - const double lane_changing_length, const double resample_interval); - std::vector generateDrivableLanes( const std::vector & original_drivable_lanes, const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes); @@ -141,8 +124,8 @@ bool isParkedObject( * If the parameter delay_lc_param.check_only_parked_vehicle is set to True, only objects * which pass isParkedObject() check will be considered. * - * @param common_data_ptr Shared pointer to CommonData that holds necessary lanes info, parameters, - * and transient data. + * @param common_data_ptr Shared pointer to CommonData that holds necessary lanes info, + * parameters, and transient data. * @param lane_change_path Candidate lane change path to apply checks on. * @param target_objects Relevant objects to consider for delay LC checks (assumed to only include * target lane leading static objects). @@ -202,8 +185,8 @@ rclcpp::Logger getLogger(const std::string & type); * The footprint is determined by the vehicle's pose and its dimensions, including the distance * from the base to the front and rear ends of the vehicle, as well as its width. * - * @param common_data_ptr Shared pointer to CommonData that holds necessary ego vehicle's dimensions - * and pose information. + * @param common_data_ptr Shared pointer to CommonData that holds necessary ego vehicle's + * dimensions and pose information. * * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle. */ @@ -233,15 +216,15 @@ bool is_within_intersection( /** * @brief Determines if a polygon is within lanes designated for turning. * - * Checks if a polygon overlaps with lanelets tagged for turning directions (excluding 'straight'). - * It evaluates the lanelet's 'turn_direction' attribute and determines overlap with the lanelet's - * area. + * Checks if a polygon overlaps with lanelets tagged for turning directions (excluding + * 'straight'). It evaluates the lanelet's 'turn_direction' attribute and determines overlap with + * the lanelet's area. * * @param lanelet Lanelet representing the road segment whose turn direction is to be evaluated. * @param polygon The polygon to be checked for its presence within turn direction lanes. * - * @return bool True if the polygon is within a lane designated for turning, false if it is within a - * straight lane or no turn direction is specified. + * @return bool True if the polygon is within a lane designated for turning, false if it is within + * a straight lane or no turn direction is specified. */ bool is_within_turn_direction_lanes( const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); @@ -276,8 +259,8 @@ double get_distance_to_next_regulatory_element( * * @param common_data_ptr Pointer to the common data structure containing parameters for lane * change. - * @param filtered_objects A collection of objects filtered by lanes, including those in the current - * lane. + * @param filtered_objects A collection of objects filtered by lanes, including those in the + * current lane. * @param dist_to_target_lane_start The distance to the start of the target lane from the ego * vehicle. * @param path The current path of the ego vehicle, containing path points and lane information. @@ -297,8 +280,8 @@ double get_min_dist_to_current_lanes_obj( * * @param common_data_ptr Pointer to the common data structure containing parameters for the lane * change. - * @param filtered_objects A collection of objects filtered by lanes, including those in the target - * lane. + * @param filtered_objects A collection of objects filtered by lanes, including those in the + * target lane. * @param stop_arc_length The arc length at which the ego vehicle is expected to stop. * @param path The current path of the ego vehicle, containing path points and lane information. * @return true if there is an object in the target lane that influences the stop point decision; @@ -365,14 +348,15 @@ bool has_overtaking_turn_lane_object( * * @param common_data_ptr Shared pointer to CommonData containing information about current lanes, * vehicle dimensions, lane polygons, and behavior parameters. - * @param object An extended predicted object representing a potential obstacle in the environment. + * @param object An extended predicted object representing a potential obstacle in the + * environment. * @param dist_ego_to_current_lanes_center Distance from the ego vehicle to the center of the * current lanes. * @param ahead_of_ego Boolean flag indicating if the object is ahead of the ego vehicle. - * @param before_terminal Boolean flag indicating if the ego vehicle is before the terminal point of - * the lane. - * @param leading_objects Reference to a structure for storing leading objects (stopped, moving, or - * outside boundaries). + * @param before_terminal Boolean flag indicating if the ego vehicle is before the terminal point + * of the lane. + * @param leading_objects Reference to a structure for storing leading objects (stopped, moving, + * or outside boundaries). * @param trailing_objects Reference to a collection for storing trailing objects. * * @return true if the object is classified as either leading or trailing, false otherwise. @@ -415,5 +399,38 @@ std::vector get_preceding_lanes(const CommonDataPtr & co */ bool object_path_overlaps_lanes( const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon); + +/** + * @brief Converts a lane change path into multiple predicted paths with varying acceleration + * profiles. + * + * This function generates a set of predicted paths for the ego vehicle during a lane change, + * using different acceleration values within the specified range. It accounts for deceleration + * sampling if the global minimum acceleration differs from the lane-changing acceleration. + * + * @param common_data_ptr Shared pointer to CommonData containing parameters and state information. + * @param lane_change_path The lane change path used to generate predicted paths. + * @param deceleration_sampling_num Number of samples for deceleration profiles to generate paths. + * + * @return std::vector> A collection of predicted paths, where + * each path is represented as a series of poses with associated velocity. + */ +std::vector> convert_to_predicted_paths( + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, + const size_t deceleration_sampling_num); + +/** + * @brief Validates whether a given pose is a valid starting point for a lane change. + * + * This function checks if the specified pose lies within the polygons representing + * the target lane or its neighboring areas. This ensures that the starting point is + * appropriately positioned for initiating a lane change, even if previous paths were adjusted. + * + * @param common_data_ptr Shared pointer to CommonData containing lane polygon information. + * @param pose The pose to validate as a potential lane change starting point. + * + * @return true if the pose is within the target or target neighbor polygons, false otherwise. + */ +bool is_valid_start_point(const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose); } // namespace autoware::behavior_path_planner::utils::lane_change #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml index dd4dbe63e41d4..fad98ecf8f8e1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml @@ -26,6 +26,7 @@ autoware_motion_utils autoware_rtc_interface autoware_universe_utils + fmt pluginlib range-v3 rclcpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index d65e51997eb32..f80aad721a07c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -111,9 +111,7 @@ BehaviorModuleOutput LaneChangeInterface::plan() path_reference_ = std::make_shared(output.reference_path); *prev_approved_path_ = getPreviousModuleOutput().path; - const auto stop_pose_opt = module_type_->getStopPose(); - stop_pose_ = stop_pose_opt.has_value() ? PoseWithDetailOpt(PoseWithDetail(stop_pose_opt.value())) - : PoseWithDetailOpt(); + stop_pose_ = module_type_->getStopPose(); const auto & lane_change_debug = module_type_->getDebugData(); for (const auto & [uuid, data] : lane_change_debug.collision_check_objects_after_approval) { @@ -171,9 +169,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() } path_reference_ = std::make_shared(out.reference_path); - const auto stop_pose_opt = module_type_->getStopPose(); - stop_pose_ = stop_pose_opt.has_value() ? PoseWithDetailOpt(PoseWithDetail(stop_pose_opt.value())) - : PoseWithDetailOpt(); + stop_pose_ = module_type_->getStopPose(); if (!module_type_->isValidPath()) { path_candidate_ = std::make_shared(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 1bb41069140e7..9f7a811770b5d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -195,6 +195,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("backward_length_buffer_for_blocking_object")); p.backward_length_from_intersection = getOrDeclareParameter(*node, parameter("backward_length_from_intersection")); + p.enable_stopped_vehicle_buffer = + getOrDeclareParameter(*node, parameter("enable_stopped_vehicle_buffer")); if (p.backward_length_buffer_for_end_of_lane < 1.0) { RCLCPP_WARN_STREAM( @@ -305,6 +307,8 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectorbackward_length_buffer_for_blocking_object); updateParam( parameters, ns + "lane_change_finish_judge_buffer", p->lane_change_finish_judge_buffer); + updateParam( + parameters, ns + "enable_stopped_vehicle_buffer", p->enable_stopped_vehicle_buffer); updateParam( parameters, ns + "finish_judge_lateral_threshold", p->th_finish_judge_lateral_diff); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index a1c2d8467d1fc..fb4f7aeca1525 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -15,6 +15,7 @@ #include "autoware/behavior_path_lane_change_module/scene.hpp" #include "autoware/behavior_path_lane_change_module/utils/calculation.hpp" +#include "autoware/behavior_path_lane_change_module/utils/path.hpp" #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" @@ -31,9 +32,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -379,48 +380,6 @@ LaneChangePath NormalLaneChange::getLaneChangePath() const return status_.lane_change_path; } -BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const -{ - auto output = prev_module_output_; - - if (isAbortState() && abort_path_) { - output.path = abort_path_->path; - extendOutputDrivableArea(output); - const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( - output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, - output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, - planner_data_->parameters.ego_nearest_yaw_threshold); - return output; - } - - const auto & current_lanes = get_current_lanes(); - if (current_lanes.empty()) { - RCLCPP_DEBUG(logger_, "Current lanes not found. Returning previous module's path as output."); - return prev_module_output_; - } - - const auto terminal_path = - calcTerminalLaneChangePath(current_lanes, get_lane_change_lanes(current_lanes)); - if (!terminal_path) { - RCLCPP_DEBUG(logger_, "Terminal path not found. Returning previous module's path as output."); - return prev_module_output_; - } - - output.path = terminal_path->path; - output.turn_signal_info = updateOutputTurnSignal(); - - extendOutputDrivableArea(output); - - const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( - output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, - output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, - planner_data_->parameters.ego_nearest_yaw_threshold); - - return output; -} - BehaviorModuleOutput NormalLaneChange::generateOutput() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -449,7 +408,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() output.path.points, output.path.points.front().point.pose.position, getEgoPosition()); const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); - set_stop_pose(stop_dist + current_dist, output.path); + set_stop_pose(stop_dist + current_dist, output.path, "incoming rear object"); } else { insert_stop_point(get_target_lanes(), output.path); } @@ -512,7 +471,7 @@ void NormalLaneChange::insert_stop_point( if (!is_current_lane) { const auto arc_length_to_stop_pose = motion_utils::calcArcLength(path.points) - common_data_ptr_->transient_data.next_dist_buffer.min; - set_stop_pose(arc_length_to_stop_pose, path); + set_stop_pose(arc_length_to_stop_pose, path, "next lc terminal"); return; } @@ -555,8 +514,11 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) const auto dist_to_terminal_stop = std::min(dist_to_terminal_start, distance_to_last_fit_width); - if (filtered_objects_.current_lane.empty()) { - set_stop_pose(dist_to_terminal_stop, path); + const auto terminal_stop_reason = status_.is_valid_path ? "no safe path" : "no valid path"; + if ( + filtered_objects_.current_lane.empty() || + !lane_change_parameters_->enable_stopped_vehicle_buffer) { + set_stop_pose(dist_to_terminal_stop, path, terminal_stop_reason); return; } @@ -582,12 +544,12 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) const auto stop_arc_length_behind_obj = arc_length_to_current_obj - stop_margin; if (stop_arc_length_behind_obj < dist_to_target_lane_start) { - set_stop_pose(dist_to_target_lane_start, path); + set_stop_pose(dist_to_target_lane_start, path, "maintain distance to front object"); return; } if (stop_arc_length_behind_obj > dist_to_terminal_stop) { - set_stop_pose(dist_to_terminal_stop, path); + set_stop_pose(dist_to_terminal_stop, path, terminal_stop_reason); return; } @@ -603,11 +565,11 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) filtered_objects_.target_lane_leading, stop_arc_length_behind_obj, path); if (has_blocking_target_lane_obj || stop_arc_length_behind_obj <= 0.0) { - set_stop_pose(dist_to_terminal_stop, path); + set_stop_pose(dist_to_terminal_stop, path, terminal_stop_reason); return; } - set_stop_pose(stop_arc_length_behind_obj, path); + set_stop_pose(stop_arc_length_behind_obj, path, "maintain distance to front object"); } PathWithLaneId NormalLaneChange::getReferencePath() const @@ -922,43 +884,6 @@ int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) return std::abs(getRouteHandler()->getNumLaneToPreferredLane(lane, get_opposite_direction)); } -bool NormalLaneChange::get_prepare_segment( - PathWithLaneId & prepare_segment, const double prepare_length) const -{ - const auto & current_lanes = common_data_ptr_->lanes_ptr->current; - const auto & target_lanes = common_data_ptr_->lanes_ptr->target; - const auto backward_path_length = common_data_ptr_->bpp_param_ptr->backward_path_length; - - if (current_lanes.empty() || target_lanes.empty()) { - return false; - } - - prepare_segment = prev_module_output_.path; - const size_t current_seg_idx = - autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prepare_segment.points, getEgoPose(), 3.0, 1.0); - utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); - - if (prepare_segment.points.empty()) return false; - - const auto & lc_start_pose = prepare_segment.points.back().point.pose; - - // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is - // larger than distance to target lane start - if (!is_valid_start_point(common_data_ptr_, lc_start_pose)) return false; - - // lane changing start is at the end of prepare segment - const auto target_length_from_lane_change_start_pose = - utils::getArcLengthToTargetLanelet(current_lanes, target_lanes.front(), lc_start_pose); - - // Check if the lane changing start point is not on the lanes next to target lanes, - if (target_length_from_lane_change_start_pose > std::numeric_limits::epsilon()) { - throw std::logic_error("lane change start is behind target lanelet!"); - } - - return true; -} - lane_change::TargetObjects NormalLaneChange::get_target_objects( const FilteredLanesObjects & filtered_objects, [[maybe_unused]] const lanelet::ConstLanelets & current_lanes) const @@ -1025,8 +950,6 @@ FilteredLanesObjects NormalLaneChange::filter_objects() const filtered_objects.target_lane_trailing.reserve(reserve_size); filtered_objects.others.reserve(reserve_size); - const auto stopped_obj_vel_th = lane_change_parameters_->safety.th_stopped_object_velocity; - for (const auto & object : objects.objects) { auto ext_object = utils::lane_change::transform(object, *lane_change_parameters_); const auto & ext_obj_pose = ext_object.initial_pose; @@ -1045,12 +968,8 @@ FilteredLanesObjects NormalLaneChange::filter_objects() const continue; } - // TODO(Azu): We have to think about how to remove is_within_vel_th without breaking AW behavior - const auto is_moving = velocity_filter( - ext_object.initial_twist, stopped_obj_vel_th, std::numeric_limits::max()); - if ( - ahead_of_ego && is_moving && is_before_terminal && + ahead_of_ego && is_before_terminal && !boost::geometry::disjoint(ext_object.initial_polygon, lanes_polygon.current)) { // check only the objects that are in front of the ego vehicle filtered_objects.current_lane.push_back(ext_object); @@ -1107,41 +1026,6 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const }); } -PathWithLaneId NormalLaneChange::getTargetSegment( - const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, - const double target_lane_length, const double lane_changing_length, - const double lane_changing_velocity, const double buffer_for_next_lane_change) const -{ - const auto & route_handler = *getRouteHandler(); - const auto forward_path_length = planner_data_->parameters.forward_path_length; - - const double s_start = std::invoke([&lane_changing_start_pose, &target_lanes, - &lane_changing_length, &target_lane_length, - &buffer_for_next_lane_change]() { - const auto arc_to_start_pose = - lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); - const double dist_from_front_target_lanelet = arc_to_start_pose.length + lane_changing_length; - const double end_of_lane_dist_without_buffer = target_lane_length - buffer_for_next_lane_change; - return std::min(dist_from_front_target_lanelet, end_of_lane_dist_without_buffer); - }); - - const double s_end = std::invoke( - [&s_start, &forward_path_length, &target_lane_length, &buffer_for_next_lane_change]() { - const double dist_from_start = s_start + forward_path_length; - const double dist_from_end = target_lane_length - buffer_for_next_lane_change; - return std::max( - std::min(dist_from_start, dist_from_end), s_start + std::numeric_limits::epsilon()); - }); - - PathWithLaneId target_segment = route_handler.getCenterLinePath(target_lanes, s_start, s_end); - for (auto & point : target_segment.points) { - point.point.longitudinal_velocity_mps = - std::min(point.point.longitudinal_velocity_mps, static_cast(lane_changing_velocity)); - } - - return target_segment; -} - std::vector NormalLaneChange::get_prepare_metrics() const { const auto & current_lanes = common_data_ptr_->lanes_ptr->current; @@ -1177,14 +1061,20 @@ std::vector NormalLaneChange::get_lane_changing_metrics( }); const auto max_path_velocity = prep_segment.points.back().point.longitudinal_velocity_mps; - return calculation::calc_shift_phase_metrics( + const auto lc_metrics = calculation::calc_shift_phase_metrics( common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, prep_metric.sampled_lon_accel, max_lane_changing_length); + + const auto max_prep_length = common_data_ptr_->transient_data.dist_to_terminal_start; + lane_change_debug_.lane_change_metrics.push_back( + {prep_metric, lc_metrics, max_prep_length, max_lane_changing_length}); + return lc_metrics; } bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const { lane_change_debug_.collision_check_objects.clear(); + lane_change_debug_.lane_change_metrics.clear(); if (!common_data_ptr_->is_lanes_available()) { RCLCPP_WARN(logger_, "lanes are not available. Not expected."); @@ -1199,7 +1089,6 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const auto & current_lanes = get_current_lanes(); const auto & target_lanes = get_target_lanes(); - const auto current_velocity = getEgoVelocity(); const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); const auto target_objects = get_target_objects(filtered_objects_, current_lanes); @@ -1239,7 +1128,8 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) PathWithLaneId prepare_segment; try { - if (!get_prepare_segment(prepare_segment, prep_metric.length)) { + if (!utils::lane_change::get_prepare_segment( + common_data_ptr_, prev_module_output_.path, prep_metric, prepare_segment)) { debug_print("Reject: failed to get valid prepare segment!"); continue; } @@ -1258,7 +1148,10 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const auto lane_changing_metrics = get_lane_changing_metrics( prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element); - utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prep_metric.velocity); + // set_prepare_velocity must only be called after computing lane change metrics, as lane change + // metrics rely on the prepare segment's original velocity as max_path_velocity. + utils::lane_change::set_prepare_velocity( + prepare_segment, common_data_ptr_->get_ego_speed(), prep_metric.velocity); for (const auto & lc_metric : lane_changing_metrics) { const auto debug_print_lat = [&](const std::string & s) { @@ -1274,9 +1167,8 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) LaneChangePath candidate_path; try { - candidate_path = get_candidate_path( - prep_metric, lc_metric, prepare_segment, sorted_lane_ids, lane_changing_start_pose, - shift_length); + candidate_path = utils::lane_change::get_candidate_path( + common_data_ptr_, prep_metric, lc_metric, prepare_segment, sorted_lane_ids, shift_length); } catch (const std::exception & e) { debug_print_lat(std::string("Reject: ") + e.what()); continue; @@ -1302,52 +1194,6 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) return false; } -LaneChangePath NormalLaneChange::get_candidate_path( - const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, - const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, - const Pose & lc_start_pose, const double shift_length) const -{ - const auto & route_handler = *getRouteHandler(); - const auto & target_lanes = common_data_ptr_->lanes_ptr->target; - - const auto resample_interval = - utils::lane_change::calcLaneChangeResampleInterval(lc_metrics.length, prep_metrics.velocity); - const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( - common_data_ptr_, lc_start_pose, lc_metrics.length, resample_interval); - - if (target_lane_reference_path.points.empty()) { - throw std::logic_error("target_lane_reference_path is empty!"); - } - - const auto lc_end_pose = std::invoke([&]() { - const auto dist_to_lc_start = - lanelet::utils::getArcCoordinates(target_lanes, lc_start_pose).length; - const auto dist_to_lc_end = dist_to_lc_start + lc_metrics.length; - return route_handler.get_pose_from_2d_arc_length(target_lanes, dist_to_lc_end); - }); - - const auto shift_line = utils::lane_change::get_lane_changing_shift_line( - lc_start_pose, lc_end_pose, target_lane_reference_path, shift_length); - - LaneChangeInfo lane_change_info{prep_metrics, lc_metrics, lc_start_pose, lc_end_pose, shift_line}; - - const auto candidate_path = utils::lane_change::construct_candidate_path( - common_data_ptr_, lane_change_info, prep_segment, target_lane_reference_path, sorted_lane_ids); - - if (!candidate_path) { - throw std::logic_error("failed to generate candidate path!"); - } - - if ( - candidate_path.value().info.length.sum() + - common_data_ptr_->transient_data.next_dist_buffer.min > - common_data_ptr_->transient_data.dist_to_terminal_end) { - throw std::logic_error("invalid candidate path length!"); - } - - return *candidate_path; -} - bool NormalLaneChange::check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const { @@ -1377,147 +1223,30 @@ bool NormalLaneChange::check_candidate_path_safety( throw std::logic_error("Path footprint exceeds target lane boundary. Skip lane change."); } + if ((target_objects.empty()) || candidate_path.path.points.empty()) { + RCLCPP_DEBUG(logger_, "There is nothing to check."); + return true; + } + constexpr size_t decel_sampling_num = 1; + const auto ego_predicted_paths = utils::lane_change::convert_to_predicted_paths( + common_data_ptr_, candidate_path, decel_sampling_num); + const auto safety_check_with_normal_rss = isLaneChangePathSafe( - candidate_path, target_objects, common_data_ptr_->lc_param_ptr->safety.rss_params, - decel_sampling_num, lane_change_debug_.collision_check_objects); + candidate_path, ego_predicted_paths, target_objects, + common_data_ptr_->lc_param_ptr->safety.rss_params, lane_change_debug_.collision_check_objects); if (!safety_check_with_normal_rss.is_safe && is_stuck) { const auto safety_check_with_stuck_rss = isLaneChangePathSafe( - candidate_path, target_objects, common_data_ptr_->lc_param_ptr->safety.rss_params_for_stuck, - decel_sampling_num, lane_change_debug_.collision_check_objects); + candidate_path, ego_predicted_paths, target_objects, + common_data_ptr_->lc_param_ptr->safety.rss_params_for_stuck, + lane_change_debug_.collision_check_objects); return safety_check_with_stuck_rss.is_safe; } return safety_check_with_normal_rss.is_safe; } -std::optional NormalLaneChange::calcTerminalLaneChangePath( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const -{ - const auto is_empty = [&](const auto & data, const auto & s) { - if (!data.empty()) return false; - RCLCPP_WARN(logger_, "%s is empty. Not expected.", s); - return true; - }; - - const auto target_lane_neighbors_polygon_2d = - common_data_ptr_->lanes_polygon_ptr->target_neighbor; - if ( - is_empty(current_lanes, "current_lanes") || is_empty(target_lanes, "target_lanes") || - is_empty(target_lane_neighbors_polygon_2d, "target_lane_neighbors_polygon_2d")) { - return {}; - } - - const auto & route_handler = *getRouteHandler(); - - const auto minimum_lane_changing_velocity = - lane_change_parameters_->trajectory.min_lane_changing_velocity; - - const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); - - const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; - const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; - - const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); - - // lane changing start getEgoPose() is at the end of prepare segment - const auto current_lane_terminal_point = - lanelet::utils::conversion::toGeomMsgPt(current_lanes.back().centerline3d().back()); - - double distance_to_terminal_from_goal = 0; - if (is_goal_in_route) { - distance_to_terminal_from_goal = - utils::getDistanceToEndOfLane(route_handler.getGoalPose(), current_lanes); - } - - const auto lane_changing_start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( - prev_module_output_.path.points, current_lane_terminal_point, - -(current_min_dist_buffer + next_min_dist_buffer + distance_to_terminal_from_goal)); - - if (!lane_changing_start_pose) { - RCLCPP_DEBUG(logger_, "Reject: lane changing start pose not found!!!"); - return {}; - } - - const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( - current_lanes, target_lanes.front(), lane_changing_start_pose.value()); - - // Check if the lane changing start point is not on the lanes next to target lanes, - if (target_length_from_lane_change_start_pose > 0.0) { - RCLCPP_DEBUG(logger_, "lane change start getEgoPose() is behind target lanelet!"); - return {}; - } - - const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet( - target_lanes, lane_changing_start_pose.value()); - - const auto [min_lateral_acc, max_lateral_acc] = - lane_change_parameters_->trajectory.lat_acc_map.find(minimum_lane_changing_velocity); - - const auto lane_changing_time = autoware::motion_utils::calc_shift_time_from_jerk( - shift_length, lane_change_parameters_->trajectory.lateral_jerk, max_lateral_acc); - - const auto target_lane_length = common_data_ptr_->transient_data.target_lane_length; - const auto target_segment = getTargetSegment( - target_lanes, lane_changing_start_pose.value(), target_lane_length, current_min_dist_buffer, - minimum_lane_changing_velocity, next_min_dist_buffer); - - if (target_segment.points.empty()) { - RCLCPP_DEBUG(logger_, "Reject: target segment is empty!! something wrong..."); - return {}; - } - - LaneChangeInfo lane_change_info; - lane_change_info.longitudinal_acceleration = LaneChangePhaseInfo{0.0, 0.0}; - lane_change_info.duration = LaneChangePhaseInfo{0.0, lane_changing_time}; - lane_change_info.velocity = - LaneChangePhaseInfo{minimum_lane_changing_velocity, minimum_lane_changing_velocity}; - lane_change_info.length = LaneChangePhaseInfo{0.0, current_min_dist_buffer}; - lane_change_info.lane_changing_start = lane_changing_start_pose.value(); - lane_change_info.lane_changing_end = target_segment.points.front().point.pose; - lane_change_info.lateral_acceleration = max_lateral_acc; - lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity; - - if (!is_valid_start_point(common_data_ptr_, lane_changing_start_pose.value())) { - RCLCPP_DEBUG( - logger_, - "Reject: lane changing points are not inside of the target preferred lanes or its " - "neighbors"); - return {}; - } - - const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( - current_min_dist_buffer, minimum_lane_changing_velocity); - const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( - common_data_ptr_, lane_changing_start_pose.value(), current_min_dist_buffer, resample_interval); - - if (target_lane_reference_path.points.empty()) { - RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); - return {}; - } - - lane_change_info.shift_line = utils::lane_change::get_lane_changing_shift_line( - lane_changing_start_pose.value(), target_segment.points.front().point.pose, - target_lane_reference_path, shift_length); - - auto reference_segment = prev_module_output_.path; - const double length_to_lane_changing_start = autoware::motion_utils::calcSignedArcLength( - reference_segment.points, reference_segment.points.front().point.pose.position, - lane_changing_start_pose->position); - utils::clipPathLength(reference_segment, 0, length_to_lane_changing_start, 0.0); - // remove terminal points because utils::clipPathLength() calculates extra long path - reference_segment.points.pop_back(); - reference_segment.points.back().point.longitudinal_velocity_mps = - static_cast(minimum_lane_changing_velocity); - - const auto terminal_lane_change_path = utils::lane_change::construct_candidate_path( - common_data_ptr_, lane_change_info, reference_segment, target_lane_reference_path, - sorted_lane_ids); - - return terminal_lane_change_path; -} - PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { const auto & path = status_.lane_change_path; @@ -1545,9 +1274,13 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return {false, false}; } + const auto decel_sampling_num = + static_cast(lane_change_parameters_->cancel.deceleration_sampling_num); + const auto ego_predicted_paths = + utils::lane_change::convert_to_predicted_paths(common_data_ptr_, path, decel_sampling_num); const auto safety_status = isLaneChangePathSafe( - path, target_objects, lane_change_parameters_->safety.rss_params_for_abort, - static_cast(lane_change_parameters_->cancel.deceleration_sampling_num), debug_data); + path, ego_predicted_paths, target_objects, lane_change_parameters_->safety.rss_params_for_abort, + debug_data); { // only for debug purpose lane_change_debug_.collision_check_objects.clear(); @@ -1779,103 +1512,57 @@ bool NormalLaneChange::calcAbortPath() PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const LaneChangePath & lane_change_path, + const std::vector> & ego_predicted_paths, const lane_change::TargetObjects & collision_check_objects, - const utils::path_safety_checker::RSSparams & rss_params, const size_t deceleration_sampling_num, + const utils::path_safety_checker::RSSparams & rss_params, CollisionCheckDebugMap & debug_data) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); constexpr auto is_safe = true; constexpr auto is_object_behind_ego = true; - if (collision_check_objects.leading.empty() && collision_check_objects.trailing.empty()) { - RCLCPP_DEBUG(logger_, "There is nothing to check."); - return {is_safe, !is_object_behind_ego}; - } - const auto is_check_prepare_phase = check_prepare_phase(); - const auto all_decel_pattern_has_collision = - [&](const utils::path_safety_checker::ExtendedPredictedObjects & objects) -> bool { - return has_collision_with_decel_patterns( - lane_change_path, objects, deceleration_sampling_num, rss_params, is_check_prepare_phase, - debug_data); + const auto all_paths_collide = [&](const auto & objects) { + const auto stopped_obj_vel_th = lane_change_parameters_->safety.th_stopped_object_velocity; + return ranges::all_of(ego_predicted_paths, [&](const auto & ego_predicted_path) { + const auto check_for_collision = [&](const auto & object) { + const auto selected_rss_param = (object.initial_twist.linear.x <= stopped_obj_vel_th) + ? lane_change_parameters_->safety.rss_params_for_parked + : rss_params; + return is_colliding( + lane_change_path, object, ego_predicted_path, selected_rss_param, is_check_prepare_phase, + debug_data); + }; + return ranges::any_of(objects, check_for_collision); + }); }; - if (all_decel_pattern_has_collision(collision_check_objects.trailing)) { + if (all_paths_collide(collision_check_objects.trailing)) { return {!is_safe, is_object_behind_ego}; } - if (all_decel_pattern_has_collision(collision_check_objects.leading)) { + if (all_paths_collide(collision_check_objects.leading)) { return {!is_safe, !is_object_behind_ego}; } return {is_safe, !is_object_behind_ego}; } -bool NormalLaneChange::has_collision_with_decel_patterns( - const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects, - const size_t deceleration_sampling_num, const RSSparams & rss_param, - const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const -{ - if (objects.empty()) { - return false; - } - - const auto & path = lane_change_path.path; - - if (path.points.empty()) { - return false; - } - - const auto bpp_param = *common_data_ptr_->bpp_param_ptr; - const auto global_min_acc = bpp_param.min_acc; - const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing; - - const auto min_acc = std::min(lane_changing_acc, global_min_acc); - const auto sampling_num = - std::abs(min_acc - lane_changing_acc) > floating_err_th ? deceleration_sampling_num : 1; - const auto acc_resolution = (min_acc - lane_changing_acc) / static_cast(sampling_num); - - std::vector acceleration_values(sampling_num); - std::iota(acceleration_values.begin(), acceleration_values.end(), 0); - - std::transform( - acceleration_values.begin(), acceleration_values.end(), acceleration_values.begin(), - [&](double n) { return lane_changing_acc + n * acc_resolution; }); - - const auto stopped_obj_vel_th = lane_change_parameters_->safety.th_stopped_object_velocity; - const auto all_collided = std::all_of( - acceleration_values.begin(), acceleration_values.end(), [&](const auto acceleration) { - const auto ego_predicted_path = utils::lane_change::convert_to_predicted_path( - common_data_ptr_, lane_change_path, acceleration); - - return std::any_of(objects.begin(), objects.end(), [&](const auto & obj) { - const auto selected_rss_param = (obj.initial_twist.linear.x <= stopped_obj_vel_th) - ? lane_change_parameters_->safety.rss_params_for_parked - : rss_param; - return is_collided( - lane_change_path, obj, ego_predicted_path, selected_rss_param, check_prepare_phase, - debug_data); - }); - }); - - return all_collided; -} - -bool NormalLaneChange::is_collided( +bool NormalLaneChange::is_colliding( const LaneChangePath & lane_change_path, const ExtendedPredictedObject & obj, const std::vector & ego_predicted_path, const RSSparams & selected_rss_param, const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const { - constexpr auto is_collided{true}; + constexpr auto is_colliding{true}; if (lane_change_path.path.points.empty()) { - return !is_collided; + return !is_colliding; } if (ego_predicted_path.empty()) { - return !is_collided; + return !is_colliding; } const auto & lanes_polygon_ptr = common_data_ptr_->lanes_polygon_ptr; @@ -1883,7 +1570,7 @@ bool NormalLaneChange::is_collided( const auto & expanded_target_polygon = lanes_polygon_ptr->target; if (current_polygon.empty() || expanded_target_polygon.empty()) { - return !is_collided; + return !is_colliding; } constexpr auto is_safe{true}; @@ -1934,10 +1621,10 @@ bool NormalLaneChange::is_collided( utils::path_safety_checker::updateCollisionCheckDebugMap( debug_data, current_debug_data, !is_safe); - return is_collided; + return is_colliding; } utils::path_safety_checker::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); - return !is_collided; + return !is_colliding; } double NormalLaneChange::get_max_velocity_for_safety_check() const @@ -2004,22 +1691,11 @@ bool NormalLaneChange::is_ego_stuck() const return has_object_blocking; } -bool NormalLaneChange::is_valid_start_point( - const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) const -{ - const lanelet::BasicPoint2d lc_start_point(pose.position.x, pose.position.y); - - const auto & target_neighbor_poly = common_data_ptr->lanes_polygon_ptr->target_neighbor; - const auto & target_lane_poly = common_data_ptr_->lanes_polygon_ptr->target; - - return boost::geometry::covered_by(lc_start_point, target_neighbor_poly) || - boost::geometry::covered_by(lc_start_point, target_lane_poly); -} - -void NormalLaneChange::set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path) +void NormalLaneChange::set_stop_pose( + const double arc_length_to_stop_pose, PathWithLaneId & path, const std::string & reason) { const auto stop_point = utils::insertStopPoint(arc_length_to_stop_pose, path); - lane_change_stop_pose_ = stop_point.point.pose; + lane_change_stop_pose_ = PoseWithDetailOpt(PoseWithDetail(stop_point.point.pose, reason)); } void NormalLaneChange::updateStopTime() diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 3ba11d62a2be7..550c0fa290a99 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -417,7 +417,7 @@ double calc_actual_prepare_duration( if (max_acc < eps) { return params.max_prepare_duration; } - return (min_lc_velocity - current_velocity) / max_acc; + return std::max((min_lc_velocity - current_velocity) / max_acc, params.min_prepare_duration); }); return std::max(params.max_prepare_duration - active_signal_duration, min_prepare_duration); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index c7514fea01535..da9ee52b038c6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp @@ -16,6 +16,7 @@ #include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" #include +#include #include #include @@ -25,10 +26,11 @@ #include #include +#include + #include #include #include -#include #include #include @@ -38,34 +40,64 @@ using autoware::universe_utils::createDefaultMarker; using autoware::universe_utils::createMarkerScale; using geometry_msgs::msg::Point; -MarkerArray showAllValidLaneChangePath(const std::vector & lanes, std::string && ns) +MarkerArray showAllValidLaneChangePath( + const std::vector & lane_change_paths, std::string && ns) { - if (lanes.empty()) { + if (lane_change_paths.empty()) { return MarkerArray{}; } MarkerArray marker_array; - int32_t id{0}; const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; const auto colors = colors::colors_list(); - const auto loop_size = std::min(lanes.size(), colors.size()); + const auto loop_size = std::min(lane_change_paths.size(), colors.size()); marker_array.markers.reserve(loop_size); for (std::size_t idx = 0; idx < loop_size; ++idx) { - if (lanes.at(idx).path.points.empty()) { + int32_t id{0}; + const auto & lc_path = lane_change_paths.at(idx); + if (lc_path.path.points.empty()) { continue; } - + std::string ns_with_idx = ns + "[" + std::to_string(idx) + "]"; const auto & color = colors.at(idx); - Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0), color); - marker.points.reserve(lanes.at(idx).path.points.size()); + const auto & points = lc_path.path.points; + auto marker = createDefaultMarker( + "map", current_time, ns_with_idx, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0), + color); + marker.points.reserve(points.size()); - for (const auto & point : lanes.at(idx).path.points) { + for (const auto & point : points) { marker.points.push_back(point.point.pose.position); } + const auto & info = lc_path.info; + auto text_marker = createDefaultMarker( + "map", current_time, ns_with_idx, ++id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerScale(0.1, 0.1, 0.8), colors::yellow()); + const auto prep_idx = points.size() / 4; + text_marker.pose = points.at(prep_idx).point.pose; + text_marker.pose.position.z += 2.0; + text_marker.text = fmt::format( + "vel: {vel:.3f}[m/s] | lon_acc: {lon_acc:.3f}[m/s2] | t: {time:.3f}[s] | L: {length:.3f}[m]", + fmt::arg("vel", info.velocity.prepare), + fmt::arg("lon_acc", info.longitudinal_acceleration.prepare), + fmt::arg("time", info.duration.prepare), fmt::arg("length", info.length.prepare)); + marker_array.markers.push_back(text_marker); + + const auto lc_idx = points.size() / 2; + text_marker.id = ++id; + text_marker.pose = points.at(lc_idx).point.pose; + text_marker.text = fmt::format( + "vel: {vel:.3f}[m/s] | lon_acc: {lon_acc:.3f}[m/s2] | lat_acc: {lat_acc:.3f}[m/s2] | t: " + "{time:.3f}[s] | L: {length:.3f}[m]", + fmt::arg("vel", info.velocity.lane_changing), + fmt::arg("lon_acc", info.longitudinal_acceleration.lane_changing), + fmt::arg("lat_acc", info.lateral_acceleration), fmt::arg("time", info.duration.lane_changing), + fmt::arg("length", info.length.lane_changing)); + marker_array.markers.push_back(text_marker); + marker_array.markers.push_back(marker); } return marker_array; @@ -146,18 +178,51 @@ MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg safety_check_info_text.pose = ego_pose; safety_check_info_text.pose.position.z += 4.0; - std::ostringstream ss; + safety_check_info_text.text = fmt::format( + "{stuck} | {return_lane} | {abort}", fmt::arg("stuck", debug_data.is_stuck ? "is stuck" : ""), + fmt::arg("return_lane", debug_data.is_able_to_return_to_current_lane ? "" : "can't return"), + fmt::arg("abort", debug_data.is_abort ? "aborting" : "")); + marker_array.markers.push_back(safety_check_info_text); + return marker_array; +} - ss << "\nDistToEndOfCurrentLane: " << std::setprecision(5) - << debug_data.distance_to_end_of_current_lane - << "\nDistToLaneChangeFinished: " << debug_data.distance_to_lane_change_finished - << (debug_data.is_stuck ? "\nVehicleStuck" : "") - << (debug_data.is_able_to_return_to_current_lane ? "\nAbleToReturnToCurrentLane" : "") - << (debug_data.is_abort ? "\nAborting" : "") - << "\nDistanceToAbortFinished: " << debug_data.distance_to_abort_finished; +MarkerArray ShowLaneChangeMetricsInfo( + const Debug & debug_data, const geometry_msgs::msg::Pose & pose) +{ + MarkerArray marker_array; + if (debug_data.lane_change_metrics.empty()) { + return marker_array; + } - safety_check_info_text.text = ss.str(); - marker_array.markers.push_back(safety_check_info_text); + auto text_marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "sampling_metrics", 0, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.6, 0.6, 0.6), colors::yellow()); + text_marker.pose = autoware::universe_utils::calcOffsetPose(pose, 10.0, 15.0, 0.0); + + text_marker.text = fmt::format("{:<12}", "") + fmt::format("{:^18}|", "lat_accel[m/s2]") + + fmt::format("{:^18}|", "lon_accel[m/s2]") + + fmt::format("{:^17}|", "velocity[m/s]") + + fmt::format("{:^15}|", "duration[s]") + fmt::format("{:^15}|", "length[m]") + + fmt::format("{:^20}\n", "max_length_th[m]"); + for (const auto & metrics : debug_data.lane_change_metrics) { + text_marker.text += fmt::format("{:-<170}\n", ""); + const auto & p_m = metrics.prep_metric; + text_marker.text += + fmt::format("{:<17}", "prep_metrics:") + fmt::format("{:^10.3f}", p_m.lat_accel) + + fmt::format("{:^21.3f}", p_m.actual_lon_accel) + fmt::format("{:^12.3f}", p_m.velocity) + + fmt::format("{:^15.3f}", p_m.duration) + fmt::format("{:^15.3f}", p_m.length) + + fmt::format("{:^17.3f}\n", metrics.max_prepare_length); + text_marker.text += fmt::format("{:<20}\n", "lc_metrics:"); + for (const auto lc_m : metrics.lc_metrics) { + text_marker.text += + fmt::format("{:<15}", "") + fmt::format("{:^10.3f}", lc_m.lat_accel) + + fmt::format("{:^21.3f}", lc_m.actual_lon_accel) + fmt::format("{:^12.3f}", lc_m.velocity) + + fmt::format("{:^15.3f}", lc_m.duration) + fmt::format("{:^15.3f}", lc_m.length) + + fmt::format("{:^17.3f}\n", metrics.max_lane_changing_length); + } + } + + marker_array.markers.push_back(text_marker); return marker_array; } @@ -188,6 +253,7 @@ MarkerArray createDebugMarkerArray( } add(showExecutionInfo(debug_data, ego_pose)); + add(ShowLaneChangeMetricsInfo(debug_data, ego_pose)); // lanes add(laneletsAsTriangleMarkerArray( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp new file mode 100644 index 0000000000000..d7303d7d1df2d --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp @@ -0,0 +1,292 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_path_lane_change_module/utils/path.hpp" + +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace +{ +using autoware::behavior_path_planner::LaneChangeInfo; +using autoware::behavior_path_planner::PathPointWithLaneId; +using autoware::behavior_path_planner::PathShifter; +using autoware::behavior_path_planner::PathWithLaneId; +using autoware::behavior_path_planner::ShiftedPath; +using autoware::behavior_path_planner::lane_change::CommonDataPtr; +using autoware::behavior_path_planner::lane_change::LCParamPtr; + +using autoware::behavior_path_planner::LaneChangePhaseMetrics; +using autoware::behavior_path_planner::ShiftLine; +using geometry_msgs::msg::Pose; + +double calc_resample_interval( + const double lane_changing_length, const double lane_changing_velocity) +{ + constexpr auto min_resampling_points{30.0}; + constexpr auto resampling_dt{0.2}; + return std::max( + lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); +} + +PathWithLaneId get_reference_path_from_target_Lane( + const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, + const double lane_changing_length, const double resample_interval) +{ + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto target_lane_length = common_data_ptr->transient_data.target_lane_length; + const auto is_goal_in_route = common_data_ptr->lanes_ptr->target_lane_in_goal_section; + const auto next_lc_buffer = common_data_ptr->transient_data.next_dist_buffer.min; + const auto forward_path_length = common_data_ptr->bpp_param_ptr->forward_path_length; + + const auto lane_change_start_arc_position = + lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); + + const double s_start = lane_change_start_arc_position.length; + const double s_end = std::invoke([&]() { + const auto dist_from_lc_start = s_start + lane_changing_length + forward_path_length; + if (is_goal_in_route) { + const double s_goal = + lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length - + next_lc_buffer; + return std::min(dist_from_lc_start, s_goal); + } + return std::min(dist_from_lc_start, target_lane_length - next_lc_buffer); + }); + + constexpr double epsilon = 1e-4; + if (s_end - s_start + epsilon < lane_changing_length) { + return PathWithLaneId(); + } + + const auto lane_changing_reference_path = + route_handler.getCenterLinePath(target_lanes, s_start, s_end); + + return autoware::behavior_path_planner::utils::resamplePathWithSpline( + lane_changing_reference_path, resample_interval, true, {0.0, lane_changing_length}); +} + +ShiftLine get_lane_changing_shift_line( + const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, + const PathWithLaneId & reference_path, const double shift_length) +{ + ShiftLine shift_line; + shift_line.end_shift_length = shift_length; + shift_line.start = lane_changing_start_pose; + shift_line.end = lane_changing_end_pose; + shift_line.start_idx = autoware::motion_utils::findNearestIndex( + reference_path.points, lane_changing_start_pose.position); + shift_line.end_idx = autoware::motion_utils::findNearestIndex( + reference_path.points, lane_changing_end_pose.position); + + return shift_line; +} + +ShiftedPath get_shifted_path( + const PathWithLaneId & target_lane_reference_path, const LaneChangeInfo & lane_change_info) +{ + const auto longitudinal_acceleration = lane_change_info.longitudinal_acceleration; + + PathShifter path_shifter; + path_shifter.setPath(target_lane_reference_path); + path_shifter.addShiftLine(lane_change_info.shift_line); + path_shifter.setLongitudinalAcceleration(longitudinal_acceleration.lane_changing); + const auto initial_lane_changing_velocity = lane_change_info.velocity.lane_changing; + path_shifter.setVelocity(initial_lane_changing_velocity); + path_shifter.setLateralAccelerationLimit(std::abs(lane_change_info.lateral_acceleration)); + + constexpr auto offset_back = false; + ShiftedPath shifted_path; + [[maybe_unused]] const auto success = path_shifter.generate(&shifted_path, offset_back); + return shifted_path; +} + +std::optional exceed_yaw_threshold( + const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, + const double yaw_th_rad) +{ + const auto & prepare = prepare_segment.points; + const auto & lane_changing = lane_changing_segment.points; + + if (prepare.size() <= 1 || lane_changing.size() <= 1) { + return std::nullopt; + } + + const auto & p1 = std::prev(prepare.end() - 1)->point.pose; + const auto & p2 = std::next(lane_changing.begin())->point.pose; + const auto yaw_diff_rad = std::abs(autoware::universe_utils::normalizeRadian( + tf2::getYaw(p1.orientation) - tf2::getYaw(p2.orientation))); + if (yaw_diff_rad > yaw_th_rad) { + return yaw_diff_rad; + } + return std::nullopt; +} +}; // namespace + +namespace autoware::behavior_path_planner::utils::lane_change +{ +using behavior_path_planner::lane_change::CommonDataPtr; + +bool get_prepare_segment( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, + const LaneChangePhaseMetrics prep_metric, PathWithLaneId & prepare_segment) +{ + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto backward_path_length = common_data_ptr->bpp_param_ptr->backward_path_length; + + if (current_lanes.empty() || target_lanes.empty()) { + throw std::logic_error("Empty lanes!"); + } + + prepare_segment = prev_module_path; + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prepare_segment.points, common_data_ptr->get_ego_pose(), 3.0, 1.0); + utils::clipPathLength(prepare_segment, current_seg_idx, prep_metric.length, backward_path_length); + + if (prepare_segment.points.empty()) return false; + + const auto & lc_start_pose = prepare_segment.points.back().point.pose; + + // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is + // larger than distance to target lane start + if (!is_valid_start_point(common_data_ptr, lc_start_pose)) return false; + + // lane changing start is at the end of prepare segment + const auto target_length_from_lane_change_start_pose = + utils::getArcLengthToTargetLanelet(current_lanes, target_lanes.front(), lc_start_pose); + + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > std::numeric_limits::epsilon()) { + throw std::logic_error("lane change start is behind target lanelet!"); + } + + return true; +} + +LaneChangePath get_candidate_path( + const CommonDataPtr & common_data_ptr, const LaneChangePhaseMetrics & prep_metric, + const LaneChangePhaseMetrics & lc_metric, const PathWithLaneId & prep_segment, + const std::vector> & sorted_lane_ids, const double shift_length) +{ + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + + const auto resample_interval = calc_resample_interval(lc_metric.length, prep_metric.velocity); + + if (prep_segment.points.empty()) { + throw std::logic_error("Empty prepare segment!"); + } + + const auto & lc_start_pose = prep_segment.points.back().point.pose; + const auto target_lane_reference_path = get_reference_path_from_target_Lane( + common_data_ptr, lc_start_pose, lc_metric.length, resample_interval); + + if (target_lane_reference_path.points.empty()) { + throw std::logic_error("Empty target reference!"); + } + + const auto lc_end_pose = std::invoke([&]() { + const auto dist_to_lc_start = + lanelet::utils::getArcCoordinates(target_lanes, lc_start_pose).length; + const auto dist_to_lc_end = dist_to_lc_start + lc_metric.length; + return route_handler.get_pose_from_2d_arc_length(target_lanes, dist_to_lc_end); + }); + + const auto shift_line = get_lane_changing_shift_line( + lc_start_pose, lc_end_pose, target_lane_reference_path, shift_length); + + LaneChangeInfo lane_change_info{prep_metric, lc_metric, lc_start_pose, lc_end_pose, shift_line}; + + if ( + lane_change_info.length.sum() + common_data_ptr->transient_data.next_dist_buffer.min > + common_data_ptr->transient_data.dist_to_terminal_end) { + throw std::logic_error("invalid candidate path length!"); + } + + return utils::lane_change::construct_candidate_path( + lane_change_info, prep_segment, target_lane_reference_path, sorted_lane_ids); +} + +LaneChangePath construct_candidate_path( + const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, + const PathWithLaneId & target_lane_reference_path, + const std::vector> & sorted_lane_ids) +{ + const auto & shift_line = lane_change_info.shift_line; + const auto terminal_lane_changing_velocity = lane_change_info.terminal_lane_changing_velocity; + + auto shifted_path = get_shifted_path(target_lane_reference_path, lane_change_info); + + if (shifted_path.path.points.empty()) { + throw std::logic_error("Failed to generate shifted path."); + } + + if (shifted_path.path.points.size() < shift_line.end_idx + 1) { + throw std::logic_error("Path points are removed by PathShifter."); + } + + const auto lc_end_idx_opt = autoware::motion_utils::findNearestIndex( + shifted_path.path.points, lane_change_info.lane_changing_end); + + if (!lc_end_idx_opt) { + throw std::logic_error("Lane change end idx not found on target path."); + } + + for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { + auto & point = shifted_path.path.points.at(i); + if (i < *lc_end_idx_opt) { + point.lane_ids = replaceWithSortedIds(point.lane_ids, sorted_lane_ids); + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, static_cast(terminal_lane_changing_velocity)); + continue; + } + const auto nearest_idx = + autoware::motion_utils::findNearestIndex(target_lane_reference_path.points, point.point.pose); + point.lane_ids = target_lane_reference_path.points.at(*nearest_idx).lane_ids; + } + + constexpr auto yaw_diff_th = autoware::universe_utils::deg2rad(5.0); + if ( + const auto yaw_diff_opt = + exceed_yaw_threshold(prepare_segment, shifted_path.path, yaw_diff_th)) { + std::stringstream err_msg; + err_msg << "Excessive yaw difference " << yaw_diff_opt.value() << " which exceeds the " + << yaw_diff_th << " radian threshold."; + throw std::logic_error(err_msg.str()); + } + + LaneChangePath candidate_path; + candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); + candidate_path.shifted_path = shifted_path; + candidate_path.info = lane_change_info; + + return candidate_path; +} +} // namespace autoware::behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 2b1c744a926e3..9360435891632 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -19,7 +19,6 @@ #include "autoware/behavior_path_planner_common/parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/object_recognition_utils/predicted_path_utils.hpp" @@ -29,15 +28,14 @@ #include #include #include -#include #include +#include #include #include #include -#include -#include -#include -#include +#include +#include +#include #include #include #include @@ -72,7 +70,6 @@ using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Pose; using tier4_planning_msgs::msg::PathWithLaneId; -using lanelet::ArcCoordinates; using tier4_planning_msgs::msg::PathPointWithLaneId; rclcpp::Logger get_logger() @@ -88,29 +85,20 @@ bool is_mandatory_lane_change(const ModuleType lc_type) lc_type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE; } -double calcLaneChangeResampleInterval( - const double lane_changing_length, const double lane_changing_velocity) -{ - constexpr auto min_resampling_points{30.0}; - constexpr auto resampling_dt{0.2}; - return std::max( - lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); -} - -void setPrepareVelocity( +void set_prepare_velocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity) { - if (current_velocity < prepare_velocity) { - // acceleration - for (auto & point : prepare_segment.points) { - point.point.longitudinal_velocity_mps = - std::min(point.point.longitudinal_velocity_mps, static_cast(prepare_velocity)); - } - } else { + if (current_velocity >= prepare_velocity) { // deceleration prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( prepare_segment.points.back().point.longitudinal_velocity_mps, static_cast(prepare_velocity)); + return; + } + // acceleration + for (auto & point : prepare_segment.points) { + point.point.longitudinal_velocity_mps = + std::min(point.point.longitudinal_velocity_mps, static_cast(prepare_velocity)); } } @@ -132,34 +120,9 @@ lanelet::ConstLanelets get_target_neighbor_lanes( } } } - return neighbor_lanes; } -bool isPathInLanelets( - const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) -{ - const auto current_lane_poly = - lanelet::utils::getPolygonFromArcLength(current_lanes, 0, std::numeric_limits::max()); - const auto target_lane_poly = - lanelet::utils::getPolygonFromArcLength(target_lanes, 0, std::numeric_limits::max()); - const auto current_lane_poly_2d = lanelet::utils::to2D(current_lane_poly).basicPolygon(); - const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_poly).basicPolygon(); - for (const auto & pt : path.points) { - const lanelet::BasicPoint2d ll_pt(pt.point.pose.position.x, pt.point.pose.position.y); - const auto is_in_current = boost::geometry::covered_by(ll_pt, current_lane_poly_2d); - if (is_in_current) { - continue; - } - const auto is_in_target = boost::geometry::covered_by(ll_pt, target_lane_poly_2d); - if (!is_in_target) { - return false; - } - } - return true; -} - bool path_footprint_exceeds_target_lane_bound( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, const double margin) @@ -190,152 +153,6 @@ bool path_footprint_exceeds_target_lane_bound( return false; } -std::optional construct_candidate_path( - const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path, - const std::vector> & sorted_lane_ids) -{ - const auto & shift_line = lane_change_info.shift_line; - const auto terminal_lane_changing_velocity = lane_change_info.terminal_lane_changing_velocity; - const auto longitudinal_acceleration = lane_change_info.longitudinal_acceleration; - const auto lane_change_velocity = lane_change_info.velocity; - - PathShifter path_shifter; - path_shifter.setPath(target_lane_reference_path); - path_shifter.addShiftLine(shift_line); - path_shifter.setLongitudinalAcceleration(longitudinal_acceleration.lane_changing); - ShiftedPath shifted_path; - - // offset front side - bool offset_back = false; - - const auto initial_lane_changing_velocity = lane_change_velocity.lane_changing; - path_shifter.setVelocity(initial_lane_changing_velocity); - path_shifter.setLateralAccelerationLimit(std::abs(lane_change_info.lateral_acceleration)); - - if (!path_shifter.generate(&shifted_path, offset_back)) { - RCLCPP_DEBUG(get_logger(), "Failed to generate shifted path."); - } - - // TODO(Zulfaqar Azmi): have to think of a more feasible solution for points being remove by path - // shifter. - if (shifted_path.path.points.size() < shift_line.end_idx + 1) { - RCLCPP_DEBUG(get_logger(), "Path points are removed by PathShifter."); - return std::nullopt; - } - - LaneChangePath candidate_path; - candidate_path.info = lane_change_info; - - const auto lane_change_end_idx = autoware::motion_utils::findNearestIndex( - shifted_path.path.points, candidate_path.info.lane_changing_end); - - if (!lane_change_end_idx) { - RCLCPP_DEBUG(get_logger(), "Lane change end idx not found on target path."); - return std::nullopt; - } - - for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { - auto & point = shifted_path.path.points.at(i); - if (i < *lane_change_end_idx) { - point.lane_ids = replaceWithSortedIds(point.lane_ids, sorted_lane_ids); - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, static_cast(terminal_lane_changing_velocity)); - continue; - } - const auto nearest_idx = - autoware::motion_utils::findNearestIndex(target_lane_reference_path.points, point.point.pose); - point.lane_ids = target_lane_reference_path.points.at(*nearest_idx).lane_ids; - } - - // TODO(Yutaka Shimizu): remove this flag after make the isPathInLanelets faster - const bool enable_path_check_in_lanelet = false; - - // check candidate path is in lanelet - const auto & current_lanes = common_data_ptr->lanes_ptr->current; - const auto & target_lanes = common_data_ptr->lanes_ptr->target; - if ( - enable_path_check_in_lanelet && - !isPathInLanelets(shifted_path.path, current_lanes, target_lanes)) { - return std::nullopt; - } - - if (prepare_segment.points.size() > 1 && shifted_path.path.points.size() > 1) { - const auto & prepare_segment_second_last_point = - std::prev(prepare_segment.points.end() - 1)->point.pose; - const auto & lane_change_start_from_shifted = - std::next(shifted_path.path.points.begin())->point.pose; - const auto yaw_diff2 = std::abs(autoware::universe_utils::normalizeRadian( - tf2::getYaw(prepare_segment_second_last_point.orientation) - - tf2::getYaw(lane_change_start_from_shifted.orientation))); - if (yaw_diff2 > autoware::universe_utils::deg2rad(5.0)) { - RCLCPP_DEBUG( - get_logger(), "Excessive yaw difference %.3f which exceeds the 5 degrees threshold.", - autoware::universe_utils::rad2deg(yaw_diff2)); - return std::nullopt; - } - } - - candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); - candidate_path.shifted_path = shifted_path; - - return std::optional{candidate_path}; -} - -PathWithLaneId get_reference_path_from_target_Lane( - const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, - const double lane_changing_length, const double resample_interval) -{ - const auto & route_handler = *common_data_ptr->route_handler_ptr; - const auto & target_lanes = common_data_ptr->lanes_ptr->target; - const auto target_lane_length = common_data_ptr->transient_data.target_lane_length; - const auto is_goal_in_route = common_data_ptr->lanes_ptr->target_lane_in_goal_section; - const auto next_lc_buffer = common_data_ptr->transient_data.next_dist_buffer.min; - const auto forward_path_length = common_data_ptr->bpp_param_ptr->forward_path_length; - - const ArcCoordinates lane_change_start_arc_position = - lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); - - const double s_start = lane_change_start_arc_position.length; - const double s_end = std::invoke([&]() { - const auto dist_from_lc_start = s_start + lane_changing_length + forward_path_length; - if (is_goal_in_route) { - const double s_goal = - lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length - - next_lc_buffer; - return std::min(dist_from_lc_start, s_goal); - } - return std::min(dist_from_lc_start, target_lane_length - next_lc_buffer); - }); - - constexpr double epsilon = 1e-4; - if (s_end - s_start + epsilon < lane_changing_length) { - return PathWithLaneId(); - } - - const auto lane_changing_reference_path = - route_handler.getCenterLinePath(target_lanes, s_start, s_end); - - return utils::resamplePathWithSpline( - lane_changing_reference_path, resample_interval, true, {0.0, lane_changing_length}); -} - -ShiftLine get_lane_changing_shift_line( - const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, - const PathWithLaneId & reference_path, const double shift_length) -{ - ShiftLine shift_line; - shift_line.end_shift_length = shift_length; - shift_line.start = lane_changing_start_pose; - shift_line.end = lane_changing_end_pose; - shift_line.start_idx = autoware::motion_utils::findNearestIndex( - reference_path.points, lane_changing_start_pose.position); - shift_line.end_idx = autoware::motion_utils::findNearestIndex( - reference_path.points, lane_changing_end_pose.position); - - return shift_line; -} - std::vector generateDrivableLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes) @@ -631,6 +448,7 @@ std::vector convert_to_predicted_path( const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; const auto resolution = lc_param_ptr->safety.collision_check.prediction_time_resolution; std::vector predicted_path; + predicted_path.reserve(static_cast(std::ceil(duration / resolution))); // prepare segment for (double t = 0.0; t < prepare_time; t += resolution) { @@ -647,6 +465,7 @@ std::vector convert_to_predicted_path( initial_velocity + prepare_acc * prepare_time, 0.0, lane_change_path.info.velocity.prepare); const auto offset = initial_velocity * prepare_time + 0.5 * prepare_acc * prepare_time * prepare_time; + for (double t = prepare_time; t < duration; t += resolution) { const auto delta_t = t - prepare_time; const auto velocity = std::clamp( @@ -1101,19 +920,17 @@ double get_min_dist_to_current_lanes_obj( continue; } - // calculate distance from path front to the stationary object polygon on the ego lane. - for (const auto & polygon_p : object.initial_polygon.outer()) { - const auto p_fp = autoware::universe_utils::toMsg(polygon_p.to_3d()); - const auto lateral_fp = motion_utils::calcLateralOffset(path_points, p_fp); - - // ignore if the point is not on ego path - if (std::abs(lateral_fp) > (common_data_ptr->bpp_param_ptr->vehicle_width / 2)) { - continue; - } - - const auto current_distance_to_obj = motion_utils::calcSignedArcLength(path_points, 0, p_fp); - min_dist_to_obj = std::min(min_dist_to_obj, current_distance_to_obj); + // check if object is on ego path + const auto obj_half_width = object.shape.dimensions.y / 2; + const auto obj_lat_dist_to_path = + std::abs(motion_utils::calcLateralOffset(path_points, object.initial_pose.position)) - + obj_half_width; + if (obj_lat_dist_to_path > (common_data_ptr->bpp_param_ptr->vehicle_width / 2)) { + continue; } + + min_dist_to_obj = std::min(min_dist_to_obj, dist_to_obj); + break; } return min_dist_to_obj; } @@ -1294,4 +1111,39 @@ bool object_path_overlaps_lanes( return !boost::geometry::disjoint(path, lanes_polygon); }); } + +std::vector> convert_to_predicted_paths( + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, + const size_t deceleration_sampling_num) +{ + static constexpr double floating_err_th{1e-3}; + const auto bpp_param = *common_data_ptr->bpp_param_ptr; + const auto global_min_acc = bpp_param.min_acc; + const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing; + + const auto min_acc = std::min(lane_changing_acc, global_min_acc); + const auto sampling_num = + std::abs(min_acc - lane_changing_acc) > floating_err_th ? deceleration_sampling_num : 1; + const auto acc_resolution = (min_acc - lane_changing_acc) / static_cast(sampling_num); + + const auto ego_predicted_path = [&](size_t n) { + auto acc = lane_changing_acc + static_cast(n) * acc_resolution; + return utils::lane_change::convert_to_predicted_path(common_data_ptr, lane_change_path, acc); + }; + + return ranges::views::iota(0UL, sampling_num) | ranges::views::transform(ego_predicted_path) | + ranges::to(); +} + +bool is_valid_start_point(const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) +{ + const lanelet::BasicPoint2d lc_start_point(pose.position.x, pose.position.y); + + const auto & target_neighbor_poly = common_data_ptr->lanes_polygon_ptr->target_neighbor; + const auto & target_lane_poly = common_data_ptr->lanes_polygon_ptr->target; + + // Check the target lane because the previous approved path might be shifted by avoidance module + return boost::geometry::covered_by(lc_start_point, target_neighbor_poly) || + boost::geometry::covered_by(lc_start_point, target_lane_poly); +} } // namespace autoware::behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp index 96b24d5aa9a9e..5fb445788672c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp @@ -242,15 +242,14 @@ TEST_F(TestNormalLaneChange, testFilteredObjects) const auto & filtered_objects = get_filtered_objects(); - // Note: There's 1 stopping object in current lanes, however, it was filtered out. const auto filtered_size = filtered_objects.current_lane.size() + filtered_objects.target_lane_leading.size() + filtered_objects.target_lane_trailing.size() + filtered_objects.others.size(); EXPECT_EQ(filtered_size, planner_data_->dynamic_object->objects.size()); - EXPECT_EQ(filtered_objects.current_lane.size(), 0); + EXPECT_EQ(filtered_objects.current_lane.size(), 1); EXPECT_EQ(filtered_objects.target_lane_leading.size(), 2); EXPECT_EQ(filtered_objects.target_lane_trailing.size(), 0); - EXPECT_EQ(filtered_objects.others.size(), 2); + EXPECT_EQ(filtered_objects.others.size(), 1); } TEST_F(TestNormalLaneChange, testGetPathWhenValid) @@ -258,6 +257,7 @@ TEST_F(TestNormalLaneChange, testGetPathWhenValid) constexpr auto is_approved = true; ego_pose_ = autoware::test_utils::createPose(1.0, 1.75, 0.0, 0.0, 0.0, 0.0); planner_data_->self_odometry = set_odometry(ego_pose_); + normal_lane_change_->setData(planner_data_); set_previous_approved_path(); normal_lane_change_->update_lanes(!is_approved); normal_lane_change_->update_filtered_objects(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp index 3bed1e6a88bd8..741202590779c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp @@ -25,6 +25,8 @@ #include #include +#include +#include #include #include @@ -44,8 +46,8 @@ using tier4_planning_msgs::msg::PathWithLaneId; using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; using DebugPublisher = autoware::universe_utils::DebugPublisher; -using DebugDoubleMsg = tier4_debug_msgs::msg::Float64Stamped; -using DebugStringMsg = tier4_debug_msgs::msg::StringStamped; +using DebugDoubleMsg = autoware_internal_debug_msgs::msg::Float64Stamped; +using DebugStringMsg = autoware_internal_debug_msgs::msg::StringStamped; struct SceneModuleUpdateInfo { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml index 6141480d1597a..7b7e2438a3fd6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml @@ -40,6 +40,7 @@ autoware_behavior_path_planner_common autoware_freespace_planning_algorithms autoware_frenet_planner + autoware_internal_debug_msgs autoware_interpolation autoware_lane_departure_checker autoware_lanelet2_extension diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 0768c620dd054..3b272fd8c6722 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -1127,9 +1127,7 @@ std::vector getBoundWithHatchedRoadMarkings( get_corresponding_polygon_index(*current_polygon, bound_point.id())); } } else { - if (!polygon) { - will_close_polygon = true; - } else if (polygon.value().id() != current_polygon.value().id()) { + if (!polygon || polygon.value().id() != current_polygon.value().id()) { will_close_polygon = true; } else { current_polygon_border_indices.push_back( @@ -1496,9 +1494,9 @@ std::vector postProcess( [](const lanelet::ConstLineString3d & points, std::vector & bound) { for (const auto & bound_p : points) { const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); - if (bound.empty()) { - bound.push_back(cp); - } else if (autoware::universe_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { + if ( + bound.empty() || + autoware::universe_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { bound.push_back(cp); } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index e65c67065bc54..33f46420ad537 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -80,7 +80,7 @@ void OccupancyGridBasedCollisionDetector::setMap(const nav_msgs::msg::OccupancyG for (uint32_t i = 0; i < height; i++) { is_obstacle_table.at(i).resize(width); for (uint32_t j = 0; j < width; j++) { - const int cost = costmap_.data[i * width + j]; + const int cost = costmap_.data[i * width + j]; // NOLINT if (cost < 0 || param_.obstacle_threshold <= cost) { is_obstacle_table[i][j] = true; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt index 3fa8ad7218fa2..e28339fb51a6c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt @@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/freespace_pull_out.cpp src/geometric_pull_out.cpp src/shift_pull_out.cpp + src/data_structs.cpp src/util.cpp ) diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp index 9c439e9b3b921..5d8331318484d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp @@ -97,6 +97,7 @@ struct StartPlannerDebugData struct StartPlannerParameters { + static StartPlannerParameters init(rclcpp::Node & node); double th_arrived_distance{0.0}; double th_stopped_velocity{0.0}; double th_stopped_time{0.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp index d1b4c9dabdd7f..5b4f78b252d22 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp @@ -56,6 +56,8 @@ class ShiftPullOut : public PullOutPlannerBase std::shared_ptr lane_departure_checker_; + friend class TestShiftPullOut; + private: // Calculate longitudinal distance based on the acceleration limit, curvature limit, and the // minimum distance requirement. diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml index c7cabb403f164..214731f96cebc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml @@ -22,6 +22,7 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_freespace_planning_algorithms autoware_motion_utils autoware_rtc_interface autoware_universe_utils diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp new file mode 100644 index 0000000000000..f613b9345ce42 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp @@ -0,0 +1,340 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_path_start_planner_module/data_structs.hpp" + +#include "autoware/behavior_path_start_planner_module/manager.hpp" + +#include +#include + +#include + +namespace autoware::behavior_path_planner +{ + +StartPlannerParameters StartPlannerParameters::init(rclcpp::Node & node) +{ + using autoware::universe_utils::getOrDeclareParameter; + StartPlannerParameters p; + { + const std::string ns = "start_planner."; + + p.th_arrived_distance = getOrDeclareParameter(node, ns + "th_arrived_distance"); + p.th_stopped_velocity = getOrDeclareParameter(node, ns + "th_stopped_velocity"); + p.th_stopped_time = getOrDeclareParameter(node, ns + "th_stopped_time"); + p.prepare_time_before_start = + getOrDeclareParameter(node, ns + "prepare_time_before_start"); + p.th_distance_to_middle_of_the_road = + getOrDeclareParameter(node, ns + "th_distance_to_middle_of_the_road"); + p.skip_rear_vehicle_check = getOrDeclareParameter(node, ns + "skip_rear_vehicle_check"); + p.extra_width_margin_for_rear_obstacle = + getOrDeclareParameter(node, ns + "extra_width_margin_for_rear_obstacle"); + p.collision_check_margins = + getOrDeclareParameter>(node, ns + "collision_check_margins"); + p.collision_check_margin_from_front_object = + getOrDeclareParameter(node, ns + "collision_check_margin_from_front_object"); + p.th_moving_object_velocity = + getOrDeclareParameter(node, ns + "th_moving_object_velocity"); + p.center_line_path_interval = + getOrDeclareParameter(node, ns + "center_line_path_interval"); + // shift pull out + p.enable_shift_pull_out = getOrDeclareParameter(node, ns + "enable_shift_pull_out"); + p.check_shift_path_lane_departure = + getOrDeclareParameter(node, ns + "check_shift_path_lane_departure"); + p.allow_check_shift_path_lane_departure_override = + getOrDeclareParameter(node, ns + "allow_check_shift_path_lane_departure_override"); + p.shift_collision_check_distance_from_end = + getOrDeclareParameter(node, ns + "shift_collision_check_distance_from_end"); + p.minimum_shift_pull_out_distance = + getOrDeclareParameter(node, ns + "minimum_shift_pull_out_distance"); + p.lateral_acceleration_sampling_num = + getOrDeclareParameter(node, ns + "lateral_acceleration_sampling_num"); + p.lateral_jerk = getOrDeclareParameter(node, ns + "lateral_jerk"); + p.maximum_lateral_acc = getOrDeclareParameter(node, ns + "maximum_lateral_acc"); + p.minimum_lateral_acc = getOrDeclareParameter(node, ns + "minimum_lateral_acc"); + p.maximum_curvature = getOrDeclareParameter(node, ns + "maximum_curvature"); + p.end_pose_curvature_threshold = + getOrDeclareParameter(node, ns + "end_pose_curvature_threshold"); + p.maximum_longitudinal_deviation = + getOrDeclareParameter(node, ns + "maximum_longitudinal_deviation"); + // geometric pull out + p.enable_geometric_pull_out = + getOrDeclareParameter(node, ns + "enable_geometric_pull_out"); + p.geometric_collision_check_distance_from_end = + getOrDeclareParameter(node, ns + "geometric_collision_check_distance_from_end"); + p.divide_pull_out_path = getOrDeclareParameter(node, ns + "divide_pull_out_path"); + p.parallel_parking_parameters.pull_out_velocity = + getOrDeclareParameter(node, ns + "geometric_pull_out_velocity"); + p.parallel_parking_parameters.pull_out_arc_path_interval = + getOrDeclareParameter(node, ns + "arc_path_interval"); + p.parallel_parking_parameters.pull_out_lane_departure_margin = + getOrDeclareParameter(node, ns + "lane_departure_margin"); + p.lane_departure_check_expansion_margin = + getOrDeclareParameter(node, ns + "lane_departure_check_expansion_margin"); + p.parallel_parking_parameters.pull_out_max_steer_angle = + getOrDeclareParameter(node, ns + "pull_out_max_steer_angle"); // 15deg + p.parallel_parking_parameters.center_line_path_interval = + p.center_line_path_interval; // for geometric parallel parking + // search start pose backward + p.search_priority = getOrDeclareParameter( + node, + ns + "search_priority"); // "efficient_path" or "short_back_distance" + p.enable_back = getOrDeclareParameter(node, ns + "enable_back"); + p.backward_velocity = getOrDeclareParameter(node, ns + "backward_velocity"); + p.max_back_distance = getOrDeclareParameter(node, ns + "max_back_distance"); + p.backward_search_resolution = + getOrDeclareParameter(node, ns + "backward_search_resolution"); + p.backward_path_update_duration = + getOrDeclareParameter(node, ns + "backward_path_update_duration"); + p.ignore_distance_from_lane_end = + getOrDeclareParameter(node, ns + "ignore_distance_from_lane_end"); + // stop condition + p.maximum_deceleration_for_stop = + getOrDeclareParameter(node, ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + getOrDeclareParameter(node, ns + "stop_condition.maximum_jerk_for_stop"); + } + { + const std::string ns = "start_planner.object_types_to_check_for_path_generation."; + p.object_types_to_check_for_path_generation.check_car = + getOrDeclareParameter(node, ns + "check_car"); + p.object_types_to_check_for_path_generation.check_truck = + getOrDeclareParameter(node, ns + "check_truck"); + p.object_types_to_check_for_path_generation.check_bus = + getOrDeclareParameter(node, ns + "check_bus"); + p.object_types_to_check_for_path_generation.check_trailer = + getOrDeclareParameter(node, ns + "check_trailer"); + p.object_types_to_check_for_path_generation.check_unknown = + getOrDeclareParameter(node, ns + "check_unknown"); + p.object_types_to_check_for_path_generation.check_bicycle = + getOrDeclareParameter(node, ns + "check_bicycle"); + p.object_types_to_check_for_path_generation.check_motorcycle = + getOrDeclareParameter(node, ns + "check_motorcycle"); + p.object_types_to_check_for_path_generation.check_pedestrian = + getOrDeclareParameter(node, ns + "check_pedestrian"); + } + // freespace planner general params + { + const std::string ns = "start_planner.freespace_planner."; + p.enable_freespace_planner = getOrDeclareParameter(node, ns + "enable_freespace_planner"); + p.freespace_planner_algorithm = + getOrDeclareParameter(node, ns + "freespace_planner_algorithm"); + p.end_pose_search_start_distance = + getOrDeclareParameter(node, ns + "end_pose_search_start_distance"); + p.end_pose_search_end_distance = + getOrDeclareParameter(node, ns + "end_pose_search_end_distance"); + p.end_pose_search_interval = + getOrDeclareParameter(node, ns + "end_pose_search_interval"); + p.freespace_planner_velocity = getOrDeclareParameter(node, ns + "velocity"); + p.vehicle_shape_margin = getOrDeclareParameter(node, ns + "vehicle_shape_margin"); + p.freespace_planner_common_parameters.time_limit = + getOrDeclareParameter(node, ns + "time_limit"); + p.freespace_planner_common_parameters.max_turning_ratio = + getOrDeclareParameter(node, ns + "max_turning_ratio"); + p.freespace_planner_common_parameters.turning_steps = + getOrDeclareParameter(node, ns + "turning_steps"); + } + // freespace planner search config + { + const std::string ns = "start_planner.freespace_planner.search_configs."; + p.freespace_planner_common_parameters.theta_size = + getOrDeclareParameter(node, ns + "theta_size"); + p.freespace_planner_common_parameters.angle_goal_range = + getOrDeclareParameter(node, ns + "angle_goal_range"); + p.freespace_planner_common_parameters.curve_weight = + getOrDeclareParameter(node, ns + "curve_weight"); + p.freespace_planner_common_parameters.reverse_weight = + getOrDeclareParameter(node, ns + "reverse_weight"); + p.freespace_planner_common_parameters.lateral_goal_range = + getOrDeclareParameter(node, ns + "lateral_goal_range"); + p.freespace_planner_common_parameters.longitudinal_goal_range = + getOrDeclareParameter(node, ns + "longitudinal_goal_range"); + } + // freespace planner costmap configs + { + const std::string ns = "start_planner.freespace_planner.costmap_configs."; + p.freespace_planner_common_parameters.obstacle_threshold = + getOrDeclareParameter(node, ns + "obstacle_threshold"); + } + // freespace planner astar + { + const std::string ns = "start_planner.freespace_planner.astar."; + p.astar_parameters.search_method = + getOrDeclareParameter(node, ns + "search_method"); + p.astar_parameters.only_behind_solutions = + getOrDeclareParameter(node, ns + "only_behind_solutions"); + p.astar_parameters.use_back = getOrDeclareParameter(node, ns + "use_back"); + p.astar_parameters.distance_heuristic_weight = + getOrDeclareParameter(node, ns + "distance_heuristic_weight"); + } + // freespace planner rrtstar + { + const std::string ns = "start_planner.freespace_planner.rrtstar."; + p.rrt_star_parameters.enable_update = getOrDeclareParameter(node, ns + "enable_update"); + p.rrt_star_parameters.use_informed_sampling = + getOrDeclareParameter(node, ns + "use_informed_sampling"); + p.rrt_star_parameters.max_planning_time = + getOrDeclareParameter(node, ns + "max_planning_time"); + p.rrt_star_parameters.neighbor_radius = + getOrDeclareParameter(node, ns + "neighbor_radius"); + p.rrt_star_parameters.margin = getOrDeclareParameter(node, ns + "margin"); + } + + const std::string base_ns = "start_planner.path_safety_check."; + // EgoPredictedPath + { + const std::string ego_path_ns = base_ns + "ego_predicted_path."; + p.ego_predicted_path_params.min_velocity = + getOrDeclareParameter(node, ego_path_ns + "min_velocity"); + p.ego_predicted_path_params.acceleration = + getOrDeclareParameter(node, ego_path_ns + "min_acceleration"); + p.ego_predicted_path_params.time_horizon_for_front_object = + getOrDeclareParameter(node, ego_path_ns + "time_horizon_for_front_object"); + p.ego_predicted_path_params.time_horizon_for_rear_object = + getOrDeclareParameter(node, ego_path_ns + "time_horizon_for_rear_object"); + p.ego_predicted_path_params.time_resolution = + getOrDeclareParameter(node, ego_path_ns + "time_resolution"); + p.ego_predicted_path_params.delay_until_departure = + getOrDeclareParameter(node, ego_path_ns + "delay_until_departure"); + } + // ObjectFilteringParams + const std::string obj_filter_ns = base_ns + "target_filtering."; + { + p.objects_filtering_params.safety_check_time_horizon = + getOrDeclareParameter(node, obj_filter_ns + "safety_check_time_horizon"); + p.objects_filtering_params.safety_check_time_resolution = + getOrDeclareParameter(node, obj_filter_ns + "safety_check_time_resolution"); + p.objects_filtering_params.object_check_forward_distance = + getOrDeclareParameter(node, obj_filter_ns + "object_check_forward_distance"); + p.objects_filtering_params.object_check_backward_distance = + getOrDeclareParameter(node, obj_filter_ns + "object_check_backward_distance"); + p.objects_filtering_params.ignore_object_velocity_threshold = + getOrDeclareParameter(node, obj_filter_ns + "ignore_object_velocity_threshold"); + p.objects_filtering_params.include_opposite_lane = + getOrDeclareParameter(node, obj_filter_ns + "include_opposite_lane"); + p.objects_filtering_params.invert_opposite_lane = + getOrDeclareParameter(node, obj_filter_ns + "invert_opposite_lane"); + p.objects_filtering_params.check_all_predicted_path = + getOrDeclareParameter(node, obj_filter_ns + "check_all_predicted_path"); + p.objects_filtering_params.use_all_predicted_path = + getOrDeclareParameter(node, obj_filter_ns + "use_all_predicted_path"); + p.objects_filtering_params.use_predicted_path_outside_lanelet = + getOrDeclareParameter(node, obj_filter_ns + "use_predicted_path_outside_lanelet"); + } + // ObjectTypesToCheck + { + const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + p.objects_filtering_params.object_types_to_check.check_car = + getOrDeclareParameter(node, obj_types_ns + "check_car"); + p.objects_filtering_params.object_types_to_check.check_truck = + getOrDeclareParameter(node, obj_types_ns + "check_truck"); + p.objects_filtering_params.object_types_to_check.check_bus = + getOrDeclareParameter(node, obj_types_ns + "check_bus"); + p.objects_filtering_params.object_types_to_check.check_trailer = + getOrDeclareParameter(node, obj_types_ns + "check_trailer"); + p.objects_filtering_params.object_types_to_check.check_unknown = + getOrDeclareParameter(node, obj_types_ns + "check_unknown"); + p.objects_filtering_params.object_types_to_check.check_bicycle = + getOrDeclareParameter(node, obj_types_ns + "check_bicycle"); + p.objects_filtering_params.object_types_to_check.check_motorcycle = + getOrDeclareParameter(node, obj_types_ns + "check_motorcycle"); + p.objects_filtering_params.object_types_to_check.check_pedestrian = + getOrDeclareParameter(node, obj_types_ns + "check_pedestrian"); + } + // ObjectLaneConfiguration + { + const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + p.objects_filtering_params.object_lane_configuration.check_current_lane = + getOrDeclareParameter(node, obj_lane_ns + "check_current_lane"); + p.objects_filtering_params.object_lane_configuration.check_right_lane = + getOrDeclareParameter(node, obj_lane_ns + "check_right_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_left_lane = + getOrDeclareParameter(node, obj_lane_ns + "check_left_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = + getOrDeclareParameter(node, obj_lane_ns + "check_shoulder_lane"); + p.objects_filtering_params.object_lane_configuration.check_other_lane = + getOrDeclareParameter(node, obj_lane_ns + "check_other_lane"); + } + // SafetyCheckParams + const std::string safety_check_ns = base_ns + "safety_check_params."; + { + p.safety_check_params.enable_safety_check = + getOrDeclareParameter(node, safety_check_ns + "enable_safety_check"); + p.safety_check_params.hysteresis_factor_expand_rate = + getOrDeclareParameter(node, safety_check_ns + "hysteresis_factor_expand_rate"); + p.safety_check_params.backward_path_length = + getOrDeclareParameter(node, safety_check_ns + "backward_path_length"); + p.safety_check_params.forward_path_length = + getOrDeclareParameter(node, safety_check_ns + "forward_path_length"); + p.safety_check_params.publish_debug_marker = + getOrDeclareParameter(node, safety_check_ns + "publish_debug_marker"); + p.safety_check_params.collision_check_yaw_diff_threshold = + getOrDeclareParameter(node, safety_check_ns + "collision_check_yaw_diff_threshold"); + } + // RSSparams + { + const std::string rss_ns = safety_check_ns + "rss_params."; + p.safety_check_params.rss_params.rear_vehicle_reaction_time = + getOrDeclareParameter(node, rss_ns + "rear_vehicle_reaction_time"); + p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = + getOrDeclareParameter(node, rss_ns + "rear_vehicle_safety_time_margin"); + p.safety_check_params.rss_params.lateral_distance_max_threshold = + getOrDeclareParameter(node, rss_ns + "lateral_distance_max_threshold"); + p.safety_check_params.rss_params.longitudinal_distance_min_threshold = + getOrDeclareParameter(node, rss_ns + "longitudinal_distance_min_threshold"); + p.safety_check_params.rss_params.longitudinal_velocity_delta_time = + getOrDeclareParameter(node, rss_ns + "longitudinal_velocity_delta_time"); + p.safety_check_params.rss_params.extended_polygon_policy = + getOrDeclareParameter(node, rss_ns + "extended_polygon_policy"); + } + // surround moving obstacle check + { + const std::string surround_moving_obstacle_check_ns = + "start_planner.surround_moving_obstacle_check."; + p.search_radius = + getOrDeclareParameter(node, surround_moving_obstacle_check_ns + "search_radius"); + p.th_moving_obstacle_velocity = getOrDeclareParameter( + node, surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity"); + // ObjectTypesToCheck + { + const std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; + p.surround_moving_obstacles_type_to_check.check_car = + getOrDeclareParameter(node, obj_types_ns + "check_car"); + p.surround_moving_obstacles_type_to_check.check_truck = + getOrDeclareParameter(node, obj_types_ns + "check_truck"); + p.surround_moving_obstacles_type_to_check.check_bus = + getOrDeclareParameter(node, obj_types_ns + "check_bus"); + p.surround_moving_obstacles_type_to_check.check_trailer = + getOrDeclareParameter(node, obj_types_ns + "check_trailer"); + p.surround_moving_obstacles_type_to_check.check_unknown = + getOrDeclareParameter(node, obj_types_ns + "check_unknown"); + p.surround_moving_obstacles_type_to_check.check_bicycle = + getOrDeclareParameter(node, obj_types_ns + "check_bicycle"); + p.surround_moving_obstacles_type_to_check.check_motorcycle = + getOrDeclareParameter(node, obj_types_ns + "check_motorcycle"); + p.surround_moving_obstacles_type_to_check.check_pedestrian = + getOrDeclareParameter(node, obj_types_ns + "check_pedestrian"); + } + } + + // debug + { + const std::string debug_ns = "start_planner.debug."; + p.print_debug_info = getOrDeclareParameter(node, debug_ns + "print_debug_info"); + } + + return p; +} +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp index f5739ce322248..9ea51d56ee1cc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -29,318 +29,18 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) // init manager interface initInterface(node, {""}); - StartPlannerParameters p; - - { - const std::string ns = "start_planner."; - - p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); - p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); - p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); - p.prepare_time_before_start = node->declare_parameter(ns + "prepare_time_before_start"); - p.th_distance_to_middle_of_the_road = - node->declare_parameter(ns + "th_distance_to_middle_of_the_road"); - p.skip_rear_vehicle_check = node->declare_parameter(ns + "skip_rear_vehicle_check"); - p.extra_width_margin_for_rear_obstacle = - node->declare_parameter(ns + "extra_width_margin_for_rear_obstacle"); - p.collision_check_margins = - node->declare_parameter>(ns + "collision_check_margins"); - p.collision_check_margin_from_front_object = - node->declare_parameter(ns + "collision_check_margin_from_front_object"); - p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); - p.center_line_path_interval = node->declare_parameter(ns + "center_line_path_interval"); - // shift pull out - p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); - p.check_shift_path_lane_departure = - node->declare_parameter(ns + "check_shift_path_lane_departure"); - p.allow_check_shift_path_lane_departure_override = - node->declare_parameter(ns + "allow_check_shift_path_lane_departure_override"); - p.shift_collision_check_distance_from_end = - node->declare_parameter(ns + "shift_collision_check_distance_from_end"); - p.minimum_shift_pull_out_distance = - node->declare_parameter(ns + "minimum_shift_pull_out_distance"); - p.lateral_acceleration_sampling_num = - node->declare_parameter(ns + "lateral_acceleration_sampling_num"); - p.lateral_jerk = node->declare_parameter(ns + "lateral_jerk"); - p.maximum_lateral_acc = node->declare_parameter(ns + "maximum_lateral_acc"); - p.minimum_lateral_acc = node->declare_parameter(ns + "minimum_lateral_acc"); - p.maximum_curvature = node->declare_parameter(ns + "maximum_curvature"); - p.end_pose_curvature_threshold = - node->declare_parameter(ns + "end_pose_curvature_threshold"); - p.maximum_longitudinal_deviation = - node->declare_parameter(ns + "maximum_longitudinal_deviation"); - // geometric pull out - p.enable_geometric_pull_out = node->declare_parameter(ns + "enable_geometric_pull_out"); - p.geometric_collision_check_distance_from_end = - node->declare_parameter(ns + "geometric_collision_check_distance_from_end"); - p.divide_pull_out_path = node->declare_parameter(ns + "divide_pull_out_path"); - p.parallel_parking_parameters.pull_out_velocity = - node->declare_parameter(ns + "geometric_pull_out_velocity"); - p.parallel_parking_parameters.pull_out_arc_path_interval = - node->declare_parameter(ns + "arc_path_interval"); - p.parallel_parking_parameters.pull_out_lane_departure_margin = - node->declare_parameter(ns + "lane_departure_margin"); - p.lane_departure_check_expansion_margin = - node->declare_parameter(ns + "lane_departure_check_expansion_margin"); - p.parallel_parking_parameters.pull_out_max_steer_angle = - node->declare_parameter(ns + "pull_out_max_steer_angle"); // 15deg - p.parallel_parking_parameters.center_line_path_interval = - p.center_line_path_interval; // for geometric parallel parking - // search start pose backward - p.search_priority = node->declare_parameter( - ns + "search_priority"); // "efficient_path" or "short_back_distance" - p.enable_back = node->declare_parameter(ns + "enable_back"); - p.backward_velocity = node->declare_parameter(ns + "backward_velocity"); - p.max_back_distance = node->declare_parameter(ns + "max_back_distance"); - p.backward_search_resolution = - node->declare_parameter(ns + "backward_search_resolution"); - p.backward_path_update_duration = - node->declare_parameter(ns + "backward_path_update_duration"); - p.ignore_distance_from_lane_end = - node->declare_parameter(ns + "ignore_distance_from_lane_end"); - // stop condition - p.maximum_deceleration_for_stop = - node->declare_parameter(ns + "stop_condition.maximum_deceleration_for_stop"); - p.maximum_jerk_for_stop = - node->declare_parameter(ns + "stop_condition.maximum_jerk_for_stop"); - } - { - const std::string ns = "start_planner.object_types_to_check_for_path_generation."; - p.object_types_to_check_for_path_generation.check_car = - node->declare_parameter(ns + "check_car"); - p.object_types_to_check_for_path_generation.check_truck = - node->declare_parameter(ns + "check_truck"); - p.object_types_to_check_for_path_generation.check_bus = - node->declare_parameter(ns + "check_bus"); - p.object_types_to_check_for_path_generation.check_trailer = - node->declare_parameter(ns + "check_trailer"); - p.object_types_to_check_for_path_generation.check_unknown = - node->declare_parameter(ns + "check_unknown"); - p.object_types_to_check_for_path_generation.check_bicycle = - node->declare_parameter(ns + "check_bicycle"); - p.object_types_to_check_for_path_generation.check_motorcycle = - node->declare_parameter(ns + "check_motorcycle"); - p.object_types_to_check_for_path_generation.check_pedestrian = - node->declare_parameter(ns + "check_pedestrian"); - } - // freespace planner general params - { - const std::string ns = "start_planner.freespace_planner."; - p.enable_freespace_planner = node->declare_parameter(ns + "enable_freespace_planner"); - p.freespace_planner_algorithm = - node->declare_parameter(ns + "freespace_planner_algorithm"); - p.end_pose_search_start_distance = - node->declare_parameter(ns + "end_pose_search_start_distance"); - p.end_pose_search_end_distance = - node->declare_parameter(ns + "end_pose_search_end_distance"); - p.end_pose_search_interval = node->declare_parameter(ns + "end_pose_search_interval"); - p.freespace_planner_velocity = node->declare_parameter(ns + "velocity"); - p.vehicle_shape_margin = node->declare_parameter(ns + "vehicle_shape_margin"); - p.freespace_planner_common_parameters.time_limit = - node->declare_parameter(ns + "time_limit"); - p.freespace_planner_common_parameters.max_turning_ratio = - node->declare_parameter(ns + "max_turning_ratio"); - p.freespace_planner_common_parameters.turning_steps = - node->declare_parameter(ns + "turning_steps"); - } - // freespace planner search config - { - const std::string ns = "start_planner.freespace_planner.search_configs."; - p.freespace_planner_common_parameters.theta_size = - node->declare_parameter(ns + "theta_size"); - p.freespace_planner_common_parameters.angle_goal_range = - node->declare_parameter(ns + "angle_goal_range"); - p.freespace_planner_common_parameters.curve_weight = - node->declare_parameter(ns + "curve_weight"); - p.freespace_planner_common_parameters.reverse_weight = - node->declare_parameter(ns + "reverse_weight"); - p.freespace_planner_common_parameters.lateral_goal_range = - node->declare_parameter(ns + "lateral_goal_range"); - p.freespace_planner_common_parameters.longitudinal_goal_range = - node->declare_parameter(ns + "longitudinal_goal_range"); - } - // freespace planner costmap configs - { - const std::string ns = "start_planner.freespace_planner.costmap_configs."; - p.freespace_planner_common_parameters.obstacle_threshold = - node->declare_parameter(ns + "obstacle_threshold"); - } - // freespace planner astar - { - const std::string ns = "start_planner.freespace_planner.astar."; - p.astar_parameters.search_method = node->declare_parameter(ns + "search_method"); - p.astar_parameters.only_behind_solutions = - node->declare_parameter(ns + "only_behind_solutions"); - p.astar_parameters.use_back = node->declare_parameter(ns + "use_back"); - p.astar_parameters.distance_heuristic_weight = - node->declare_parameter(ns + "distance_heuristic_weight"); - } - // freespace planner rrtstar - { - const std::string ns = "start_planner.freespace_planner.rrtstar."; - p.rrt_star_parameters.enable_update = node->declare_parameter(ns + "enable_update"); - p.rrt_star_parameters.use_informed_sampling = - node->declare_parameter(ns + "use_informed_sampling"); - p.rrt_star_parameters.max_planning_time = - node->declare_parameter(ns + "max_planning_time"); - p.rrt_star_parameters.neighbor_radius = node->declare_parameter(ns + "neighbor_radius"); - p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); - } - - const std::string base_ns = "start_planner.path_safety_check."; - // EgoPredictedPath - { - const std::string ego_path_ns = base_ns + "ego_predicted_path."; - p.ego_predicted_path_params.min_velocity = - node->declare_parameter(ego_path_ns + "min_velocity"); - p.ego_predicted_path_params.acceleration = - node->declare_parameter(ego_path_ns + "min_acceleration"); - p.ego_predicted_path_params.time_horizon_for_front_object = - node->declare_parameter(ego_path_ns + "time_horizon_for_front_object"); - p.ego_predicted_path_params.time_horizon_for_rear_object = - node->declare_parameter(ego_path_ns + "time_horizon_for_rear_object"); - p.ego_predicted_path_params.time_resolution = - node->declare_parameter(ego_path_ns + "time_resolution"); - p.ego_predicted_path_params.delay_until_departure = - node->declare_parameter(ego_path_ns + "delay_until_departure"); - } - // ObjectFilteringParams - const std::string obj_filter_ns = base_ns + "target_filtering."; - { - p.objects_filtering_params.safety_check_time_horizon = - node->declare_parameter(obj_filter_ns + "safety_check_time_horizon"); - p.objects_filtering_params.safety_check_time_resolution = - node->declare_parameter(obj_filter_ns + "safety_check_time_resolution"); - p.objects_filtering_params.object_check_forward_distance = - node->declare_parameter(obj_filter_ns + "object_check_forward_distance"); - p.objects_filtering_params.object_check_backward_distance = - node->declare_parameter(obj_filter_ns + "object_check_backward_distance"); - p.objects_filtering_params.ignore_object_velocity_threshold = - node->declare_parameter(obj_filter_ns + "ignore_object_velocity_threshold"); - p.objects_filtering_params.include_opposite_lane = - node->declare_parameter(obj_filter_ns + "include_opposite_lane"); - p.objects_filtering_params.invert_opposite_lane = - node->declare_parameter(obj_filter_ns + "invert_opposite_lane"); - p.objects_filtering_params.check_all_predicted_path = - node->declare_parameter(obj_filter_ns + "check_all_predicted_path"); - p.objects_filtering_params.use_all_predicted_path = - node->declare_parameter(obj_filter_ns + "use_all_predicted_path"); - p.objects_filtering_params.use_predicted_path_outside_lanelet = - node->declare_parameter(obj_filter_ns + "use_predicted_path_outside_lanelet"); - } - // ObjectTypesToCheck - { - const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; - p.objects_filtering_params.object_types_to_check.check_car = - node->declare_parameter(obj_types_ns + "check_car"); - p.objects_filtering_params.object_types_to_check.check_truck = - node->declare_parameter(obj_types_ns + "check_truck"); - p.objects_filtering_params.object_types_to_check.check_bus = - node->declare_parameter(obj_types_ns + "check_bus"); - p.objects_filtering_params.object_types_to_check.check_trailer = - node->declare_parameter(obj_types_ns + "check_trailer"); - p.objects_filtering_params.object_types_to_check.check_unknown = - node->declare_parameter(obj_types_ns + "check_unknown"); - p.objects_filtering_params.object_types_to_check.check_bicycle = - node->declare_parameter(obj_types_ns + "check_bicycle"); - p.objects_filtering_params.object_types_to_check.check_motorcycle = - node->declare_parameter(obj_types_ns + "check_motorcycle"); - p.objects_filtering_params.object_types_to_check.check_pedestrian = - node->declare_parameter(obj_types_ns + "check_pedestrian"); - } - // ObjectLaneConfiguration - { - const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; - p.objects_filtering_params.object_lane_configuration.check_current_lane = - node->declare_parameter(obj_lane_ns + "check_current_lane"); - p.objects_filtering_params.object_lane_configuration.check_right_lane = - node->declare_parameter(obj_lane_ns + "check_right_side_lane"); - p.objects_filtering_params.object_lane_configuration.check_left_lane = - node->declare_parameter(obj_lane_ns + "check_left_side_lane"); - p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = - node->declare_parameter(obj_lane_ns + "check_shoulder_lane"); - p.objects_filtering_params.object_lane_configuration.check_other_lane = - node->declare_parameter(obj_lane_ns + "check_other_lane"); - } - // SafetyCheckParams - const std::string safety_check_ns = base_ns + "safety_check_params."; - { - p.safety_check_params.enable_safety_check = - node->declare_parameter(safety_check_ns + "enable_safety_check"); - p.safety_check_params.hysteresis_factor_expand_rate = - node->declare_parameter(safety_check_ns + "hysteresis_factor_expand_rate"); - p.safety_check_params.backward_path_length = - node->declare_parameter(safety_check_ns + "backward_path_length"); - p.safety_check_params.forward_path_length = - node->declare_parameter(safety_check_ns + "forward_path_length"); - p.safety_check_params.publish_debug_marker = - node->declare_parameter(safety_check_ns + "publish_debug_marker"); - p.safety_check_params.collision_check_yaw_diff_threshold = - node->declare_parameter(safety_check_ns + "collision_check_yaw_diff_threshold"); - } - // RSSparams - { - const std::string rss_ns = safety_check_ns + "rss_params."; - p.safety_check_params.rss_params.rear_vehicle_reaction_time = - node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); - p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = - node->declare_parameter(rss_ns + "rear_vehicle_safety_time_margin"); - p.safety_check_params.rss_params.lateral_distance_max_threshold = - node->declare_parameter(rss_ns + "lateral_distance_max_threshold"); - p.safety_check_params.rss_params.longitudinal_distance_min_threshold = - node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); - p.safety_check_params.rss_params.longitudinal_velocity_delta_time = - node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); - p.safety_check_params.rss_params.extended_polygon_policy = - node->declare_parameter(rss_ns + "extended_polygon_policy"); - } - // surround moving obstacle check - { - const std::string surround_moving_obstacle_check_ns = - "start_planner.surround_moving_obstacle_check."; - p.search_radius = - node->declare_parameter(surround_moving_obstacle_check_ns + "search_radius"); - p.th_moving_obstacle_velocity = node->declare_parameter( - surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity"); - // ObjectTypesToCheck - { - const std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; - p.surround_moving_obstacles_type_to_check.check_car = - node->declare_parameter(obj_types_ns + "check_car"); - p.surround_moving_obstacles_type_to_check.check_truck = - node->declare_parameter(obj_types_ns + "check_truck"); - p.surround_moving_obstacles_type_to_check.check_bus = - node->declare_parameter(obj_types_ns + "check_bus"); - p.surround_moving_obstacles_type_to_check.check_trailer = - node->declare_parameter(obj_types_ns + "check_trailer"); - p.surround_moving_obstacles_type_to_check.check_unknown = - node->declare_parameter(obj_types_ns + "check_unknown"); - p.surround_moving_obstacles_type_to_check.check_bicycle = - node->declare_parameter(obj_types_ns + "check_bicycle"); - p.surround_moving_obstacles_type_to_check.check_motorcycle = - node->declare_parameter(obj_types_ns + "check_motorcycle"); - p.surround_moving_obstacles_type_to_check.check_pedestrian = - node->declare_parameter(obj_types_ns + "check_pedestrian"); - } - } - - // debug - { - const std::string debug_ns = "start_planner.debug."; - p.print_debug_info = node->declare_parameter(debug_ns + "print_debug_info"); - } - + StartPlannerParameters parameters = StartPlannerParameters::init(*node); // validation of parameters - if (p.lateral_acceleration_sampling_num < 1) { + if (parameters.lateral_acceleration_sampling_num < 1) { RCLCPP_FATAL_STREAM( node->get_logger().get_child(name()), "lateral_acceleration_sampling_num must be positive integer. Given parameter: " - << p.lateral_acceleration_sampling_num << std::endl + << parameters.lateral_acceleration_sampling_num << std::endl << "Terminating the program..."); exit(EXIT_FAILURE); } - parameters_ = std::make_shared(p); + parameters_ = std::make_shared(parameters); } void StartPlannerModuleManager::updateModuleParams( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp new file mode 100644 index 0000000000000..156cf62f9ac4a --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp @@ -0,0 +1,179 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +using autoware::behavior_path_planner::ShiftPullOut; +using autoware::behavior_path_planner::StartPlannerParameters; +using autoware::lane_departure_checker::LaneDepartureChecker; +using autoware::test_utils::get_absolute_path_to_config; +using autoware_planning_msgs::msg::LaneletRoute; +using RouteSections = std::vector; +using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; + +namespace autoware::behavior_path_planner +{ + +class TestShiftPullOut : public ::testing::Test +{ +public: + std::optional call_plan( + const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) + { + return shift_pull_out_->plan(start_pose, goal_pose, planner_debug_data); + } + +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + node_ = rclcpp::Node::make_shared("shift_pull_out", make_node_options()); + + initialize_lane_departure_checker(); + initialize_shift_pull_out_planner(); + } + + void TearDown() override { rclcpp::shutdown(); } + + PlannerData make_planner_data( + const Pose & start_pose, const int route_start_lane_id, const int route_goal_lane_id) + { + PlannerData planner_data; + planner_data.init_parameters(*node_); + + // Load a sample lanelet map and create a route handler + const auto shoulder_map_path = autoware::test_utils::get_absolute_path_to_lanelet_map( + "autoware_test_utils", "road_shoulder/lanelet2_map.osm"); + const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(shoulder_map_path, 0.5); + auto route_handler = std::make_shared(map_bin_msg); + + // Set up current odometry at start pose + auto odometry = std::make_shared(); + odometry->pose.pose = start_pose; + odometry->header.frame_id = "map"; + planner_data.self_odometry = odometry; + + // Setup route + const auto route = makeBehaviorRouteFromLaneId( + route_start_lane_id, route_goal_lane_id, "autoware_test_utils", + "road_shoulder/lanelet2_map.osm"); + route_handler->setRoute(route); + + // Update planner data with the route handler + planner_data.route_handler = route_handler; + + return planner_data; + } + + // Member variables + std::shared_ptr node_; + std::shared_ptr shift_pull_out_; + std::shared_ptr lane_departure_checker_; + +private: + rclcpp::NodeOptions make_node_options() const + { + // Load common configuration files + auto node_options = rclcpp::NodeOptions{}; + + const auto common_param_path = + get_absolute_path_to_config("autoware_test_utils", "test_common.param.yaml"); + const auto nearest_search_param_path = + get_absolute_path_to_config("autoware_test_utils", "test_nearest_search.param.yaml"); + const auto vehicle_info_param_path = + get_absolute_path_to_config("autoware_test_utils", "test_vehicle_info.param.yaml"); + const auto behavior_path_planner_param_path = get_absolute_path_to_config( + "autoware_behavior_path_planner", "behavior_path_planner.param.yaml"); + const auto drivable_area_expansion_param_path = get_absolute_path_to_config( + "autoware_behavior_path_planner", "drivable_area_expansion.param.yaml"); + const auto scene_module_manager_param_path = get_absolute_path_to_config( + "autoware_behavior_path_planner", "scene_module_manager.param.yaml"); + const auto start_planner_param_path = get_absolute_path_to_config( + "autoware_behavior_path_start_planner_module", "start_planner.param.yaml"); + + autoware::test_utils::updateNodeOptions( + node_options, {common_param_path, nearest_search_param_path, vehicle_info_param_path, + behavior_path_planner_param_path, drivable_area_expansion_param_path, + scene_module_manager_param_path, start_planner_param_path}); + + return node_options; + } + + void initialize_lane_departure_checker() + { + const auto vehicle_info = + autoware::vehicle_info_utils::VehicleInfoUtils(*node_).getVehicleInfo(); + lane_departure_checker_ = std::make_shared(); + lane_departure_checker_->setVehicleInfo(vehicle_info); + + autoware::lane_departure_checker::Param lane_departure_checker_params{}; + lane_departure_checker_->setParam(lane_departure_checker_params); + } + + void initialize_shift_pull_out_planner() + { + auto parameters = StartPlannerParameters::init(*node_); + + auto time_keeper = std::make_shared(); + shift_pull_out_ = + std::make_shared(*node_, parameters, lane_departure_checker_, time_keeper); + } +}; + +TEST_F(TestShiftPullOut, GenerateValidShiftPullOutPath) +{ + const auto start_pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(362.181).y(362.164).z(100.000)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(0.709650).w( + 0.704554)); + + const auto goal_pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(365.658).y(507.253).z(100.000)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(0.705897).w( + 0.708314)); + const auto planner_data = make_planner_data(start_pose, 4619, 4635); + + shift_pull_out_->setPlannerData(std::make_shared(planner_data)); + + // Plan the pull out path + PlannerDebugData debug_data; + auto result = call_plan(start_pose, goal_pose, debug_data); + + // Assert that a valid shift pull out path is generated + ASSERT_TRUE(result.has_value()) << "shift pull out path generation failed."; + EXPECT_EQ(result->partial_paths.size(), 1UL) + << "Generated shift pull out path does not have the expected number of partial paths."; + EXPECT_EQ(debug_data.conditions_evaluation.back(), "success") + << "shift pull out path planning did not succeed."; +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index be4ce4a125c68..48729c9c4fa0c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -233,11 +233,7 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( std::for_each( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { - if (!not_use_adjacent_lane) { - data.drivable_lanes.push_back( - utils::static_obstacle_avoidance::generateExpandedDrivableLanes( - lanelet, planner_data_, parameters_)); - } else if (red_signal_lane_itr->id() != lanelet.id()) { + if (!not_use_adjacent_lane || red_signal_lane_itr->id() != lanelet.id()) { data.drivable_lanes.push_back( utils::static_obstacle_avoidance::generateExpandedDrivableLanes( lanelet, planner_data_, parameters_)); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index 744af35641a59..52d5ee49c9a68 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -264,9 +264,9 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( AvoidOutlines outlines; for (auto & o : data.target_objects) { if (!o.avoid_margin.has_value()) { - if (!data.red_signal_lane.has_value()) { - o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE; - } else if (data.red_signal_lane.value().id() == o.overhang_lanelet.id()) { + if ( + data.red_signal_lane.has_value() && + data.red_signal_lane.value().id() == o.overhang_lanelet.id()) { o.info = ObjectInfo::LIMIT_DRIVABLE_SPACE_TEMPORARY; } else { o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp index f296ad03cbac6..bf01986d5c28f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -42,8 +42,8 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class BlindSpotModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp index 1bfa41d86ffbe..50ce8634ec600 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp @@ -16,8 +16,8 @@ #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_ #include -#include #include +#include #include #include @@ -59,7 +59,7 @@ struct Safe using BlindSpotDecision = std::variant; -class BlindSpotModule : public SceneModuleInterface +class BlindSpotModule : public SceneModuleInterfaceWithRTC { public: struct DebugData diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml index a2ea4a82a884d..9739370387df7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp index cc63b42df68e4..a1410d2fecfd5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp @@ -83,7 +83,7 @@ void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa } } -std::function &)> +std::function &)> BlindSpotModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index 7697786adb19d..7d1a68c5006a0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -41,7 +41,7 @@ BlindSpotModule::BlindSpotModule( const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, const std::shared_ptr planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), +: SceneModuleInterfaceWithRTC(module_id, logger, clock), lane_id_(lane_id), planner_param_{planner_param}, turn_direction_(turn_direction), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml index 6b939a7dd0be9..ff01e85aa5456 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml @@ -22,7 +22,9 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_grid_map_utils + autoware_internal_debug_msgs autoware_interpolation autoware_lanelet2_extension autoware_motion_utils @@ -41,8 +43,6 @@ pluginlib rclcpp sensor_msgs - tier4_api_msgs - tier4_debug_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py index 8a9de1a563249..5b13b6f5752d3 100755 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py @@ -19,10 +19,10 @@ from copy import deepcopy from ament_index_python.packages import get_package_share_directory +from autoware_internal_debug_msgs.msg import StringStamped import matplotlib.pyplot as plt import rclpy from rclpy.node import Node -from tier4_debug_msgs.msg import StringStamped import yaml diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp index 8ba05be36ae56..cdaff7225a7d2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp @@ -216,7 +216,7 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) } } -std::function &)> +std::function &)> CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; @@ -233,7 +233,7 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) crosswalk_id_set.insert(crosswalk.first->crosswalkLanelet().id()); } - return [crosswalk_id_set](const std::shared_ptr & scene_module) { + return [crosswalk_id_set](const std::shared_ptr & scene_module) { return crosswalk_id_set.count(scene_module->getModuleId()) == 0; }; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp index 693f6c27a380d..091a427e14949 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp @@ -19,11 +19,10 @@ #include #include -#include +#include #include #include -#include #include #include @@ -49,8 +48,8 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const PathWithLaneId & path) override; }; class CrosswalkModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 1bea0626b0b2f..64f7e9e14dcd1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -153,11 +153,11 @@ StopFactor createStopFactor( return stop_factor; } -tier4_debug_msgs::msg::StringStamped createStringStampedMessage( +autoware_internal_debug_msgs::msg::StringStamped createStringStampedMessage( const rclcpp::Time & now, const int64_t module_id_, const std::vector> & collision_points) { - tier4_debug_msgs::msg::StringStamped msg; + autoware_internal_debug_msgs::msg::StringStamped msg; msg.stamp = now; for (const auto & collision_point : collision_points) { std::stringstream ss; @@ -176,7 +176,7 @@ CrosswalkModule::CrosswalkModule( const std::optional & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), +: SceneModuleInterfaceWithRTC(module_id, logger, clock), module_id_(module_id), planner_param_(planner_param), use_regulatory_element_(reg_elem_id) @@ -199,8 +199,8 @@ CrosswalkModule::CrosswalkModule( road_ = lanelet_map_ptr->laneletLayer.get(lane_id); - collision_info_pub_ = - node.create_publisher("~/debug/collision_info", 1); + collision_info_pub_ = node.create_publisher( + "~/debug/collision_info", 1); } bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 8577ed1669b48..d5a9e463c730b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -17,15 +17,15 @@ #include "autoware/behavior_velocity_crosswalk_module/util.hpp" -#include +#include #include #include #include #include +#include #include #include -#include #include #include @@ -57,7 +57,6 @@ using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::TrafficLightElement; using lanelet::autoware::Crosswalk; -using tier4_api_msgs::msg::CrosswalkStatus; using tier4_planning_msgs::msg::PathWithLaneId; namespace @@ -113,7 +112,7 @@ double InterpolateMap( } } // namespace -class CrosswalkModule : public SceneModuleInterface +class CrosswalkModule : public SceneModuleInterfaceWithRTC { public: struct PlannerParam @@ -457,7 +456,8 @@ class CrosswalkModule : public SceneModuleInterface const int64_t module_id_; - rclcpp::Publisher::SharedPtr collision_info_pub_; + rclcpp::Publisher::SharedPtr + collision_info_pub_; lanelet::ConstLanelet crosswalk_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml index ff91cf40a32a6..90aaddf96feef 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml @@ -18,6 +18,7 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_lanelet2_extension autoware_motion_utils autoware_planning_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp index 3f39696ff6bcc..62f5b88699a37 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp @@ -74,16 +74,17 @@ void DetectionAreaModuleManager::launchNewModules( } } -std::function &)> +std::function &)> DetectionAreaModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto detection_area_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [detection_area_id_set](const std::shared_ptr & scene_module) { - return detection_area_id_set.count(scene_module->getModuleId()) == 0; - }; + return + [detection_area_id_set](const std::shared_ptr & scene_module) { + return detection_area_id_set.count(scene_module->getModuleId()) == 0; + }; } } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp index 7aa60cbbaa18b..1fbcc16461ebc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -41,8 +41,8 @@ class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class DetectionAreaModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 61b3b185999d8..6d7754624485c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -37,7 +37,7 @@ DetectionAreaModule::DetectionAreaModule( const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), +: SceneModuleInterfaceWithRTC(module_id, logger, clock), lane_id_(lane_id), detection_area_reg_elem_(detection_area_reg_elem), state_(State::GO), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp index 9224cf4624687..bdf2d05a35bcb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp @@ -23,8 +23,8 @@ #define EIGEN_MPL2_ONLY #include -#include #include +#include #include #include @@ -38,7 +38,7 @@ using PathIndexWithPoint2d = std::pair; // front using PathIndexWithOffset = std::pair; // front index, offset using tier4_planning_msgs::msg::PathWithLaneId; -class DetectionAreaModule : public SceneModuleInterface +class DetectionAreaModule : public SceneModuleInterfaceWithRTC { public: enum class State { GO, STOP }; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml index b056f71614da3..208e7ed49de71 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml @@ -20,6 +20,8 @@ autoware_cmake autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface + autoware_internal_debug_msgs autoware_interpolation autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/ttc.py b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/ttc.py index 1eb6ae1ffafc1..c1bed003ea3a9 100755 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/ttc.py +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/ttc.py @@ -22,13 +22,13 @@ import time from PIL import Image +from autoware_internal_debug_msgs.msg import Float64MultiArrayStamped import imageio import matplotlib import matplotlib.pyplot as plt import numpy as np import rclpy from rclpy.node import Node -from tier4_debug_msgs.msg import Float64MultiArrayStamped matplotlib.use("TKAgg") diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp index 39ed8e80412a6..41bdbe3e43c8b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -353,14 +353,14 @@ void IntersectionModuleManager::launchNewModules( } } -std::function &)> +std::function &)> IntersectionModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [lane_set](const std::shared_ptr & scene_module) { + return [lane_set](const std::shared_ptr & scene_module) { const auto intersection_module = std::dynamic_pointer_cast(scene_module); const auto & associative_ids = intersection_module->getAssociativeIds(); for (const auto & lane : lane_set) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp index 8189cbe7bc5d7..c6f76d7c39640 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp @@ -21,10 +21,10 @@ #include #include #include +#include #include #include -#include #include #include @@ -48,8 +48,8 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; @@ -64,7 +64,7 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC tl_observation_pub_; }; -class MergeFromPrivateModuleManager : public SceneModuleManagerInterface +class MergeFromPrivateModuleManager : public SceneModuleManagerInterface<> { public: explicit MergeFromPrivateModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 6f7841ebb0bbb..e33416860c319 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -53,7 +53,7 @@ IntersectionModule::IntersectionModule( const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), +: SceneModuleInterfaceWithRTC(module_id, logger, clock), planner_param_(planner_param), lane_id_(lane_id), associative_ids_(associative_ids), @@ -88,10 +88,11 @@ IntersectionModule::IntersectionModule( static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); } - ego_ttc_pub_ = node.create_publisher( + ego_ttc_pub_ = node.create_publisher( "~/debug/intersection/ego_ttc", 1); - object_ttc_pub_ = node.create_publisher( - "~/debug/intersection/object_ttc", 1); + object_ttc_pub_ = + node.create_publisher( + "~/debug/intersection/object_ttc", 1); } bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path) @@ -231,7 +232,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat // calculate the expected vehicle speed and obtain the spatiotemporal profile of ego to the // exit of intersection // ========================================================================================== - tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; const auto time_distance_array = calcIntersectionPassingTime(*path, is_prioritized, intersection_stoplines, &ego_ttc_time_array); @@ -240,7 +241,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat // passed each pass judge line for the first time, save current collision status for late // diagnosis // ========================================================================================== - tier4_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; updateObjectInfoManagerCollision( path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line, safely_passed_2nd_judge_line, &object_ttc_time_array); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index b718dd84d33af..6c31be2ce83b9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -22,12 +22,12 @@ #include "object_manager.hpp" #include "result.hpp" -#include #include +#include #include #include -#include +#include #include #include @@ -46,7 +46,7 @@ namespace autoware::behavior_velocity_planner { -class IntersectionModule : public SceneModuleInterface +class IntersectionModule : public SceneModuleInterfaceWithRTC { public: struct PlannerParam @@ -748,7 +748,7 @@ class IntersectionModule : public SceneModuleInterface const PathLanelets & path_lanelets, const TimeDistanceArray & time_distance_array, const TrafficPrioritizedLevel & traffic_prioritized_level, const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time, - tier4_debug_msgs::msg::Float64MultiArrayStamped * object_ttc_time_array); + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped * object_ttc_time_array); void cutPredictPathWithinDuration( const builtin_interfaces::msg::Time & object_stamp, const double time_thr, @@ -809,13 +809,15 @@ class IntersectionModule : public SceneModuleInterface TimeDistanceArray calcIntersectionPassingTime( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const IntersectionStopLines & intersection_stoplines, - tier4_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const; + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const; /** @} */ mutable DebugData debug_data_; mutable InternalDebugData internal_debug_data_{}; - rclcpp::Publisher::SharedPtr ego_ttc_pub_; - rclcpp::Publisher::SharedPtr object_ttc_pub_; + rclcpp::Publisher::SharedPtr + ego_ttc_pub_; + rclcpp::Publisher::SharedPtr + object_ttc_pub_; }; } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 5a66bec15bab1..5103cd6cc46e4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -152,7 +152,7 @@ void IntersectionModule::updateObjectInfoManagerCollision( const IntersectionModule::TimeDistanceArray & time_distance_array, const IntersectionModule::TrafficPrioritizedLevel & traffic_prioritized_level, const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time, - tier4_debug_msgs::msg::Float64MultiArrayStamped * object_ttc_time_array) + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped * object_ttc_time_array) { const auto & intersection_lanelets = intersection_lanelets_.value(); @@ -815,7 +815,7 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const IntersectionStopLines & intersection_stoplines, - tier4_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const { const double intersection_velocity = planner_param_.collision_detection.velocity_profile.default_velocity; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp index 545af1576c6a8..bea068f5b9579 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp @@ -29,7 +29,7 @@ namespace autoware::behavior_velocity_planner { -class NoDrivableLaneModuleManager : public SceneModuleManagerInterface +class NoDrivableLaneModuleManager : public SceneModuleManagerInterface<> { public: explicit NoDrivableLaneModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml index 88fafeb5b90dc..4415eadef62c7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_interpolation autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp index 9d66aa6672d36..dca2dde33b693 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -69,16 +69,17 @@ void NoStoppingAreaModuleManager::launchNewModules( } } -std::function &)> +std::function &)> NoStoppingAreaModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto no_stopping_area_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [no_stopping_area_id_set](const std::shared_ptr & scene_module) { - return no_stopping_area_id_set.count(scene_module->getModuleId()) == 0; - }; + return + [no_stopping_area_id_set](const std::shared_ptr & scene_module) { + return no_stopping_area_id_set.count(scene_module->getModuleId()) == 0; + }; } } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp index 7009e94612580..523cbba291632 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -41,8 +41,8 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class NoStoppingAreaModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 3769aed71a1ec..5c79ec69a9d98 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -41,7 +41,7 @@ NoStoppingAreaModule::NoStoppingAreaModule( const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), +: SceneModuleInterfaceWithRTC(module_id, logger, clock), lane_id_(lane_id), no_stopping_area_reg_elem_(no_stopping_area_reg_elem), planner_param_(planner_param), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 51f3a0d261ebd..1eafcf157623d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -17,9 +17,9 @@ #include "utils.hpp" -#include #include #include +#include #include #include @@ -31,7 +31,7 @@ namespace autoware::behavior_velocity_planner { -class NoStoppingAreaModule : public SceneModuleInterface +class NoStoppingAreaModule : public SceneModuleInterfaceWithRTC { public: struct PlannerParam diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp index 32b1818214952..08c1516dea67a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp @@ -39,7 +39,7 @@ namespace autoware::behavior_velocity_planner { -class OcclusionSpotModuleManager : public SceneModuleManagerInterface +class OcclusionSpotModuleManager : public SceneModuleManagerInterface<> { public: explicit OcclusionSpotModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt index 37a02b844dfe9..31900a23d00e5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt @@ -16,6 +16,7 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/node.cpp src/planner_manager.cpp + src/test_utils.cpp ) rclcpp_components_register_node(${PROJECT_NAME}_lib @@ -34,7 +35,7 @@ endif() if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - test/src/test_node_interface.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} gtest_main diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp similarity index 96% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp index e260f582aae60..4c1f10c355226 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NODE_HPP_ -#define NODE_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ +#include "autoware/behavior_velocity_planner/planner_manager.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" -#include "planner_manager.hpp" #include #include @@ -31,8 +31,6 @@ #include #include #include -#include -#include #include #include #include @@ -152,4 +150,4 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node }; } // namespace autoware::behavior_velocity_planner -#endif // NODE_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp similarity index 91% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp index fddd658cef06e..9bd423ecfef21 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNER_MANAGER_HPP_ -#define PLANNER_MANAGER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ #include #include @@ -57,4 +57,4 @@ class BehaviorVelocityPlannerManager }; } // namespace autoware::behavior_velocity_planner -#endif // PLANNER_MANAGER_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp new file mode 100644 index 0000000000000..f34182e77e6f1 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp @@ -0,0 +1,47 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_ + +#include "autoware/behavior_velocity_planner/node.hpp" + +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; +using autoware::planning_test_manager::PlanningInterfaceTestManager; + +struct PluginInfo +{ + std::string module_name; // e.g. crosswalk + std::string plugin_name; // e.g. autoware::behavior_velocity_planner::CrosswalkModulePlugin +}; + +std::shared_ptr generateTestManager(); + +std::shared_ptr generateNode( + const std::vector & plugin_info_vec); + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node); +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml index 8ba8272712364..04b4cf0328fd1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml @@ -56,7 +56,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_api_msgs tier4_planning_msgs tier4_v2x_msgs visualization_msgs @@ -65,19 +64,6 @@ ament_cmake_ros ament_lint_auto - autoware_behavior_velocity_blind_spot_module - autoware_behavior_velocity_crosswalk_module - autoware_behavior_velocity_detection_area_module - autoware_behavior_velocity_intersection_module - autoware_behavior_velocity_no_drivable_lane_module - autoware_behavior_velocity_no_stopping_area_module - autoware_behavior_velocity_occlusion_spot_module - autoware_behavior_velocity_run_out_module - autoware_behavior_velocity_speed_bump_module - autoware_behavior_velocity_stop_line_module - autoware_behavior_velocity_traffic_light_module - autoware_behavior_velocity_virtual_traffic_light_module - autoware_behavior_velocity_walkway_module autoware_lint_common diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp index d78bc883e6b35..5f78e4c670b49 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "node.hpp" +#include "autoware/behavior_velocity_planner/node.hpp" #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp index 4820c340058ff..45ee83260d53a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planner_manager.hpp" +#include "autoware/behavior_velocity_planner/planner_manager.hpp" #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp similarity index 50% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp index fe79450d0def8..ee1bb8f89fc46 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,22 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "node.hpp" +#include "autoware/behavior_velocity_planner/test_utils.hpp" #include #include #include -#include - -#include #include #include #include -using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; -using autoware::planning_test_manager::PlanningInterfaceTestManager; - +namespace autoware::behavior_velocity_planner +{ std::shared_ptr generateTestManager() { auto test_manager = std::make_shared(); @@ -45,7 +41,8 @@ std::shared_ptr generateTestManager() return test_manager; } -std::shared_ptr generateNode() +std::shared_ptr generateNode( + const std::vector & plugin_info_vec) { auto node_options = rclcpp::NodeOptions{}; @@ -64,49 +61,29 @@ std::shared_ptr generateNode() return package_path + "/config/" + module + ".param.yaml"; }; - std::vector module_names; - module_names.emplace_back("autoware::behavior_velocity_planner::CrosswalkModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::WalkwayModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::TrafficLightModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::IntersectionModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::MergeFromPrivateModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::BlindSpotModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::DetectionAreaModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::NoStoppingAreaModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::StopLineModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::OcclusionSpotModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::RunOutModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::SpeedBumpModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::NoDrivableLaneModulePlugin"); + std::vector plugin_names; + for (const auto & plugin_info : plugin_info_vec) { + plugin_names.emplace_back(plugin_info.plugin_name); + } std::vector params; - params.emplace_back("launch_modules", module_names); + params.emplace_back("launch_modules", plugin_names); params.emplace_back("is_simulation", false); node_options.parameter_overrides(params); - autoware::test_utils::updateNodeOptions( - node_options, - {autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", - velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", - velocity_smoother_dir + "/config/Analytical.param.yaml", - behavior_velocity_planner_common_dir + "/config/behavior_velocity_planner_common.param.yaml", - behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", - get_behavior_velocity_module_config("blind_spot"), - get_behavior_velocity_module_config("crosswalk"), - get_behavior_velocity_module_config("walkway"), - get_behavior_velocity_module_config("detection_area"), - get_behavior_velocity_module_config("intersection"), - get_behavior_velocity_module_config("no_stopping_area"), - get_behavior_velocity_module_config("occlusion_spot"), - get_behavior_velocity_module_config("run_out"), - get_behavior_velocity_module_config("speed_bump"), - get_behavior_velocity_module_config("stop_line"), - get_behavior_velocity_module_config("traffic_light"), - get_behavior_velocity_module_config("virtual_traffic_light"), - get_behavior_velocity_module_config("no_drivable_lane")}); + auto yaml_files = std::vector{ + autoware_test_utils_dir + "/config/test_common.param.yaml", + autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", + autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", + velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", + velocity_smoother_dir + "/config/Analytical.param.yaml", + behavior_velocity_planner_common_dir + "/config/behavior_velocity_planner_common.param.yaml", + behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml"}; + for (const auto & plugin_info : plugin_info_vec) { + yaml_files.push_back(get_behavior_velocity_module_config(plugin_info.module_name)); + } + + autoware::test_utils::updateNodeOptions(node_options, yaml_files); // TODO(Takagi, Isamu): set launch_modules // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light @@ -141,39 +118,4 @@ void publishMandatoryTopics( test_manager->publishOccupancyGrid( test_target_node, "behavior_velocity_planner_node/input/occupancy_grid"); } - -TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) -{ - rclcpp::init(0, nullptr); - auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); - - publishMandatoryTopics(test_manager, test_target_node); - - // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); - EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - - // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); - rclcpp::shutdown(); -} - -TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) -{ - rclcpp::init(0, nullptr); - - auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); - publishMandatoryTopics(test_manager, test_target_node); - - // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); - - // make sure behavior_path_planner is running - EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); - - rclcpp::shutdown(); -} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp new file mode 100644 index 0000000000000..6e232318c1711 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp @@ -0,0 +1,61 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_velocity_planner/test_utils.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md index 2abbb83575af5..c4c8d97b4b390 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md @@ -1,3 +1,3 @@ # Behavior Velocity Planner Common -This package provides common functions as a library, which are used in the `behavior_velocity_planner` node and modules. +This package provides a behavior velocity interface without RTC, and common functions as a library, which are used in the `behavior_velocity_planner` node and modules. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp index 309ba33a3498a..983aaec2acf4f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp @@ -29,8 +29,6 @@ #include #include #include -#include -#include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index fadda66f12562..9dbed9009a93e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -16,21 +16,22 @@ #define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ #include +#include #include #include +#include #include -#include #include #include +#include #include #include #include #include +#include #include -#include #include -#include #include #include @@ -54,14 +55,12 @@ using autoware::motion_utils::PlanningBehavior; using autoware::motion_utils::VelocityFactor; using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; -using autoware::rtc_interface::RTCInterface; using autoware::universe_utils::DebugPublisher; using autoware::universe_utils::getOrDeclareParameter; +using autoware::universe_utils::StopWatch; +using autoware_internal_debug_msgs::msg::Float64Stamped; using builtin_interfaces::msg::Time; -using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::PathWithLaneId; -using tier4_rtc_msgs::msg::Module; -using tier4_rtc_msgs::msg::State; using unique_identifier_msgs::msg::UUID; struct ObjectOfInterest @@ -114,12 +113,6 @@ class SceneModuleInterface std::shared_ptr getTimeKeeper() { return time_keeper_; } - void setActivation(const bool activated) { activated_ = activated; } - void setRTCEnabled(const bool enable_rtc) { rtc_enabled_ = enable_rtc; } - bool isActivated() const { return activated_; } - bool isSafe() const { return safe_; } - double getDistance() const { return distance_; } - void resetVelocityFactor() { velocity_factor_.reset(); } VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); } std::vector getObjectsOfInterestData() const { return objects_of_interest_; } @@ -127,10 +120,6 @@ class SceneModuleInterface protected: const int64_t module_id_; - bool activated_; - bool safe_; - bool rtc_enabled_; - double distance_; rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; @@ -139,16 +128,6 @@ class SceneModuleInterface std::vector objects_of_interest_; mutable std::shared_ptr time_keeper_; - void setSafe(const bool safe) - { - safe_ = safe; - if (!rtc_enabled_) { - syncActivation(); - } - } - void setDistance(const double distance) { distance_ = distance; } - void syncActivation() { setActivation(isSafe()); } - void setObjectsOfInterestData( const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, const ColorName & color_name) @@ -160,10 +139,39 @@ class SceneModuleInterface const std::vector & points) const; }; +template class SceneModuleManagerInterface { public: - SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name); + SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name) + : node_(node), clock_(node.get_clock()), logger_(node.get_logger()) + { + const auto ns = std::string("~/debug/") + module_name; + pub_debug_ = node.create_publisher(ns, 1); + if (!node.has_parameter("is_publish_debug_path")) { + is_publish_debug_path_ = node.declare_parameter("is_publish_debug_path"); + } else { + is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool(); + } + if (is_publish_debug_path_) { + pub_debug_path_ = node.create_publisher( + std::string("~/debug/path_with_lane_id/") + module_name, 1); + } + pub_virtual_wall_ = node.create_publisher( + std::string("~/virtual_wall/") + module_name, 5); + pub_velocity_factor_ = node.create_publisher( + std::string("/planning/velocity_factors/") + module_name, 1); + pub_infrastructure_commands_ = + node.create_publisher( + "~/output/infrastructure_commands", 1); + + processing_time_publisher_ = std::make_shared(&node, "~/debug"); + + pub_processing_time_detail_ = node.create_publisher( + "~/debug/processing_time_detail_ms/" + std::string(module_name), 1); + + time_keeper_ = std::make_shared(pub_processing_time_detail_); + } virtual ~SceneModuleManagerInterface() = default; @@ -171,31 +179,111 @@ class SceneModuleManagerInterface void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & path); + const tier4_planning_msgs::msg::PathWithLaneId & path) + { + planner_data_ = planner_data; + + launchNewModules(path); + deleteExpiredModules(path); + } virtual void plan(tier4_planning_msgs::msg::PathWithLaneId * path) { modifyPathVelocity(path); } protected: - virtual void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path); + virtual void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) + { + universe_utils::ScopedTimeTrack st( + "SceneModuleManagerInterface::modifyPathVelocity", *time_keeper_); + StopWatch stop_watch; + stop_watch.tic("Total"); + visualization_msgs::msg::MarkerArray debug_marker_array; + autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; + velocity_factor_array.header.frame_id = "map"; + velocity_factor_array.header.stamp = clock_->now(); + + tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; + infrastructure_command_array.stamp = clock_->now(); + + for (const auto & scene_module : scene_modules_) { + scene_module->resetVelocityFactor(); + scene_module->setPlannerData(planner_data_); + scene_module->modifyPathVelocity(path); + + // The velocity factor must be called after modifyPathVelocity. + const auto velocity_factor = scene_module->getVelocityFactor(); + if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { + velocity_factor_array.factors.emplace_back(velocity_factor); + } + + if (const auto command = scene_module->getInfrastructureCommand()) { + infrastructure_command_array.commands.push_back(*command); + } + + for (const auto & marker : scene_module->createDebugMarkerArray().markers) { + debug_marker_array.markers.push_back(marker); + } + + virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); + } + + pub_velocity_factor_->publish(velocity_factor_array); + pub_infrastructure_commands_->publish(infrastructure_command_array); + pub_debug_->publish(debug_marker_array); + if (is_publish_debug_path_) { + tier4_planning_msgs::msg::PathWithLaneId debug_path; + debug_path.header = path->header; + debug_path.points = path->points; + pub_debug_path_->publish(debug_path); + } + pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); + processing_time_publisher_->publish( + std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total")); + } virtual void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; - virtual std::function &)> - getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; + virtual std::function &)> getModuleExpiredFunction( + const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; - virtual void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path); + virtual void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) + { + const auto isModuleExpired = getModuleExpiredFunction(path); + + auto itr = scene_modules_.begin(); + while (itr != scene_modules_.end()) { + if (isModuleExpired(*itr)) { + itr = scene_modules_.erase(itr); + } else { + itr++; + } + } + } bool isModuleRegistered(const int64_t module_id) { return registered_module_id_set_.count(module_id) != 0; } - void registerModule(const std::shared_ptr & scene_module); + void registerModule(const std::shared_ptr & scene_module) + { + RCLCPP_DEBUG( + logger_, "register task: module = %s, id = %lu", getModuleName(), + scene_module->getModuleId()); + registered_module_id_set_.emplace(scene_module->getModuleId()); + scene_module->setTimeKeeper(time_keeper_); + scene_modules_.insert(scene_module); + } size_t findEgoSegmentIndex( - const std::vector & points) const; + const std::vector & points) const + { + const auto & p = planner_data_; + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, p->current_odometry->pose, p->ego_nearest_dist_threshold, + p->ego_nearest_yaw_threshold); + } - std::set> scene_modules_; + std::set> scene_modules_; std::set registered_module_id_set_; std::shared_ptr planner_data_; @@ -220,66 +308,19 @@ class SceneModuleManagerInterface std::shared_ptr time_keeper_; }; - -class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface -{ -public: - SceneModuleManagerInterfaceWithRTC( - rclcpp::Node & node, const char * module_name, const bool enable_rtc = true); - - void plan(tier4_planning_msgs::msg::PathWithLaneId * path) override; - -protected: - RTCInterface rtc_interface_; - std::unordered_map map_uuid_; - - ObjectsOfInterestMarkerInterface objects_of_interest_marker_interface_; - - virtual void sendRTC(const Time & stamp); - - virtual void setActivation(); - - void updateRTCStatus( - const UUID & uuid, const bool safe, const uint8_t state, const double distance, - const Time & stamp) - { - rtc_interface_.updateCooperateStatus(uuid, safe, state, distance, distance, stamp); - } - - void removeRTCStatus(const UUID & uuid) { rtc_interface_.removeCooperateStatus(uuid); } - - void publishRTCStatus(const Time & stamp) - { - rtc_interface_.removeExpiredCooperateStatus(); - rtc_interface_.publishCooperateStatus(stamp); - } - - UUID getUUID(const int64_t & module_id) const; - - void generateUUID(const int64_t & module_id); - - void removeUUID(const int64_t & module_id); - - void publishObjectsOfInterestMarker(); - - void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - - static bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) - { - bool enable_rtc = true; - - try { - enable_rtc = getOrDeclareParameter(node, "enable_all_modules_auto_mode") - ? false - : getOrDeclareParameter(node, param_name); - } catch (const std::exception & e) { - enable_rtc = getOrDeclareParameter(node, param_name); - } - - return enable_rtc; - } -}; - +extern template SceneModuleManagerInterface::SceneModuleManagerInterface( + rclcpp::Node & node, [[maybe_unused]] const char * module_name); +extern template size_t SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +extern template void SceneModuleManagerInterface::updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const tier4_planning_msgs::msg::PathWithLaneId & path); +extern template void SceneModuleManagerInterface::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path); +extern template void SceneModuleManagerInterface::deleteExpiredModules( + const tier4_planning_msgs::msg::PathWithLaneId & path); +extern template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); } // namespace autoware::behavior_velocity_planner #endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index 77b26aac09161..002c3362260cc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -20,6 +20,7 @@ eigen3_cmake_module autoware_adapi_v1_msgs + autoware_internal_debug_msgs autoware_interpolation autoware_lanelet2_extension autoware_map_msgs @@ -28,7 +29,6 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler - autoware_rtc_interface autoware_universe_utils autoware_vehicle_info_utils autoware_velocity_smoother @@ -44,7 +44,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_api_msgs tier4_planning_msgs tier4_v2x_msgs visualization_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index cdfde1ce51205..ffd454012d13e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -13,29 +13,20 @@ // limitations under the License. #include -#include #include -#include -#include #include #include #include #include -#include #include namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::StopWatch; - SceneModuleInterface::SceneModuleInterface( const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) : module_id_(module_id), - activated_(false), - safe_(false), - distance_(std::numeric_limits::lowest()), logger_(logger), clock_(clock), time_keeper_(std::shared_ptr()) @@ -50,222 +41,17 @@ size_t SceneModuleInterface::findEgoSegmentIndex( points, p->current_odometry->pose, p->ego_nearest_dist_threshold); } -SceneModuleManagerInterface::SceneModuleManagerInterface( - rclcpp::Node & node, [[maybe_unused]] const char * module_name) -: node_(node), clock_(node.get_clock()), logger_(node.get_logger()) -{ - const auto ns = std::string("~/debug/") + module_name; - pub_debug_ = node.create_publisher(ns, 1); - if (!node.has_parameter("is_publish_debug_path")) { - is_publish_debug_path_ = node.declare_parameter("is_publish_debug_path"); - } else { - is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool(); - } - if (is_publish_debug_path_) { - pub_debug_path_ = node.create_publisher( - std::string("~/debug/path_with_lane_id/") + module_name, 1); - } - pub_virtual_wall_ = node.create_publisher( - std::string("~/virtual_wall/") + module_name, 5); - pub_velocity_factor_ = node.create_publisher( - std::string("/planning/velocity_factors/") + module_name, 1); - pub_infrastructure_commands_ = - node.create_publisher( - "~/output/infrastructure_commands", 1); - - processing_time_publisher_ = std::make_shared(&node, "~/debug"); - - pub_processing_time_detail_ = node.create_publisher( - "~/debug/processing_time_detail_ms/" + std::string(module_name), 1); - - time_keeper_ = std::make_shared(pub_processing_time_detail_); -} - -size_t SceneModuleManagerInterface::findEgoSegmentIndex( - const std::vector & points) const -{ - const auto & p = planner_data_; - return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - points, p->current_odometry->pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); -} - -void SceneModuleManagerInterface::updateSceneModuleInstances( +template SceneModuleManagerInterface::SceneModuleManagerInterface( + rclcpp::Node & node, [[maybe_unused]] const char * module_name); +template size_t SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +template void SceneModuleManagerInterface::updateSceneModuleInstances( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - planner_data_ = planner_data; - - launchNewModules(path); - deleteExpiredModules(path); -} - -void SceneModuleManagerInterface::modifyPathVelocity( - tier4_planning_msgs::msg::PathWithLaneId * path) -{ - universe_utils::ScopedTimeTrack st( - "SceneModuleManagerInterface::modifyPathVelocity", *time_keeper_); - StopWatch stop_watch; - stop_watch.tic("Total"); - visualization_msgs::msg::MarkerArray debug_marker_array; - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); - - tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; - infrastructure_command_array.stamp = clock_->now(); - - for (const auto & scene_module : scene_modules_) { - scene_module->resetVelocityFactor(); - scene_module->setPlannerData(planner_data_); - scene_module->modifyPathVelocity(path); - - // The velocity factor must be called after modifyPathVelocity. - const auto velocity_factor = scene_module->getVelocityFactor(); - if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } - - if (const auto command = scene_module->getInfrastructureCommand()) { - infrastructure_command_array.commands.push_back(*command); - } - - for (const auto & marker : scene_module->createDebugMarkerArray().markers) { - debug_marker_array.markers.push_back(marker); - } - - virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); - } - - pub_velocity_factor_->publish(velocity_factor_array); - pub_infrastructure_commands_->publish(infrastructure_command_array); - pub_debug_->publish(debug_marker_array); - if (is_publish_debug_path_) { - tier4_planning_msgs::msg::PathWithLaneId debug_path; - debug_path.header = path->header; - debug_path.points = path->points; - pub_debug_path_->publish(debug_path); - } - pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); - processing_time_publisher_->publish( - std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total")); -} - -void SceneModuleManagerInterface::deleteExpiredModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - const auto isModuleExpired = getModuleExpiredFunction(path); - - auto itr = scene_modules_.begin(); - while (itr != scene_modules_.end()) { - if (isModuleExpired(*itr)) { - itr = scene_modules_.erase(itr); - } else { - itr++; - } - } -} - -void SceneModuleManagerInterface::registerModule( - const std::shared_ptr & scene_module) -{ - RCLCPP_DEBUG( - logger_, "register task: module = %s, id = %lu", getModuleName(), scene_module->getModuleId()); - registered_module_id_set_.emplace(scene_module->getModuleId()); - scene_module->setTimeKeeper(time_keeper_); - scene_modules_.insert(scene_module); -} - -SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( - rclcpp::Node & node, const char * module_name, const bool enable_rtc) -: SceneModuleManagerInterface(node, module_name), - rtc_interface_(&node, module_name, enable_rtc), - objects_of_interest_marker_interface_(&node, module_name) -{ -} - -void SceneModuleManagerInterfaceWithRTC::plan(tier4_planning_msgs::msg::PathWithLaneId * path) -{ - setActivation(); - modifyPathVelocity(path); - sendRTC(path->header.stamp); - publishObjectsOfInterestMarker(); -} - -void SceneModuleManagerInterfaceWithRTC::sendRTC(const Time & stamp) -{ - for (const auto & scene_module : scene_modules_) { - const UUID uuid = getUUID(scene_module->getModuleId()); - const auto state = !scene_module->isActivated() && scene_module->isSafe() - ? State::WAITING_FOR_EXECUTION - : State::RUNNING; - updateRTCStatus(uuid, scene_module->isSafe(), state, scene_module->getDistance(), stamp); - } - publishRTCStatus(stamp); -} - -void SceneModuleManagerInterfaceWithRTC::setActivation() -{ - for (const auto & scene_module : scene_modules_) { - const UUID uuid = getUUID(scene_module->getModuleId()); - scene_module->setActivation(rtc_interface_.isActivated(uuid)); - scene_module->setRTCEnabled(rtc_interface_.isRTCEnabled(uuid)); - } -} - -UUID SceneModuleManagerInterfaceWithRTC::getUUID(const int64_t & module_id) const -{ - if (map_uuid_.count(module_id) == 0) { - const UUID uuid; - return uuid; - } - return map_uuid_.at(module_id); -} - -void SceneModuleManagerInterfaceWithRTC::generateUUID(const int64_t & module_id) -{ - map_uuid_.insert({module_id, autoware::universe_utils::generateUUID()}); -} - -void SceneModuleManagerInterfaceWithRTC::removeUUID(const int64_t & module_id) -{ - const auto result = map_uuid_.erase(module_id); - if (result == 0) { - RCLCPP_WARN_STREAM(logger_, "[removeUUID] module_id = " << module_id << " is not registered."); - } -} - -void SceneModuleManagerInterfaceWithRTC::publishObjectsOfInterestMarker() -{ - for (const auto & scene_module : scene_modules_) { - const auto objects = scene_module->getObjectsOfInterestData(); - for (const auto & obj : objects) { - objects_of_interest_marker_interface_.insertObjectData(obj.pose, obj.shape, obj.color); - } - scene_module->clearObjectsOfInterestData(); - } - - objects_of_interest_marker_interface_.publishMarkerArray(); -} - -void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - const auto isModuleExpired = getModuleExpiredFunction(path); - - auto itr = scene_modules_.begin(); - while (itr != scene_modules_.end()) { - if (isModuleExpired(*itr)) { - const UUID uuid = getUUID((*itr)->getModuleId()); - updateRTCStatus( - uuid, (*itr)->isSafe(), State::SUCCEEDED, std::numeric_limits::lowest(), - clock_->now()); - removeUUID((*itr)->getModuleId()); - registered_module_id_set_.erase((*itr)->getModuleId()); - itr = scene_modules_.erase(itr); - } else { - itr++; - } - } -} - + const tier4_planning_msgs::msg::PathWithLaneId & path); +template void SceneModuleManagerInterface::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path); +template void SceneModuleManagerInterface::deleteExpiredModules( + const tier4_planning_msgs::msg::PathWithLaneId & path); +template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/CMakeLists.txt new file mode 100644 index 0000000000000..2bd9b9b8d20f0 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_velocity_rtc_interface) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene_module_interface_with_rtc.cpp +) + +ament_auto_package(INSTALL_TO_SHARE) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/README.md new file mode 100644 index 0000000000000..79b5a4e3d7b95 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/README.md @@ -0,0 +1,3 @@ +# Behavior Velocity RTC Interface + +This package provides a behavior velocity interface with RTC, which are used in the `behavior_velocity_planner` node and modules. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp new file mode 100644 index 0000000000000..4e30ab019aa4e --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp @@ -0,0 +1,154 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_RTC_INTERFACE__SCENE_MODULE_INTERFACE_WITH_RTC_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_RTC_INTERFACE__SCENE_MODULE_INTERFACE_WITH_RTC_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +// Debug +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +using autoware::rtc_interface::RTCInterface; +using autoware::universe_utils::getOrDeclareParameter; +using builtin_interfaces::msg::Time; +using tier4_planning_msgs::msg::PathWithLaneId; +using tier4_rtc_msgs::msg::Module; +using tier4_rtc_msgs::msg::State; +using unique_identifier_msgs::msg::UUID; + +class SceneModuleInterfaceWithRTC : public SceneModuleInterface +{ +public: + explicit SceneModuleInterfaceWithRTC( + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); + virtual ~SceneModuleInterfaceWithRTC() = default; + + void setActivation(const bool activated) { activated_ = activated; } + void setRTCEnabled(const bool enable_rtc) { rtc_enabled_ = enable_rtc; } + bool isActivated() const { return activated_; } + bool isSafe() const { return safe_; } + double getDistance() const { return distance_; } + +protected: + bool activated_; + bool safe_; + bool rtc_enabled_; + double distance_; + + void setSafe(const bool safe) + { + safe_ = safe; + if (!rtc_enabled_) { + syncActivation(); + } + } + void setDistance(const double distance) { distance_ = distance; } + void syncActivation() { setActivation(isSafe()); } +}; + +class SceneModuleManagerInterfaceWithRTC +: public SceneModuleManagerInterface +{ +public: + SceneModuleManagerInterfaceWithRTC( + rclcpp::Node & node, const char * module_name, const bool enable_rtc = true); + + void plan(tier4_planning_msgs::msg::PathWithLaneId * path) override; + +protected: + RTCInterface rtc_interface_; + std::unordered_map map_uuid_; + + ObjectsOfInterestMarkerInterface objects_of_interest_marker_interface_; + + virtual void sendRTC(const Time & stamp); + + virtual void setActivation(); + + void updateRTCStatus( + const UUID & uuid, const bool safe, const uint8_t state, const double distance, + const Time & stamp) + { + rtc_interface_.updateCooperateStatus(uuid, safe, state, distance, distance, stamp); + } + + void removeRTCStatus(const UUID & uuid) { rtc_interface_.removeCooperateStatus(uuid); } + + void publishRTCStatus(const Time & stamp) + { + rtc_interface_.removeExpiredCooperateStatus(); + rtc_interface_.publishCooperateStatus(stamp); + } + + UUID getUUID(const int64_t & module_id) const; + + void generateUUID(const int64_t & module_id); + + void removeUUID(const int64_t & module_id); + + void publishObjectsOfInterestMarker(); + + void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + + static bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) + { + bool enable_rtc = true; + + try { + enable_rtc = getOrDeclareParameter(node, "enable_all_modules_auto_mode") + ? false + : getOrDeclareParameter(node, param_name); + } catch (const std::exception & e) { + enable_rtc = getOrDeclareParameter(node, param_name); + } + + return enable_rtc; + } +}; + +extern template size_t +SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +extern template void +SceneModuleManagerInterface::updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const tier4_planning_msgs::msg::PathWithLaneId & path); +extern template void SceneModuleManagerInterface::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path); +extern template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_RTC_INTERFACE__SCENE_MODULE_INTERFACE_WITH_RTC_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml new file mode 100644 index 0000000000000..88b252106a90a --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml @@ -0,0 +1,38 @@ + + + + autoware_behavior_velocity_rtc_interface + 0.40.0 + The autoware_behavior_velocity_rtc_interface package + + Mamoru Sobue + Takayuki Murooka + Tomoya Kimura + Shumpei Wakabayashi + Takagi, Isamu + Fumiya Watanabe + + Apache License 2.0 + + Takayuki Murooka + + ament_cmake_auto + autoware_cmake + + autoware_behavior_velocity_planner_common + autoware_motion_utils + autoware_planning_msgs + autoware_rtc_interface + autoware_universe_utils + rclcpp + rclcpp_components + tier4_planning_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp new file mode 100644 index 0000000000000..abac509fd2b2b --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp @@ -0,0 +1,140 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +SceneModuleInterfaceWithRTC::SceneModuleInterfaceWithRTC( + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), + activated_(false), + safe_(false), + distance_(std::numeric_limits::lowest()) +{ +} + +SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( + rclcpp::Node & node, const char * module_name, const bool enable_rtc) +: SceneModuleManagerInterface(node, module_name), + rtc_interface_(&node, module_name, enable_rtc), + objects_of_interest_marker_interface_(&node, module_name) +{ +} + +void SceneModuleManagerInterfaceWithRTC::plan(tier4_planning_msgs::msg::PathWithLaneId * path) +{ + setActivation(); + modifyPathVelocity(path); + sendRTC(path->header.stamp); + publishObjectsOfInterestMarker(); +} + +void SceneModuleManagerInterfaceWithRTC::sendRTC(const Time & stamp) +{ + for (const auto & scene_module : scene_modules_) { + const UUID uuid = getUUID(scene_module->getModuleId()); + const auto state = !scene_module->isActivated() && scene_module->isSafe() + ? State::WAITING_FOR_EXECUTION + : State::RUNNING; + updateRTCStatus(uuid, scene_module->isSafe(), state, scene_module->getDistance(), stamp); + } + publishRTCStatus(stamp); +} + +void SceneModuleManagerInterfaceWithRTC::setActivation() +{ + for (const auto & scene_module : scene_modules_) { + const UUID uuid = getUUID(scene_module->getModuleId()); + scene_module->setActivation(rtc_interface_.isActivated(uuid)); + scene_module->setRTCEnabled(rtc_interface_.isRTCEnabled(uuid)); + } +} + +UUID SceneModuleManagerInterfaceWithRTC::getUUID(const int64_t & module_id) const +{ + if (map_uuid_.count(module_id) == 0) { + const UUID uuid; + return uuid; + } + return map_uuid_.at(module_id); +} + +void SceneModuleManagerInterfaceWithRTC::generateUUID(const int64_t & module_id) +{ + map_uuid_.insert({module_id, autoware::universe_utils::generateUUID()}); +} + +void SceneModuleManagerInterfaceWithRTC::removeUUID(const int64_t & module_id) +{ + const auto result = map_uuid_.erase(module_id); + if (result == 0) { + RCLCPP_WARN_STREAM(logger_, "[removeUUID] module_id = " << module_id << " is not registered."); + } +} + +void SceneModuleManagerInterfaceWithRTC::publishObjectsOfInterestMarker() +{ + for (const auto & scene_module : scene_modules_) { + const auto objects = scene_module->getObjectsOfInterestData(); + for (const auto & obj : objects) { + objects_of_interest_marker_interface_.insertObjectData(obj.pose, obj.shape, obj.color); + } + scene_module->clearObjectsOfInterestData(); + } + + objects_of_interest_marker_interface_.publishMarkerArray(); +} + +void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto isModuleExpired = getModuleExpiredFunction(path); + + auto itr = scene_modules_.begin(); + while (itr != scene_modules_.end()) { + if (isModuleExpired(*itr)) { + const UUID uuid = getUUID((*itr)->getModuleId()); + updateRTCStatus( + uuid, (*itr)->isSafe(), State::SUCCEEDED, std::numeric_limits::lowest(), + clock_->now()); + removeUUID((*itr)->getModuleId()); + registered_module_id_set_.erase((*itr)->getModuleId()); + itr = scene_modules_.erase(itr); + } else { + itr++; + } + } +} + +template size_t SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +template void SceneModuleManagerInterface::updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const tier4_planning_msgs::msg::PathWithLaneId & path); +template void SceneModuleManagerInterface::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path); +template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml index 90e1d32198854..b3ced8b2e9b9f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml @@ -21,6 +21,7 @@ autoware_behavior_velocity_crosswalk_module autoware_behavior_velocity_planner_common + autoware_internal_debug_msgs autoware_motion_utils autoware_object_recognition_utils autoware_perception_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp index 2823648e184d3..3f4b9f330e2d0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp @@ -316,7 +316,7 @@ void RunOutDebug::setAccelReason(const AccelReason & accel_reason) void RunOutDebug::publishDebugValue() { // publish debug values - tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; + autoware_internal_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; debug_msg.stamp = node_.now(); for (const auto & v : debug_values_.getValues()) { debug_msg.data.push_back(v); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp index 3e913f57f30c0..1ffa826c4d1d1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp @@ -18,17 +18,17 @@ #include -#include -#include +#include +#include #include #include #include namespace autoware::behavior_velocity_planner { +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Int32Stamped; using sensor_msgs::msg::PointCloud2; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; -using tier4_debug_msgs::msg::Int32Stamped; class DebugValues { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp index 131ba32f32100..068ed81015fb1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp @@ -25,7 +25,7 @@ namespace autoware::behavior_velocity_planner { -class RunOutModuleManager : public SceneModuleManagerInterface +class RunOutModuleManager : public SceneModuleManagerInterface<> { public: explicit RunOutModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp index a32a65a0d8909..efc2d458ae2a7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -811,7 +811,7 @@ void RunOutModule::insertVelocityForState( // insert velocity for each state switch (state) { - case State::GO: { + case State::GO: { // NOLINT insertStoppingVelocity(target_obstacle, current_pose, current_vel, current_acc, output_path); break; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp index 3673a215bb749..83123c71f461e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp @@ -32,10 +32,10 @@ namespace autoware::behavior_velocity_planner { +using autoware_internal_debug_msgs::msg::Float32Stamped; using autoware_perception_msgs::msg::PredictedObjects; using run_out_utils::PlannerParam; using run_out_utils::PoseWithRange; -using tier4_debug_msgs::msg::Float32Stamped; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; using BasicPolygons2d = std::vector; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp index 1f948cc7faaa1..c48e4f867fac0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp @@ -21,10 +21,10 @@ #include #include +#include #include #include #include -#include #include #include @@ -39,11 +39,11 @@ using autoware::universe_utils::LineString2d; using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; +using autoware_internal_debug_msgs::msg::Float32Stamped; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; using autoware_planning_msgs::msg::PathPoint; -using tier4_debug_msgs::msg::Float32Stamped; using tier4_planning_msgs::msg::PathWithLaneId; using PathPointsWithLaneId = std::vector; struct CommonParam diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp index 950bb8471cc22..f98db8b88b7a9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp @@ -29,7 +29,7 @@ namespace autoware::behavior_velocity_planner { -class SpeedBumpModuleManager : public SceneModuleManagerInterface +class SpeedBumpModuleManager : public SceneModuleManagerInterface<> { public: explicit SpeedBumpModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp index bef8a5eef4ac0..c746e2bf6a314 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp @@ -33,7 +33,7 @@ namespace autoware::behavior_velocity_planner { using StopLineWithLaneId = std::pair; -class StopLineModuleManager : public SceneModuleManagerInterface +class StopLineModuleManager : public SceneModuleManagerInterface<> { public: explicit StopLineModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp index a7a0d83be6368..c5b9293fcdcc5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp @@ -38,7 +38,7 @@ namespace autoware::behavior_velocity_planner * @param node A reference to the ROS node. */ class TemplateModuleManager -: public autoware::behavior_velocity_planner::SceneModuleManagerInterface +: public autoware::behavior_velocity_planner::SceneModuleManagerInterface<> { public: explicit TemplateModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml index 082973d9431e5..ebc45d372f92d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml @@ -20,6 +20,7 @@ autoware_adapi_v1_msgs autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index b6747724ba6f7..1b66824d04623 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp @@ -123,7 +123,7 @@ void TrafficLightModuleManager::launchNewModules( } } -std::function &)> +std::function &)> TrafficLightModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { @@ -131,7 +131,7 @@ TrafficLightModuleManager::getModuleExpiredFunction( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); return [this, lanelet_id_set]( - [[maybe_unused]] const std::shared_ptr & scene_module) { + [[maybe_unused]] const std::shared_ptr & scene_module) { for (const auto & id : lanelet_id_set) { if (isModuleRegisteredFromExistingAssociatedModule(id)) { return false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp index eb8ef53ddb604..5ac32d1107880 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -42,8 +42,8 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index 5eb0d5aa5f267..458d8e1588a5b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -44,7 +44,7 @@ TrafficLightModule::TrafficLightModule( const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(lane_id, logger, clock), +: SceneModuleInterfaceWithRTC(lane_id, logger, clock), lane_id_(lane_id), traffic_light_reg_elem_(traffic_light_reg_elem), lane_(lane), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp index 8221bb3740273..3af13bc1927ce 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp @@ -23,8 +23,8 @@ #define EIGEN_MPL2_ONLY #include #include -#include #include +#include #include #include @@ -33,7 +33,7 @@ namespace autoware::behavior_velocity_planner { -class TrafficLightModule : public SceneModuleInterface +class TrafficLightModule : public SceneModuleInterfaceWithRTC { public: using TrafficSignal = autoware_perception_msgs::msg::TrafficLightGroup; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp index ba5a4b23a6fe0..8e0abc98c90de 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp @@ -29,7 +29,7 @@ namespace autoware::behavior_velocity_planner { -class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface +class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface<> { public: explicit VirtualTrafficLightModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp index 85d4495914823..b323d6d201795 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp @@ -35,7 +35,7 @@ namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathWithLaneId; -class WalkwayModuleManager : public SceneModuleManagerInterface +class WalkwayModuleManager : public SceneModuleManagerInterface<> { public: explicit WalkwayModuleManager(rclcpp::Node & node); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml index eef7b6af1f9af..1063abfb61bbb 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml @@ -24,7 +24,6 @@ pluginlib rclcpp tf2 - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp index 12cb59ac46195..40aa985ee42e2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp @@ -51,8 +51,9 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo node.create_publisher("~/" + ns_ + "/virtual_walls", 1); processing_diag_publisher_ = std::make_shared( &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); - processing_time_publisher_ = node.create_publisher( - "~/debug/" + ns_ + "/processing_time_ms", 1); + processing_time_publisher_ = + node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); using autoware::universe_utils::getOrDeclareParameter; auto & p = params_; @@ -190,7 +191,7 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( processing_times["collisions"] = collisions_duration_us / 1000; processing_times["Total"] = total_time_us / 1000; processing_diag_publisher_->publish(processing_times); - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = clock_->now(); processing_time_msg.data = processing_times["Total"]; processing_time_publisher_->publish(processing_time_msg); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp index 79b817c51169c..1d980e220ccef 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp @@ -16,6 +16,7 @@ #include "../src/types.hpp" #include +#include #include #include @@ -55,45 +56,53 @@ polygon_t random_polygon() int main() { - Obstacles obstacles; - std::vector polygons; - polygons.reserve(100); - for (auto i = 0; i < 100; ++i) { - polygons.push_back(random_polygon()); - } - for (auto nb_lines = 1lu; nb_lines < 1000lu; nb_lines += 10) { - obstacles.lines.push_back(random_line()); - obstacles.points.clear(); - for (auto nb_points = 1lu; nb_points < 1000lu; nb_points += 10) { - obstacles.points.push_back(random_point()); - const auto rtt_constr_start = std::chrono::system_clock::now(); - CollisionChecker rtree_collision_checker(obstacles, 0, 0); - const auto rtt_constr_end = std::chrono::system_clock::now(); - const auto naive_constr_start = std::chrono::system_clock::now(); - CollisionChecker naive_collision_checker(obstacles, nb_points + 1, nb_lines * 100); - const auto naive_constr_end = std::chrono::system_clock::now(); - const auto rtt_check_start = std::chrono::system_clock::now(); - for (const auto & polygon : polygons) - // cppcheck-suppress unreadVariable - const auto rtree_result = rtree_collision_checker.intersections(polygon); - const auto rtt_check_end = std::chrono::system_clock::now(); - const auto naive_check_start = std::chrono::system_clock::now(); - for (const auto & polygon : polygons) - // cppcheck-suppress unreadVariable - const auto naive_result = naive_collision_checker.intersections(polygon); - const auto naive_check_end = std::chrono::system_clock::now(); - const auto rtt_constr_time = - std::chrono::duration_cast(rtt_constr_end - rtt_constr_start); - const auto naive_constr_time = - std::chrono::duration_cast(naive_constr_end - naive_constr_start); - const auto rtt_check_time = - std::chrono::duration_cast(rtt_check_end - rtt_check_start); - const auto naive_check_time = - std::chrono::duration_cast(naive_check_end - naive_check_start); - std::printf( - "%lu, %lu, %ld, %ld, %ld, %ld\n", nb_lines, nb_points, rtt_constr_time.count(), - rtt_check_time.count(), naive_constr_time.count(), naive_check_time.count()); + try { + Obstacles obstacles; + std::vector polygons; + polygons.reserve(100); + for (auto i = 0; i < 100; ++i) { + polygons.push_back(random_polygon()); + } + for (auto nb_lines = 1lu; nb_lines < 1000lu; nb_lines += 10) { + obstacles.lines.push_back(random_line()); + obstacles.points.clear(); + for (auto nb_points = 1lu; nb_points < 1000lu; nb_points += 10) { + obstacles.points.push_back(random_point()); + const auto rtt_constr_start = std::chrono::system_clock::now(); + CollisionChecker rtree_collision_checker(obstacles, 0, 0); + const auto rtt_constr_end = std::chrono::system_clock::now(); + const auto naive_constr_start = std::chrono::system_clock::now(); + CollisionChecker naive_collision_checker(obstacles, nb_points + 1, nb_lines * 100); + const auto naive_constr_end = std::chrono::system_clock::now(); + const auto rtt_check_start = std::chrono::system_clock::now(); + for (const auto & polygon : polygons) + // cppcheck-suppress unreadVariable + const auto rtree_result = rtree_collision_checker.intersections(polygon); + const auto rtt_check_end = std::chrono::system_clock::now(); + const auto naive_check_start = std::chrono::system_clock::now(); + for (const auto & polygon : polygons) + // cppcheck-suppress unreadVariable + const auto naive_result = naive_collision_checker.intersections(polygon); + const auto naive_check_end = std::chrono::system_clock::now(); + const auto rtt_constr_time = + std::chrono::duration_cast(rtt_constr_end - rtt_constr_start); + const auto naive_constr_time = std::chrono::duration_cast( + naive_constr_end - naive_constr_start); + const auto rtt_check_time = + std::chrono::duration_cast(rtt_check_end - rtt_check_start); + const auto naive_check_time = + std::chrono::duration_cast(naive_check_end - naive_check_start); + std::printf( + "%lu, %lu, %ld, %ld, %ld, %ld\n", nb_lines, nb_points, rtt_constr_time.count(), + rtt_check_time.count(), naive_constr_time.count(), naive_check_time.count()); + } } + } catch (const std::exception & e) { + std::cerr << "Exception in main(): " << e.what() << std::endl; + return {}; + } catch (...) { + std::cerr << "Unknown exception in main()" << std::endl; + return {}; } return 0; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml index 68e2ead5a5ec7..e608e158b3ffe 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml @@ -33,7 +33,6 @@ sensor_msgs tf2_eigen tf2_geometry_msgs - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index 3fa3ec7cbf782..532cc834c19ac 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -56,8 +56,9 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string node.create_publisher("~/" + ns_ + "/virtual_walls", 1); processing_diag_publisher_ = std::make_shared( &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); - processing_time_publisher_ = node.create_publisher( - "~/debug/" + ns_ + "/processing_time_ms", 1); + processing_time_publisher_ = + node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); const auto vehicle_info = vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); vehicle_lateral_offset_ = static_cast(vehicle_info.max_lateral_offset_m); @@ -236,7 +237,7 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( processing_times["slowdowns"] = slowdowns_us / 1000; processing_times["Total"] = total_us / 1000; processing_diag_publisher_->publish(processing_times); - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = clock_->now(); processing_time_msg.data = processing_times["Total"]; processing_time_publisher_->publish(processing_time_msg); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml index 857716819d8cc..f0f663355821c 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml @@ -29,7 +29,6 @@ pluginlib rclcpp tf2 - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index 63ca1b5784fe8..bd85546cb6c56 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -66,8 +66,9 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) node.create_publisher("~/" + ns_ + "/virtual_walls", 1); processing_diag_publisher_ = std::make_shared( &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); - processing_time_publisher_ = node.create_publisher( - "~/debug/" + ns_ + "/processing_time_ms", 1); + processing_time_publisher_ = + node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); } void OutOfLaneModule::init_parameters(rclcpp::Node & node) { @@ -349,7 +350,7 @@ VelocityPlanningResult OutOfLaneModule::plan( processing_times["publish_markers"] = pub_markers_us / 1000; processing_times["Total"] = total_time_us / 1000; processing_diag_publisher_->publish(processing_times); - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = clock_->now(); processing_time_msg.data = processing_times["Total"]; processing_time_publisher_->publish(processing_time_msg); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp index c39c6e17101c5..72f20f1c63d96 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp @@ -24,8 +24,6 @@ #include #include -#include -#include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp index a96587c4465d6..9ef3bf5c7f756 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp @@ -31,10 +31,6 @@ #include #include #include -#include -#include -#include -#include #include #include @@ -75,8 +71,6 @@ struct PlannerData // last observed infomation for UNKNOWN std::map traffic_light_id_map_raw_; std::map traffic_light_id_map_last_observed_; - std::optional external_velocity_limit; - tier4_v2x_msgs::msg::VirtualTrafficLightStateArray virtual_traffic_light_states; // velocity smoother std::shared_ptr velocity_smoother_; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp index 9bd662af697ea..53860c390b4db 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp @@ -22,8 +22,8 @@ #include #include +#include #include -#include #include #include @@ -47,7 +47,8 @@ class PluginModuleInterface rclcpp::Publisher::SharedPtr debug_publisher_; rclcpp::Publisher::SharedPtr virtual_wall_publisher_; std::shared_ptr processing_diag_publisher_; - rclcpp::Publisher::SharedPtr processing_time_publisher_; + rclcpp::Publisher::SharedPtr + processing_time_publisher_; autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{}; }; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml index 3f0c027a5c986..2fc682e695f6b 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -17,6 +17,7 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_internal_debug_msgs autoware_motion_utils autoware_perception_msgs autoware_planning_msgs @@ -27,8 +28,6 @@ geometry_msgs libboost-dev rclcpp - tier4_debug_msgs - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md index 494446828e134..01062f81e77a2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md @@ -17,17 +17,16 @@ This means that to stop before a wall, a stop point is inserted in the trajector ## Input topics -| Name | Type | Description | -| -------------------------------------- | ----------------------------------------------------- | ----------------------------- | -| `~/input/trajectory` | autoware_planning_msgs::msg::Trajectory | input trajectory | -| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | -| `~/input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle position and velocity | -| `~/input/accel` | geometry_msgs::msg::AccelWithCovarianceStamped | vehicle acceleration | -| `~/input/dynamic_objects` | autoware_perception_msgs::msg::PredictedObjects | dynamic objects | -| `~/input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | -| `~/input/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | traffic light states | -| `~/input/virtual_traffic_light_states` | tier4_v2x_msgs::msg::VirtualTrafficLightStateArray | virtual traffic light states | -| `~/input/occupancy_grid` | nav_msgs::msg::OccupancyGrid | occupancy grid | +| Name | Type | Description | +| ------------------------------ | ----------------------------------------------------- | ----------------------------- | +| `~/input/trajectory` | autoware_planning_msgs::msg::Trajectory | input trajectory | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle position and velocity | +| `~/input/accel` | geometry_msgs::msg::AccelWithCovarianceStamped | vehicle acceleration | +| `~/input/dynamic_objects` | autoware_perception_msgs::msg::PredictedObjects | dynamic objects | +| `~/input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | +| `~/input/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | traffic light states | +| `~/input/occupancy_grid` | nav_msgs::msg::OccupancyGrid | occupancy grid | ## Output topics diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml index 3732d86ef380a..963394323caeb 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml @@ -24,7 +24,6 @@ - diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml index 186140cba6e3c..07b719689f51f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml @@ -18,6 +18,7 @@ rosidl_default_generators + autoware_internal_debug_msgs autoware_map_msgs autoware_motion_utils autoware_motion_velocity_planner_common @@ -37,9 +38,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_debug_msgs tier4_metric_msgs - tier4_planning_msgs visualization_msgs rosidl_default_runtime diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 96865fec1c197..cefc84836beda 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -86,7 +86,8 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & this->create_publisher( "~/output/velocity_factors", 1); processing_time_publisher_ = - this->create_publisher("~/debug/processing_time_ms", 1); + this->create_publisher( + "~/debug/processing_time_ms", 1); debug_viz_pub_ = this->create_publisher("~/debug/markers", 1); metrics_pub_ = this->create_publisher("~/metrics", 1); @@ -185,9 +186,6 @@ bool MotionVelocityPlannerNode::update_planner_data( // optional data const auto traffic_signals_ptr = sub_traffic_signals_.takeData(); if (traffic_signals_ptr) process_traffic_signals(traffic_signals_ptr); - const auto virtual_traffic_light_states_ptr = sub_virtual_traffic_light_states_.takeData(); - if (virtual_traffic_light_states_ptr) - planner_data_.virtual_traffic_light_states = *virtual_traffic_light_states_ptr; processing_times["update_planner_data.traffic_lights"] = sw.toc(true); return is_ready; @@ -302,7 +300,7 @@ void MotionVelocityPlannerNode::on_trajectory( trajectory_pub_, output_trajectory_msg.header.stamp); processing_times["Total"] = stop_watch.toc("Total"); processing_diag_publisher_.publish(processing_times); - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = processing_times["Total"]; processing_time_publisher_->publish(processing_time_msg); @@ -359,7 +357,6 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s const geometry_msgs::msg::Pose current_pose = planner_data.current_odometry.pose.pose; const double v0 = planner_data.current_odometry.twist.twist.linear.x; const double a0 = planner_data.current_acceleration.accel.accel.linear.x; - const auto & external_v_limit = planner_data.external_velocity_limit; const auto & smoother = planner_data.velocity_smoother_; const auto traj_lateral_acc_filtered = @@ -384,10 +381,6 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) { RCLCPP_ERROR(get_logger(), "failed to smooth"); } - if (external_v_limit) { - autoware::velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( - 0LU, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); - } return traj_smoothed; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index 757be518e018a..18d41f5d7fe5d 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -32,7 +33,6 @@ #include #include #include -#include #include #include @@ -80,9 +80,6 @@ class MotionVelocityPlannerNode : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber< autoware_perception_msgs::msg::TrafficLightGroupArray> sub_traffic_signals_{this, "~/input/traffic_signals"}; - autoware::universe_utils::InterProcessPollingSubscriber< - tier4_v2x_msgs::msg::VirtualTrafficLightStateArray> - sub_virtual_traffic_light_states_{this, "~/input/virtual_traffic_light_states"}; rclcpp::Subscription::SharedPtr sub_lanelet_map_; void on_trajectory( @@ -100,7 +97,8 @@ class MotionVelocityPlannerNode : public rclcpp::Node velocity_factor_publisher_; autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{ this, "~/debug/processing_time_ms_diag"}; - rclcpp::Publisher::SharedPtr processing_time_publisher_; + rclcpp::Publisher::SharedPtr + processing_time_publisher_; autoware::universe_utils::PublishedTimePublisher published_time_publisher_{this}; rclcpp::Publisher::SharedPtr metrics_pub_; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp index 44ff7820c1566..89a732ead861b 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp @@ -101,8 +101,6 @@ void publishMandatoryTopics( test_manager->publishMap(test_target_node, "motion_velocity_planner_node/input/vector_map"); test_manager->publishTrafficSignals( test_target_node, "motion_velocity_planner_node/input/traffic_signals"); - test_manager->publishVirtualTrafficLightState( - test_target_node, "motion_velocity_planner_node/input/virtual_traffic_light_states"); test_manager->publishOccupancyGrid( test_target_node, "motion_velocity_planner_node/input/occupancy_grid"); } diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/type_alias.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/type_alias.hpp index 95d833c4df47d..7398e28d18b28 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/type_alias.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/type_alias.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE_PATH_SAMPLER__TYPE_ALIAS_HPP_ #define AUTOWARE_PATH_SAMPLER__TYPE_ALIAS_HPP_ +#include "autoware_internal_debug_msgs/msg/string_stamped.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/path.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" @@ -25,7 +26,6 @@ #include "geometry_msgs/msg/twist.hpp" #include "nav_msgs/msg/odometry.hpp" #include "std_msgs/msg/header.hpp" -#include "tier4_debug_msgs/msg/string_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" namespace autoware::path_sampler @@ -45,7 +45,7 @@ using nav_msgs::msg::Odometry; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; // debug -using tier4_debug_msgs::msg::StringStamped; +using autoware_internal_debug_msgs::msg::StringStamped; } // namespace autoware::path_sampler #endif // AUTOWARE_PATH_SAMPLER__TYPE_ALIAS_HPP_ diff --git a/planning/sampling_based_planner/autoware_path_sampler/package.xml b/planning/sampling_based_planner/autoware_path_sampler/package.xml index 142eafdfa4bfe..a4119b03a2dd7 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/package.xml +++ b/planning/sampling_based_planner/autoware_path_sampler/package.xml @@ -15,6 +15,7 @@ autoware_bezier_sampler autoware_frenet_planner + autoware_internal_debug_msgs autoware_interpolation autoware_motion_utils autoware_perception_msgs diff --git a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp index 681e9ae70a89c..1625cba5b72aa 100644 --- a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp +++ b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp @@ -171,11 +171,9 @@ void ImageDiagNode::ImageChecker(const sensor_msgs::msg::Image::ConstSharedPtr i cv::rectangle( diag_block_image, cv::Point(x, y), cv::Point(x + block_size_h, y + block_size_v), state_color_map_["BLOCKAGE"], -1, cv::LINE_AA); - } else if (region_state_vec[j] == Image_State::LOW_VIS) { - cv::rectangle( - diag_block_image, cv::Point(x, y), cv::Point(x + block_size_h, y + block_size_v), - state_color_map_["BACKLIGHT"], -1, cv::LINE_AA); - } else if (region_state_vec[j] == Image_State::BACKLIGHT) { + } else if ( + region_state_vec[j] == Image_State::LOW_VIS || + region_state_vec[j] == Image_State::BACKLIGHT) { cv::rectangle( diag_block_image, cv::Point(x, y), cv::Point(x + block_size_h, y + block_size_v), state_color_map_["BACKLIGHT"], -1, cv::LINE_AA); diff --git a/sensing/autoware_image_transport_decompressor/src/image_transport_decompressor.cpp b/sensing/autoware_image_transport_decompressor/src/image_transport_decompressor.cpp index b1971f892a352..045d1ea564fe6 100644 --- a/sensing/autoware_image_transport_decompressor/src/image_transport_decompressor.cpp +++ b/sensing/autoware_image_transport_decompressor/src/image_transport_decompressor.cpp @@ -107,13 +107,12 @@ void ImageTransportDecompressor::onCompressedImage( } } else { std::string image_encoding; - if (encoding_ == std::string("default")) { - image_encoding = input_compressed_image_msg->format.substr(0, split_pos); - } else if (encoding_ == std::string("rgb8")) { + if (encoding_ == std::string("rgb8")) { image_encoding = "rgb8"; } else if (encoding_ == std::string("bgr8")) { image_encoding = "bgr8"; } else { + // default encoding image_encoding = input_compressed_image_msg->format.substr(0, split_pos); } diff --git a/sensing/autoware_pointcloud_preprocessor/docs/blockage_diag.md b/sensing/autoware_pointcloud_preprocessor/docs/blockage-diag.md similarity index 100% rename from sensing/autoware_pointcloud_preprocessor/docs/blockage_diag.md rename to sensing/autoware_pointcloud_preprocessor/docs/blockage-diag.md diff --git a/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md index 75cdccc4453ba..032798e9db125 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md @@ -55,16 +55,9 @@ ros2 launch autoware_pointcloud_preprocessor distortion_corrector.launch.xml - `hesai`: (x: 90 degrees, y: 0 degrees) - `others`: (x: 0 degrees, y: 90 degrees) and (x: 270 degrees, y: 0 degrees) - - - - - - - - - -
velodyne azimuth coordinatehesai azimuth coordinate

Velodyne azimuth coordinate

Hesai azimuth coordinate

+| ![Velodyne Azimuth Coordinate](./image/velodyne.drawio.png) | ![Hesai Azimuth Coordinate](./image/hesai.drawio.png) | +| :---------------------------------------------------------: | :---------------------------------------------------: | +| **Velodyne azimuth coordinate** | **Hesai azimuth coordinate** | ## References/External links diff --git a/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md index 92ca1d3b3081c..f237a1871b5e5 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md @@ -48,7 +48,7 @@ These implementations inherit `autoware::pointcloud_preprocessor::Filter` class, ### Pickup Based Voxel Grid Downsample Filter -{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/pickup_based_voxel_grid_downsample_filter.schema.json") }} +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/pickup_based_voxel_grid_downsample_filter_node.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/docs/passthrough-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/passthrough-filter.md index 58da60c8fe90e..25d2f3d4127a0 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/passthrough-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/passthrough-filter.md @@ -25,8 +25,7 @@ The `passthrough_filter` is a node that removes points on the outside of a range ### Core Parameters -{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/passthrough_filter_uint16_node.schema.json -") }} +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/passthrough_filter_uint16_node.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/schema/blockage_diag_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/blockage_diag_node.schema.json index 0e4a02d37bd16..cdca58a6a73f0 100644 --- a/sensing/autoware_pointcloud_preprocessor/schema/blockage_diag_node.schema.json +++ b/sensing/autoware_pointcloud_preprocessor/schema/blockage_diag_node.schema.json @@ -99,7 +99,8 @@ "type": "number", "minimum": 0.0, "maximum": 360.0 - } + }, + "default": [0.0, 360.0] }, "vertical_bins": { "type": "integer", diff --git a/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json index 091695716c610..c59541d1d2fba 100644 --- a/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json +++ b/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json @@ -29,7 +29,7 @@ "has_static_tf_only": { "type": "boolean", "description": "Flag to indicate if only static TF is used.", - "default": false + "default": "false" } }, "required": [ diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp index 10e2fa2511478..3d2bc0a206c94 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp @@ -124,15 +124,7 @@ void PickupBasedVoxelGridDownsampleFilterComponent::filter( size_t output_global_offset = 0; output.data.resize(voxel_map.size() * input->point_step); for (const auto & kv : voxel_map) { - std::memcpy( - &output.data[output_global_offset + x_offset], &input->data[kv.second + x_offset], - sizeof(float)); - std::memcpy( - &output.data[output_global_offset + y_offset], &input->data[kv.second + y_offset], - sizeof(float)); - std::memcpy( - &output.data[output_global_offset + z_offset], &input->data[kv.second + z_offset], - sizeof(float)); + std::memcpy(&output.data[output_global_offset], &input->data[kv.second], input->point_step); output_global_offset += input->point_step; } diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h index 55307a2aa5552..0748516665e40 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h @@ -2189,12 +2189,6 @@ class Table return 0 == mNumElements; } - ROBIN_HOOD(NODISCARD) size_t mask() const noexcept - { - ROBIN_HOOD_TRACE(this) - return mMask; - } - ROBIN_HOOD(NODISCARD) size_t calcMaxNumElementsAllowed(size_t maxElements) const noexcept { if (ROBIN_HOOD_LIKELY(maxElements <= (std::numeric_limits::max)() / 100)) { diff --git a/system/autoware_default_adapi/config/default_adapi.param.yaml b/system/autoware_default_adapi/config/default_adapi.param.yaml index ddbf103878953..f2782e313e785 100644 --- a/system/autoware_default_adapi/config/default_adapi.param.yaml +++ b/system/autoware_default_adapi/config/default_adapi.param.yaml @@ -6,3 +6,7 @@ ros__parameters: require_accept_start: false stop_check_duration: 1.0 + +/adapi/node/vehicle_door: + ros__parameters: + check_autoware_control: true diff --git a/system/autoware_default_adapi/launch/test_default_adapi.launch.xml b/system/autoware_default_adapi/launch/test_default_adapi.launch.xml new file mode 100644 index 0000000000000..3a00beeb623d4 --- /dev/null +++ b/system/autoware_default_adapi/launch/test_default_adapi.launch.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/system/autoware_default_adapi/src/interface.cpp b/system/autoware_default_adapi/src/interface.cpp index fb924f06f42ea..6535dcfca30fd 100644 --- a/system/autoware_default_adapi/src/interface.cpp +++ b/system/autoware_default_adapi/src/interface.cpp @@ -21,7 +21,7 @@ InterfaceNode::InterfaceNode(const rclcpp::NodeOptions & options) : Node("interf { const auto on_interface_version = [](auto, auto res) { res->major = 1; - res->minor = 5; + res->minor = 6; res->patch = 0; }; diff --git a/system/autoware_default_adapi/src/routing.cpp b/system/autoware_default_adapi/src/routing.cpp index 4eaa98376b147..93f31ff693344 100644 --- a/system/autoware_default_adapi/src/routing.cpp +++ b/system/autoware_default_adapi/src/routing.cpp @@ -68,6 +68,7 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing", adaptor.init_cli(cli_operation_mode_, group_cli_); adaptor.init_sub(sub_operation_mode_, this, &RoutingNode::on_operation_mode); + is_autoware_control_ = false; is_auto_mode_ = false; state_.state = State::Message::UNKNOWN; } @@ -85,6 +86,7 @@ void RoutingNode::change_stop_mode() void RoutingNode::on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg) { + is_autoware_control_ = msg->is_autoware_control_enabled; is_auto_mode_ = msg->mode == OperationModeState::Message::AUTONOMOUS; } @@ -119,7 +121,14 @@ void RoutingNode::on_clear_route( const autoware::adapi_specs::routing::ClearRoute::Service::Request::SharedPtr req, const autoware::adapi_specs::routing::ClearRoute::Service::Response::SharedPtr res) { - change_stop_mode(); + // For safety, do not clear the route while it is in use. + // https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/routing/clear_route/ + if (is_auto_mode_ && is_autoware_control_) { + res->status.success = false; + res->status.code = ResponseStatus::UNKNOWN; + res->status.message = "The route cannot be cleared while it is in use."; + return; + } res->status = conversion::convert_call(cli_clear_route_, req); } diff --git a/system/autoware_default_adapi/src/routing.hpp b/system/autoware_default_adapi/src/routing.hpp index 10eed606a5f6b..f37b8a978a235 100644 --- a/system/autoware_default_adapi/src/routing.hpp +++ b/system/autoware_default_adapi/src/routing.hpp @@ -52,6 +52,7 @@ class RoutingNode : public rclcpp::Node Cli cli_clear_route_; Cli cli_operation_mode_; Sub sub_operation_mode_; + bool is_autoware_control_; bool is_auto_mode_; State::Message state_; diff --git a/system/autoware_default_adapi/src/utils/route_conversion.cpp b/system/autoware_default_adapi/src/utils/route_conversion.cpp index e8a748d52edee..25e8204770f8a 100644 --- a/system/autoware_default_adapi/src/utils/route_conversion.cpp +++ b/system/autoware_default_adapi/src/utils/route_conversion.cpp @@ -115,13 +115,13 @@ ExternalState convert_state(const InternalState & internal) const auto convert = [](InternalState::_state_type state) { switch(state) { // TODO(Takagi, Isamu): Add adapi state. - case InternalState::INITIALIZING: return ExternalState::UNSET; + case InternalState::INITIALIZING: return ExternalState::UNSET; // NOLINT case InternalState::UNSET: return ExternalState::UNSET; case InternalState::ROUTING: return ExternalState::UNSET; case InternalState::SET: return ExternalState::SET; case InternalState::REROUTING: return ExternalState::CHANGING; case InternalState::ARRIVED: return ExternalState::ARRIVED; - case InternalState::ABORTED: return ExternalState::SET; + case InternalState::ABORTED: return ExternalState::SET; // NOLINT case InternalState::INTERRUPTED: return ExternalState::SET; default: return ExternalState::UNKNOWN; } diff --git a/system/autoware_default_adapi/src/vehicle_door.cpp b/system/autoware_default_adapi/src/vehicle_door.cpp index 23588bdf4597a..d3ad5d8ddccee 100644 --- a/system/autoware_default_adapi/src/vehicle_door.cpp +++ b/system/autoware_default_adapi/src/vehicle_door.cpp @@ -24,18 +24,50 @@ VehicleDoorNode::VehicleDoorNode(const rclcpp::NodeOptions & options) { const auto adaptor = autoware::component_interface_utils::NodeAdaptor(this); group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - adaptor.relay_service(cli_command_, srv_command_, group_cli_, 3.0); - adaptor.relay_service(cli_layout_, srv_layout_, group_cli_, 3.0); + adaptor.relay_service(cli_layout_, srv_layout_, group_cli_); + adaptor.init_cli(cli_command_, group_cli_); + adaptor.init_srv(srv_command_, this, &VehicleDoorNode::on_command); adaptor.init_pub(pub_status_); adaptor.init_sub(sub_status_, this, &VehicleDoorNode::on_status); + adaptor.init_sub(sub_operation_mode_, this, &VehicleDoorNode::on_operation_mode); + + check_autoware_control_ = declare_parameter("check_autoware_control"); + is_autoware_control_ = false; + is_stop_mode_ = false; +} + +void VehicleDoorNode::on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg) +{ + is_autoware_control_ = msg->is_autoware_control_enabled; + is_stop_mode_ = msg->mode == OperationModeState::Message::STOP; +} +void VehicleDoorNode::on_status(InternalDoorStatus::Message::ConstSharedPtr msg) +{ + utils::notify(pub_status_, status_, *msg, utils::ignore_stamp); } -void VehicleDoorNode::on_status( - autoware::component_interface_specs::vehicle::DoorStatus::Message::ConstSharedPtr msg) +void VehicleDoorNode::on_command( + const ExternalDoorCommand::Service::Request::SharedPtr req, + const ExternalDoorCommand::Service::Response::SharedPtr res) { - utils::notify( - pub_status_, status_, *msg, - utils::ignore_stamp); + // For safety, do not open the door if the vehicle is not stopped. + // https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/vehicle/doors/command/ + if (!is_stop_mode_ || (!is_autoware_control_ && check_autoware_control_)) { + bool is_open = false; + for (const auto & door : req->doors) { + if (door.command == autoware_adapi_v1_msgs::msg::DoorCommand::OPEN) { + is_open = true; + break; + } + } + if (is_open) { + res->status.success = false; + res->status.code = autoware_adapi_v1_msgs::msg::ResponseStatus::UNKNOWN; + res->status.message = "Doors cannot be opened if the vehicle is not stopped."; + return; + } + } + autoware::component_interface_utils::status::copy(cli_command_->call(req), res); } } // namespace autoware::default_adapi diff --git a/system/autoware_default_adapi/src/vehicle_door.hpp b/system/autoware_default_adapi/src/vehicle_door.hpp index 50312a38a4cb7..a75232a2b2c74 100644 --- a/system/autoware_default_adapi/src/vehicle_door.hpp +++ b/system/autoware_default_adapi/src/vehicle_door.hpp @@ -16,7 +16,9 @@ #define VEHICLE_DOOR_HPP_ #include +#include #include +#include #include #include @@ -33,16 +35,33 @@ class VehicleDoorNode : public rclcpp::Node explicit VehicleDoorNode(const rclcpp::NodeOptions & options); private: - void on_status( - autoware::component_interface_specs::vehicle::DoorStatus::Message::ConstSharedPtr msg); + using OperationModeState = autoware::component_interface_specs::system::OperationModeState; + using InternalDoorStatus = autoware::component_interface_specs::vehicle::DoorStatus; + using InternalDoorLayout = autoware::component_interface_specs::vehicle::DoorLayout; + using InternalDoorCommand = autoware::component_interface_specs::vehicle::DoorCommand; + using ExternalDoorStatus = autoware::adapi_specs::vehicle::DoorStatus; + using ExternalDoorLayout = autoware::adapi_specs::vehicle::DoorLayout; + using ExternalDoorCommand = autoware::adapi_specs::vehicle::DoorCommand; + + void on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg); + void on_status(InternalDoorStatus::Message::ConstSharedPtr msg); + void on_command( + const ExternalDoorCommand::Service::Request::SharedPtr req, + const ExternalDoorCommand::Service::Response::SharedPtr res); + rclcpp::CallbackGroup::SharedPtr group_cli_; - Srv srv_command_; - Srv srv_layout_; - Pub pub_status_; - Cli cli_command_; - Cli cli_layout_; - Sub sub_status_; - std::optional status_; + Srv srv_command_; + Srv srv_layout_; + Pub pub_status_; + Cli cli_command_; + Cli cli_layout_; + Sub sub_status_; + std::optional status_; + + bool check_autoware_control_; + bool is_autoware_control_; + bool is_stop_mode_; + Sub sub_operation_mode_; }; } // namespace autoware::default_adapi diff --git a/system/autoware_default_adapi/test/node/interface_version.py b/system/autoware_default_adapi/test/node/interface_version.py index c91889149cb0e..8db70ca5cba9a 100644 --- a/system/autoware_default_adapi/test/node/interface_version.py +++ b/system/autoware_default_adapi/test/node/interface_version.py @@ -30,7 +30,7 @@ if response.major != 1: exit(1) -if response.minor != 5: +if response.minor != 6: exit(1) if response.patch != 0: exit(1) diff --git a/system/autoware_processing_time_checker/src/processing_time_checker.cpp b/system/autoware_processing_time_checker/src/processing_time_checker.cpp index 1e3f04af8fa89..5455cdab422d5 100644 --- a/system/autoware_processing_time_checker/src/processing_time_checker.cpp +++ b/system/autoware_processing_time_checker/src/processing_time_checker.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -103,45 +104,51 @@ ProcessingTimeChecker::~ProcessingTimeChecker() return; } - // generate json data - nlohmann::json j; - for (const auto & accumulator_iterator : processing_time_accumulator_map_) { - const auto module_name = accumulator_iterator.first; - const auto processing_time_accumulator = accumulator_iterator.second; - j[module_name + "/min"] = processing_time_accumulator.min(); - j[module_name + "/max"] = processing_time_accumulator.max(); - j[module_name + "/mean"] = processing_time_accumulator.mean(); - j[module_name + "/count"] = processing_time_accumulator.count(); - j[module_name + "/description"] = "processing time of " + module_name + "[ms]"; - } + try { + // generate json data + nlohmann::json j; + for (const auto & accumulator_iterator : processing_time_accumulator_map_) { + const auto module_name = accumulator_iterator.first; + const auto processing_time_accumulator = accumulator_iterator.second; + j[module_name + "/min"] = processing_time_accumulator.min(); + j[module_name + "/max"] = processing_time_accumulator.max(); + j[module_name + "/mean"] = processing_time_accumulator.mean(); + j[module_name + "/count"] = processing_time_accumulator.count(); + j[module_name + "/description"] = "processing time of " + module_name + "[ms]"; + } - // get output folder - const std::string output_folder_str = - rclcpp::get_logging_directory().string() + "/autoware_metrics"; - if (!std::filesystem::exists(output_folder_str)) { - if (!std::filesystem::create_directories(output_folder_str)) { - RCLCPP_ERROR( - this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); - return; + // get output folder + const std::string output_folder_str = + rclcpp::get_logging_directory().string() + "/autoware_metrics"; + if (!std::filesystem::exists(output_folder_str)) { + if (!std::filesystem::create_directories(output_folder_str)) { + RCLCPP_ERROR( + this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); + return; + } } - } - // get time stamp - std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); - std::tm * local_time = std::localtime(&now_time_t); - std::ostringstream oss; - oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); - std::string cur_time_str = oss.str(); - - // Write metrics .json to file - const std::string output_file_str = - output_folder_str + "/autoware_processing_time_checker-" + cur_time_str + ".json"; - std::ofstream f(output_file_str); - if (f.is_open()) { - f << j.dump(4); - f.close(); - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + // get time stamp + std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + std::tm * local_time = std::localtime(&now_time_t); + std::ostringstream oss; + oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); + std::string cur_time_str = oss.str(); + + // Write metrics .json to file + const std::string output_file_str = + output_folder_str + "/autoware_processing_time_checker-" + cur_time_str + ".json"; + std::ofstream f(output_file_str); + if (f.is_open()) { + f << j.dump(4); + f.close(); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + } + } catch (const std::exception & e) { + std::cerr << "Exception in ProcessingTimeChecker: " << e.what() << std::endl; + } catch (...) { + std::cerr << "Unknown exception in ProcessingTimeChecker" << std::endl; } } diff --git a/system/diagnostic_graph_aggregator/test/src/test2.cpp b/system/diagnostic_graph_aggregator/test/src/test2.cpp index 4a0199a89f37e..169e3017f8cd4 100644 --- a/system/diagnostic_graph_aggregator/test/src/test2.cpp +++ b/system/diagnostic_graph_aggregator/test/src/test2.cpp @@ -29,7 +29,6 @@ using namespace diagnostic_graph_aggregator; // NOLINT(build/namespaces) using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; -using tier4_system_msgs::msg::DiagnosticGraph; constexpr auto OK = DiagnosticStatus::OK; constexpr auto WARN = DiagnosticStatus::WARN; diff --git a/system/diagnostic_graph_utils/package.xml b/system/diagnostic_graph_utils/package.xml index 03cf1fa369774..b777ee530ecc9 100644 --- a/system/diagnostic_graph_utils/package.xml +++ b/system/diagnostic_graph_utils/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs diagnostic_msgs rclcpp rclcpp_components diff --git a/system/diagnostic_graph_utils/src/node/logging.cpp b/system/diagnostic_graph_utils/src/node/logging.cpp index 5d360a00aee82..2dfc939469172 100644 --- a/system/diagnostic_graph_utils/src/node/logging.cpp +++ b/system/diagnostic_graph_utils/src/node/logging.cpp @@ -34,7 +34,7 @@ LoggingNode::LoggingNode(const rclcpp::NodeOptions & options) : Node("logging", sub_graph_.register_create_callback(std::bind(&LoggingNode::on_create, this, _1)); sub_graph_.subscribe(*this, 1); - pub_error_graph_text_ = create_publisher( + pub_error_graph_text_ = create_publisher( "~/debug/error_graph_text", rclcpp::QoS(1)); const auto period = rclcpp::Rate(declare_parameter("show_rate")).period(); @@ -68,12 +68,12 @@ void LoggingNode::on_timer() RCLCPP_WARN_STREAM(get_logger(), prefix_message << std::endl << dump_text_.str()); } - tier4_debug_msgs::msg::StringStamped message; + autoware_internal_debug_msgs::msg::StringStamped message; message.stamp = now(); message.data = dump_text_.str(); pub_error_graph_text_->publish(message); } else { - tier4_debug_msgs::msg::StringStamped message; + autoware_internal_debug_msgs::msg::StringStamped message; message.stamp = now(); pub_error_graph_text_->publish(message); } diff --git a/system/diagnostic_graph_utils/src/node/logging.hpp b/system/diagnostic_graph_utils/src/node/logging.hpp index 17d33499a1513..81acfcd2ad82f 100644 --- a/system/diagnostic_graph_utils/src/node/logging.hpp +++ b/system/diagnostic_graph_utils/src/node/logging.hpp @@ -19,7 +19,7 @@ #include -#include "tier4_debug_msgs/msg/string_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/string_stamped.hpp" #include #include @@ -37,7 +37,8 @@ class LoggingNode : public rclcpp::Node void on_timer(); void dump_unit(DiagUnit * unit, int depth, const std::string & indent); DiagGraphSubscription sub_graph_; - rclcpp::Publisher::SharedPtr pub_error_graph_text_; + rclcpp::Publisher::SharedPtr + pub_error_graph_text_; rclcpp::TimerBase::SharedPtr timer_; DiagUnit * root_unit_; diff --git a/system/dummy_infrastructure/schema/dummy_infrastructure_node.schema.json b/system/dummy_infrastructure/schema/dummy_infrastructure.schema.json similarity index 100% rename from system/dummy_infrastructure/schema/dummy_infrastructure_node.schema.json rename to system/dummy_infrastructure/schema/dummy_infrastructure.schema.json diff --git a/system/system_monitor/README.md b/system/system_monitor/README.md index 6203ad45d4b3a..d00c57c895279 100644 --- a/system/system_monitor/README.md +++ b/system/system_monitor/README.md @@ -77,6 +77,7 @@ Every topic is published in 1 minute interval. | | Network Usage | ✓ | ✓ | ✓ | Notification of usage only, normally error not generated. | | | Network CRC Error | ✓ | ✓ | ✓ | Warning occurs when the number of CRC errors in the period reaches the threshold value. The number of CRC errors that occur is the same as the value that can be confirmed with the ip command. | | | IP Packet Reassembles Failed | ✓ | ✓ | ✓ | | +| | UDP Buf Errors | ✓ | ✓ | ✓ | | | NTP Monitor | NTP Offset | ✓ | ✓ | ✓ | | | Process Monitor | Tasks Summary | ✓ | ✓ | ✓ | | | | High-load Proc[0-9] | ✓ | ✓ | ✓ | | diff --git a/system/system_monitor/config/net_monitor.param.yaml b/system/system_monitor/config/net_monitor.param.yaml index 7a1e5aeff2db1..b366e26395641 100644 --- a/system/system_monitor/config/net_monitor.param.yaml +++ b/system/system_monitor/config/net_monitor.param.yaml @@ -7,3 +7,5 @@ crc_error_count_threshold: 1 reassembles_failed_check_duration: 1 reassembles_failed_check_count: 1 + udp_buf_errors_check_duration: 1 + udp_buf_errors_check_count: 1 diff --git a/system/system_monitor/docs/ros_parameters.md b/system/system_monitor/docs/ros_parameters.md index 1d081d513c85c..8ec952e60e242 100644 --- a/system/system_monitor/docs/ros_parameters.md +++ b/system/system_monitor/docs/ros_parameters.md @@ -69,6 +69,8 @@ net_monitor: | crc_error_count_threshold | int | n/a | 1 | Generates warning when count of CRC errors during CRC error check duration reaches a specified value or higher. | | reassembles_failed_check_duration | int | sec | 1 | IP packet reassembles failed check duration. | | reassembles_failed_check_count | int | n/a | 1 | Generates warning when count of IP packet reassembles failed during IP packet reassembles failed check duration reaches a specified value or higher. | +| udp_buf_errors_check_duration | int | sec | 1 | UDP buf errors check duration. | +| udp_buf_errors_check_count | int | n/a | 1 | Generates warning when count of UDP buf errors during udp_buf_errors_check_duration reaches a specified value or higher. | ## NTP Monitor diff --git a/system/system_monitor/docs/topics_net_monitor.md b/system/system_monitor/docs/topics_net_monitor.md index edb067c0a5be7..84afb250929b8 100644 --- a/system/system_monitor/docs/topics_net_monitor.md +++ b/system/system_monitor/docs/topics_net_monitor.md @@ -106,3 +106,23 @@ | --------------------------------------- | --------------- | | total packet reassembles failed | 0 | | packet reassembles failed per unit time | 0 | + +## UDP Buf Errors + +/diagnostics/net_monitor: UDP Buf Errors + +[summary] + +| level | message | +| ----- | -------------- | +| OK | OK | +| WARN | UDP buf errors | + +[values] + +| key | value (example) | +| -------------------------------- | --------------- | +| total UDP rcv buf errors | 0 | +| UDP rcv buf errors per unit time | 0 | +| total UDP snd buf errors | 0 | +| UDP snd buf errors per unit time | 0 | diff --git a/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp b/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp index 5aa2cc9790143..77536c4a60615 100644 --- a/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp +++ b/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp @@ -81,6 +81,93 @@ struct CrcErrors unsigned int last_rx_crc_errors{0}; //!< @brief rx_crc_error at the time of the last monitoring }; +/** + * @brief /proc/net/snmp information + */ +class NetSnmp +{ +public: + enum class Result { + OK, + CHECK_WARNING, + READ_ERROR, + }; + + /** + * @brief Constructor + * @param [in] node node using this class. + */ + explicit NetSnmp(rclcpp::Node * node); + + /** + * @brief Constructor + */ + NetSnmp() = delete; + + /** + * @brief Copy constructor + */ + NetSnmp(const NetSnmp &) = delete; + + /** + * @brief Copy assignment operator + */ + NetSnmp & operator=(const NetSnmp &) = delete; + + /** + * @brief Move constructor + */ + NetSnmp(const NetSnmp &&) = delete; + + /** + * @brief Move assignment operator + */ + NetSnmp & operator=(const NetSnmp &&) = delete; + + /** + * @brief Set parameters for check + * @param [in] check_duration the value for check_duration + * @param [in] check_count the value for check_count + */ + void set_check_parameters(unsigned int check_duration, unsigned int check_count); + + /** + * @brief Find index in `/proc/net/snmp` + * @param [in] protocol Protocol name (the first column string). e.g. "Ip:" or "Udp:" + * @param [in] metrics Metrics name. e.g. "ReasmFails" + */ + void find_index(const std::string & protocol, const std::string & metrics); + + /** + * @brief Check metrics + * @param [out] current_value the value read from snmp + * @param [out] value_per_unit_time the increase of the value during the duration + * @return the result of check + */ + Result check_metrics(uint64_t & current_value, uint64_t & value_per_unit_time); + +private: + /** + * @brief Read value from `/proc/net/snmp` + * @param [in] index_row row in `/proc/net/snmp` + * @param [in] index_col col in `/proc/net/snmp` + * @param [out] output_value retrieved value + * @return execution result + */ + bool read_value_from_proc( + unsigned int index_row, unsigned int index_col, uint64_t & output_value); + + rclcpp::Logger logger_; //!< @brief logger gotten from user node + unsigned int check_duration_; //!< @brief check duration + unsigned int check_count_; //!< @brief check count threshold + unsigned int index_row_; //!< @brief index for the target metrics in /proc/net/snmp + unsigned int index_col_; //!< @brief index for the target metrics in /proc/net/snmp + uint64_t current_value_; //!< @brief the value read from snmp + uint64_t last_value_; //!< @brief the value read from snmp at the last monitoring + uint64_t value_per_unit_time_; //!< @brief the increase of the value during the duration + std::deque queue_; //!< @brief queue that holds the delta of the value +}; + namespace local = boost::asio::local; class NetMonitor : public rclcpp::Node @@ -150,6 +237,12 @@ class NetMonitor : public rclcpp::Node */ void check_reassembles_failed(diagnostic_updater::DiagnosticStatusWrapper & status); + /** + * @brief Check UDP buf errors + * @param [out] status diagnostic message passed directly to diagnostic publish calls + */ + void check_udp_buf_errors(diagnostic_updater::DiagnosticStatusWrapper & status); + /** * @brief Timer callback */ @@ -273,18 +366,6 @@ class NetMonitor : public rclcpp::Node */ void close_connection(); - /** - * @brief Get column index of IP packet reassembles failed from `/proc/net/snmp` - */ - void get_reassembles_failed_column_index(); - - /** - * @brief get IP packet reassembles failed - * @param [out] reassembles_failed IP packet reassembles failed - * @return execution result - */ - bool get_reassembles_failed(uint64_t & reassembles_failed); - diagnostic_updater::Updater updater_; //!< @brief Updater class which advertises to /diagnostics rclcpp::TimerBase::SharedPtr timer_; //!< @brief timer to get Network information @@ -307,16 +388,9 @@ class NetMonitor : public rclcpp::Node unsigned int crc_error_check_duration_; //!< @brief CRC error check duration unsigned int crc_error_count_threshold_; //!< @brief CRC error count threshold - std::deque - reassembles_failed_queue_; //!< @brief queue that holds count of IP packet reassembles failed - uint64_t last_reassembles_failed_; //!< @brief IP packet reassembles failed at the time of the - //!< last monitoring - unsigned int - reassembles_failed_check_duration_; //!< @brief IP packet reassembles failed check duration - unsigned int - reassembles_failed_check_count_; //!< @brief IP packet reassembles failed check count threshold - unsigned int reassembles_failed_column_index_; //!< @brief column index of IP Reassembles failed - //!< in /proc/net/snmp + NetSnmp reassembles_failed_info_; //!< @brief information of IP packet reassembles failed + NetSnmp udp_rcvbuf_errors_info_; //!< @brief information of UDP rcv buf errors + NetSnmp udp_sndbuf_errors_info_; //!< @brief information of UDP snd buf errors /** * @brief Network connection status messages diff --git a/system/system_monitor/src/net_monitor/net_monitor.cpp b/system/system_monitor/src/net_monitor/net_monitor.cpp index d99150f0b3037..fd00d7f7b895b 100644 --- a/system/system_monitor/src/net_monitor/net_monitor.cpp +++ b/system/system_monitor/src/net_monitor/net_monitor.cpp @@ -28,6 +28,7 @@ #include #include +#include #include #include @@ -51,11 +52,9 @@ NetMonitor::NetMonitor(const rclcpp::NodeOptions & options) socket_path_(declare_parameter("socket_path", traffic_reader_service::socket_path)), crc_error_check_duration_(declare_parameter("crc_error_check_duration", 1)), crc_error_count_threshold_(declare_parameter("crc_error_count_threshold", 1)), - last_reassembles_failed_(0), - reassembles_failed_check_duration_( - declare_parameter("reassembles_failed_check_duration", 1)), - reassembles_failed_check_count_(declare_parameter("reassembles_failed_check_count", 1)), - reassembles_failed_column_index_(0) + reassembles_failed_info_(this), + udp_rcvbuf_errors_info_(this), + udp_sndbuf_errors_info_(this) { if (monitor_program_.empty()) { monitor_program_ = "*"; @@ -68,6 +67,7 @@ NetMonitor::NetMonitor(const rclcpp::NodeOptions & options) updater_.add("Network Traffic", this, &NetMonitor::monitor_traffic); updater_.add("Network CRC Error", this, &NetMonitor::check_crc_error); updater_.add("IP Packet Reassembles Failed", this, &NetMonitor::check_reassembles_failed); + updater_.add("UDP Buf Errors", this, &NetMonitor::check_udp_buf_errors); nl80211_.init(); @@ -83,8 +83,21 @@ NetMonitor::NetMonitor(const rclcpp::NodeOptions & options) using namespace std::literals::chrono_literals; timer_ = rclcpp::create_timer(this, get_clock(), 1s, std::bind(&NetMonitor::on_timer, this)); - // Get column index of IP packet reassembles failed from `/proc/net/snmp` - get_reassembles_failed_column_index(); + // Initialize information for `/proc/net/snmp` + int reassembles_failed_check_duration = + declare_parameter("reassembles_failed_check_duration", 1); + int reassembles_failed_check_count = declare_parameter("reassembles_failed_check_count", 1); + int udp_buf_errors_check_duration = declare_parameter("udp_buf_errors_check_duration", 1); + int udp_buf_errors_check_count = declare_parameter("udp_buf_errors_check_count", 1); + reassembles_failed_info_.set_check_parameters( + reassembles_failed_check_duration, reassembles_failed_check_count); + udp_rcvbuf_errors_info_.set_check_parameters( + udp_buf_errors_check_duration, udp_buf_errors_check_count); + udp_sndbuf_errors_info_.set_check_parameters( + udp_buf_errors_check_duration, udp_buf_errors_check_count); + reassembles_failed_info_.find_index("Ip:", "ReasmFails"); + udp_rcvbuf_errors_info_.find_index("Udp:", "RcvbufErrors"); + udp_sndbuf_errors_info_.find_index("Udp:", "SndbufErrors"); // Send request to start nethogs if (enable_traffic_monitor_) { @@ -292,41 +305,66 @@ void NetMonitor::check_reassembles_failed(diagnostic_updater::DiagnosticStatusWr // Remember start time to measure elapsed time const auto t_start = SystemMonitorUtility::startMeasurement(); - int whole_level = DiagStatus::OK; - std::string error_message; uint64_t total_reassembles_failed = 0; + uint64_t unit_reassembles_failed = 0; + NetSnmp::Result ret = + reassembles_failed_info_.check_metrics(total_reassembles_failed, unit_reassembles_failed); + status.add("total packet reassembles failed", total_reassembles_failed); + status.add("packet reassembles failed per unit time", unit_reassembles_failed); - if (get_reassembles_failed(total_reassembles_failed)) { - reassembles_failed_queue_.push_back(total_reassembles_failed - last_reassembles_failed_); - while (reassembles_failed_queue_.size() > reassembles_failed_check_duration_) { - reassembles_failed_queue_.pop_front(); - } + int whole_level = DiagStatus::OK; + std::string error_message = "OK"; + switch (ret) { + case NetSnmp::Result::OK: + default: + break; + case NetSnmp::Result::CHECK_WARNING: + whole_level = DiagStatus::WARN; + error_message = "reassembles failed"; + break; + case NetSnmp::Result::READ_ERROR: + whole_level = DiagStatus::ERROR; + error_message = "failed to read /proc/net/snmp"; + break; + } - uint64_t unit_reassembles_failed = 0; - for (auto reassembles_failed : reassembles_failed_queue_) { - unit_reassembles_failed += reassembles_failed; - } + status.summary(whole_level, error_message); - status.add(fmt::format("total packet reassembles failed"), total_reassembles_failed); - status.add(fmt::format("packet reassembles failed per unit time"), unit_reassembles_failed); + // Measure elapsed time since start time and report + SystemMonitorUtility::stopMeasurement(t_start, status); +} - if (unit_reassembles_failed >= reassembles_failed_check_count_) { - whole_level = std::max(whole_level, static_cast(DiagStatus::WARN)); - error_message = "reassembles failed"; - } +void NetMonitor::check_udp_buf_errors(diagnostic_updater::DiagnosticStatusWrapper & status) +{ + // Remember start time to measure elapsed time + const auto t_start = SystemMonitorUtility::startMeasurement(); - last_reassembles_failed_ = total_reassembles_failed; - } else { - reassembles_failed_queue_.push_back(0); - whole_level = std::max(whole_level, static_cast(DiagStatus::ERROR)); + uint64_t total_udp_rcvbuf_errors = 0; + uint64_t unit_udp_rcvbuf_errors = 0; + NetSnmp::Result ret_rcv = + udp_rcvbuf_errors_info_.check_metrics(total_udp_rcvbuf_errors, unit_udp_rcvbuf_errors); + status.add("total UDP rcv buf errors", total_udp_rcvbuf_errors); + status.add("UDP rcv buf errors per unit time", unit_udp_rcvbuf_errors); + + uint64_t total_udp_sndbuf_errors = 0; + uint64_t unit_udp_sndbuf_errors = 0; + NetSnmp::Result ret_snd = + udp_sndbuf_errors_info_.check_metrics(total_udp_sndbuf_errors, unit_udp_sndbuf_errors); + status.add("total UDP snd buf errors", total_udp_sndbuf_errors); + status.add("UDP snd buf errors per unit time", unit_udp_sndbuf_errors); + + int whole_level = DiagStatus::OK; + std::string error_message = "OK"; + if (ret_rcv == NetSnmp::Result::READ_ERROR || ret_snd == NetSnmp::Result::READ_ERROR) { + whole_level = DiagStatus::ERROR; error_message = "failed to read /proc/net/snmp"; + } else if ( + ret_rcv == NetSnmp::Result::CHECK_WARNING || ret_snd == NetSnmp::Result::CHECK_WARNING) { + whole_level = DiagStatus::WARN; + error_message = "UDP buf errors"; } - if (!error_message.empty()) { - status.summary(whole_level, error_message); - } else { - status.summary(whole_level, "OK"); - } + status.summary(whole_level, error_message); // Measure elapsed time since start time and report SystemMonitorUtility::stopMeasurement(t_start, status); @@ -544,90 +582,6 @@ void NetMonitor::update_crc_error(NetworkInfomation & network, const struct rtnl crc_errors.last_rx_crc_errors = stats->rx_crc_errors; } -void NetMonitor::get_reassembles_failed_column_index() -{ - std::ifstream ifs("/proc/net/snmp"); - if (!ifs) { - RCLCPP_WARN(get_logger(), "Failed to open /proc/net/snmp."); - return; - } - - // Find column index of 'ReasmFails' - std::string line; - if (!std::getline(ifs, line)) { - RCLCPP_WARN(get_logger(), "Failed to get header of /proc/net/snmp."); - return; - } - - // /proc/net/snmp - // Ip: Forwarding DefaultTTL InReceives ... ReasmTimeout ReasmReqds ReasmOKs ReasmFails ... - // Ip: 2 64 5636471397 ... 135 2303339 216166 270 .. - std::vector header_list; - boost::split(header_list, line, boost::is_space()); - - if (header_list.empty()) { - RCLCPP_WARN(get_logger(), "Failed to get header list of /proc/net/snmp."); - return; - } - if (header_list[0] != "Ip:") { - RCLCPP_WARN( - get_logger(), "Header column is invalid in /proc/net/snmp. %s", header_list[0].c_str()); - return; - } - - int index = 0; - for (const auto & header : header_list) { - if (header == "ReasmFails") { - reassembles_failed_column_index_ = index; - break; - } - ++index; - } -} - -bool NetMonitor::get_reassembles_failed(uint64_t & reassembles_failed) -{ - if (reassembles_failed_column_index_ == 0) { - RCLCPP_WARN(get_logger(), "Column index is invalid. : %d", reassembles_failed_column_index_); - return false; - } - - std::ifstream ifs("/proc/net/snmp"); - if (!ifs) { - RCLCPP_WARN(get_logger(), "Failed to open /proc/net/snmp."); - return false; - } - - std::string line; - - // Skip header row - if (!std::getline(ifs, line)) { - RCLCPP_WARN(get_logger(), "Failed to get header of /proc/net/snmp."); - return false; - } - - // Find a value of 'ReasmFails' - if (!std::getline(ifs, line)) { - RCLCPP_WARN(get_logger(), "Failed to get a line of /proc/net/snmp."); - return false; - } - - std::vector value_list; - boost::split(value_list, line, boost::is_space()); - - if (reassembles_failed_column_index_ >= value_list.size()) { - RCLCPP_WARN( - get_logger(), - "There are not enough columns for reassembles failed column index. : columns=%d index=%d", - static_cast(value_list.size()), reassembles_failed_column_index_); - return false; - } - - reassembles_failed = std::stoull(value_list[reassembles_failed_column_index_]); - - return true; -} - void NetMonitor::send_start_nethogs_request() { // Connect to boot/shutdown service @@ -701,7 +655,7 @@ bool NetMonitor::connect_service() socket_->connect(endpoint, error_code); if (error_code) { - RCLCPP_ERROR(get_logger(), "Failed to connect socket. %s", error_code.message().c_str()); + RCLCPP_ERROR_ONCE(get_logger(), "Failed to connect socket. %s", error_code.message().c_str()); return false; } @@ -782,5 +736,145 @@ void NetMonitor::close_connection() socket_->close(); } +NetSnmp::NetSnmp(rclcpp::Node * node) +: logger_(node->get_logger().get_child("net_snmp")), + check_duration_(1), + check_count_(1), + index_row_(0), + index_col_(0), + current_value_(0), + last_value_(0), + value_per_unit_time_(0), + queue_() +{ +} + +void NetSnmp::set_check_parameters(unsigned int check_duration, unsigned int check_count) +{ + check_duration_ = check_duration; + check_count_ = check_count; +} + +void NetSnmp::find_index(const std::string & protocol, const std::string & metrics) +{ + // /proc/net/snmp + // Ip: Forwarding DefaultTTL InReceives ... ReasmTimeout ReasmReqds ReasmOKs ReasmFails ... + // Ip: 2 64 5636471397 ... 135 2303339 216166 270 .. + std::ifstream ifs("/proc/net/snmp"); + if (!ifs) { + RCLCPP_WARN(logger_, "Failed to open /proc/net/snmp."); + index_row_ = index_col_ = 0; + return; + } + + std::vector target_header_list; + std::string line; + while (std::getline(ifs, line)) { + std::vector header_list; + boost::split(header_list, line, boost::is_space()); + if (header_list.empty()) continue; + if (header_list[0] == protocol) { + target_header_list = header_list; + break; + } + ++index_row_; + } + + ++index_row_; // The values are placed in the row following the header + + for (const auto & header : target_header_list) { + if (header == metrics) { + return; + } + ++index_col_; + } + + RCLCPP_WARN(logger_, "Failed to get header of /proc/net/snmp."); + index_row_ = index_col_ = 0; + return; +} + +NetSnmp::Result NetSnmp::check_metrics(uint64_t & current_value, uint64_t & value_per_unit_time) +{ + if (!read_value_from_proc(index_row_, index_col_, current_value_)) { + queue_.push_back(0); + current_value = value_per_unit_time = 0; + return Result::READ_ERROR; + } + + if (queue_.empty()) { + last_value_ = current_value_; + } + queue_.push_back(current_value_ - last_value_); + last_value_ = current_value_; + while (queue_.size() > check_duration_) { + queue_.pop_front(); + } + + value_per_unit_time_ = std::accumulate(queue_.begin(), queue_.end(), static_cast(0)); + + current_value = current_value_; + value_per_unit_time = value_per_unit_time_; + + if (value_per_unit_time_ >= check_count_) { + return Result::CHECK_WARNING; + } else { + return Result::OK; + } +} + +bool NetSnmp::read_value_from_proc( + unsigned int index_row, unsigned int index_col, uint64_t & output_value) +{ + if (index_row == 0 && index_col == 0) { + RCLCPP_WARN_ONCE(logger_, "index is invalid. : %u, %u", index_row, index_col); + return false; + } + + std::ifstream ifs("/proc/net/snmp"); + if (!ifs) { + RCLCPP_WARN_ONCE(logger_, "Failed to open /proc/net/snmp."); + return false; + } + + std::string target_line; + std::string line; + for (unsigned int row_index = 0; std::getline(ifs, line); ++row_index) { + if (row_index == index_row) { + target_line = line; + break; + } + } + + if (target_line.empty()) { + RCLCPP_WARN_ONCE(logger_, "Failed to get a line of /proc/net/snmp."); + return false; + } + + std::vector value_list; + boost::split(value_list, target_line, boost::is_space()); + if (index_col >= value_list.size()) { + RCLCPP_WARN_ONCE( + logger_, "There are not enough columns for the column index. : column size=%lu index=%u, %u", + value_list.size(), index_row, index_col); + return false; + } + + std::string value_str = value_list[index_col]; + if (value_str.empty()) { + RCLCPP_WARN_ONCE(logger_, "The value is empty. : index=%u, %u", index_row, index_col); + return false; + } + + if (value_str[0] == '-') { + RCLCPP_WARN_ONCE(logger_, "The value is minus. : %s", value_str.c_str()); + output_value = 0; + return false; + } else { + output_value = std::stoull(value_str); + return true; + } +} + #include RCLCPP_COMPONENTS_REGISTER_NODE(NetMonitor) diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt b/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt index 9e5b7439e1de2..0d5b6734418c4 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt +++ b/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt @@ -13,11 +13,18 @@ ament_auto_add_library(actuation_map_converter SHARED src/vgr.cpp ) +ament_auto_add_library(vehicle_adaptor SHARED + src/vehicle_adaptor/vehicle_adaptor.cpp +) + ament_auto_add_library(autoware_raw_vehicle_cmd_converter_node_component SHARED src/node.cpp ) -target_link_libraries(autoware_raw_vehicle_cmd_converter_node_component actuation_map_converter) +target_link_libraries(autoware_raw_vehicle_cmd_converter_node_component + actuation_map_converter + vehicle_adaptor +) rclcpp_components_register_node(autoware_raw_vehicle_cmd_converter_node_component PLUGIN "autoware::raw_vehicle_cmd_converter::RawVehicleCommandConverterNode" @@ -30,7 +37,11 @@ if(BUILD_TESTING) ) set(TEST_RAW_VEHICLE_CMD_CONVERTER_EXE test_autoware_raw_vehicle_cmd_converter) ament_add_ros_isolated_gtest(${TEST_RAW_VEHICLE_CMD_CONVERTER_EXE} ${TEST_SOURCES}) - target_link_libraries(${TEST_RAW_VEHICLE_CMD_CONVERTER_EXE} actuation_map_converter autoware_raw_vehicle_cmd_converter_node_component) + target_link_libraries(${TEST_RAW_VEHICLE_CMD_CONVERTER_EXE} + actuation_map_converter + vehicle_adaptor + autoware_raw_vehicle_cmd_converter_node_component + ) endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/README.md b/vehicle/autoware_raw_vehicle_cmd_converter/README.md index 1df083f5c5370..a4d72a684e7d3 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/README.md +++ b/vehicle/autoware_raw_vehicle_cmd_converter/README.md @@ -45,6 +45,12 @@ vgr_coef_c: 0.042 When `convert_steer_cmd_method: "vgr"` is selected, the node receives the control command from the controller as the desired tire angle and calculates the desired steering angle to output. Also, when `convert_actuation_to_steering_status: true`, this node receives the `actuation_status` topic and calculates the steer tire angle from the `steer_wheel_angle` and publishes it. +### Vehicle Adaptor + +**Under development** +A feature that compensates for control commands according to the dynamic characteristics of the vehicle. +This feature works when `use_vehicle_adaptor: true` is set and requires `control_horizon` to be enabled, so you need to set `enable_control_cmd_horizon_pub: true` in the trajectory_follower node. + ## Input topics | Name | Type | Description | @@ -54,6 +60,14 @@ Also, when `convert_actuation_to_steering_status: true`, this node receives the | `~/input/odometry` | navigation_msgs::Odometry | twist topic in odometry is used. | | `~/input/actuation_status` | tier4_vehicle_msgs::msg::ActuationStatus | actuation status is assumed to receive the same type of status as sent to the vehicle side. For example, if throttle/brake pedal/steer_wheel_angle is sent, the same type of status is received. In the case of steer_wheel_angle, it is used to calculate steer_tire_angle and VGR in this node. | +Input topics when vehicle_adaptor is enabled + +| Name | Type | Description | +| ------------------------------ | ----------------------------------------------- | ----------------------- | +| `~/input/accel` | geometry_msgs::msg::AccelWithCovarianceStamped; | acceleration status | +| `~/input/operation_mode_state` | autoware_adapi_v1_msgs::msg::OperationModeState | operation mode status | +| `~/input/control_horizon` | autoware_control_msgs::msg::ControlHorizon | control horizon command | + ## Output topics | Name | Type | Description | diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp index b5e13985c036e..8f8e543dc680e 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp @@ -21,10 +21,12 @@ #include "autoware_raw_vehicle_cmd_converter/brake_map.hpp" #include "autoware_raw_vehicle_cmd_converter/pid.hpp" #include "autoware_raw_vehicle_cmd_converter/steer_map.hpp" +#include "autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp" #include "autoware_raw_vehicle_cmd_converter/vgr.hpp" #include +#include #include #include #include @@ -46,7 +48,8 @@ using tier4_vehicle_msgs::msg::ActuationStatusStamped; using TwistStamped = geometry_msgs::msg::TwistStamped; using Odometry = nav_msgs::msg::Odometry; using Steering = autoware_vehicle_msgs::msg::SteeringReport; - +using autoware_adapi_v1_msgs::msg::OperationModeState; +using geometry_msgs::msg::AccelWithCovarianceStamped; class DebugValues { public: @@ -79,6 +82,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node //!< @brief topic publisher for low level vehicle command rclcpp::Publisher::SharedPtr pub_actuation_cmd_; rclcpp::Publisher::SharedPtr pub_steering_status_; + rclcpp::Publisher::SharedPtr pub_compensated_control_cmd_; //!< @brief subscriber for vehicle command rclcpp::Subscription::SharedPtr sub_control_cmd_; rclcpp::Subscription::SharedPtr sub_actuation_status_; @@ -86,17 +90,27 @@ class RawVehicleCommandConverterNode : public rclcpp::Node // polling subscribers autoware::universe_utils::InterProcessPollingSubscriber sub_odometry_{ this, "~/input/odometry"}; + // polling subscribers for vehicle_adaptor + autoware::universe_utils::InterProcessPollingSubscriber sub_accel_{ + this, "~/input/accel"}; + autoware::universe_utils::InterProcessPollingSubscriber sub_operation_mode_{ + this, "~/input/operation_mode_state"}; + // control_horizon is an experimental topic, but vehicle_adaptor uses it to improve performance, + autoware::universe_utils::InterProcessPollingSubscriber sub_control_horizon_{ + this, "~/input/control_horizon"}; rclcpp::TimerBase::SharedPtr timer_; std::unique_ptr current_twist_ptr_; // [m/s] std::unique_ptr current_steer_ptr_; ActuationStatusStamped::ConstSharedPtr actuation_status_ptr_; + Odometry::ConstSharedPtr current_odometry_; Control::ConstSharedPtr control_cmd_ptr_; AccelMap accel_map_; BrakeMap brake_map_; SteerMap steer_map_; VGR vgr_; + VehicleAdaptor vehicle_adaptor_; // TODO(tanaka): consider accel/brake pid too PIDController steer_pid_; bool ff_map_initialized_; @@ -112,6 +126,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node bool convert_brake_cmd_; //!< @brief use brake or not std::optional convert_steer_cmd_method_{std::nullopt}; //!< @brief method to convert bool need_to_subscribe_actuation_status_{false}; + bool use_vehicle_adaptor_{false}; rclcpp::Time prev_time_steer_calculation_{0, 0, RCL_ROS_TIME}; // Whether to subscribe to actuation_status and calculate and publish steering_status diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp new file mode 100644 index 0000000000000..595cddd29817f --- /dev/null +++ b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp @@ -0,0 +1,50 @@ +// Copyright 2024 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__VEHICLE_ADAPTOR__VEHICLE_ADAPTOR_HPP_ +#define AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__VEHICLE_ADAPTOR__VEHICLE_ADAPTOR_HPP_ + +#include +#include +#include +#include +#include +#include + +namespace autoware::raw_vehicle_cmd_converter +{ + +using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_control_msgs::msg::Control; +using autoware_control_msgs::msg::ControlHorizon; +using autoware_vehicle_msgs::msg::SteeringReport; +using geometry_msgs::msg::AccelWithCovarianceStamped; +using nav_msgs::msg::Odometry; + +class VehicleAdaptor +{ +public: + VehicleAdaptor() = default; + Control compensate( + const Control & input_control_cmd, [[maybe_unused]] const Odometry & odometry, + [[maybe_unused]] const AccelWithCovarianceStamped & accel, + [[maybe_unused]] const double steering, + [[maybe_unused]] const OperationModeState & operation_mode, + [[maybe_unused]] const ControlHorizon & control_horizon); + +private: +}; +} // namespace autoware::raw_vehicle_cmd_converter + +#endif // AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__VEHICLE_ADAPTOR__VEHICLE_ADAPTOR_HPP_ diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml b/vehicle/autoware_raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml index 441d924a0e6c3..dc96533c505cd 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml +++ b/vehicle/autoware_raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml @@ -2,8 +2,11 @@ + + + @@ -13,8 +16,11 @@ + + + diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/package.xml b/vehicle/autoware_raw_vehicle_cmd_converter/package.xml index 468e039312520..ff00f09a7efd8 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/package.xml +++ b/vehicle/autoware_raw_vehicle_cmd_converter/package.xml @@ -21,6 +21,7 @@ ament_cmake_auto autoware_cmake + autoware_adapi_v1_msgs autoware_control_msgs autoware_interpolation autoware_vehicle_msgs diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json b/vehicle/autoware_raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json index 30642663a39f5..84bc78e1e7ace 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json +++ b/vehicle/autoware_raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json @@ -180,6 +180,11 @@ "type": "boolean", "default": "true", "description": "convert actuation to steering status or not. Whether to subscribe to actuation_status and calculate and publish steering_status For example, receive the steering wheel angle and calculate the steering wheel angle based on the gear ratio. If false, the vehicle interface must publish steering_status." + }, + "use_vehicle_adaptor": { + "type": "boolean", + "default": "false", + "description": "flag to enable feature that compensates control commands according to vehicle dynamics." } }, "required": [ diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp index e4cdbe60fd4cd..7624705b15b37 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp @@ -40,6 +40,8 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( // for steering steer controller use_steer_ff_ = declare_parameter("use_steer_ff"); use_steer_fb_ = declare_parameter("use_steer_fb"); + use_vehicle_adaptor_ = declare_parameter("use_vehicle_adaptor", false); + if (convert_accel_cmd_) { if (!accel_map_.readAccelMapFromCSV(csv_path_accel_map, true)) { throw std::invalid_argument("Accel map is invalid."); @@ -116,15 +118,21 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( debug_pub_steer_pid_ = create_publisher( "/vehicle/raw_vehicle_cmd_converter/debug/steer_pid", 1); + if (use_vehicle_adaptor_) { + pub_compensated_control_cmd_ = create_publisher( + "/vehicle/raw_vehicle_cmd_converter/debug/compensated_control_cmd", 1); + } + logger_configure_ = std::make_unique(this); } void RawVehicleCommandConverterNode::publishActuationCmd() { - if (!current_twist_ptr_ || !control_cmd_ptr_ || !current_steer_ptr_) { + /* check if all necessary data is available */ + if (!current_odometry_ || !control_cmd_ptr_ || !current_steer_ptr_) { RCLCPP_WARN_EXPRESSION( get_logger(), is_debugging_, "some pointers are null: %s, %s, %s", - !current_twist_ptr_ ? "twist" : "", !control_cmd_ptr_ ? "cmd" : "", + !current_odometry_ ? "odometry" : "", !control_cmd_ptr_ ? "cmd" : "", !current_steer_ptr_ ? "steer" : ""); return; } @@ -136,14 +144,34 @@ void RawVehicleCommandConverterNode::publishActuationCmd() } } + /* compensate control command if vehicle adaptor is enabled */ + Control control_cmd = *control_cmd_ptr_; + if (use_vehicle_adaptor_) { + const auto current_accel = sub_accel_.takeData(); + const auto current_operation_mode = sub_operation_mode_.takeData(); + const auto control_horizon = sub_control_horizon_.takeData(); + if (!current_accel || !current_operation_mode || !control_horizon) { + RCLCPP_WARN_EXPRESSION( + get_logger(), is_debugging_, "some pointers are null: %s, %s %s", + !current_accel ? "accel" : "", !current_operation_mode ? "operation_mode" : "", + !control_horizon ? "control_horizon" : ""); + return; + } + control_cmd = vehicle_adaptor_.compensate( + *control_cmd_ptr_, *current_odometry_, *current_accel, *current_steer_ptr_, + *current_operation_mode, *control_horizon); + pub_compensated_control_cmd_->publish(control_cmd); + } + + /* calculate actuation command */ double desired_accel_cmd = 0.0; double desired_brake_cmd = 0.0; double desired_steer_cmd = 0.0; ActuationCommandStamped actuation_cmd; - const double acc = control_cmd_ptr_->longitudinal.acceleration; - const double vel = current_twist_ptr_->twist.linear.x; - const double steer = control_cmd_ptr_->lateral.steering_tire_angle; - const double steer_rate = control_cmd_ptr_->lateral.steering_tire_rotation_rate; + const double acc = control_cmd.longitudinal.acceleration; + const double vel = current_odometry_->twist.twist.linear.x; + const double steer = control_cmd.lateral.steering_tire_angle; + const double steer_rate = control_cmd.lateral.steering_tire_rotation_rate; bool accel_cmd_is_zero = true; if (convert_accel_cmd_) { desired_accel_cmd = calculateAccelMap(vel, acc, accel_cmd_is_zero); @@ -173,7 +201,7 @@ void RawVehicleCommandConverterNode::publishActuationCmd() desired_steer_cmd = calculateSteerFromMap(vel, steer, steer_rate); } actuation_cmd.header.frame_id = "base_link"; - actuation_cmd.header.stamp = control_cmd_ptr_->stamp; + actuation_cmd.header.stamp = control_cmd.stamp; actuation_cmd.actuation.accel_cmd = desired_accel_cmd; actuation_cmd.actuation.brake_cmd = desired_brake_cmd; actuation_cmd.actuation.steer_cmd = desired_steer_cmd; @@ -252,12 +280,7 @@ double RawVehicleCommandConverterNode::calculateBrakeMap( void RawVehicleCommandConverterNode::onControlCmd(const Control::ConstSharedPtr msg) { - const auto odometry_msg = sub_odometry_.takeData(); - if (odometry_msg) { - current_twist_ptr_ = std::make_unique(); - current_twist_ptr_->header = odometry_msg->header; - current_twist_ptr_->twist = odometry_msg->twist.twist; - } + current_odometry_ = sub_odometry_.takeData(); control_cmd_ptr_ = msg; publishActuationCmd(); } @@ -277,14 +300,11 @@ void RawVehicleCommandConverterNode::onActuationStatus( } // calculate steering status from actuation status - const auto odometry_msg = sub_odometry_.takeData(); - if (odometry_msg) { + current_odometry_ = sub_odometry_.takeData(); + if (current_odometry_) { if (convert_steer_cmd_method_.value() == "vgr") { - current_twist_ptr_ = std::make_unique(); - current_twist_ptr_->header = odometry_msg->header; - current_twist_ptr_->twist = odometry_msg->twist.twist; current_steer_ptr_ = std::make_unique(vgr_.calculateSteeringTireState( - current_twist_ptr_->twist.linear.x, actuation_status_ptr_->status.steer_status)); + current_odometry_->twist.twist.linear.x, actuation_status_ptr_->status.steer_status)); Steering steering_msg{}; steering_msg.steering_tire_angle = *current_steer_ptr_; pub_steering_status_->publish(steering_msg); diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/vehicle_adaptor/vehicle_adaptor.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/vehicle_adaptor/vehicle_adaptor.cpp new file mode 100644 index 0000000000000..7bc9076ae944b --- /dev/null +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/vehicle_adaptor/vehicle_adaptor.cpp @@ -0,0 +1,32 @@ +// Copyright 2024 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp" + +#include + +namespace autoware::raw_vehicle_cmd_converter +{ +Control VehicleAdaptor::compensate( + const Control & input_control_cmd, [[maybe_unused]] const Odometry & odometry, + [[maybe_unused]] const AccelWithCovarianceStamped & accel, [[maybe_unused]] const double steering, + [[maybe_unused]] const OperationModeState & operation_mode, + [[maybe_unused]] const ControlHorizon & control_horizon) +{ + // TODO(someone): implement the control command compensation + Control output_control_cmd = input_control_cmd; + std::cerr << "vehicle adaptor: compensate control command" << std::endl; + return output_control_cmd; +} +} // namespace autoware::raw_vehicle_cmd_converter diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CMakeLists.txt b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..7ff4e19c69419 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_string_stamped_rviz_plugin) + +# find dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugins_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + plugins +) diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/README.md b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/README.md new file mode 100644 index 0000000000000..e5add261325f4 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/README.md @@ -0,0 +1,7 @@ +# autoware_string_stamped_rviz_plugin + +Plugin for displaying 2D overlays over the RViz2 3D scene. + +## Purpose + +This plugin displays the ROS message whose topic type is `autoware_internal_debug_msgs::msg::StringStamped` in rviz. diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/package.xml b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/package.xml new file mode 100644 index 0000000000000..c22bc75f55290 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/package.xml @@ -0,0 +1,30 @@ + + + + autoware_string_stamped_rviz_plugin + 0.39.0 + + RViz2 plugin for 2D overlays in the 3D view. Mainly a port of the JSK overlay plugin + (https://github.com/jsk-ros-pkg/jsk_visualization). + + Takayuki Murooka + Satoshi Ota + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + ament_index_cpp + autoware_internal_debug_msgs + rviz_common + rviz_ogre_vendor + rviz_rendering + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/plugins/plugins_description.xml b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/plugins/plugins_description.xml new file mode 100644 index 0000000000000..302bcc629b892 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/plugins/plugins_description.xml @@ -0,0 +1,5 @@ + + + String stamped overlay plugin for the 3D view. + + diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.cpp b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.cpp new file mode 100644 index 0000000000000..03fd8bca5aee8 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.cpp @@ -0,0 +1,225 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "jsk_overlay_utils.hpp" + +#include + +namespace jsk_rviz_plugins +{ +ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer) +: pixel_buffer_(pixel_buffer) +{ + pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL); +} + +ScopedPixelBuffer::~ScopedPixelBuffer() +{ + pixel_buffer_->unlock(); +} + +Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() +{ + return pixel_buffer_; +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height) +{ + const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock(); + Ogre::uint8 * pDest = static_cast(pixelBox.data); + memset(pDest, 0, width * height); + return QImage(pDest, width, height, QImage::Format_ARGB32); +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height, QColor & bg_color) +{ + QImage Hud = getQImage(width, height); + for (unsigned int i = 0; i < width; i++) { + for (unsigned int j = 0; j < height; j++) { + Hud.setPixel(i, j, bg_color.rgba()); + } + } + return Hud; +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight()); +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay, QColor & bg_color) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight(), bg_color); +} + +OverlayObject::OverlayObject( + Ogre::SceneManager * manager, rclcpp::Logger logger, const std::string & name) +: name_(name), logger_(logger) +{ + rviz_rendering::RenderSystem::get()->prepareOverlays(manager); + std::string material_name = name_ + "Material"; + Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + overlay_ = mOverlayMgr->create(name_); + panel_ = static_cast( + mOverlayMgr->createOverlayElement("Panel", name_ + "Panel")); + panel_->setMetricsMode(Ogre::GMM_PIXELS); + + panel_material_ = Ogre::MaterialManager::getSingleton().create( + material_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + panel_->setMaterialName(panel_material_->getName()); + overlay_->add2D(panel_); +} + +OverlayObject::~OverlayObject() +{ + hide(); + panel_material_->unload(); + Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); + // Ogre::OverlayManager* mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + // mOverlayMgr->destroyOverlayElement(panel_); + // delete panel_; + // delete overlay_; +} + +std::string OverlayObject::getName() +{ + return name_; +} + +void OverlayObject::hide() +{ + if (overlay_->isVisible()) { + overlay_->hide(); + } +} + +void OverlayObject::show() +{ + if (!overlay_->isVisible()) { + overlay_->show(); + } +} + +bool OverlayObject::isTextureReady() +{ + return static_cast(texture_); +} + +void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) +{ + const std::string texture_name = name_ + "Texture"; + if (width == 0) { + RCLCPP_WARN(logger_, "width=0 is specified as texture size"); + width = 1; + } + if (height == 0) { + RCLCPP_WARN(logger_, "height=0 is specified as texture size"); + height = 1; + } + if (!isTextureReady() || ((width != texture_->getWidth()) || (height != texture_->getHeight()))) { + if (isTextureReady()) { + Ogre::TextureManager::getSingleton().remove(texture_name); + panel_material_->getTechnique(0)->getPass(0)->removeAllTextureUnitStates(); + } + texture_ = Ogre::TextureManager::getSingleton().createManual( + texture_name, // name + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, // type + width, height, // width & height of the render window + 0, // number of mipmaps + Ogre::PF_A8R8G8B8, // pixel format chosen to match a format Qt can use + Ogre::TU_DEFAULT // usage + ); + panel_material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name); + + panel_material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + } +} + +ScopedPixelBuffer OverlayObject::getBuffer() +{ + if (isTextureReady()) { + return ScopedPixelBuffer(texture_->getBuffer()); + } else { + return ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr()); + } +} + +void OverlayObject::setPosition(double left, double top) +{ + panel_->setPosition(left, top); +} + +void OverlayObject::setDimensions(double width, double height) +{ + panel_->setDimensions(width, height); +} + +bool OverlayObject::isVisible() +{ + return overlay_->isVisible(); +} + +unsigned int OverlayObject::getTextureWidth() +{ + if (isTextureReady()) { + return texture_->getWidth(); + } else { + return 0; + } +} + +unsigned int OverlayObject::getTextureHeight() +{ + if (isTextureReady()) { + return texture_->getHeight(); + } else { + return 0; + } +} + +} // namespace jsk_rviz_plugins diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.hpp b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.hpp new file mode 100644 index 0000000000000..e69abed49f371 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.hpp @@ -0,0 +1,143 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef JSK_OVERLAY_UTILS_HPP_ +#define JSK_OVERLAY_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +// see OGRE/OgrePrerequisites.h +// #define OGRE_VERSION +// ((OGRE_VERSION_MAJOR << 16) | (OGRE_VERSION_MINOR << 8) | OGRE_VERSION_PATCH) +#if OGRE_VERSION < ((1 << 16) | (9 << 8) | 0) +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#endif + +#include +#include +#include +#include +#include +#include + +namespace jsk_rviz_plugins +{ +class OverlayObject; + +class ScopedPixelBuffer +{ +public: + explicit ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer); + virtual ~ScopedPixelBuffer(); + virtual Ogre::HardwarePixelBufferSharedPtr getPixelBuffer(); + virtual QImage getQImage(unsigned int width, unsigned int height); + virtual QImage getQImage(OverlayObject & overlay); + virtual QImage getQImage(unsigned int width, unsigned int height, QColor & bg_color); + virtual QImage getQImage(OverlayObject & overlay, QColor & bg_color); + +protected: + Ogre::HardwarePixelBufferSharedPtr pixel_buffer_; + +private: +}; + +// this is a class for put overlay object on rviz 3D panel. +// This class suppose to be instantiated in onInitialize method +// of rviz::Display class. +class OverlayObject +{ +public: + typedef std::shared_ptr Ptr; + + OverlayObject(Ogre::SceneManager * manager, rclcpp::Logger logger, const std::string & name); + virtual ~OverlayObject(); + + virtual std::string getName(); + /*virtual*/ void hide(); // remove "virtual" for cppcheck + virtual void show(); + virtual bool isTextureReady(); + virtual void updateTextureSize(unsigned int width, unsigned int height); + virtual ScopedPixelBuffer getBuffer(); + virtual void setPosition(double left, double top); + virtual void setDimensions(double width, double height); + virtual bool isVisible(); + virtual unsigned int getTextureWidth(); + virtual unsigned int getTextureHeight(); + +protected: + const std::string name_; + rclcpp::Logger logger_; + Ogre::Overlay * overlay_; + Ogre::PanelOverlayElement * panel_; + Ogre::MaterialPtr panel_material_; + Ogre::TexturePtr texture_; + +private: +}; + +// Ogre::Overlay* createOverlay(std::string name); +// Ogre::PanelOverlayElement* createOverlayPanel(Ogre::Overlay* overlay); +// Ogre::MaterialPtr createOverlayMaterial(Ogre::Overlay* overlay); +} // namespace jsk_rviz_plugins + +#endif // JSK_OVERLAY_UTILS_HPP_ diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.cpp b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.cpp new file mode 100644 index 0000000000000..d5746e99ff084 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.cpp @@ -0,0 +1,237 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "string_stamped_overlay_display.hpp" + +#include "jsk_overlay_utils.hpp" + +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace autoware::string_stamped_rviz_plugin +{ +StringStampedOverlayDisplay::StringStampedOverlayDisplay() +{ + const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL)); + + constexpr float hight_4k = 2160.0; + const float scale = static_cast(screen_info->height) / hight_4k; + const auto left = static_cast(std::round(1024 * scale)); + const auto top = static_cast(std::round(128 * scale)); + + property_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this); + property_left_ = new rviz_common::properties::IntProperty( + "Left", left, "Left of the plotter window", this, SLOT(updateVisualization()), this); + property_left_->setMin(0); + property_top_ = new rviz_common::properties::IntProperty( + "Top", top, "Top of the plotter window", this, SLOT(updateVisualization())); + property_top_->setMin(0); + + property_value_height_offset_ = new rviz_common::properties::IntProperty( + "Value height offset", 0, "Height offset of the plotter window", this, + SLOT(updateVisualization())); + property_font_size_ = new rviz_common::properties::IntProperty( + "Font Size", 15, "Font Size", this, SLOT(updateVisualization()), this); + property_font_size_->setMin(1); + property_max_letter_num_ = new rviz_common::properties::IntProperty( + "Max Letter Num", 100, "Max Letter Num", this, SLOT(updateVisualization()), this); + property_max_letter_num_->setMin(10); + + property_last_diag_keep_time_ = new rviz_common::properties::FloatProperty( + "Time To Keep Last Diag", 1.0, "Time To Keep Last Diag", this, SLOT(updateVisualization()), + this); + property_last_diag_keep_time_->setMin(0); + + property_last_diag_erase_time_ = new rviz_common::properties::FloatProperty( + "Time To Erase Last Diag", 2.0, "Time To Erase Last Diag", this, SLOT(updateVisualization()), + this); + property_last_diag_erase_time_->setMin(0.001); +} + +StringStampedOverlayDisplay::~StringStampedOverlayDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void StringStampedOverlayDisplay::onInitialize() +{ + RTDClass::onInitialize(); + + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "StringOverlayDisplayObject" << count++; + auto logger = context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger(); + overlay_.reset(new jsk_rviz_plugins::OverlayObject(scene_manager_, logger, ss.str())); + + overlay_->show(); + + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +void StringStampedOverlayDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void StringStampedOverlayDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void StringStampedOverlayDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + { + std::lock_guard message_lock(mutex_); + if (!last_non_empty_msg_ptr_) { + return; + } + } + + // calculate text and alpha + const auto text_with_alpha = [&]() { + std::lock_guard message_lock(mutex_); + if (last_msg_text_.empty()) { + const auto current_time = context_->getRosNodeAbstraction().lock()->get_raw_node()->now(); + const auto duration = (current_time - last_non_empty_msg_ptr_->stamp).seconds(); + if ( + duration < + property_last_diag_keep_time_->getFloat() + property_last_diag_erase_time_->getFloat()) { + const int dynamic_alpha = static_cast(std::max( + (1.0 - std::max(duration - property_last_diag_keep_time_->getFloat(), 0.0) / + property_last_diag_erase_time_->getFloat()) * + 255, + 0.0)); + return std::make_pair(last_non_empty_msg_ptr_->data, dynamic_alpha); + } + } + return std::make_pair(last_msg_text_, 255); + }(); + + // Display + QColor background_color; + background_color.setAlpha(0); + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(background_color); + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + const int w = overlay_->getTextureWidth() - line_width_; + const int h = overlay_->getTextureHeight() - line_width_; + + // text + QColor text_color(property_text_color_->getColor()); + text_color.setAlpha(text_with_alpha.second); + painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); + QFont font = painter.font(); + font.setPixelSize(property_font_size_->getInt()); + font.setBold(true); + painter.setFont(font); + + // same as above, but align on right side + painter.drawText( + 0, std::min(property_value_height_offset_->getInt(), h - 1), w, + std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignLeft | Qt::AlignTop, + text_with_alpha.first.c_str()); + painter.end(); + updateVisualization(); +} + +void StringStampedOverlayDisplay::processMessage( + const autoware_internal_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + { + std::lock_guard message_lock(mutex_); + last_msg_text_ = msg_ptr->data; + + // keep the non empty last message for visualization + if (!msg_ptr->data.empty()) { + last_non_empty_msg_ptr_ = msg_ptr; + } + } + + queueRender(); +} + +void StringStampedOverlayDisplay::updateVisualization() +{ + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +} // namespace autoware::string_stamped_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::string_stamped_rviz_plugin::StringStampedOverlayDisplay, rviz_common::Display) diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.hpp b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.hpp new file mode 100644 index 0000000000000..6f7cd84b91aaa --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.hpp @@ -0,0 +1,110 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef STRING_STAMPED_OVERLAY_DISPLAY_HPP_ +#define STRING_STAMPED_OVERLAY_DISPLAY_HPP_ + +#include +#include +#include + +#ifndef Q_MOC_RUN +#include "jsk_overlay_utils.hpp" + +#include +#include +#include +#include + +#endif + +#include + +namespace autoware::string_stamped_rviz_plugin +{ +class StringStampedOverlayDisplay +: public rviz_common::RosTopicDisplay + +{ + Q_OBJECT + +public: + StringStampedOverlayDisplay(); + ~StringStampedOverlayDisplay() override; + + void onInitialize() override; + void onEnable() override; + void onDisable() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void update(float wall_dt, float ros_dt) override; + void processMessage( + const autoware_internal_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) override; + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::ColorProperty * property_text_color_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_value_height_offset_; + rviz_common::properties::IntProperty * property_font_size_; + rviz_common::properties::IntProperty * property_max_letter_num_; + rviz_common::properties::FloatProperty * property_last_diag_keep_time_; + rviz_common::properties::FloatProperty * property_last_diag_erase_time_; + +private: + static constexpr int line_width_ = 2; + static constexpr int hand_width_ = 4; + + std::mutex mutex_; + std::string last_msg_text_; + autoware_internal_debug_msgs::msg::StringStamped::ConstSharedPtr last_non_empty_msg_ptr_; +}; +} // namespace autoware::string_stamped_rviz_plugin + +#endif // STRING_STAMPED_OVERLAY_DISPLAY_HPP_