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 cda3c8ceaca89..5bbc4d1c6177b 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -170,6 +170,7 @@ planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_m planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp yukinari.hisaki.2@tier4.jp zhe.shen@tier4.jp @@ -234,6 +235,7 @@ vehicle/autoware_raw_vehicle_cmd_converter/** kosuke.takeuchi@tier4.jp kyoichi.s vehicle/autoware_steer_offset_estimator/** taiki.tanaka@tier4.jp visualization/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai +visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/** satoshi.ota@tier4.jp takayuki.murooka@tier4.jp visualization/autoware_perception_rviz_plugin/** opensource@apex.ai shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp visualization/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp visualization/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp 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/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..c25f6b24c5e47 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -1,57 +1,40 @@ 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 + run-condition: + default: true + required: false + type: boolean + 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 + if: ${{ inputs.run-condition }} + 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 +46,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 +61,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..c0442694b5c77 --- /dev/null +++ b/.github/workflows/build-test-tidy-pr.yaml @@ -0,0 +1,83 @@ +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: + if: ${{ always() }} + needs: + - require-label + uses: ./.github/workflows/build-and-test-differential.yaml + with: + container: ghcr.io/autowarefoundation/autoware:universe-devel + run-condition: ${{ needs.require-label.outputs.result == 'true' }} + secrets: + codecov-token: ${{ secrets.CODECOV_TOKEN }} + + build-and-test-differential-cuda: + if: ${{ always() }} + needs: check-if-cuda-job-is-needed + uses: ./.github/workflows/build-and-test-differential.yaml + with: + container: ghcr.io/autowarefoundation/autoware:universe-devel + container-suffix: -cuda + run-condition: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'true' }} + secrets: + codecov-token: ${{ secrets.CODECOV_TOKEN }} + + clang-tidy-differential: + if: ${{ always() }} # always run to provide report for status check + needs: + - check-if-cuda-job-is-needed + - build-and-test-differential + uses: ./.github/workflows/clang-tidy-differential.yaml + with: + container: ghcr.io/autowarefoundation/autoware:universe-devel + run-condition: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'false' && needs.build-and-test-differential.result == 'success' }} + + clang-tidy-differential-cuda: + if: ${{ always() }} # always run to provide report for status check + needs: + - check-if-cuda-job-is-needed + - build-and-test-differential-cuda + uses: ./.github/workflows/clang-tidy-differential.yaml + with: + container: ghcr.io/autowarefoundation/autoware:universe-devel + container-suffix: -cuda + run-condition: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'true' && needs.build-and-test-differential-cuda.result == 'success' }} diff --git a/.github/workflows/clang-tidy-differential.yaml b/.github/workflows/clang-tidy-differential.yaml new file mode 100644 index 0000000000000..41d722c8e07b0 --- /dev/null +++ b/.github/workflows/clang-tidy-differential.yaml @@ -0,0 +1,80 @@ +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 + run-condition: + default: true + required: false + type: boolean + +jobs: + clang-tidy-differential: + if: ${{ inputs.run-condition }} + 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/.pre-commit-config-optional.yaml b/.pre-commit-config-optional.yaml index 56000d93a8af5..f0b5bdba2d24d 100644 --- a/.pre-commit-config-optional.yaml +++ b/.pre-commit-config-optional.yaml @@ -2,9 +2,16 @@ # 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-optional): autofix" + # we already have our own daily update mechanism, we set this to quarterly + autoupdate_schedule: quarterly + autoupdate_commit_msg: "ci(pre-commit-optional): quarterly autoupdate" + repos: - repo: https://github.com/tcort/markdown-link-check - rev: v3.13.6 + rev: v3.12.2 hooks: - id: markdown-link-check args: [--quiet, --config=.markdown-link-check.json] diff --git a/build_depends.repos b/build_depends.repos index 7e547be7409cf..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.2.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/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_point_types/CHANGELOG.rst b/common/autoware_point_types/CHANGELOG.rst deleted file mode 100644 index 5e26002677a36..0000000000000 --- a/common/autoware_point_types/CHANGELOG.rst +++ /dev/null @@ -1,150 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_point_types -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(autoware_point_types): prefix namespace with autoware::point_types (`#9169 `_) -* feat: migrating pointcloud types (`#6996 `_) - * feat: changed most of sensing to the new type - * chore: started applying changes to the perception stack - * feat: confirmed operation until centerpoint - * feat: reverted to the original implementation of pointpainting - * chore: forgot to push a header - * feat: also implemented the changes for the subsample filters that were out of scope before - * fix: some point type changes were missing from the latest merge from main - * chore: removed unused code, added comments, and brought back a removed publish - * chore: replaced pointcloud_raw for pointcloud_raw_ex to avoid extra processing time in the drivers - * feat: added memory layout checks - * chore: updated documentation regarding the point types - * chore: added hyperlinks to the point definitions. will be valid only once the PR is merged - * fix: fixed compilation due to moving the utilities files to the base library - * chore: separated the utilities functions due to a dependency issue - * chore: forgot that perception also uses the filter class - * feature: adapted the undistortion tests to the new point type - --------- - Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> - Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> -* chore: updated maintainers for the autoware_point_types package (`#7797 `_) -* docs(common): adding .pages file (`#7148 `_) - * docs(common): adding .pages file - * fix naming - * fix naming - * fix naming - * include main page plus explanation to autoware tools - * style(pre-commit): autofix - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* Contributors: Esteve Fernandez, Kenzo Lobos Tsunekawa, Yutaka Kondo, Zulfaqar Azmi - -0.26.0 (2024-04-03) -------------------- -* build: mark autoware_cmake as (`#3616 `_) - * build: mark autoware_cmake as - with , autoware_cmake is automatically exported with ament_target_dependencies() (unecessary) - * style(pre-commit): autofix - * chore: fix pre-commit errors - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> - Co-authored-by: Kenji Miyake -* feat: isolate gtests in all packages (`#693 `_) -* chore: upgrade cmake_minimum_required to 3.14 (`#856 `_) -* refactor: use autoware cmake (`#849 `_) - * remove autoware_auto_cmake - * add build_depend of autoware_cmake - * use autoware_cmake in CMakeLists.txt - * fix bugs - * fix cmake lint errors -* feat: add blockage diagnostics (`#461 `_) - * feat!: add blockage diagnostic - * fix: typo - * docs: add documentation - * ci(pre-commit): autofix - * fix: typo - * ci(pre-commit): autofix - * fix: typo - * chore: add adjustable param - * ci(pre-commit): autofix - * feat!: add blockage diagnostic - * fix: typo - * docs: add documentation - * ci(pre-commit): autofix - * fix: typo - * ci(pre-commit): autofix - * fix: typo - * chore: add adjustable param - * ci(pre-commit): autofix - * chore: rearrange header file - * chore: fix typo - * chore: rearrange header - * fix: revert accident change - * chore: fix typo - * docs: add limits - * chore: check overflow - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* ci: check include guard (`#438 `_) - * ci: check include guard - * apply pre-commit - * Update .pre-commit-config.yaml - Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> - * fix: pre-commit - Co-authored-by: Kenji Miyake - Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> -* feat: add point_types for wrapper (`#784 `_) (`#215 `_) - * add point_types - * Revert "add point_types" - This reverts commit 5810000cd1cbd876bc22372e2bb74ccaca06187b. - * create autoware_point_types pkg - * add include - * add cmath - * fix author - * fix bug - * define epsilon as argument - * add test - * remove unnamed namespace - * update test - * fix test name - * use std::max - * change comparison method - * remove unnencessary line - * fix test - * fix comparison method name - Co-authored-by: Taichi Higashide -* Contributors: Daisuke Nishimatsu, Kenji Miyake, Maxime CLEMENT, Takagi, Isamu, Vincent Richard, badai nguyen diff --git a/common/autoware_point_types/CMakeLists.txt b/common/autoware_point_types/CMakeLists.txt deleted file mode 100644 index c149f79dab71f..0000000000000 --- a/common/autoware_point_types/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_point_types) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -include_directories( - include - SYSTEM - ${PCL_INCLUDE_DIRS} -) - -if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_autoware_point_types - test/test_point_types.cpp - ) - target_include_directories(test_autoware_point_types - PRIVATE include - ) - ament_target_dependencies(test_autoware_point_types - point_cloud_msg_wrapper - ) -endif() - -ament_auto_package() diff --git a/common/autoware_point_types/README.md b/common/autoware_point_types/README.md deleted file mode 100644 index 92f19d2bc353a..0000000000000 --- a/common/autoware_point_types/README.md +++ /dev/null @@ -1 +0,0 @@ -# Autoware Point Types diff --git a/common/autoware_point_types/include/autoware/point_types/types.hpp b/common/autoware_point_types/include/autoware/point_types/types.hpp deleted file mode 100644 index 0fde62222e276..0000000000000 --- a/common/autoware_point_types/include/autoware/point_types/types.hpp +++ /dev/null @@ -1,188 +0,0 @@ -// Copyright 2021 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__POINT_TYPES__TYPES_HPP_ -#define AUTOWARE__POINT_TYPES__TYPES_HPP_ - -#include - -#include - -#include -#include - -namespace autoware::point_types -{ -template -bool float_eq(const T a, const T b, const T eps = 10e-6) -{ - return std::fabs(a - b) < eps; -} - -struct PointXYZI -{ - float x{0.0F}; - float y{0.0F}; - float z{0.0F}; - float intensity{0.0F}; - friend bool operator==(const PointXYZI & p1, const PointXYZI & p2) noexcept - { - return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && - float_eq(p1.z, p2.z) && float_eq(p1.intensity, p2.intensity); - } -}; - -enum ReturnType : uint8_t { - INVALID = 0, - SINGLE_STRONGEST, - SINGLE_LAST, - DUAL_STRONGEST_FIRST, - DUAL_STRONGEST_LAST, - DUAL_WEAK_FIRST, - DUAL_WEAK_LAST, - DUAL_ONLY, -}; - -struct PointXYZIRC -{ - float x{0.0F}; - float y{0.0F}; - float z{0.0F}; - std::uint8_t intensity{0U}; - std::uint8_t return_type{0U}; - std::uint16_t channel{0U}; - - friend bool operator==(const PointXYZIRC & p1, const PointXYZIRC & p2) noexcept - { - return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && - float_eq(p1.z, p2.z) && p1.intensity == p2.intensity && - p1.return_type == p2.return_type && p1.channel == p2.channel; - } -}; - -struct PointXYZIRADRT -{ - float x{0.0F}; - float y{0.0F}; - float z{0.0F}; - float intensity{0.0F}; - uint16_t ring{0U}; - float azimuth{0.0F}; - float distance{0.0F}; - uint8_t return_type{0U}; - double time_stamp{0.0}; - friend bool operator==(const PointXYZIRADRT & p1, const PointXYZIRADRT & p2) noexcept - { - return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && - float_eq(p1.z, p2.z) && float_eq(p1.intensity, p2.intensity) && - p1.ring == p2.ring && float_eq(p1.azimuth, p2.azimuth) && - float_eq(p1.distance, p2.distance) && p1.return_type == p2.return_type && - float_eq(p1.time_stamp, p2.time_stamp); - } -}; - -struct PointXYZIRCAEDT -{ - float x{0.0F}; - float y{0.0F}; - float z{0.0F}; - std::uint8_t intensity{0U}; - std::uint8_t return_type{0U}; - std::uint16_t channel{0U}; - float azimuth{0.0F}; - float elevation{0.0F}; - float distance{0.0F}; - std::uint32_t time_stamp{0U}; - - friend bool operator==(const PointXYZIRCAEDT & p1, const PointXYZIRCAEDT & p2) noexcept - { - return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && - float_eq(p1.z, p2.z) && p1.intensity == p2.intensity && - p1.return_type == p2.return_type && p1.channel == p2.channel && - float_eq(p1.azimuth, p2.azimuth) && float_eq(p1.distance, p2.distance) && - p1.time_stamp == p2.time_stamp; - } -}; - -enum class PointXYZIIndex { X, Y, Z, Intensity }; -enum class PointXYZIRCIndex { X, Y, Z, Intensity, ReturnType, Channel }; -enum class PointXYZIRADRTIndex { - X, - Y, - Z, - Intensity, - Ring, - Azimuth, - Distance, - ReturnType, - TimeStamp -}; -enum class PointXYZIRCAEDTIndex { - X, - Y, - Z, - Intensity, - ReturnType, - Channel, - Azimuth, - Elevation, - Distance, - TimeStamp -}; - -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(azimuth); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(elevation); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(distance); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(return_type); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(time_stamp); - -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(channel); - -using PointXYZIRCGenerator = std::tuple< - point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, - point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, - field_return_type_generator, field_channel_generator>; - -using PointXYZIRADRTGenerator = std::tuple< - point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, - point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, - point_cloud_msg_wrapper::field_ring_generator, field_azimuth_generator, field_distance_generator, - field_return_type_generator, field_time_stamp_generator>; - -using PointXYZIRCAEDTGenerator = std::tuple< - point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, - point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, - field_return_type_generator, field_channel_generator, field_azimuth_generator, - field_elevation_generator, field_distance_generator, field_time_stamp_generator>; - -} // namespace autoware::point_types - -POINT_CLOUD_REGISTER_POINT_STRUCT( - autoware::point_types::PointXYZIRC, - (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)( - std::uint8_t, return_type, return_type)(std::uint16_t, channel, channel)) - -POINT_CLOUD_REGISTER_POINT_STRUCT( - autoware::point_types::PointXYZIRADRT, - (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)( - float, azimuth, azimuth)(float, distance, distance)(std::uint8_t, return_type, return_type)( - double, time_stamp, time_stamp)) - -POINT_CLOUD_REGISTER_POINT_STRUCT( - autoware::point_types::PointXYZIRCAEDT, - (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)( - std::uint8_t, return_type, - return_type)(std::uint16_t, channel, channel)(float, azimuth, azimuth)( - float, elevation, elevation)(float, distance, distance)(std::uint32_t, time_stamp, time_stamp)) -#endif // AUTOWARE__POINT_TYPES__TYPES_HPP_ diff --git a/common/autoware_point_types/package.xml b/common/autoware_point_types/package.xml deleted file mode 100644 index e35e6a63de648..0000000000000 --- a/common/autoware_point_types/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - autoware_point_types - 0.40.0 - The point types definition to use point_cloud_msg_wrapper - David Wong - Max Schmeller - Apache License 2.0 - - ament_cmake_core - ament_cmake_export_dependencies - ament_cmake_test - autoware_cmake - - ament_cmake_core - ament_cmake_test - - ament_cmake_copyright - ament_cmake_cppcheck - ament_cmake_lint_cmake - ament_cmake_xmllint - pcl_ros - point_cloud_msg_wrapper - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - point_cloud_msg_wrapper - - - ament_cmake - - diff --git a/common/autoware_point_types/test/test_point_types.cpp b/common/autoware_point_types/test/test_point_types.cpp deleted file mode 100644 index 08696a9948f60..0000000000000 --- a/common/autoware_point_types/test/test_point_types.cpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2021 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/point_types/types.hpp" - -#include - -#include - -TEST(PointEquality, PointXYZI) -{ - using autoware::point_types::PointXYZI; - - PointXYZI pt0{0, 1, 2, 3}; - PointXYZI pt1{0, 1, 2, 3}; - EXPECT_EQ(pt0, pt1); -} - -TEST(PointEquality, PointXYZIRADRT) -{ - using autoware::point_types::PointXYZIRADRT; - - PointXYZIRADRT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8}; - PointXYZIRADRT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8}; - EXPECT_EQ(pt0, pt1); -} - -TEST(PointEquality, PointXYZIRCAEDT) -{ - using autoware::point_types::PointXYZIRCAEDT; - - PointXYZIRCAEDT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; - PointXYZIRCAEDT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; - EXPECT_EQ(pt0, pt1); -} - -TEST(PointEquality, FloatEq) -{ - // test template - EXPECT_TRUE(autoware::point_types::float_eq(1, 1)); - EXPECT_TRUE(autoware::point_types::float_eq(1, 1)); - - // test floating point error - EXPECT_TRUE(autoware::point_types::float_eq(1, 1 + std::numeric_limits::epsilon())); - - // test difference of sign - EXPECT_FALSE(autoware::point_types::float_eq(2, -2)); - EXPECT_FALSE(autoware::point_types::float_eq(-2, 2)); - - // small value difference - EXPECT_FALSE(autoware::point_types::float_eq(2, 2 + 10e-6)); - - // expect same value if epsilon is larger than difference - EXPECT_TRUE(autoware::point_types::float_eq(2, 2 + 10e-6, 10e-5)); -} diff --git a/common/autoware_test_utils/config/sample_topic_snapshot.yaml b/common/autoware_test_utils/config/sample_topic_snapshot.yaml index d9eff5d579540..2ff746b84c3ac 100644 --- a/common/autoware_test_utils/config/sample_topic_snapshot.yaml +++ b/common/autoware_test_utils/config/sample_topic_snapshot.yaml @@ -24,6 +24,9 @@ fields: - name: dynamic_object type: PredictedObjects # autoware_perception_msgs::msg::PredictedObjects topic: /perception/object_recognition/objects -# - name: tracked_object -# type: TrackedObjects # autoware_perception_msgs::msg::TrackedObjects -# topic: /perception/object_recognition/tracking/objects + # - name: tracked_object + # type: TrackedObjects # autoware_perception_msgs::msg::TrackedObjects + # topic: /perception/object_recognition/tracking/objects + # - name: path_with_lane_id + # type: PathWithLaneId # tier4_planning_msgs::msg::PathWithLaneId + # topic: /planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id diff --git a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp index 5e07237e2c495..8375d3e731683 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp @@ -59,6 +59,7 @@ using autoware_perception_msgs::msg::TrafficLightGroupArray; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; +using autoware_planning_msgs::msg::PathPoint; using builtin_interfaces::msg::Duration; using builtin_interfaces::msg::Time; using geometry_msgs::msg::Accel; @@ -97,6 +98,9 @@ Duration parse(const YAML::Node & node); template <> Time parse(const YAML::Node & node); +template <> +Point parse(const YAML::Node & node); + template <> std::vector parse(const YAML::Node & node); @@ -145,6 +149,15 @@ std::vector parse(const YAML::Node & node); template <> UUID parse(const YAML::Node & node); +template <> +PathPoint parse(const YAML::Node & node); + +template <> +PathPointWithLaneId parse(const YAML::Node & node); + +template <> +PathWithLaneId parse(const YAML::Node & node); + template <> PredictedPath parse(const YAML::Node & node); diff --git a/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp b/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp index c2185e65422e8..dbd3dd6638c95 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp @@ -19,6 +19,9 @@ #include #include +#include + +#include #include #include @@ -150,7 +153,8 @@ inline void plot_lanelet2_object( 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()))); + axes.text( + Args(center.x(), center.y(), std::to_string(lanelet.id())), Kwargs("clip_on"_a = true)); } if (config_opt && config_opt.value().label) { @@ -214,16 +218,111 @@ inline void plot_lanelet2_object( axes.add_patch(Args(poly.unwrap())); } +struct DrivableAreaConfig +{ + static DrivableAreaConfig defaults() { return {"turquoise", 2.0}; } + std::optional color{}; + std::optional linewidth{}; +}; + +struct PathWithLaneIdConfig +{ + static PathWithLaneIdConfig defaults() + { + return {std::nullopt, "k", 1.0, std::nullopt, false, 1.0}; + } + std::optional label{}; + std::optional color{}; + std::optional linewidth{}; + std::optional da{}; + bool lane_id{}; // & config_opt = std::nullopt); -*/ +inline void plot_autoware_object( + const tier4_planning_msgs::msg::PathWithLaneId & path, 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.label) { + kwargs["label"] = config.label.value(); + } + if (config.color) { + kwargs["color"] = config.color.value(); + } + if (config.linewidth) { + kwargs["linewidth"] = config.linewidth.value(); + } + } + std::vector xs; + std::vector ys; + std::vector yaw_cos; + std::vector yaw_sin; + std::vector> ids; + const bool plot_lane_id = config_opt ? config_opt.value().lane_id : false; + for (const auto & point : path.points) { + xs.push_back(point.point.pose.position.x); + ys.push_back(point.point.pose.position.y); + const auto th = autoware::universe_utils::getRPY(point.point.pose.orientation).z; + yaw_cos.push_back(std::cos(th)); + yaw_sin.push_back(std::sin(th)); + if (plot_lane_id) { + ids.emplace_back(); + for (const auto & id : point.lane_ids) { + ids.back().push_back(id); + } + } + } + // plot centerline + axes.plot(Args(xs, ys), kwargs); + const auto quiver_scale = + config_opt ? config_opt.value().quiver_size : PathWithLaneIdConfig::defaults().quiver_size; + const auto quiver_color = + config_opt ? (config_opt.value().color ? config_opt.value().color.value() : "k") : "k"; + axes.quiver( + Args(xs, ys, yaw_cos, yaw_sin), Kwargs( + "angles"_a = "xy", "scale_units"_a = "xy", + "scale"_a = quiver_scale, "color"_a = quiver_color)); + if (plot_lane_id) { + for (size_t i = 0; i < xs.size(); ++i) { + std::stringstream ss; + const char * delimiter = ""; + for (const auto id : ids[i]) { + ss << std::exchange(delimiter, ",") << id; + } + axes.text(Args(xs[i], ys[i], ss.str()), Kwargs("clip_on"_a = true)); + } + } + // plot drivable area + if (config_opt && config_opt.value().da) { + auto plot_boundary = [&](const decltype(path.left_bound) & points) { + std::vector xs; + std::vector ys; + for (const auto & point : points) { + xs.push_back(point.x); + ys.push_back(point.y); + } + const auto & cfg = config_opt.value().da.value(); + py::dict kwargs{}; + if (cfg.color) { + kwargs["color"] = cfg.color.value(); + } + if (cfg.linewidth) { + kwargs["linewidth"] = cfg.linewidth.value(); + } + axes.plot(Args(xs, ys), kwargs); + }; + plot_boundary(path.left_bound); + plot_boundary(path.right_bound); + } +} + } // 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 c328f37ba357d..740e7f840141d 100644 --- a/common/autoware_test_utils/package.xml +++ b/common/autoware_test_utils/package.xml @@ -8,6 +8,7 @@ Takamasa Horibe Zulfaqar Azmi Mamoru Sobue + Yukinari Hisaki Apache License 2.0 Kyoichi Sugahara diff --git a/common/autoware_test_utils/src/mock_data_parser.cpp b/common/autoware_test_utils/src/mock_data_parser.cpp index 6e7002501bf30..29aed75357441 100644 --- a/common/autoware_test_utils/src/mock_data_parser.cpp +++ b/common/autoware_test_utils/src/mock_data_parser.cpp @@ -65,6 +65,16 @@ std::array parse(const YAML::Node & node) return msg; } +template <> +Point parse(const YAML::Node & node) +{ + Point geom_point; + geom_point.x = node["x"].as(); + geom_point.y = node["y"].as(); + geom_point.z = node["z"].as(); + return geom_point; +} + template <> std::vector parse(const YAML::Node & node) { @@ -285,6 +295,46 @@ Shape parse(const YAML::Node & node) return msg; } +template <> +PathPoint parse(const YAML::Node & node) +{ + PathPoint point; + point.pose = parse(node["pose"]); + point.longitudinal_velocity_mps = node["longitudinal_velocity_mps"].as(); + point.lateral_velocity_mps = node["lateral_velocity_mps"].as(); + point.heading_rate_rps = node["heading_rate_rps"].as(); + point.is_final = node["is_final"].as(); + return point; +} + +template <> +PathPointWithLaneId parse(const YAML::Node & node) +{ + PathPointWithLaneId point; + point.point = parse(node["point"]); + for (const auto & lane_id_node : node["lane_ids"]) { + point.lane_ids.push_back(lane_id_node.as()); + } + return point; +} + +template <> +PathWithLaneId parse(const YAML::Node & node) +{ + PathWithLaneId path; + path.header = parse
(node["header"]); + for (const auto & point_node : node["points"]) { + path.points.push_back(parse(point_node)); + } + for (const auto & left_bound_node : node["left_bound"]) { + path.left_bound.push_back(parse(left_bound_node)); + } + for (const auto & right_bound_node : node["right_bound"]) { + path.right_bound.push_back(parse(right_bound_node)); + } + return path; +} + template <> PredictedPath parse(const YAML::Node & node) { diff --git a/common/autoware_test_utils/src/topic_snapshot_saver.cpp b/common/autoware_test_utils/src/topic_snapshot_saver.cpp index b2cb2a17c9621..bd9af2d5672b8 100644 --- a/common/autoware_test_utils/src/topic_snapshot_saver.cpp +++ b/common/autoware_test_utils/src/topic_snapshot_saver.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include @@ -46,7 +47,8 @@ using MessageType = std::variant< autoware_adapi_v1_msgs::msg::OperationModeState, // 3 autoware_planning_msgs::msg::LaneletRoute, // 4 autoware_perception_msgs::msg::TrafficLightGroupArray, // 5 - autoware_perception_msgs::msg::TrackedObjects // 6 + autoware_perception_msgs::msg::TrackedObjects, // 6 + tier4_planning_msgs::msg::PathWithLaneId // 7 >; std::optional get_topic_index(const std::string & name) @@ -72,6 +74,9 @@ std::optional get_topic_index(const std::string & name) if (name == "TrackedObjects") { return 6; } + if (name == "PathWithLaneId") { + return 7; + } return std::nullopt; } @@ -196,6 +201,7 @@ class TopicSnapShotSaver REGISTER_CALLBACK(4); REGISTER_CALLBACK(5); REGISTER_CALLBACK(6); + REGISTER_CALLBACK(7); } } @@ -243,6 +249,7 @@ class TopicSnapShotSaver REGISTER_WRITE_TYPE(4); REGISTER_WRITE_TYPE(5); REGISTER_WRITE_TYPE(6); + REGISTER_WRITE_TYPE(7); } const std::string desc = std::string(R"(# @@ -253,7 +260,7 @@ class TopicSnapShotSaver # map_path_uri: package:/// # fields(this is array) # - name: -# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | TBD} +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | PathWithLaneId | TBD} # topic: # )"); diff --git a/common/autoware_test_utils/test_map/intersection/lanelet2_map.osm b/common/autoware_test_utils/test_map/intersection/lanelet2_map.osm index c5c5b7b1fc657..9f28ed79a91ae 100644 --- a/common/autoware_test_utils/test_map/intersection/lanelet2_map.osm +++ b/common/autoware_test_utils/test_map/intersection/lanelet2_map.osm @@ -1,6 +1,6 @@ - + @@ -8002,391 +8002,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -10004,16 +9619,6 @@ - - - - - - - - - - @@ -15586,6 +15191,181 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -18669,142 +18449,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -19364,12 +19008,6 @@ - - - - - - @@ -21131,6 +20769,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -22269,15 +21969,6 @@ - - - - - - - - - @@ -22853,6 +22544,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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/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 e00851de63b9c..7605ed2a5e859 100644 --- a/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml +++ b/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml @@ -14,7 +14,8 @@ - lateral_deviation - yaw_deviation - velocity_deviation - - lateral_trajectory_displacement + - lateral_trajectory_displacement_local + - lateral_trajectory_displacement_lookahead - stability - stability_frechet - obstacle_distance @@ -25,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 0e08398ffa87e..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 @@ -45,7 +45,7 @@ Accumulator calcLateralDeviation(const Trajectory & ref, const Trajector * @param [in] base_pose base pose * @return calculated statistics */ -Accumulator calcLateralTrajectoryDisplacement( +Accumulator calcLocalLateralTrajectoryDisplacement( const Trajectory & prev, const Trajectory & traj, const Pose & base_pose); /** 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 13dad65b239b1..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,7 +37,8 @@ enum class Metric { lateral_deviation, yaw_deviation, velocity_deviation, - lateral_trajectory_displacement, + lateral_trajectory_displacement_local, + lateral_trajectory_displacement_lookahead, stability, stability_frechet, obstacle_distance, @@ -63,7 +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", Metric::lateral_trajectory_displacement}, + {"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}, @@ -84,7 +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, "lateral_trajectory_displacement"}, + {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"}, @@ -106,7 +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, "Nearest Pose Lateral Deviation[m]"}, + {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/src/metrics/deviation_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp index ffb56baf29f17..82ba86c65d6af 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp @@ -45,7 +45,7 @@ Accumulator calcLateralDeviation(const Trajectory & ref, const Trajector return stat; } -Accumulator calcLateralTrajectoryDisplacement( +Accumulator calcLocalLateralTrajectoryDisplacement( const Trajectory & prev, const Trajectory & traj, const Pose & ego_pose) { 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 b7676c2fdab6c..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,23 +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: - return metrics::calcLateralTrajectoryDisplacement(previous_trajectory_, traj, ego_pose_); + 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); @@ -107,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() @@ -117,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_ekf_localizer/README.md b/localization/autoware_ekf_localizer/README.md index aad65da2516d1..802bf7dbb16c3 100644 --- a/localization/autoware_ekf_localizer/README.md +++ b/localization/autoware_ekf_localizer/README.md @@ -191,6 +191,7 @@ Note that, although the dimension gets larger since the analytical expansion can ### The conditions that result in a WARN state - The node is not in the activate state. +- The initial pose is not set. - The number of consecutive no measurement update via the Pose/Twist topic exceeds the `pose_no_update_count_threshold_warn`/`twist_no_update_count_threshold_warn`. - The timestamp of the Pose/Twist topic is beyond the delay compensation range. - The Pose/Twist topic is beyond the range of Mahalanobis distance for covariance estimation. diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp index 20a77354c0996..b194c3e956341 100644 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp +++ b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp @@ -24,6 +24,7 @@ namespace autoware::ekf_localizer { diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_activated); +diagnostic_msgs::msg::DiagnosticStatus check_set_initialpose(const bool is_set_initialpose); diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated( const std::string & measurement_type, const size_t no_update_count, diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp index 1b437f26e9c7d..0561e250298ac 100644 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp +++ b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp @@ -119,6 +119,7 @@ class EKFLocalizer : public rclcpp::Node double ekf_dt_; bool is_activated_; + bool is_set_initialpose_; EKFDiagnosticInfo pose_diag_info_; EKFDiagnosticInfo twist_diag_info_; diff --git a/localization/autoware_ekf_localizer/media/ekf_diagnostics.png b/localization/autoware_ekf_localizer/media/ekf_diagnostics.png index 234b2f1e19b6d..a1561c3f52707 100644 Binary files a/localization/autoware_ekf_localizer/media/ekf_diagnostics.png and b/localization/autoware_ekf_localizer/media/ekf_diagnostics.png differ diff --git a/localization/autoware_ekf_localizer/src/diagnostics.cpp b/localization/autoware_ekf_localizer/src/diagnostics.cpp index a1af492487fe4..45468abf72d6c 100644 --- a/localization/autoware_ekf_localizer/src/diagnostics.cpp +++ b/localization/autoware_ekf_localizer/src/diagnostics.cpp @@ -41,6 +41,25 @@ diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_act return stat; } +diagnostic_msgs::msg::DiagnosticStatus check_set_initialpose(const bool is_set_initialpose) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "is_set_initialpose"; + key_value.value = is_set_initialpose ? "True" : "False"; + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + if (!is_set_initialpose) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "[WARN]initial pose is not set"; + } + + return stat; +} + diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated( const std::string & measurement_type, const size_t no_update_count, const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error) diff --git a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp index d34be2a537ef1..11d7788adfade 100644 --- a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp +++ b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp @@ -56,6 +56,7 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) twist_queue_(params_.twist_smoothing_steps) { is_activated_ = false; + is_set_initialpose_ = false; /* initialize ros system */ timer_control_ = rclcpp::create_timer( @@ -143,6 +144,13 @@ void EKFLocalizer::timer_callback() return; } + if (!is_set_initialpose_) { + warning_->warn_throttle( + "Initial pose is not set. Provide initial pose to pose_initializer", 2000); + publish_diagnostics(geometry_msgs::msg::PoseStamped{}, current_time); + return; + } + DEBUG_INFO(get_logger(), "========================= timer called ========================="); /* update predict frequency with measured timer rate */ @@ -264,6 +272,8 @@ void EKFLocalizer::callback_initial_pose( params_.pose_frame_id.c_str(), msg->header.frame_id.c_str()); } ekf_module_->initialize(*msg, transform); + + is_set_initialpose_ = true; } /* @@ -272,7 +282,7 @@ void EKFLocalizer::callback_initial_pose( void EKFLocalizer::callback_pose_with_covariance( geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - if (!is_activated_) { + if (!is_activated_ && !is_set_initialpose_) { return; } @@ -359,8 +369,9 @@ void EKFLocalizer::publish_diagnostics( std::vector diag_status_array; diag_status_array.push_back(check_process_activated(is_activated_)); + diag_status_array.push_back(check_set_initialpose(is_set_initialpose_)); - if (is_activated_) { + if (is_activated_ && is_set_initialpose_) { diag_status_array.push_back(check_measurement_updated( "pose", pose_diag_info_.no_update_count, params_.pose_no_update_count_threshold_warn, params_.pose_no_update_count_threshold_error)); @@ -439,6 +450,7 @@ void EKFLocalizer::service_trigger_node( is_activated_ = true; } else { is_activated_ = false; + is_set_initialpose_ = false; } res->success = true; } diff --git a/localization/autoware_ekf_localizer/test/test_diagnostics.cpp b/localization/autoware_ekf_localizer/test/test_diagnostics.cpp index 165102adec1d7..5ce39df484e98 100644 --- a/localization/autoware_ekf_localizer/test/test_diagnostics.cpp +++ b/localization/autoware_ekf_localizer/test/test_diagnostics.cpp @@ -35,6 +35,19 @@ TEST(TestEkfDiagnostics, check_process_activated) EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); } +TEST(TestEkfDiagnostics, check_set_initialpose) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + bool is_set_initialpose = true; + stat = check_set_initialpose(is_set_initialpose); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + is_set_initialpose = false; + stat = check_set_initialpose(is_set_initialpose); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); +} + TEST(TestEkfDiagnostics, check_measurement_updated) { diagnostic_msgs::msg::DiagnosticStatus stat; 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/README.md b/localization/autoware_ndt_scan_matcher/README.md index 22e56930a0048..ee177608bdefe 100644 --- a/localization/autoware_ndt_scan_matcher/README.md +++ b/localization/autoware_ndt_scan_matcher/README.md @@ -25,26 +25,26 @@ One optional function is regularization. Please see the regularization chapter i ### Output -| Name | Type | Description | -| --------------------------------- | ----------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------- | -| `ndt_pose` | `geometry_msgs::msg::PoseStamped` | estimated pose | -| `ndt_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | estimated pose with covariance | -| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | -| `points_aligned` | `sensor_msgs::msg::PointCloud2` | [debug topic] pointcloud aligned by scan matching | -| `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] no ground pointcloud aligned by scan matching | -| `initial_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] initial pose used in scan matching | -| `multi_ndt_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] estimated poses from multiple initial poses in real-time covariance estimation | -| `multi_initial_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] initial poses for real-time covariance estimation | -| `exe_time_ms` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] execution time for scan matching [ms] | -| `transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching | -| `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on no ground LiDAR scan | -| `iteration_num` | `tier4_debug_msgs::msg::Int32Stamped` | [debug topic] number of scan matching iterations | -| `initial_to_result_relative_pose` | `geometry_msgs::msg::PoseStamped` | [debug topic] relative pose between the initial point and the convergence point | -| `initial_to_result_distance` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the initial point and the convergence point [m] | -| `initial_to_result_distance_old` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the older of the two initial points used in linear interpolation and the convergence point [m] | -| `initial_to_result_distance_new` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the newer of the two initial points used in linear interpolation and the convergence point [m] | -| `ndt_marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] markers for debugging | -| `monte_carlo_initial_pose_marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] particles used in initial position estimation | +| Name | Type | Description | +| --------------------------------- | --------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------- | +| `ndt_pose` | `geometry_msgs::msg::PoseStamped` | estimated pose | +| `ndt_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | estimated pose with covariance | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | +| `points_aligned` | `sensor_msgs::msg::PointCloud2` | [debug topic] pointcloud aligned by scan matching | +| `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] no ground pointcloud aligned by scan matching | +| `initial_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] initial pose used in scan matching | +| `multi_ndt_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] estimated poses from multiple initial poses in real-time covariance estimation | +| `multi_initial_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] initial poses for real-time covariance estimation | +| `exe_time_ms` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] execution time for scan matching [ms] | +| `transform_probability` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching | +| `no_ground_transform_probability` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on no ground LiDAR scan | +| `iteration_num` | `autoware_internal_debug_msgs::msg::Int32Stamped` | [debug topic] number of scan matching iterations | +| `initial_to_result_relative_pose` | `geometry_msgs::msg::PoseStamped` | [debug topic] relative pose between the initial point and the convergence point | +| `initial_to_result_distance` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the initial point and the convergence point [m] | +| `initial_to_result_distance_old` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the older of the two initial points used in linear interpolation and the convergence point [m] | +| `initial_to_result_distance_new` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the newer of the two initial points used in linear interpolation and the convergence point [m] | +| `ndt_marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] markers for debugging | +| `monte_carlo_initial_pose_marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] particles used in initial position estimation | ### Service 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..d6e629ee2f1c3 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" @@ -26,6 +26,8 @@ #include #include +#include +#include #include #include #include @@ -33,8 +35,6 @@ #include #include #include -#include -#include #include #include @@ -166,23 +166,24 @@ class NDTScanMatcher : public rclcpp::Node initial_pose_with_covariance_pub_; rclcpp::Publisher::SharedPtr multi_ndt_pose_pub_; rclcpp::Publisher::SharedPtr multi_initial_pose_pub_; - rclcpp::Publisher::SharedPtr exe_time_pub_; - rclcpp::Publisher::SharedPtr transform_probability_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr exe_time_pub_; + rclcpp::Publisher::SharedPtr + transform_probability_pub_; + rclcpp::Publisher::SharedPtr nearest_voxel_transformation_likelihood_pub_; rclcpp::Publisher::SharedPtr voxel_score_points_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr no_ground_transform_probability_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr no_ground_nearest_voxel_transformation_likelihood_pub_; - rclcpp::Publisher::SharedPtr iteration_num_pub_; + rclcpp::Publisher::SharedPtr iteration_num_pub_; rclcpp::Publisher::SharedPtr initial_to_result_relative_pose_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr initial_to_result_distance_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr initial_to_result_distance_old_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr initial_to_result_distance_new_pub_; rclcpp::Publisher::SharedPtr ndt_marker_pub_; rclcpp::Publisher::SharedPtr @@ -211,12 +212,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/package.xml b/localization/autoware_ndt_scan_matcher/package.xml index f2047418310aa..ebd1ebda2d573 100644 --- a/localization/autoware_ndt_scan_matcher/package.xml +++ b/localization/autoware_ndt_scan_matcher/package.xml @@ -17,6 +17,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_localization_util autoware_map_msgs autoware_universe_utils @@ -35,7 +36,6 @@ tf2_geometry_msgs tf2_ros tf2_sensor_msgs - tier4_debug_msgs tier4_localization_msgs visualization_msgs 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..3d06dfffa16ed 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,22 +49,22 @@ 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( +autoware_internal_debug_msgs::msg::Float32Stamped make_float32_stamped( const builtin_interfaces::msg::Time & stamp, const float data) { - using T = tier4_debug_msgs::msg::Float32Stamped; - return tier4_debug_msgs::build().stamp(stamp).data(data); + using T = autoware_internal_debug_msgs::msg::Float32Stamped; + return autoware_internal_debug_msgs::build().stamp(stamp).data(data); } -tier4_debug_msgs::msg::Int32Stamped make_int32_stamped( +autoware_internal_debug_msgs::msg::Int32Stamped make_int32_stamped( const builtin_interfaces::msg::Time & stamp, const int32_t data) { - using T = tier4_debug_msgs::msg::Int32Stamped; - return tier4_debug_msgs::build().stamp(stamp).data(data); + using T = autoware_internal_debug_msgs::msg::Int32Stamped; + return autoware_internal_debug_msgs::build().stamp(stamp).data(data); } std::array rotate_covariance( @@ -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_ = @@ -158,31 +158,34 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) multi_ndt_pose_pub_ = this->create_publisher("multi_ndt_pose", 10); multi_initial_pose_pub_ = this->create_publisher("multi_initial_pose", 10); - exe_time_pub_ = this->create_publisher("exe_time_ms", 10); + exe_time_pub_ = + this->create_publisher("exe_time_ms", 10); transform_probability_pub_ = - this->create_publisher("transform_probability", 10); + this->create_publisher( + "transform_probability", 10); nearest_voxel_transformation_likelihood_pub_ = - this->create_publisher( + this->create_publisher( "nearest_voxel_transformation_likelihood", 10); voxel_score_points_pub_ = this->create_publisher("voxel_score_points", 10); no_ground_transform_probability_pub_ = - this->create_publisher( + this->create_publisher( "no_ground_transform_probability", 10); no_ground_nearest_voxel_transformation_likelihood_pub_ = - this->create_publisher( + this->create_publisher( "no_ground_nearest_voxel_transformation_likelihood", 10); iteration_num_pub_ = - this->create_publisher("iteration_num", 10); + this->create_publisher("iteration_num", 10); initial_to_result_relative_pose_pub_ = this->create_publisher("initial_to_result_relative_pose", 10); initial_to_result_distance_pub_ = - this->create_publisher("initial_to_result_distance", 10); + this->create_publisher( + "initial_to_result_distance", 10); initial_to_result_distance_old_pub_ = - this->create_publisher( + this->create_publisher( "initial_to_result_distance_old", 10); initial_to_result_distance_new_pub_ = - this->create_publisher( + this->create_publisher( "initial_to_result_distance_new", 10); ndt_marker_pub_ = this->create_publisher("ndt_marker", 10); ndt_monte_carlo_initial_pose_marker_pub_ = @@ -209,13 +212,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_pose2twist/README.md b/localization/autoware_pose2twist/README.md index 55ca4667c423d..7f81e0dece5b0 100644 --- a/localization/autoware_pose2twist/README.md +++ b/localization/autoware_pose2twist/README.md @@ -17,11 +17,11 @@ The `twist.angular` is calculated as `relative_rotation_vector / dt` for each fi ### Output -| Name | Type | Description | -| --------- | ------------------------------------- | --------------------------------------------- | -| twist | geometry_msgs::msg::TwistStamped | twist calculated from the input pose history. | -| linear_x | tier4_debug_msgs::msg::Float32Stamped | linear-x field of the output twist. | -| angular_z | tier4_debug_msgs::msg::Float32Stamped | angular-z field of the output twist. | +| Name | Type | Description | +| --------- | ------------------------------------------------- | --------------------------------------------- | +| twist | geometry_msgs::msg::TwistStamped | twist calculated from the input pose history. | +| linear_x | autoware_internal_debug_msgs::msg::Float32Stamped | linear-x field of the output twist. | +| angular_z | autoware_internal_debug_msgs::msg::Float32Stamped | angular-z field of the output twist. | ## Parameters diff --git a/localization/autoware_pose2twist/package.xml b/localization/autoware_pose2twist/package.xml index 3fd16856ef6af..cfde18d430cfd 100644 --- a/localization/autoware_pose2twist/package.xml +++ b/localization/autoware_pose2twist/package.xml @@ -17,11 +17,11 @@ ament_cmake autoware_cmake + autoware_internal_debug_msgs geometry_msgs rclcpp rclcpp_components tf2_geometry_msgs - tier4_debug_msgs ament_lint_auto autoware_lint_common diff --git a/localization/autoware_pose2twist/src/pose2twist_core.cpp b/localization/autoware_pose2twist/src/pose2twist_core.cpp index 4dc7b5fb04209..f071d7c8c66e2 100644 --- a/localization/autoware_pose2twist/src/pose2twist_core.cpp +++ b/localization/autoware_pose2twist/src/pose2twist_core.cpp @@ -29,9 +29,10 @@ Pose2Twist::Pose2Twist(const rclcpp::NodeOptions & options) : rclcpp::Node("pose durable_qos.transient_local(); twist_pub_ = create_publisher("twist", durable_qos); - linear_x_pub_ = create_publisher("linear_x", durable_qos); + linear_x_pub_ = + create_publisher("linear_x", durable_qos); angular_z_pub_ = - create_publisher("angular_z", durable_qos); + create_publisher("angular_z", durable_qos); // Note: this callback publishes topics above pose_sub_ = create_subscription( "pose", queue_size, std::bind(&Pose2Twist::callback_pose, this, _1)); @@ -105,12 +106,12 @@ void Pose2Twist::callback_pose(geometry_msgs::msg::PoseStamped::SharedPtr pose_m twist_msg.header.frame_id = "base_link"; twist_pub_->publish(twist_msg); - tier4_debug_msgs::msg::Float32Stamped linear_x_msg; + autoware_internal_debug_msgs::msg::Float32Stamped linear_x_msg; linear_x_msg.stamp = this->now(); linear_x_msg.data = static_cast(twist_msg.twist.linear.x); linear_x_pub_->publish(linear_x_msg); - tier4_debug_msgs::msg::Float32Stamped angular_z_msg; + autoware_internal_debug_msgs::msg::Float32Stamped angular_z_msg; angular_z_msg.stamp = this->now(); angular_z_msg.data = static_cast(twist_msg.twist.angular.z); angular_z_pub_->publish(angular_z_msg); diff --git a/localization/autoware_pose2twist/src/pose2twist_core.hpp b/localization/autoware_pose2twist/src/pose2twist_core.hpp index ed3e542beb857..22540c9aee473 100644 --- a/localization/autoware_pose2twist/src/pose2twist_core.hpp +++ b/localization/autoware_pose2twist/src/pose2twist_core.hpp @@ -17,9 +17,9 @@ #include +#include #include #include -#include #ifdef ROS_DISTRO_GALACTIC #include @@ -44,8 +44,8 @@ class Pose2Twist : public rclcpp::Node rclcpp::Subscription::SharedPtr pose_sub_; rclcpp::Publisher::SharedPtr twist_pub_; - rclcpp::Publisher::SharedPtr linear_x_pub_; - rclcpp::Publisher::SharedPtr angular_z_pub_; + rclcpp::Publisher::SharedPtr linear_x_pub_; + rclcpp::Publisher::SharedPtr angular_z_pub_; }; } // namespace autoware::pose2twist 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/localization/autoware_stop_filter/README.md b/localization/autoware_stop_filter/README.md index 9904707a59996..9136f6b6fc8a0 100644 --- a/localization/autoware_stop_filter/README.md +++ b/localization/autoware_stop_filter/README.md @@ -18,10 +18,10 @@ This node aims to: ### Output -| Name | Type | Description | -| ----------------- | ------------------------------------ | -------------------------------------------------------- | -| `output/odom` | `nav_msgs::msg::Odometry` | odometry with suppressed longitudinal and yaw twist | -| `debug/stop_flag` | `tier4_debug_msgs::msg::BoolStamped` | flag to represent whether the vehicle is stopping or not | +| Name | Type | Description | +| ----------------- | ------------------------------------------------ | -------------------------------------------------------- | +| `output/odom` | `nav_msgs::msg::Odometry` | odometry with suppressed longitudinal and yaw twist | +| `debug/stop_flag` | `autoware_internal_debug_msgs::msg::BoolStamped` | flag to represent whether the vehicle is stopping or not | ## Parameters diff --git a/localization/autoware_stop_filter/package.xml b/localization/autoware_stop_filter/package.xml index 1ed5e2c2082be..50ba98484f59d 100644 --- a/localization/autoware_stop_filter/package.xml +++ b/localization/autoware_stop_filter/package.xml @@ -17,12 +17,12 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs geometry_msgs nav_msgs rclcpp rclcpp_components tf2 - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/localization/autoware_stop_filter/src/stop_filter.cpp b/localization/autoware_stop_filter/src/stop_filter.cpp index 17eaafdc3002a..b0f1bc8eba053 100644 --- a/localization/autoware_stop_filter/src/stop_filter.cpp +++ b/localization/autoware_stop_filter/src/stop_filter.cpp @@ -34,12 +34,13 @@ StopFilter::StopFilter(const rclcpp::NodeOptions & node_options) "input/odom", 1, std::bind(&StopFilter::callback_odometry, this, std::placeholders::_1)); pub_odom_ = create_publisher("output/odom", 1); - pub_stop_flag_ = create_publisher("debug/stop_flag", 1); + pub_stop_flag_ = + create_publisher("debug/stop_flag", 1); } void StopFilter::callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg) { - tier4_debug_msgs::msg::BoolStamped stop_flag_msg; + autoware_internal_debug_msgs::msg::BoolStamped stop_flag_msg; stop_flag_msg.stamp = msg->header.stamp; stop_flag_msg.data = false; diff --git a/localization/autoware_stop_filter/src/stop_filter.hpp b/localization/autoware_stop_filter/src/stop_filter.hpp index b6d56b5f77059..71864fab0a580 100644 --- a/localization/autoware_stop_filter/src/stop_filter.hpp +++ b/localization/autoware_stop_filter/src/stop_filter.hpp @@ -17,10 +17,10 @@ #include +#include #include #include #include -#include #include #include @@ -43,7 +43,7 @@ class StopFilter : public rclcpp::Node private: rclcpp::Publisher::SharedPtr pub_odom_; //!< @brief odom publisher - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr pub_stop_flag_; //!< @brief stop flag publisher rclcpp::Subscription::SharedPtr sub_odom_; //!< @brief measurement odometry subscriber diff --git a/localization/autoware_twist2accel/package.xml b/localization/autoware_twist2accel/package.xml index 75cea94546516..092b227c2ad40 100644 --- a/localization/autoware_twist2accel/package.xml +++ b/localization/autoware_twist2accel/package.xml @@ -23,7 +23,6 @@ rclcpp rclcpp_components tf2 - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/localization/autoware_twist2accel/src/twist2accel.hpp b/localization/autoware_twist2accel/src/twist2accel.hpp index d338b256fec77..48bc03a326091 100644 --- a/localization/autoware_twist2accel/src/twist2accel.hpp +++ b/localization/autoware_twist2accel/src/twist2accel.hpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include diff --git a/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp b/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp index 8efb90cc87c89..085e423db38ba 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp +++ b/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp @@ -97,6 +97,8 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node void removeDuplicateIds(TrafficSignalArray & signal_array) const; + bool isInvalidDetectionStatus(const TrafficSignal & signal) const; + // Node param bool use_last_detect_color_; double last_detect_color_hold_time_; diff --git a/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp b/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp index b0ec4d0e5ffd0..d7cc6c725edfd 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp @@ -298,6 +298,14 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal( if (valid_id2idx_map.count(id)) { size_t idx = valid_id2idx_map[id]; auto signal = msg.traffic_light_groups[idx]; + if (isInvalidDetectionStatus(signal)) { + TrafficSignalElement output_traffic_signal_element; + output_traffic_signal_element.color = color; + output_traffic_signal_element.shape = TrafficSignalElement::CIRCLE; + output_traffic_signal_element.confidence = 1.0; + output.traffic_light_groups[idx].elements[0] = output_traffic_signal_element; + continue; + } updateFlashingState(signal); // check if it is flashing // update output msg according to flashing and current state output.traffic_light_groups[idx].elements[0].color = updateAndGetColorState(signal); @@ -314,6 +322,19 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal( } } +bool CrosswalkTrafficLightEstimatorNode::isInvalidDetectionStatus( + const TrafficSignal & signal) const +{ + // check occlusion, backlight(shape is unknown) and no detection(shape is circle) + if ( + signal.elements.front().color == TrafficSignalElement::UNKNOWN && + signal.elements.front().confidence == 0.0) { + return true; + } + + return false; +} + void CrosswalkTrafficLightEstimatorNode::updateFlashingState(const TrafficSignal & signal) { const auto id = signal.traffic_light_group_id; 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 347cb57b484e9..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 @@ -13,5 +14,11 @@ 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 04ecf89faa0d4..e05339771f667 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,6 +15,7 @@ #ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ #define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ +#include #include #include #include @@ -39,7 +40,6 @@ #include #include -#include #include #include #include @@ -51,13 +51,33 @@ using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; using sensor_msgs::msg::CameraInfo; using sensor_msgs::msg::Image; -using sensor_msgs::msg::PointCloud2; -using tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using PointCloudMsgType = sensor_msgs::msg::PointCloud2; +using RoiMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using ClusterMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using ClusterObjType = tier4_perception_msgs::msg::DetectedObjectWithFeature; using tier4_perception_msgs::msg::DetectedObjectWithFeature; using PointCloud = pcl::PointCloud; +using autoware::image_projection_based_fusion::CameraProjection; using autoware_perception_msgs::msg::ObjectClassification; -template +template +struct Det2dStatus +{ + // camera index + std::size_t id = 0; + // camera projection + std::unique_ptr camera_projector_ptr; + bool project_to_unrectified_image = false; + bool approximate_camera_projection = false; + // process flags + bool is_fused = false; + // timing + double input_offset_ms = 0.0; + // cache + std::map cached_det2d_msgs; +}; + +template class FusionNode : public rclcpp::Node { public: @@ -69,70 +89,71 @@ class FusionNode : public rclcpp::Node explicit FusionNode( const std::string & node_name, const rclcpp::NodeOptions & options, int queue_size); -protected: +private: + // Common process methods void cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id); - virtual void preprocess(TargetMsg3D & output_msg); - - // callback for Msg subscription - virtual void subCallback(const typename TargetMsg3D::ConstSharedPtr input_msg); - - // callback for roi subscription + // callback for timer + void timer_callback(); + void setPeriod(const int64_t new_period); + void exportProcess(); + + // 2d detection management methods + bool checkAllDet2dFused() + { + for (const auto & det2d : det2d_list_) { + if (!det2d.is_fused) { + return false; + } + } + return true; + } + + // camera projection + float approx_grid_cell_w_size_; + float approx_grid_cell_h_size_; + + // timer + rclcpp::TimerBase::SharedPtr timer_; + double timeout_ms_{}; + double match_threshold_ms_{}; - virtual void roiCallback( - const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i); + std::vector::SharedPtr> rois_subs_; + std::vector::SharedPtr> camera_info_subs_; - 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; + // cache for fusion + int64_t cached_det3d_msg_timestamp_; + typename Msg3D::SharedPtr cached_det3d_msg_ptr_; - // set args if you need - virtual void postprocess(TargetMsg3D & output_msg); +protected: + void setDet2DStatus(std::size_t rois_number); - virtual void publish(const TargetMsg3D & output_msg); + // callback for main subscription + void subCallback(const typename Msg3D::ConstSharedPtr input_msg); + // callback for roi subscription + void roiCallback(const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i); - void timer_callback(); - void setPeriod(const int64_t new_period); + // Custom process methods + virtual void preprocess(Msg3D & output_msg); + virtual void fuseOnSingleImage( + const Msg3D & input_msg, const Det2dStatus & det2d, const Msg2D & input_roi_msg, + Msg3D & output_msg) = 0; + virtual void postprocess(const Msg3D & processing_msg, ExportObj & output_msg); + virtual void publish(const ExportObj & output_msg); - std::size_t rois_number_{1}; + // Members tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - bool point_project_to_unrectified_image_{false}; - - // camera_info - std::map camera_info_map_; - std::vector::SharedPtr> camera_info_subs_; - - rclcpp::TimerBase::SharedPtr timer_; - double timeout_ms_{}; - double match_threshold_ms_{}; - std::vector input_rois_topics_; - std::vector input_camera_info_topics_; - std::vector input_camera_topics_; + // 2d detection management + std::vector> det2d_list_; /** \brief A vector of subscriber. */ - typename rclcpp::Subscription::SharedPtr sub_; - std::vector::SharedPtr> rois_subs_; - - // offsets between cameras and the lidars - std::vector input_offset_ms_; - - // cache for fusion - std::vector is_fused_; - std::pair - cached_msg_; // first element is the timestamp in nanoseconds, second element is the message - std::vector> cached_roi_msgs_; - std::mutex mutex_cached_msgs_; - - // output publisher - typename rclcpp::Publisher::SharedPtr pub_ptr_; + typename rclcpp::Subscription::SharedPtr sub_; - // debugger - std::shared_ptr debugger_; - virtual bool out_of_scope(const ObjType & obj) = 0; + // parameters for out_of_scope filter float filter_scope_min_x_; float filter_scope_max_x_; float filter_scope_min_y_; @@ -140,6 +161,12 @@ class FusionNode : public rclcpp::Node float filter_scope_min_z_; float filter_scope_max_z_; + // output publisher + typename rclcpp::Publisher::SharedPtr pub_ptr_; + + // debugger + std::shared_ptr debugger_; + /** \brief processing time publisher. **/ std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; 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..9505a926df2f8 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 @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -41,29 +42,25 @@ inline bool isInsideBbox( proj_y >= roi.y_offset * zc && proj_y <= (roi.y_offset + roi.height) * zc; } -class PointPaintingFusionNode -: public FusionNode +class PointPaintingFusionNode : public FusionNode { public: explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options); -protected: - void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; +private: + void preprocess(PointCloudMsgType & pointcloud_msg) override; 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; + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, PointCloudMsgType & painted_pointcloud_msg) override; - void postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override; + void postprocess( + const PointCloudMsgType & painted_pointcloud_msg, DetectedObjects & output_msg) override; - rclcpp::Publisher::SharedPtr obj_pub_ptr_; - - std::vector tan_h_; // horizontal field of view + rclcpp::Publisher::SharedPtr point_pub_ptr_; + std::unique_ptr diagnostics_interface_ptr_; int omp_num_threads_{1}; - float score_threshold_{0.0}; std::vector class_names_; std::map class_index_; std::map> isClassTable_; @@ -75,8 +72,6 @@ class PointPaintingFusionNode autoware::lidar_centerpoint::DetectionClassRemapper detection_class_remapper_; std::unique_ptr detector_ptr_{nullptr}; - - bool out_of_scope(const DetectedObjects & obj) override; }; } // namespace autoware::image_projection_based_fusion #endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__NODE_HPP_ 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 be05c0a1c4bc6..b7bf8738b68a4 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 @@ -24,22 +24,19 @@ namespace autoware::image_projection_based_fusion { const std::map IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}}; -class RoiClusterFusionNode -: public FusionNode< - DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature> +class RoiClusterFusionNode : public FusionNode { public: explicit RoiClusterFusionNode(const rclcpp::NodeOptions & options); -protected: - void preprocess(DetectedObjectsWithFeature & output_cluster_msg) override; - void postprocess(DetectedObjectsWithFeature & output_cluster_msg) override; +private: + void preprocess(ClusterMsgType & output_cluster_msg) override; 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; + const ClusterMsgType & input_cluster_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) override; + + void postprocess(const ClusterMsgType & output_cluster_msg, ClusterMsgType & output_msg) override; std::string trust_object_iou_mode_{"iou"}; bool use_cluster_semantic_type_{false}; @@ -54,12 +51,11 @@ class RoiClusterFusionNode 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; + bool is_far_enough(const ClusterObjType & obj, const double distance_threshold); + bool out_of_scope(const ClusterObjType & obj); double cal_iou_by_mode( const sensor_msgs::msg::RegionOfInterest & roi_1, const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode); - // bool CheckUnknown(const DetectedObjectsWithFeature & obj); }; } // namespace autoware::image_projection_based_fusion 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..ba197126277a0 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 @@ -31,35 +31,31 @@ namespace autoware::image_projection_based_fusion using sensor_msgs::msg::RegionOfInterest; -class RoiDetectedObjectFusionNode -: public FusionNode +class RoiDetectedObjectFusionNode : public FusionNode { public: explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options); -protected: +private: void preprocess(DetectedObjects & output_msg) override; 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 DetectedObjects & input_object_msg, const Det2dStatus & det2d, + const RoiMsgType & 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 Det2dStatus & det2d, + const Eigen::Affine3d & object2camera_affine); void fuseObjectsOnImage( const DetectedObjects & input_object_msg, const std::vector & image_rois, const std::map & object_roi_map); - void publish(const DetectedObjects & output_msg) override; + void postprocess(const DetectedObjects & processing_msg, DetectedObjects & output_msg) override; - bool out_of_scope(const DetectedObject & obj) override; + bool out_of_scope(const DetectedObject & obj); -private: struct { std::vector passthrough_lower_bound_probability_thresholds{}; 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 9baf754e224a7..77a1745faa7e5 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 @@ -25,33 +25,29 @@ namespace autoware::image_projection_based_fusion { -class RoiPointCloudFusionNode -: public FusionNode +class RoiPointCloudFusionNode : public FusionNode { +public: + explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options); + private: - int min_cluster_size_{1}; - int max_cluster_size_{20}; - bool fuse_unknown_only_{true}; - double cluster_2d_tolerance_; + rclcpp::Publisher::SharedPtr point_pub_ptr_; + rclcpp::Publisher::SharedPtr cluster_debug_pub_; - rclcpp::Publisher::SharedPtr pub_objects_ptr_; - std::vector output_fused_objects_; - rclcpp::Publisher::SharedPtr cluster_debug_pub_; + void fuseOnSingleImage( + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, PointCloudMsgType & output_pointcloud_msg) override; - /* data */ -public: - explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options); + void postprocess(const PointCloudMsgType & pointcloud_msg, ClusterMsgType & output_msg) override; -protected: - void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + void publish(const ClusterMsgType & output_msg) override; - void postprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + int min_cluster_size_{1}; + int max_cluster_size_{20}; + bool fuse_unknown_only_{true}; + double cluster_2d_tolerance_; - 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; - bool out_of_scope(const DetectedObjectWithFeature & obj) override; + std::vector output_fused_objects_; }; } // namespace autoware::image_projection_based_fusion 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 0bec3195bb402..5414f98e142cd 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 @@ -34,10 +34,32 @@ namespace autoware::image_projection_based_fusion { -class SegmentPointCloudFusionNode : public FusionNode +class SegmentPointCloudFusionNode : public FusionNode { +public: + explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options); + private: - rclcpp::Publisher::SharedPtr pub_pointcloud_ptr_; + void preprocess(PointCloudMsgType & pointcloud_msg) override; + + void fuseOnSingleImage( + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + const Image & input_mask, PointCloudMsgType & output_pointcloud_msg) override; + + inline void copyPointCloud( + const PointCloudMsgType & input, const int point_step, const size_t global_offset, + PointCloudMsgType & output, size_t & output_pointcloud_size) + { + std::memcpy(&output.data[output_pointcloud_size], &input.data[global_offset], point_step); + output_pointcloud_size += point_step; + } + + void postprocess( + const PointCloudMsgType & pointcloud_msg, PointCloudMsgType & output_msg) override; + + // debug + image_transport::Publisher pub_debug_mask_ptr_; + std::vector filter_semantic_label_target_; float filter_distance_threshold_; // declare list of semantic label target, depend on trained data of yolox segmentation model @@ -47,30 +69,8 @@ class SegmentPointCloudFusionNode : public FusionNode filter_global_offset_set_; - -public: - explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options); - -protected: - void preprocess(PointCloud2 & pointcloud_msg) override; - - void postprocess(PointCloud2 & pointcloud_msg) override; - - void fuseOnSingleImage( - const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, const Image & input_mask, - const CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override; - - bool out_of_scope(const PointCloud2 & filtered_cloud) override; - inline void copyPointCloud( - const PointCloud2 & input, const int point_step, const size_t global_offset, - PointCloud2 & output, size_t & output_pointcloud_size) - { - std::memcpy(&output.data[output_pointcloud_size], &input.data[global_offset], point_step); - output_pointcloud_size += point_step; - } }; } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp index 44679f7509687..044a71ab57533 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp @@ -56,7 +56,7 @@ namespace autoware::image_projection_based_fusion { using PointCloud = pcl::PointCloud; -using PointCloud2 = sensor_msgs::msg::PointCloud2; + struct PointData { float distance; @@ -65,10 +65,6 @@ struct PointData bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info); -Eigen::Vector2d calcRawImageProjectedPoint( - const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d, - const bool & unrectify = false); - 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); @@ -76,17 +72,17 @@ std::optional getTransformStamped( Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t); void closest_cluster( - const PointCloud2 & cluster, const double cluster_2d_tolerance, const int min_cluster_size, - const pcl::PointXYZ & center, PointCloud2 & out_cluster); + const PointCloudMsgType & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center, PointCloudMsgType & out_cluster); void updateOutputFusedObjects( - std::vector & output_objs, std::vector & clusters, - const std::vector & clusters_data_size, const PointCloud2 & in_cloud, + std::vector & output_objs, std::vector & clusters, + const std::vector & clusters_data_size, const PointCloudMsgType & in_cloud, const std_msgs::msg::Header & in_roi_header, const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const int max_cluster_size, const float cluster_2d_tolerance, std::vector & output_fused_objects); -geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); +geometry_msgs::msg::Point getCentroid(const PointCloudMsgType & pointcloud); } // namespace autoware::image_projection_based_fusion 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 bd4d57e45c582..700d249831000 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 @@ -27,6 +28,7 @@ #include #include #include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -44,79 +46,66 @@ namespace autoware::image_projection_based_fusion { using autoware::universe_utils::ScopedTimeTrack; -template -FusionNode::FusionNode( +template +FusionNode::FusionNode( const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { // set rois_number - rois_number_ = static_cast(declare_parameter("rois_number")); - if (rois_number_ < 1) { - RCLCPP_WARN( - this->get_logger(), "minimum rois_number is 1. current rois_number is %zu", rois_number_); - rois_number_ = 1; + const std::size_t rois_number = + static_cast(declare_parameter("rois_number")); + if (rois_number < 1) { + RCLCPP_ERROR( + this->get_logger(), "minimum rois_number is 1. current rois_number is %zu", rois_number); } - if (rois_number_ > 8) { + if (rois_number > 8) { RCLCPP_WARN( this->get_logger(), - "Current rois_number is %zu. Large rois number may cause performance issue.", rois_number_); + "Current rois_number is %zu. Large rois number may cause performance issue.", rois_number); } // Set parameters match_threshold_ms_ = declare_parameter("match_threshold_ms"); timeout_ms_ = declare_parameter("timeout_ms"); - input_rois_topics_.resize(rois_number_); - input_camera_topics_.resize(rois_number_); - input_camera_info_topics_.resize(rois_number_); + std::vector input_rois_topics; + std::vector input_camera_info_topics; + + input_rois_topics.resize(rois_number); + input_camera_info_topics.resize(rois_number); - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - input_rois_topics_.at(roi_i) = declare_parameter( + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { + input_rois_topics.at(roi_i) = declare_parameter( "input/rois" + std::to_string(roi_i), "/perception/object_recognition/detection/rois" + std::to_string(roi_i)); - input_camera_info_topics_.at(roi_i) = declare_parameter( + input_camera_info_topics.at(roi_i) = declare_parameter( "input/camera_info" + std::to_string(roi_i), "/sensing/camera/camera" + std::to_string(roi_i) + "/camera_info"); - - input_camera_topics_.at(roi_i) = declare_parameter( - "input/image" + std::to_string(roi_i), - "/sensing/camera/camera" + std::to_string(roi_i) + "/image_rect_color"); - } - - input_offset_ms_ = declare_parameter>("input_offset_ms"); - if (!input_offset_ms_.empty() && rois_number_ > input_offset_ms_.size()) { - throw std::runtime_error("The number of offsets does not match the number of topics."); } - // sub camera info - camera_info_subs_.resize(rois_number_); - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { + // subscribe camera info + camera_info_subs_.resize(rois_number); + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { std::function fnc = std::bind(&FusionNode::cameraInfoCallback, this, std::placeholders::_1, roi_i); camera_info_subs_.at(roi_i) = this->create_subscription( - input_camera_info_topics_.at(roi_i), rclcpp::QoS{1}.best_effort(), fnc); + input_camera_info_topics.at(roi_i), rclcpp::QoS{1}.best_effort(), fnc); } - // sub rois - rois_subs_.resize(rois_number_); - cached_roi_msgs_.resize(rois_number_); - is_fused_.resize(rois_number_, false); - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { + // subscribe rois + rois_subs_.resize(rois_number); + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { std::function roi_callback = std::bind(&FusionNode::roiCallback, this, std::placeholders::_1, roi_i); rois_subs_.at(roi_i) = this->create_subscription( - input_rois_topics_.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); + input_rois_topics.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); } - // subscribers - std::function sub_callback = + // subscribe 3d detection + std::function sub_callback = std::bind(&FusionNode::subCallback, this, std::placeholders::_1); - sub_ = - this->create_subscription("input", rclcpp::QoS(1).best_effort(), sub_callback); - - // publisher - pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); + sub_ = this->create_subscription("input", rclcpp::QoS(1).best_effort(), sub_callback); // Set timer const auto period_ns = std::chrono::duration_cast( @@ -124,12 +113,34 @@ FusionNode::FusionNode( timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&FusionNode::timer_callback, this)); + // initialization on each 2d detections + setDet2DStatus(rois_number); + + // parameters for approximation grid + approx_grid_cell_w_size_ = declare_parameter("approximation_grid_cell_width"); + approx_grid_cell_h_size_ = declare_parameter("approximation_grid_cell_height"); + + // parameters for out_of_scope filter + filter_scope_min_x_ = declare_parameter("filter_scope_min_x"); + filter_scope_max_x_ = declare_parameter("filter_scope_max_x"); + filter_scope_min_y_ = declare_parameter("filter_scope_min_y"); + filter_scope_max_y_ = declare_parameter("filter_scope_max_y"); + filter_scope_min_z_ = declare_parameter("filter_scope_min_z"); + filter_scope_max_z_ = declare_parameter("filter_scope_max_z"); + // debugger if (declare_parameter("debug_mode", false)) { + std::vector input_camera_topics; + input_camera_topics.resize(rois_number); + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { + input_camera_topics.at(roi_i) = declare_parameter( + "input/image" + std::to_string(roi_i), + "/sensing/camera/camera" + std::to_string(roi_i) + "/image_rect_color"); + } std::size_t image_buffer_size = static_cast(declare_parameter("image_buffer_size")); debugger_ = - std::make_shared(this, rois_number_, image_buffer_size, input_camera_topics_); + std::make_shared(this, rois_number, image_buffer_size, input_camera_topics); } // time keeper @@ -142,9 +153,6 @@ FusionNode::FusionNode( time_keeper_ = std::make_shared(time_keeper); } - point_project_to_unrectified_image_ = - declare_parameter("point_project_to_unrectified_image"); - // initialize debug tool { using autoware::universe_utils::DebugPublisher; @@ -154,66 +162,125 @@ FusionNode::FusionNode( stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } +} - filter_scope_min_x_ = declare_parameter("filter_scope_min_x"); - filter_scope_max_x_ = declare_parameter("filter_scope_max_x"); - filter_scope_min_y_ = declare_parameter("filter_scope_min_y"); - filter_scope_max_y_ = declare_parameter("filter_scope_max_y"); - filter_scope_min_z_ = declare_parameter("filter_scope_min_z"); - filter_scope_max_z_ = declare_parameter("filter_scope_max_z"); +template +void FusionNode::setDet2DStatus(std::size_t rois_number) +{ + // camera offset settings + std::vector input_offset_ms = declare_parameter>("input_offset_ms"); + if (!input_offset_ms.empty() && rois_number > input_offset_ms.size()) { + throw std::runtime_error("The number of offsets does not match the number of topics."); + } + + // camera projection settings + std::vector 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."); + } + std::vector 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( + 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; + } + } + } + + // 2d detection status initialization + det2d_list_.resize(rois_number); + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { + det2d_list_.at(roi_i).id = roi_i; + det2d_list_.at(roi_i).project_to_unrectified_image = + point_project_to_unrectified_image.at(roi_i); + det2d_list_.at(roi_i).approximate_camera_projection = approx_camera_projection.at(roi_i); + det2d_list_.at(roi_i).input_offset_ms = input_offset_ms.at(roi_i); + } } -template -void FusionNode::cameraInfoCallback( +template +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 + auto & det2d = det2d_list_.at(camera_id); + if (!det2d.camera_projector_ptr && checkCameraInfo(*input_camera_info_msg)) { + det2d.camera_projector_ptr = std::make_unique( + *input_camera_info_msg, approx_grid_cell_w_size_, approx_grid_cell_h_size_, + det2d.project_to_unrectified_image, det2d.approximate_camera_projection); + det2d.camera_projector_ptr->initialize(); + } } -template -void FusionNode::preprocess(TargetMsg3D & ouput_msg - __attribute__((unused))) +template +void FusionNode::preprocess(Msg3D & ouput_msg __attribute__((unused))) { // do nothing by default } -template -void FusionNode::subCallback( - const typename TargetMsg3D::ConstSharedPtr input_msg) +template +void FusionNode::exportProcess() +{ + timer_->cancel(); + + ExportObj output_msg; + postprocess(*(cached_det3d_msg_ptr_), output_msg); + publish(output_msg); + + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - cached_det3d_msg_ptr_->header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", + processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); + processing_time_ms = 0; + } + cached_det3d_msg_ptr_ = nullptr; +} + +template +void FusionNode::subCallback( + const typename Msg3D::ConstSharedPtr det3d_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - if (cached_msg_.second != nullptr) { + if (cached_det3d_msg_ptr_ != nullptr) { + // PROCESS: if the main message is remained (and roi is not collected all) publish the main + // message may processed partially with arrived 2d rois stop_watch_ptr_->toc("processing_time", true); - timer_->cancel(); - postprocess(*(cached_msg_.second)); - publish(*(cached_msg_.second)); - std::fill(is_fused_.begin(), is_fused_.end(), false); - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - const double pipeline_latency_ms = - std::chrono::duration( - std::chrono::nanoseconds( - (this->get_clock()->now() - cached_msg_.second->header.stamp).nanoseconds())) - .count(); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", - processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); - debug_publisher_->publish( - "debug/pipeline_latency_ms", pipeline_latency_ms); - processing_time_ms = 0; - } + exportProcess(); - cached_msg_.second = nullptr; + // reset flags + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; + } } - std::lock_guard lock(mutex_cached_msgs_); + // TIMING: reset timer to the timeout time auto period = std::chrono::duration_cast( std::chrono::duration(timeout_ms_)); try { @@ -225,157 +292,141 @@ void FusionNode::subCallback( stop_watch_ptr_->toc("processing_time", true); - typename TargetMsg3D::SharedPtr output_msg = std::make_shared(*input_msg); - + // PROCESS: preprocess the main message + typename Msg3D::SharedPtr output_msg = std::make_shared(*det3d_msg); preprocess(*output_msg); + // PROCESS: fuse the main message with the cached roi messages + // (please ask maintainers before parallelize this loop because debugger is not thread safe) int64_t timestamp_nsec = (*output_msg).header.stamp.sec * static_cast(1e9) + (*output_msg).header.stamp.nanosec; + // for loop for each roi + for (auto & det2d : det2d_list_) { + const auto roi_i = det2d.id; - // if matching rois exist, fuseOnSingle - // please ask maintainers before parallelize this loop because debugger is not thread safe - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { + // check camera info + if (det2d.camera_projector_ptr == nullptr) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); continue; } - - if ((cached_roi_msgs_.at(roi_i)).size() > 0) { - int64_t min_interval = 1e9; - int64_t matched_stamp = -1; - std::list outdate_stamps; - - for (const auto & [k, v] : cached_roi_msgs_.at(roi_i)) { - int64_t new_stamp = timestamp_nsec + input_offset_ms_.at(roi_i) * static_cast(1e6); - int64_t interval = abs(static_cast(k) - new_stamp); - - if ( - interval <= min_interval && interval <= match_threshold_ms_ * static_cast(1e6)) { - min_interval = interval; - matched_stamp = k; - } else if ( - static_cast(k) < new_stamp && - interval > match_threshold_ms_ * static_cast(1e6)) { - outdate_stamps.push_back(static_cast(k)); - } + auto & det2d_msgs = det2d.cached_det2d_msgs; + + // check if the roi is collected + if (det2d_msgs.size() == 0) continue; + + // MATCH: get the closest roi message, and remove outdated messages + int64_t min_interval = 1e9; + int64_t matched_stamp = -1; + std::list outdate_stamps; + for (const auto & [roi_stamp, value] : det2d_msgs) { + int64_t new_stamp = timestamp_nsec + det2d.input_offset_ms * static_cast(1e6); + int64_t interval = abs(static_cast(roi_stamp) - new_stamp); + + if (interval <= min_interval && interval <= match_threshold_ms_ * static_cast(1e6)) { + min_interval = interval; + matched_stamp = roi_stamp; + } else if ( + static_cast(roi_stamp) < new_stamp && + interval > match_threshold_ms_ * static_cast(1e6)) { + outdate_stamps.push_back(static_cast(roi_stamp)); } + } + for (auto stamp : outdate_stamps) { + det2d_msgs.erase(stamp); + } - // remove outdated stamps - for (auto stamp : outdate_stamps) { - (cached_roi_msgs_.at(roi_i)).erase(stamp); + // PROCESS: if matched, fuse the main message with the roi message + if (matched_stamp != -1) { + if (debugger_) { + debugger_->clear(); } - // fuseOnSingle - if (matched_stamp != -1) { - if (debugger_) { - debugger_->clear(); - } + fuseOnSingleImage(*det3d_msg, det2d, *(det2d_msgs[matched_stamp]), *output_msg); + det2d_msgs.erase(matched_stamp); + det2d.is_fused = true; - fuseOnSingleImage( - *input_msg, roi_i, *((cached_roi_msgs_.at(roi_i))[matched_stamp]), - camera_info_map_.at(roi_i), *output_msg); - (cached_roi_msgs_.at(roi_i)).erase(matched_stamp); - is_fused_.at(roi_i) = true; - - // add timestamp interval for debug - if (debug_publisher_) { - double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6; - debug_publisher_->publish( - "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); - debug_publisher_->publish( - "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", - timestamp_interval_ms - input_offset_ms_.at(roi_i)); - } + // add timestamp interval for debug + if (debug_publisher_) { + double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6; + debug_publisher_->publish( + "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); + debug_publisher_->publish( + "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", + timestamp_interval_ms - det2d.input_offset_ms); } } } - // if all camera fused, postprocess; else, publish the old Msg(if exists) and cache the current - // Msg - if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { - timer_->cancel(); - postprocess(*output_msg); - publish(*output_msg); - std::fill(is_fused_.begin(), is_fused_.end(), false); - cached_msg_.second = nullptr; - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", processing_time_ms); - processing_time_ms = 0; + // PROCESS: check if the fused message is ready to publish + cached_det3d_msg_timestamp_ = timestamp_nsec; + cached_det3d_msg_ptr_ = output_msg; + if (checkAllDet2dFused()) { + // if all camera fused, postprocess and publish the main message + exportProcess(); + + // reset flags + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; } } else { - cached_msg_.first = timestamp_nsec; - cached_msg_.second = output_msg; + // if all of rois are not collected, publish the old Msg(if exists) and cache the + // current Msg processing_time_ms = stop_watch_ptr_->toc("processing_time", true); } } -template -void FusionNode::roiCallback( - const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i) +template +void FusionNode::roiCallback( + const typename Msg2D::ConstSharedPtr det2d_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) + - (*input_roi_msg).header.stamp.nanosec; + auto & det2d = det2d_list_.at(roi_i); + int64_t timestamp_nsec = + (*det2d_msg).header.stamp.sec * static_cast(1e9) + (*det2d_msg).header.stamp.nanosec; // if cached Msg exist, try to match - if (cached_msg_.second != nullptr) { - int64_t new_stamp = cached_msg_.first + input_offset_ms_.at(roi_i) * static_cast(1e6); + if (cached_det3d_msg_ptr_ != nullptr) { + int64_t new_stamp = + cached_det3d_msg_timestamp_ + det2d.input_offset_ms * static_cast(1e6); int64_t interval = abs(timestamp_nsec - new_stamp); - if ( - interval < match_threshold_ms_ * static_cast(1e6) && is_fused_.at(roi_i) == false) { - if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { + // PROCESS: if matched, fuse the main message with the roi message + if (interval < match_threshold_ms_ * static_cast(1e6) && det2d.is_fused == false) { + // check camera info + if (det2d.camera_projector_ptr == nullptr) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); - (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; + det2d.cached_det2d_msgs[timestamp_nsec] = det2d_msg; return; } + if (debugger_) { debugger_->clear(); } - - fuseOnSingleImage( - *(cached_msg_.second), roi_i, *input_roi_msg, camera_info_map_.at(roi_i), - *(cached_msg_.second)); - is_fused_.at(roi_i) = true; + // PROCESS: fuse the main message with the roi message + fuseOnSingleImage(*(cached_det3d_msg_ptr_), det2d, *det2d_msg, *(cached_det3d_msg_ptr_)); + det2d.is_fused = true; if (debug_publisher_) { - double timestamp_interval_ms = (timestamp_nsec - cached_msg_.first) / 1e6; + double timestamp_interval_ms = (timestamp_nsec - cached_det3d_msg_timestamp_) / 1e6; debug_publisher_->publish( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); debug_publisher_->publish( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", - timestamp_interval_ms - input_offset_ms_.at(roi_i)); + timestamp_interval_ms - det2d.input_offset_ms); } - if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { - timer_->cancel(); - postprocess(*(cached_msg_.second)); - publish(*(cached_msg_.second)); - std::fill(is_fused_.begin(), is_fused_.end(), false); - cached_msg_.second = nullptr; - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", - processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); - processing_time_ms = 0; + // PROCESS: if all camera fused, postprocess and publish the main message + if (checkAllDet2dFused()) { + exportProcess(); + // reset flags + for (auto & status : det2d_list_) { + status.is_fused = false; } } processing_time_ms = processing_time_ms + stop_watch_ptr_->toc("processing_time", true); @@ -383,60 +434,32 @@ void FusionNode::roiCallback( } } // store roi msg if not matched - (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; + det2d.cached_det2d_msgs[timestamp_nsec] = det2d_msg; } -template -void FusionNode::postprocess(TargetMsg3D & output_msg - __attribute__((unused))) -{ - // do nothing by default -} - -template -void FusionNode::timer_callback() +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()) { - // timeout, postprocess cached msg - if (cached_msg_.second != nullptr) { - stop_watch_ptr_->toc("processing_time", true); - - postprocess(*(cached_msg_.second)); - publish(*(cached_msg_.second)); - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", - processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); - processing_time_ms = 0; - } - } - std::fill(is_fused_.begin(), is_fused_.end(), false); - cached_msg_.second = nullptr; + // PROCESS: if timeout, postprocess cached msg + if (cached_det3d_msg_ptr_ != nullptr) { + stop_watch_ptr_->toc("processing_time", true); + exportProcess(); + } - mutex_cached_msgs_.unlock(); - } else { - try { - std::chrono::nanoseconds period = 10ms; - setPeriod(period.count()); - } catch (rclcpp::exceptions::RCLError & ex) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); - } - timer_->reset(); + // reset flags whether the message is fused or not + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; } } -template -void FusionNode::setPeriod(const int64_t new_period) +template +void FusionNode::setPeriod(const int64_t new_period) { if (!timer_) { return; @@ -452,8 +475,16 @@ void FusionNode::setPeriod(const int64_t new_period) } } -template -void FusionNode::publish(const TargetMsg3D & output_msg) +template +void FusionNode::postprocess( + const Msg3D & processing_msg __attribute__((unused)), + ExportObj & output_msg __attribute__((unused))) +{ + // do nothing by default +} + +template +void FusionNode::publish(const ExportObj & output_msg) { if (pub_ptr_->get_subscription_count() < 1) { return; @@ -461,10 +492,21 @@ void FusionNode::publish(const TargetMsg3D & output_msg pub_ptr_->publish(output_msg); } -template class FusionNode; -template class FusionNode< - DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature>; -template class FusionNode; -template class FusionNode; -template class FusionNode; +// Explicit instantiation for the supported types + +// pointpainting fusion +template class FusionNode; + +// roi cluster fusion +template class FusionNode; + +// roi detected-object fusion +template class FusionNode; + +// roi pointcloud fusion +template class FusionNode; + +// segment pointcloud fusion +template class FusionNode; + } // namespace autoware::image_projection_based_fusion 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 4d12de2685c95..bcd62383319c1 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 @@ -91,8 +91,7 @@ inline bool isUnknown(int label2d) } PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "pointpainting_fusion", options) +: FusionNode("pointpainting_fusion", options) { omp_num_threads_ = this->declare_parameter("omp_params.num_threads"); const float score_threshold = @@ -156,17 +155,15 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); - std::function sub_callback = + // subscriber + std::function sub_callback = std::bind(&PointPaintingFusionNode::subCallback, this, std::placeholders::_1); - sub_ = this->create_subscription( + 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; - } + // publisher + point_pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); + pub_ptr_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); detection_class_remapper_.setParameters( allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); @@ -193,8 +190,8 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt // create detector detector_ptr_ = std::make_unique( encoder_param, head_param, densification_param, config); - - obj_pub_ptr_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); + diagnostics_interface_ptr_ = + std::make_unique(this, "pointpainting_trt"); if (this->declare_parameter("build_only", false)) { RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); @@ -202,7 +199,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt } } -void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) +void PointPaintingFusionNode::preprocess(PointCloudMsgType & painted_pointcloud_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -259,11 +256,9 @@ 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, - sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) + __attribute__((unused)) const PointCloudMsgType & input_pointcloud_msg, + const Det2dStatus & det2d, const RoiMsgType & input_roi_msg, + PointCloudMsgType & painted_pointcloud_msg) { if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { RCLCPP_WARN_STREAM_THROTTLE( @@ -276,8 +271,6 @@ 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_); @@ -297,10 +290,6 @@ void PointPaintingFusionNode::fuseOnSingleImage( lidar2cam_affine = _transformToEigen(transform_stamped_optional.value().transform); } - // transform - // sensor_msgs::msg::PointCloud2 transformed_pointcloud; - // tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped); - const auto x_offset = painted_pointcloud_msg.fields .at(static_cast(autoware::point_types::PointXYZIRCIndex::X)) .offset; @@ -312,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 @@ -330,73 +316,87 @@ dc | dc dc dc dc ||zc| // iterate points // Requires 'OMP_NUM_THREADS=N' omp_set_num_threads(omp_num_threads_); -#pragma omp parallel for - for (int i = 0; i < iterations; i++) { - int stride = p_step * i; - unsigned char * data = &painted_pointcloud_msg.data[0]; - unsigned char * output = &painted_pointcloud_msg.data[0]; - // cppcheck-suppress-begin invalidPointerCast - float p_x = *reinterpret_cast(&data[stride + x_offset]); - float p_y = *reinterpret_cast(&data[stride + y_offset]); - float p_z = *reinterpret_cast(&data[stride + z_offset]); - // cppcheck-suppress-end invalidPointerCast - point_lidar << p_x, p_y, p_z; - point_camera = lidar2cam_affine * point_lidar; - p_x = point_camera.x(); - 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)) { - 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; - } + std::vector> local_vectors(omp_num_threads_); +#pragma omp parallel + { +#pragma omp for + for (int i = 0; i < iterations; i++) { + int stride = p_step * i; + unsigned char * data = &painted_pointcloud_msg.data[0]; + unsigned char * output = &painted_pointcloud_msg.data[0]; + // cppcheck-suppress-begin invalidPointerCast + float p_x = *reinterpret_cast(&data[stride + x_offset]); + float p_y = *reinterpret_cast(&data[stride + y_offset]); + float p_z = *reinterpret_cast(&data[stride + z_offset]); + // cppcheck-suppress-end invalidPointerCast + point_lidar << p_x, p_y, p_z; + point_camera = lidar2cam_affine * point_lidar; + p_x = point_camera.x(); + p_y = point_camera.y(); + p_z = point_camera.z(); + + if (det2d.camera_projector_ptr->isOutsideHorizontalView(p_x, p_z)) { + continue; } -#if 0 - // Parallelizing loop don't support push_back - if (debugger_) { - debug_image_points.push_back(projected_point); + + // project + Eigen::Vector2d projected_point; + if (det2d.camera_projector_ptr->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 (debugger_) { + int thread_id = omp_get_thread_num(); + local_vectors[thread_id].push_back(projected_point); + } + } } -#endif } - } + 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); - } + 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 & local_vec : local_vectors) { + debug_image_points.insert(debug_image_points.end(), local_vec.begin(), local_vec.end()); + } - debugger_->image_rois_ = debug_image_rois; - debugger_->obstacle_points_ = debug_image_points; - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->image_rois_ = debug_image_rois; + debugger_->obstacle_points_ = debug_image_points; + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); + } } } -void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) +void PointPaintingFusionNode::postprocess( + const PointCloudMsgType & painted_pointcloud_msg, DetectedObjects & output_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + diagnostics_interface_ptr_->clear(); + + output_msg.header = painted_pointcloud_msg.header; + output_msg.objects.clear(); const auto objects_sub_count = - obj_pub_ptr_->get_subscription_count() + obj_pub_ptr_->get_intra_process_subscription_count(); + pub_ptr_->get_subscription_count() + pub_ptr_->get_intra_process_subscription_count(); if (objects_sub_count < 1) { return; } @@ -408,10 +408,21 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte } std::vector det_boxes3d; - bool is_success = detector_ptr_->detect(painted_pointcloud_msg, tf_buffer_, det_boxes3d); + bool is_num_pillars_within_range = true; + bool is_success = detector_ptr_->detect( + painted_pointcloud_msg, tf_buffer_, det_boxes3d, is_num_pillars_within_range); if (!is_success) { return; } + diagnostics_interface_ptr_->add_key_value( + "is_num_pillars_within_range", is_num_pillars_within_range); + if (!is_num_pillars_within_range) { + std::stringstream message; + message << "PointPaintingTRT::detect: The actual number of pillars exceeds its maximum value, " + << "which may limit the detection performance."; + diagnostics_interface_ptr_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } std::vector raw_objects; raw_objects.reserve(det_boxes3d.size()); @@ -421,21 +432,18 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte raw_objects.emplace_back(obj); } - autoware_perception_msgs::msg::DetectedObjects output_msg; - output_msg.header = painted_pointcloud_msg.header; + // prepare output message output_msg.objects = iou_bev_nms_.apply(raw_objects); detection_class_remapper_.mapClasses(output_msg); - if (objects_sub_count > 0) { - obj_pub_ptr_->publish(output_msg); + // publish debug message: painted pointcloud + if (point_pub_ptr_->get_subscription_count() > 0) { + point_pub_ptr_->publish(painted_pointcloud_msg); } + diagnostics_interface_ptr_->publish(painted_pointcloud_msg.header.stamp); } -bool PointPaintingFusionNode::out_of_scope(__attribute__((unused)) const DetectedObjects & obj) -{ - return false; -} } // namespace autoware::image_projection_based_fusion #include 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 7520647544684..0a9d9e3391882 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 @@ -41,8 +41,7 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "roi_cluster_fusion", options) +: FusionNode("roi_cluster_fusion", options) { trust_object_iou_mode_ = declare_parameter("trust_object_iou_mode"); non_trust_object_iou_mode_ = declare_parameter("non_trust_object_iou_mode"); @@ -54,9 +53,12 @@ RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) remove_unknown_ = declare_parameter("remove_unknown"); fusion_distance_ = declare_parameter("fusion_distance"); trust_object_distance_ = declare_parameter("trust_object_distance"); + + // publisher + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); } -void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) +void RoiClusterFusionNode::preprocess(ClusterMsgType & output_cluster_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -71,40 +73,14 @@ 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; - } - DetectedObjectsWithFeature known_objects; - known_objects.feature_objects.reserve(output_cluster_msg.feature_objects.size()); - for (auto & feature_object : output_cluster_msg.feature_objects) { - bool is_roi_label_known = feature_object.object.classification.front().label != - autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; - if ( - is_roi_label_known || - feature_object.object.existence_probability >= min_roi_existence_prob_) { - known_objects.feature_objects.push_back(feature_object); - } - } - output_cluster_msg.feature_objects = known_objects.feature_objects; -} - 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 ClusterMsgType & input_cluster_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, ClusterMsgType & 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 = det2d.camera_projector_ptr->getCameraInfo(); // get transform from cluster frame id to camera optical frame id geometry_msgs::msg::TransformStamped transform_stamped; @@ -152,18 +128,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 (det2d.camera_projector_ptr->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); } @@ -173,7 +148,6 @@ void RoiClusterFusionNode::fuseOnSingleImage( } sensor_msgs::msg::RegionOfInterest roi; - // roi.do_rectify = m_camera_info_.at(id).do_rectify; roi.x_offset = min_x; roi.y_offset = min_y; roi.width = max_x - min_x; @@ -236,7 +210,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( // note: debug objects are safely cleared in fusion_node.cpp if (debugger_) { - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); } } @@ -296,6 +270,28 @@ double RoiClusterFusionNode::cal_iou_by_mode( } } +void RoiClusterFusionNode::postprocess( + const ClusterMsgType & processing_msg, ClusterMsgType & output_msg) +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + output_msg = processing_msg; + + if (remove_unknown_) { + // filter by object classification and existence probability + output_msg.feature_objects.clear(); + for (const auto & feature_object : processing_msg.feature_objects) { + if ( + feature_object.object.classification.front().label != + autoware_perception_msgs::msg::ObjectClassification::UNKNOWN || + feature_object.object.existence_probability >= min_roi_existence_prob_) { + output_msg.feature_objects.push_back(feature_object); + } + } + } +} + } // namespace autoware::image_projection_based_fusion #include 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 035bc259ab73c..37769ad73e8c7 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 @@ -31,8 +31,7 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "roi_detected_object_fusion", options) +: FusionNode("roi_detected_object_fusion", options) { fusion_params_.passthrough_lower_bound_probability_thresholds = declare_parameter>("passthrough_lower_bound_probability_thresholds"); @@ -48,6 +47,9 @@ RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptio can_assign_vector.data(), label_num, label_num); fusion_params_.can_assign_matrix = can_assign_matrix_tmp.transpose(); } + + // publisher + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); } void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) @@ -82,16 +84,12 @@ 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))) + const DetectedObjects & input_object_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, DetectedObjects & output_object_msg __attribute__((unused))) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - if (!checkCameraInfo(camera_info)) return; - Eigen::Affine3d object2camera_affine; { const auto transform_stamped_optional = getTransformStamped( @@ -103,12 +101,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, det2d, object2camera_affine); fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map); if (debugger_) { @@ -116,15 +110,14 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( for (std::size_t roi_i = 0; roi_i < input_roi_msg.feature_objects.size(); ++roi_i) { debugger_->image_rois_.push_back(input_roi_msg.feature_objects.at(roi_i).feature.roi); } - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); } } 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 Det2dStatus & det2d, + const Eigen::Affine3d & object2camera_affine) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -139,6 +132,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 = det2d.camera_projector_ptr->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); @@ -165,18 +162,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 (det2d.camera_projector_ptr->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_) { @@ -294,14 +290,15 @@ bool RoiDetectedObjectFusionNode::out_of_scope(const DetectedObject & obj) return is_out; } -void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) +void RoiDetectedObjectFusionNode::postprocess( + const DetectedObjects & processing_msg, DetectedObjects & output_msg) { - if (pub_ptr_->get_subscription_count() < 1) { - return; - } + output_msg.header = processing_msg.header; + output_msg.objects.clear(); - int64_t timestamp_nsec = - output_msg.header.stamp.sec * static_cast(1e9) + output_msg.header.stamp.nanosec; + // filter out ignored objects + int64_t timestamp_nsec = processing_msg.header.stamp.sec * static_cast(1e9) + + processing_msg.header.stamp.nanosec; if ( passthrough_object_flags_map_.size() == 0 || fused_object_flags_map_.size() == 0 || ignored_object_flags_map_.size() == 0) { @@ -313,33 +310,34 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) ignored_object_flags_map_.count(timestamp_nsec) == 0) { return; } + auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); auto & fused_object_flags = fused_object_flags_map_.at(timestamp_nsec); - auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); - - DetectedObjects output_objects_msg, debug_fused_objects_msg, debug_ignored_objects_msg; - output_objects_msg.header = output_msg.header; - debug_fused_objects_msg.header = output_msg.header; - debug_ignored_objects_msg.header = output_msg.header; - for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) { - const auto & obj = output_msg.objects.at(obj_i); - if (passthrough_object_flags.at(obj_i)) { - output_objects_msg.objects.emplace_back(obj); + for (std::size_t obj_i = 0; obj_i < processing_msg.objects.size(); ++obj_i) { + const auto & obj = processing_msg.objects.at(obj_i); + if (passthrough_object_flags.at(obj_i) || fused_object_flags.at(obj_i)) { + output_msg.objects.emplace_back(obj); } + } + + // debug messages + auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); + DetectedObjects debug_fused_objects_msg, debug_ignored_objects_msg; + debug_fused_objects_msg.header = processing_msg.header; + debug_ignored_objects_msg.header = processing_msg.header; + for (std::size_t obj_i = 0; obj_i < processing_msg.objects.size(); ++obj_i) { + const auto & obj = processing_msg.objects.at(obj_i); if (fused_object_flags.at(obj_i)) { - output_objects_msg.objects.emplace_back(obj); debug_fused_objects_msg.objects.emplace_back(obj); } if (ignored_object_flags.at(obj_i)) { debug_ignored_objects_msg.objects.emplace_back(obj); } } - - pub_ptr_->publish(output_objects_msg); - debug_publisher_->publish("debug/fused_objects", debug_fused_objects_msg); debug_publisher_->publish("debug/ignored_objects", debug_ignored_objects_msg); + // clear flags passthrough_object_flags_map_.erase(timestamp_nsec); fused_object_flags_map_.erase(timestamp_nsec); ignored_object_flags_map_.erase(timestamp_nsec); 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 206a5f77a0235..b1eef00053946 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 @@ -37,60 +37,23 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "roi_pointcloud_fusion", options) +: FusionNode("roi_pointcloud_fusion", options) { fuse_unknown_only_ = declare_parameter("fuse_unknown_only"); min_cluster_size_ = declare_parameter("min_cluster_size"); max_cluster_size_ = declare_parameter("max_cluster_size"); cluster_2d_tolerance_ = declare_parameter("cluster_2d_tolerance"); - pub_objects_ptr_ = - this->create_publisher("output_clusters", rclcpp::QoS{1}); - cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); -} -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; + // publisher + point_pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); + pub_ptr_ = this->create_publisher("output_clusters", rclcpp::QoS{1}); + cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); } -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) { - return; - } - - DetectedObjectsWithFeature output_msg; - output_msg.header = pointcloud_msg.header; - output_msg.feature_objects = output_fused_objects_; - - if (objects_sub_count > 0) { - pub_objects_ptr_->publish(output_msg); - } - output_fused_objects_.clear(); - // publish debug cluster - if (cluster_debug_pub_->get_subscription_count() > 0) { - sensor_msgs::msg::PointCloud2 debug_cluster_msg; - autoware::euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); - cluster_debug_pub_->publish(debug_cluster_msg); - } -} void RoiPointCloudFusionNode::fuseOnSingleImage( - 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, - __attribute__((unused)) sensor_msgs::msg::PointCloud2 & output_pointcloud_msg) + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, + __attribute__((unused)) PointCloudMsgType & output_pointcloud_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -98,7 +61,6 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( if (input_pointcloud_msg.data.empty()) { return; } - if (!checkCameraInfo(camera_info)) return; std::vector output_objs; std::vector debug_image_rois; @@ -122,10 +84,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( @@ -136,15 +94,18 @@ 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; - - sensor_msgs::msg::PointCloud2 transformed_cloud; + 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; + + PointCloudMsgType transformed_cloud; tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); - std::vector clusters; + std::vector clusters; std::vector clusters_data_size; clusters.resize(output_objs.size()); for (auto & cluster : clusters) { @@ -164,33 +125,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 (det2d.camera_projector_ptr->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); } } @@ -203,14 +167,40 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( if (debugger_) { debugger_->image_rois_ = debug_image_rois; debugger_->obstacle_points_ = debug_image_points; - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); + } +} + +void RoiPointCloudFusionNode::postprocess( + const PointCloudMsgType & pointcloud_msg, ClusterMsgType & output_msg) +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + output_msg.header = pointcloud_msg.header; + output_msg.feature_objects = output_fused_objects_; + + output_fused_objects_.clear(); + + // publish debug cluster + if (cluster_debug_pub_->get_subscription_count() > 0) { + PointCloudMsgType debug_cluster_msg; + autoware::euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); + cluster_debug_pub_->publish(debug_cluster_msg); + } + if (point_pub_ptr_->get_subscription_count() > 0) { + point_pub_ptr_->publish(pointcloud_msg); } } -bool RoiPointCloudFusionNode::out_of_scope(__attribute__((unused)) - const DetectedObjectWithFeature & obj) +void RoiPointCloudFusionNode::publish(const ClusterMsgType & output_msg) { - return false; + const auto objects_sub_count = + pub_ptr_->get_subscription_count() + pub_ptr_->get_intra_process_subscription_count(); + if (objects_sub_count < 1) { + return; + } + pub_ptr_->publish(output_msg); } } // namespace autoware::image_projection_based_fusion 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 afeff213c0afe..7d63cac7273c6 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 @@ -36,7 +36,7 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("segmentation_pointcloud_fusion", options) +: FusionNode("segmentation_pointcloud_fusion", options) { filter_distance_threshold_ = declare_parameter("filter_distance_threshold"); for (auto & item : filter_semantic_label_target_list_) { @@ -48,48 +48,24 @@ SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptio } is_publish_debug_mask_ = declare_parameter("is_publish_debug_mask"); pub_debug_mask_ptr_ = image_transport::create_publisher(this, "~/debug/mask"); -} - -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; + // publisher + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); } -void SegmentPointCloudFusionNode::postprocess(PointCloud2 & pointcloud_msg) +void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) + PointCloudMsgType & 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; - size_t output_pointcloud_size = 0; - pointcloud_msg.data.clear(); - pointcloud_msg.data.resize(original_cloud->data.size()); - - for (size_t global_offset = 0; global_offset < original_cloud->data.size(); - global_offset += point_step) { - if (filter_global_offset_set_.find(global_offset) == filter_global_offset_set_.end()) { - copyPointCloud( - *original_cloud, point_step, global_offset, pointcloud_msg, output_pointcloud_size); - } - } - - pointcloud_msg.data.resize(output_pointcloud_size); - pointcloud_msg.row_step = output_pointcloud_size / pointcloud_msg.height; - pointcloud_msg.width = output_pointcloud_size / pointcloud_msg.point_step / pointcloud_msg.height; - - filter_global_offset_set_.clear(); return; } 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 PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + [[maybe_unused]] const Image & input_mask, + __attribute__((unused)) PointCloudMsgType & output_cloud) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -97,10 +73,11 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( 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 = det2d.camera_projector_ptr->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); @@ -115,8 +92,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 @@ -130,7 +105,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( transform_stamped = transform_stamped_optional.value(); } - PointCloud2 transformed_cloud; + PointCloudMsgType transformed_cloud; tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); int point_step = input_pointcloud_msg.point_step; @@ -151,13 +126,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 (!det2d.camera_projector_ptr->calcImageProjectedPoint( + cv::Point3d(transformed_x, transformed_y, transformed_z), projected_point)) { continue; } @@ -174,11 +145,33 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( } } -bool SegmentPointCloudFusionNode::out_of_scope(__attribute__((unused)) - const PointCloud2 & filtered_cloud) +void SegmentPointCloudFusionNode::postprocess( + const PointCloudMsgType & pointcloud_msg, PointCloudMsgType & output_msg) { - return false; + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + output_msg.header = pointcloud_msg.header; + output_msg.data.clear(); + output_msg.data.resize(pointcloud_msg.data.size()); + const int point_step = pointcloud_msg.point_step; + + size_t output_pointcloud_size = 0; + for (size_t global_offset = 0; global_offset < pointcloud_msg.data.size(); + global_offset += point_step) { + if (filter_global_offset_set_.find(global_offset) == filter_global_offset_set_.end()) { + copyPointCloud(pointcloud_msg, point_step, global_offset, output_msg, output_pointcloud_size); + } + } + + output_msg.data.resize(output_pointcloud_size); + output_msg.row_step = output_pointcloud_size / output_msg.height; + output_msg.width = output_pointcloud_size / output_msg.point_step / output_msg.height; + + filter_global_offset_set_.clear(); + return; } + } // namespace autoware::image_projection_based_fusion #include 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..81424d4c23a34 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) @@ -81,8 +67,8 @@ Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t) } void closest_cluster( - const PointCloud2 & cluster, const double cluster_2d_tolerance, const int min_cluster_size, - const pcl::PointXYZ & center, PointCloud2 & out_cluster) + const PointCloudMsgType & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center, PointCloudMsgType & out_cluster) { // sort point by distance to camera origin @@ -137,8 +123,8 @@ void closest_cluster( } void updateOutputFusedObjects( - std::vector & output_objs, std::vector & clusters, - const std::vector & clusters_data_size, const PointCloud2 & in_cloud, + std::vector & output_objs, std::vector & clusters, + const std::vector & clusters_data_size, const PointCloudMsgType & in_cloud, const std_msgs::msg::Header & in_roi_header, const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const int max_cluster_size, const float cluster_2d_tolerance, std::vector & output_fused_objects) @@ -176,7 +162,7 @@ void updateOutputFusedObjects( // TODO(badai-nguyen): change to interface to select refine criteria like closest, largest // to output refine cluster and centroid - sensor_msgs::msg::PointCloud2 refine_cluster; + PointCloudMsgType refine_cluster; closest_cluster( cluster, cluster_2d_tolerance, min_cluster_size, camera_orig_point_frame, refine_cluster); if ( @@ -198,7 +184,7 @@ void updateOutputFusedObjects( } } -geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) +geometry_msgs::msg::Point getCentroid(const PointCloudMsgType & pointcloud) { geometry_msgs::msg::Point centroid; centroid.x = 0.0f; diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp index ce5ec3ea0abfe..671ff54f8ce0d 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp @@ -62,7 +62,7 @@ class CenterPointTRT bool detect( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d); + std::vector & det_boxes3d, bool & is_num_pillars_within_range); protected: void initPtr(); diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp index a49451fde9d0d..1748db0e48392 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp @@ -20,6 +20,7 @@ #include "autoware/lidar_centerpoint/postprocess/non_maximum_suppression.hpp" #include +#include #include #include #include @@ -59,6 +60,7 @@ class LidarCenterPointNode : public rclcpp::Node DetectionClassRemapper detection_class_remapper_; std::unique_ptr detector_ptr_{nullptr}; + std::unique_ptr diagnostics_interface_ptr_; // debugger std::unique_ptr> stop_watch_ptr_{ diff --git a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp index 6ee2b3733bdb0..530f59179d25b 100644 --- a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp @@ -19,6 +19,7 @@ #include "autoware/lidar_centerpoint/preprocess/preprocess_kernel.hpp" #include +#include #include #include @@ -127,8 +128,10 @@ void CenterPointTRT::initPtr() bool CenterPointTRT::detect( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d) + std::vector & det_boxes3d, bool & is_num_pillars_within_range) { + is_num_pillars_within_range = true; + CHECK_CUDA_ERROR(cudaMemsetAsync( encoder_in_features_d_.get(), 0, encoder_in_feature_size_ * sizeof(float), stream_)); CHECK_CUDA_ERROR( @@ -155,6 +158,7 @@ bool CenterPointTRT::detect( "The actual number of pillars (%u) exceeds its maximum value (%zu). " "Please considering increasing it since it may limit the detection performance.", num_pillars, config_.max_voxel_size_); + is_num_pillars_within_range = false; } return true; diff --git a/perception/autoware_lidar_centerpoint/lib/preprocess/preprocess_kernel.cu b/perception/autoware_lidar_centerpoint/lib/preprocess/preprocess_kernel.cu index f300411a44aad..95ac7b4353e90 100644 --- a/perception/autoware_lidar_centerpoint/lib/preprocess/preprocess_kernel.cu +++ b/perception/autoware_lidar_centerpoint/lib/preprocess/preprocess_kernel.cu @@ -148,6 +148,8 @@ __global__ void generateVoxels_random_kernel( int voxel_idx = floorf((point.x - min_x_range) / pillar_x_size); int voxel_idy = floorf((point.y - min_y_range) / pillar_y_size); + voxel_idx = voxel_idx < 0 ? 0 : voxel_idx >= grid_x_size ? grid_x_size - 1 : voxel_idx; + voxel_idy = voxel_idy < 0 ? 0 : voxel_idy >= grid_y_size ? grid_y_size - 1 : voxel_idy; unsigned int voxel_index = (grid_x_size - 1 - voxel_idx) * grid_y_size + voxel_idy; unsigned int point_id = atomicAdd(&(mask[voxel_index]), 1); diff --git a/perception/autoware_lidar_centerpoint/src/node.cpp b/perception/autoware_lidar_centerpoint/src/node.cpp index 59acceeac54d6..825fc40234120 100644 --- a/perception/autoware_lidar_centerpoint/src/node.cpp +++ b/perception/autoware_lidar_centerpoint/src/node.cpp @@ -107,6 +107,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti circle_nms_dist_threshold, yaw_norm_thresholds, has_variance_); detector_ptr_ = std::make_unique(encoder_param, head_param, densification_param, config); + diagnostics_interface_ptr_ = + std::make_unique(this, "centerpoint_trt"); pointcloud_sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1), @@ -144,12 +146,24 @@ void LidarCenterPointNode::pointCloudCallback( if (stop_watch_ptr_) { stop_watch_ptr_->toc("processing_time", true); } + diagnostics_interface_ptr_->clear(); std::vector det_boxes3d; - bool is_success = detector_ptr_->detect(*input_pointcloud_msg, tf_buffer_, det_boxes3d); + bool is_num_pillars_within_range = true; + bool is_success = detector_ptr_->detect( + *input_pointcloud_msg, tf_buffer_, det_boxes3d, is_num_pillars_within_range); if (!is_success) { return; } + diagnostics_interface_ptr_->add_key_value( + "is_num_pillars_within_range", is_num_pillars_within_range); + if (!is_num_pillars_within_range) { + std::stringstream message; + message << "CenterPointTRT::detect: The actual number of pillars exceeds its maximum value, " + << "which may limit the detection performance."; + diagnostics_interface_ptr_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } std::vector raw_objects; raw_objects.reserve(det_boxes3d.size()); @@ -169,6 +183,7 @@ void LidarCenterPointNode::pointCloudCallback( objects_pub_->publish(output_msg); published_time_publisher_->publish_if_subscribed(objects_pub_, output_msg.header.stamp); } + diagnostics_interface_ptr_->publish(input_pointcloud_msg->header.stamp); // add processing time for debug if (debug_publisher_ptr_ && stop_watch_ptr_) { diff --git a/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu b/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu index 49131218ff698..a89268e646cfc 100644 --- a/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu +++ b/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu @@ -176,6 +176,8 @@ __global__ void generateVoxels_random_kernel( int voxel_idx = floorf((x - min_x_range) / pillar_x_size); int voxel_idy = floorf((y - min_y_range) / pillar_y_size); + voxel_idx = voxel_idx < 0 ? 0 : voxel_idx >= grid_x_size ? grid_x_size - 1 : voxel_idx; + voxel_idy = voxel_idy < 0 ? 0 : voxel_idy >= grid_y_size ? grid_y_size - 1 : voxel_idy; unsigned int voxel_index = voxel_idy * grid_x_size + voxel_idx; unsigned int point_id = atomicAdd(&(mask[voxel_index]), 1); diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 1675f8b1fbfa2..b047cd28114ba 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -71,8 +71,8 @@ struct hash } // namespace std namespace autoware::map_based_prediction { +using autoware_internal_debug_msgs::msg::StringStamped; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_debug_msgs::msg::StringStamped; using TrajectoryPoints = std::vector; class MapBasedPredictionNode : public rclcpp::Node diff --git a/perception/autoware_map_based_prediction/package.xml b/perception/autoware_map_based_prediction/package.xml index 9012590a45b14..58dfcb17c7631 100644 --- a/perception/autoware_map_based_prediction/package.xml +++ b/perception/autoware_map_based_prediction/package.xml @@ -16,6 +16,7 @@ ament_cmake autoware_cmake + autoware_internal_debug_msgs autoware_interpolation autoware_lanelet2_extension autoware_motion_utils @@ -27,7 +28,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_debug_msgs unique_identifier_msgs visualization_msgs diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index 42dbf5b83fa8c..b65020d6b4708 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -670,9 +670,9 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt if (stop_watch_ptr_) { const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true); const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } 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 69a9edd9c9d9e..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 @@ -235,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 @@ -260,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); @@ -269,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 c65efffe99a47..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 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..3761fc1e6f43a 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp @@ -127,7 +127,7 @@ void TrackerDebugger::startMeasurementTime( stamp_process_start_ = now; if (debug_settings_.publish_processing_time) { double input_latency_ms = (now - last_input_stamp_).seconds() * 1e3; - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/input_latency_ms", input_latency_ms); } // initialize debug time stamps @@ -169,15 +169,15 @@ void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Tim double measurement_to_object_ms = (object_time - last_input_stamp_).seconds() * 1e3; // starting from the measurement time - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms_); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_time_ms", processing_time_ms); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_latency_ms", processing_latency_ms); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/meas_to_tracked_object_ms", measurement_to_object_ms); } stamp_publish_output_ = now; @@ -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 d5b12ed7b2b82..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 @@ -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/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_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_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_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/planning/README.md b/planning/README.md index ccf8288df3911..3fadafe54ed4c 100644 --- a/planning/README.md +++ b/planning/README.md @@ -18,7 +18,7 @@ Enabling and disabling modules involves managing settings in key configuration a ### Key Files for Configuration -The `default_preset.yaml` file acts as the primary configuration file, where planning modules can be disable or enabled. Furthermore, users can also set the type of motion planner across various motion planners. For example: +The `default_preset.yaml` file acts as the primary configuration file, where planning modules can be disabled or enabled. Furthermore, users can also set the type of motion planner across various motion planners. For example: - `launch_avoidance_module`: Set to `true` to enable the avoidance module, or `false` to disable it. - `motion_stop_planner_type`: Set `default` to either `obstacle_stop_planner` or `obstacle_cruise_planner`. @@ -35,7 +35,7 @@ The [launch files](https://github.com/autowarefoundation/autoware.universe/tree/ corresponds to launch_avoidance_module from `default_preset.yaml`. -### Parameters configuration +### Parameter Configuration There are multiple parameters available for configuration, and users have the option to modify them in [here](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning). It's important to note that not all parameters are adjustable via `rqt_reconfigure`. To ensure the changes are effective, modify the parameters and then restart Autoware. Additionally, detailed information about each parameter is available in the corresponding documents under the planning tab. @@ -43,7 +43,7 @@ There are multiple parameters available for configuration, and users have the op This guide outlines the steps for integrating your custom module into Autoware: -- Add your modules to the `default_preset.yaml` file. For example +- Add your modules to the `default_preset.yaml` file. For example: ```yaml - arg: @@ -51,7 +51,7 @@ This guide outlines the steps for integrating your custom module into Autoware: default: "true" ``` -- Incorporate your modules into the [launcher](https://github.com/autowarefoundation/autoware.universe/tree/main/launch/tier4_planning_launch/launch/scenario_planning). For example in [behavior_planning.launch.xml](https://github.com/autowarefoundation/autoware.universe/blob/main/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml): +- Incorporate your modules into the [launcher](https://github.com/autowarefoundation/autoware.universe/tree/main/launch/tier4_planning_launch/launch/scenario_planning). For example, in [behavior_planning.launch.xml](https://github.com/autowarefoundation/autoware.universe/blob/main/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml): ```xml @@ -63,14 +63,14 @@ This guide outlines the steps for integrating your custom module into Autoware: /> ``` -- If applicable, place your parameter folder within the appropriate existing parameter folder. For example [intersection_module's parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml) is in [behavior_velocity_planner](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner). -- Insert the path of your parameters in the [tier4_planning_component.launch.xml](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/launch/components/tier4_planning_component.launch.xml). For example `behavior_velocity_planner_intersection_module_param_path` is used. +- If applicable, place your parameter folder within the appropriate existing parameter folder. For example, [intersection_module's parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml) are in [behavior_velocity_planner](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner). +- Insert the path of your parameters in the [tier4_planning_component.launch.xml](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/launch/components/tier4_planning_component.launch.xml). For example, `behavior_velocity_planner_intersection_module_param_path` is used. ```xml ``` -- Define your parameter path variable within the corresponding launcher. For example in [behavior_planning.launch.xml](https://github.com/autowarefoundation/autoware.universe/blob/04aa54bf5fb0c88e70198ca74b9ac343cc3457bf/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml#L191) +- Define your parameter path variable within the corresponding launcher. For example, in [behavior_planning.launch.xml](https://github.com/autowarefoundation/autoware.universe/blob/04aa54bf5fb0c88e70198ca74b9ac343cc3457bf/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml#L191) ```xml @@ -82,7 +82,7 @@ This guide outlines the steps for integrating your custom module into Autoware: ## Join Our Community-Driven Effort -Autoware thrives on community collaboration. Every contribution, big or small, is invaluable to us. Whether it's reporting bugs, suggesting improvements, offering new ideas, or anything else you can think of – we welcome it all with open arms. +Autoware thrives on community collaboration. Every contribution, big or small, is invaluable to us. Whether it's reporting bugs, suggesting improvements, offering new ideas, or anything else you can think of – we welcome all contributions with open arms. ### How to Contribute? @@ -105,7 +105,7 @@ Interested in joining our meetings? We’d love to have you! For more informatio Occasionally, we publish papers specific to the Planning Component in Autoware. We encourage you to explore these publications and find valuable insights for your work. If you find them useful and incorporate any of our methodologies or algorithms in your projects, citing our papers would be immensely helpful. This support allows us to reach a broader audience and continue contributing to the field. -If you use the Jerk Constrained Velocity Planning algorithm in [Motion Velocity Smoother](./autoware_velocity_smoother/README.md) module in the Planning Component, we kindly request you to cite the relevant paper. +If you use the Jerk Constrained Velocity Planning algorithm in the [Motion Velocity Smoother](./autoware_velocity_smoother/README.md) module in the Planning Component, we kindly request you cite the relevant paper. 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_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_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_scenario_selector/src/node.cpp b/planning/autoware_scenario_selector/src/node.cpp index 4bb64e27368d8..defbde4f6aabc 100644 --- a/planning/autoware_scenario_selector/src/node.cpp +++ b/planning/autoware_scenario_selector/src/node.cpp @@ -156,9 +156,8 @@ std::string ScenarioSelectorNode::selectScenarioByPosition() return tier4_planning_msgs::msg::Scenario::LANEDRIVING; } else if (is_in_parking_lot) { return tier4_planning_msgs::msg::Scenario::PARKING; - } else { - return tier4_planning_msgs::msg::Scenario::LANEDRIVING; } + return tier4_planning_msgs::msg::Scenario::LANEDRIVING; } if (current_scenario_ == tier4_planning_msgs::msg::Scenario::LANEDRIVING) { diff --git a/planning/autoware_static_centerline_generator/CHANGELOG.rst b/planning/autoware_static_centerline_generator/CHANGELOG.rst deleted file mode 100644 index 0338cdc8b66bb..0000000000000 --- a/planning/autoware_static_centerline_generator/CHANGELOG.rst +++ /dev/null @@ -1,287 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_static_centerline_generator -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix(static_centerline_generator): several bug fixes (`#9426 `_) - * fix: dependent packages - * feat: use steer angle, use warn for steer angle failure, calc curvature dicontinuously - * fix cppcheck - --------- -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - planning (`#9570 `_) -* refactor(global_parameter_loader): prefix package and namespace with autoware (`#9303 `_) -* feat!: replace tier4_map_msgs with autoware_map_msgs for MapProjectorInfo (`#9392 `_) -* refactor(map_loader)!: prefix package and namespace with autoware (`#8927 `_) - * make lanelet2_map_visualization independent - * remove unused files - * remove unused package - * fix package name - * add autoware\_ prefix - * add autoware to exec name - * add autoware prefix - * removed unnecessary dependency - --------- -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix(static_centerline_generator): map_tf_generator package name needs update (`#9383 `_) - fix map_tf_generator name in autoware_static_centerline_generator.launch -* refactor(map_loader)!: prefix package and namespace with autoware (`#8927 `_) - * make lanelet2_map_visualization independent - * remove unused files - * remove unused package - * fix package name - * add autoware\_ prefix - * add autoware to exec name - * add autoware prefix - * removed unnecessary dependency - --------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Masaki Baba, Ryohsuke Mitsudome, Takayuki Murooka, Yutaka Kondo, Zhanhong Yan - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(osqp_interface): added autoware prefix to osqp_interface (`#8958 `_) -* refactor(autoware_interpolation): prefix package and namespace with autoware (`#8088 `_) - Co-authored-by: kosuke55 -* fix(other_planning_packages): align the parameters with launcher (`#8793 `_) - * parameters in planning/others aligned - * update json - --------- -* refactor(map_projection_loader)!: prefix package and namespace with autoware (`#8420 `_) - * add autoware\_ prefix - * add autoware\_ prefix - --------- - Co-authored-by: SakodaShintaro -* fix(autoware_static_centerline_generator): fix unusedFunction (`#8647 `_) - * fix:unusedFunction - * fix:unusedFunction - * fix:compile error - --------- -* refactor(geography_utils): prefix package and namespace with autoware (`#7790 `_) - * refactor(geography_utils): prefix package and namespace with autoware - * move headers to include/autoware/ - --------- -* fix(autoware_static_centerline_generator): fix funcArgNamesDifferent (`#8019 `_) - fix:funcArgNamesDifferent -* fix(static_centerline_generator): save_map only once (`#7770 `_) -* refactor(static_centerline_optimizer): clean up the code (`#7756 `_) -* feat: add `autoware\_` prefix to `lanelet2_extension` (`#7640 `_) -* feat(static_centerline_generator): organize AUTO/GUI/VMB modes (`#7432 `_) -* refactor(universe_utils/motion_utils)!: add autoware namespace (`#7594 `_) -* refactor(motion_utils)!: add autoware prefix and include dir (`#7539 `_) - refactor(motion_utils): add autoware prefix and include dir -* feat(autoware_universe_utils)!: rename from tier4_autoware_utils (`#7538 `_) - Co-authored-by: kosuke55 -* refactor(route_handler)!: rename to include/autoware/{package_name} (`#7530 `_) - refactor(route_handler)!: rename to include/autoware/{package_name} -* feat(map_loader): add waypoints flag (`#7480 `_) - * feat(map_loader): handle centelrine and waypoints - * update README - * fix doc - * update schema - * fix - * fix - --------- -* feat(path_optimizer): rename to include/autoware/{package_name} (`#7529 `_) -* feat(path_smoother): rename to include/autoware/{package_name} (`#7527 `_) - * feat(path_smoother): rename to include/autoware/{package_name} - * fix - --------- -* refactor(behaivor_path_planner)!: rename to include/autoware/{package_name} (`#7522 `_) - * refactor(behavior_path_planner)!: make autoware dir in include - * refactor(start_planner): make autoware include dir - * refactor(goal_planner): make autoware include dir - * sampling planner module - * fix sampling planner build - * dynamic_avoidance - * lc - * side shift - * autoware_behavior_path_static_obstacle_avoidance_module - * autoware_behavior_path_planner_common - * make behavior_path dir - * pre-commit - * fix pre-commit - * fix build - --------- -* feat(mission_planner): rename to include/autoware/{package_name} (`#7513 `_) - * feat(mission_planner): rename to include/autoware/{package_name} - * feat(mission_planner): rename to include/autoware/{package_name} - * feat(mission_planner): rename to include/autoware/{package_name} - --------- -* fix(static_centerline_generator): fix dependency (`#7442 `_) - * fix: deps - * fix: package name - * fix: package name - --------- -* refactor(route_handler): route handler add autoware prefix (`#7341 `_) - * rename route handler package - * update packages dependencies - * update include guards - * update includes - * put in autoware namespace - * fix formats - * keep header and source file name as before - --------- -* refactor(mission_planner)!: add autoware prefix and namespace (`#7414 `_) - * refactor(mission_planner)!: add autoware prefix and namespace - * fix svg - --------- -* refactor(vehicle_info_utils)!: prefix package and namespace with autoware (`#7353 `_) - * chore(autoware_vehicle_info_utils): rename header - * chore(bpp-common): vehicle info - * chore(path_optimizer): vehicle info - * chore(velocity_smoother): vehicle info - * chore(bvp-common): vehicle info - * chore(static_centerline_generator): vehicle info - * chore(obstacle_cruise_planner): vehicle info - * chore(obstacle_velocity_limiter): vehicle info - * chore(mission_planner): vehicle info - * chore(obstacle_stop_planner): vehicle info - * chore(planning_validator): vehicle info - * chore(surround_obstacle_checker): vehicle info - * chore(goal_planner): vehicle info - * chore(start_planner): vehicle info - * chore(control_performance_analysis): vehicle info - * chore(lane_departure_checker): vehicle info - * chore(predicted_path_checker): vehicle info - * chore(vehicle_cmd_gate): vehicle info - * chore(obstacle_collision_checker): vehicle info - * chore(operation_mode_transition_manager): vehicle info - * chore(mpc): vehicle info - * chore(control): vehicle info - * chore(common): vehicle info - * chore(perception): vehicle info - * chore(evaluator): vehicle info - * chore(freespace): vehicle info - * chore(planning): vehicle info - * chore(vehicle): vehicle info - * chore(simulator): vehicle info - * chore(launch): vehicle info - * chore(system): vehicle info - * chore(sensing): vehicle info - * fix(autoware_joy_controller): remove unused deps - --------- -* refactor(path_smoother)!: prefix package and namespace with autoware (`#7381 `_) - * git mv - * fix - * fix launch - * rever a part of prefix - * fix test - * fix - * fix static_centerline_optimizer - * fix - --------- -* refactor(path_optimizer, velocity_smoother)!: prefix package and namespace with autoware (`#7354 `_) - * chore(autoware_velocity_smoother): update namespace - * chore(autoware_path_optimizer): update namespace - --------- -* chore(bpp): add prefix `autoware\_` (`#7288 `_) - * chore(common): rename package - * fix(static_obstacle_avoidance): fix header - * fix(dynamic_obstacle_avoidance): fix header - * fix(side_shift): fix header - * fix(sampling_planner): fix header - * fix(start_planner): fix header - * fix(goal_planner): fix header - * fix(lane_change): fix header - * fix(external_lane_change): fix header - * fix(AbLC): fix header - * fix(bpp-node): fix header - * fix(static_centerline_generator): fix header - * fix(.pages): update link - --------- -* feat!: replace autoware_auto_msgs with autoware_msgs for planning modules (`#7246 `_) - Co-authored-by: Cynthia Liu - Co-authored-by: NorahXiong - Co-authored-by: beginningfan -* chore(autoware_velocity_smoother, autoware_path_optimizer): rename packages (`#7202 `_) - * chore(autoware_path_optimizer): rename package and namespace - * chore(autoware_static_centerline_generator): rename package and namespace - * chore: update module name - * chore(autoware_velocity_smoother): rename package and namespace - * chore(tier4_planning_launch): update module name - * chore: update module name - * fix: test - * fix: test - * fix: test - --------- -* refactor(behavior_velocity_planner)!: prefix package and namespace with autoware\_ (`#6693 `_) -* fix(autoware_static_centerline_generator): update the centerline correctly with map projector (`#6825 `_) - * fix(static_centerline_generator): fixed the bug of offset lat/lon values - * fix typo - --------- -* fix(autoware_static_centerline_generator): remove prefix from topics and node names (`#7028 `_) -* build(static_centerline_generator): prefix package and namespace with autoware\_ (`#6817 `_) - * build(static_centerline_generator): prefix package and namespace with autoware\_ - * style(pre-commit): autofix - * build: fix CMake target - * build(autoware_static_centerline_generator): more renames - * style(pre-commit): autofix - * build(autoware_static_centerline_generator): fix namespace - * fix(autoware_static_centerline_generator): fix clang-tidy issues - * style(pre-commit): autofix - * style(pre-commit): autofix - * fix(autoware_static_centerline_generator): fix clang-tidy issues - * fix(autoware_static_centerline_generator): fix build issues - * fix(autoware_static_centerline_generator): fix build issues - * style(pre-commit): autofix - * fix(autoware_static_centerline_optimizer): fix clang-tidy issues - * style(pre-commit): autofix - * build: fix build errors - * fix: remove else statements after return - * fix(autoware_static_centerline_generator): fix clang-tidy issues - * style(pre-commit): autofix - * revert changes for static_centerline_generator - * fix(autoware_static_centerline_generator): add autoware\_ prefix - * style(pre-commit): autofix - * fix(autoware_static_centerline_generator): fix filenames - * fix(autoware_static_centerline_generator): fix namespaces - * style(pre-commit): autofix - * fix: added prefix to missing strings - * refactor(autoware_static_centerline_generator): move header files to src - * refactor(autoware_static_centerline_generator): fix include paths - * style(pre-commit): autofix - * refactor(autoware_static_centerline_generator): rename base folder - * Update planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml - Co-authored-by: M. Fatih Cırıt - * build(autoware_static_centerline_generator): fix include in CMake - * build(autoware_static_centerline_generator): fix missing includes - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> - Co-authored-by: M. Fatih Cırıt -* Contributors: Esteve Fernandez, Kosuke Takeuchi, Masaki Baba, Ryohsuke Mitsudome, Satoshi OTA, Takayuki Murooka, Yutaka Kondo, Zhe Shen, kobayu858, mkquda - -0.26.0 (2024-04-03) -------------------- diff --git a/planning/autoware_static_centerline_generator/CMakeLists.txt b/planning/autoware_static_centerline_generator/CMakeLists.txt deleted file mode 100644 index 261e8beb0022f..0000000000000 --- a/planning/autoware_static_centerline_generator/CMakeLists.txt +++ /dev/null @@ -1,66 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_static_centerline_generator) - -find_package(autoware_cmake REQUIRED) - -find_package(builtin_interfaces REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(std_msgs REQUIRED) - -rosidl_generate_interfaces( - autoware_static_centerline_generator - "srv/LoadMap.srv" - "srv/PlanRoute.srv" - "srv/PlanPath.srv" - "msg/PointsWithLaneId.msg" - DEPENDENCIES builtin_interfaces geometry_msgs -) - -autoware_package() - -ament_auto_add_executable(main - src/main.cpp - src/static_centerline_generator_node.cpp - src/centerline_source/optimization_trajectory_based_centerline.cpp - src/centerline_source/bag_ego_trajectory_based_centerline.cpp - src/utils.cpp -) - -if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(main - autoware_static_centerline_generator "rosidl_typesupport_cpp") -else() - rosidl_get_typesupport_target( - cpp_typesupport_target autoware_static_centerline_generator "rosidl_typesupport_cpp") - target_link_libraries(main "${cpp_typesupport_target}") -endif() - -ament_auto_package( - INSTALL_TO_SHARE - launch - config - rviz -) - -target_include_directories(main PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/src -) - -if(BUILD_TESTING) - add_launch_test( - test/test_static_centerline_generator.test.py - TIMEOUT "30" - ) - install(DIRECTORY - test/data/ - DESTINATION share/${PROJECT_NAME}/test/data/ - ) -endif() - -install(PROGRAMS - scripts/app.py - scripts/centerline_updater_helper.py - scripts/show_lanelet2_map_diff.py - DESTINATION lib/${PROJECT_NAME} -) diff --git a/planning/autoware_static_centerline_generator/README.md b/planning/autoware_static_centerline_generator/README.md deleted file mode 100644 index 426d5487cf0cb..0000000000000 --- a/planning/autoware_static_centerline_generator/README.md +++ /dev/null @@ -1,83 +0,0 @@ -# Static Centerline Generator - -## Purpose - -This package statically calculates the centerline satisfying path footprints inside the drivable area. - -On narrow-road driving, the default centerline, which is the middle line between lanelets' right and left boundaries, often causes path footprints outside the drivable area. -To make path footprints inside the drivable area, we use online path shape optimization by [the autoware_path_optimizer package](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/autoware_path_optimizer/). - -Instead of online path shape optimization, we introduce static centerline optimization. -With this static centerline optimization, we have following advantages. - -- We can see the optimized centerline shape in advance. - - With the default autoware, path shape is not determined until the vehicle drives there. - - This enables offline path shape evaluation. -- We do not have to calculate a heavy and sometimes unstable path optimization since the path footprints are already inside the drivable area. - -## Use cases - -There are two interfaces to communicate with the centerline optimizer. - -### Vector Map Builder Interface - -Note: This function of Vector Map Builder has not been released. Please wait for a while. -Currently there is no documentation about Vector Map Builder's operation for this function. - -The optimized centerline can be generated from Vector Map Builder's operation. - -We can run - -- path planning server -- http server to connect path planning server and Vector Map Builder - -with the following command by designating `` - -```sh -ros2 launch autoware_static_centerline_generator run_planning_server.launch.xml vehicle_model:= -``` - -FYI, port ID of the http server is 4010 by default. - -### Command Line Interface - -The optimized centerline can be generated from the command line interface by designating - -- `` -- `` (not mandatory) -- `` -- `` -- `` - -```sh -ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml run_backgrond:=false lanelet2_input_file_path:= lanelet2_output_file_path:= start_lanelet_id:= end_lanelet_id:= vehicle_model:= -``` - -The default output map path containing the optimized centerline locates `/tmp/lanelet2_map.osm`. -If you want to change the output map path, you can remap the path by designating ``. - -## Visualization - -When launching the path planning server, rviz is launched as well as follows. -![rviz](./media/rviz.png) - -- The yellow footprints are the original ones from the osm map file. - - FYI: Footprints are generated based on the centerline and vehicle size. -- The red footprints are the optimized ones. -- The gray area is the drivable area. -- You can see that the red footprints are inside the drivable area although the yellow ones are outside. - -### Unsafe footprints - -Sometimes the optimized centerline footprints are close to the lanes' boundaries. -We can check how close they are with `unsafe footprints` marker as follows. - -Footprints' color depends on its distance to the boundaries, and text expresses its distance. - -![rviz](./media/unsafe_footprints.png) - -By default, footprints' color is - -- when the distance is less than 0.1 [m] : red -- when the distance is less than 0.2 [m] : green -- when the distance is less than 0.3 [m] : blue diff --git a/planning/autoware_static_centerline_generator/config/common.param.yaml b/planning/autoware_static_centerline_generator/config/common.param.yaml deleted file mode 100644 index 6c547c8cd83dc..0000000000000 --- a/planning/autoware_static_centerline_generator/config/common.param.yaml +++ /dev/null @@ -1,17 +0,0 @@ -/**: - ros__parameters: - # constraints param for normal driving - max_vel: 11.1 # max velocity limit [m/s] - - normal: - min_acc: -1.0 # min deceleration [m/ss] - max_acc: 1.0 # max acceleration [m/ss] - min_jerk: -1.0 # min jerk [m/sss] - max_jerk: 1.0 # max jerk [m/sss] - - # constraints to be observed - limit: - min_acc: -2.5 # min deceleration limit [m/ss] - max_acc: 1.0 # max acceleration limit [m/ss] - min_jerk: -1.5 # min jerk limit [m/sss] - max_jerk: 1.5 # max jerk limit [m/sss] diff --git a/planning/autoware_static_centerline_generator/config/nearest_search.param.yaml b/planning/autoware_static_centerline_generator/config/nearest_search.param.yaml deleted file mode 100644 index eb6709764ce3e..0000000000000 --- a/planning/autoware_static_centerline_generator/config/nearest_search.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - # ego - ego_nearest_dist_threshold: 3.0 # [m] - ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] diff --git a/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml b/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml deleted file mode 100644 index 060590803428a..0000000000000 --- a/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml +++ /dev/null @@ -1,9 +0,0 @@ -/**: - ros__parameters: - marker_color: ["FF0000", "FF5A00", "FFFF00"] - marker_color_dist_thresh : [0.1, 0.2, 0.3] - output_trajectory_interval: 1.0 - - validation: - dist_threshold_to_road_border: 0.0 - max_steer_angle_margin: 0.0 # [rad] NOTE: Positive value makes max steer angle threshold to decrease. diff --git a/planning/autoware_static_centerline_generator/config/vehicle_info.param.yaml b/planning/autoware_static_centerline_generator/config/vehicle_info.param.yaml deleted file mode 100644 index 8941b92b4e78e..0000000000000 --- a/planning/autoware_static_centerline_generator/config/vehicle_info.param.yaml +++ /dev/null @@ -1,12 +0,0 @@ -/**: - ros__parameters: - wheel_radius: 0.39 - wheel_width: 0.42 - wheel_base: 2.74 # between front wheel center and rear wheel center - wheel_tread: 1.63 # between left wheel center and right wheel center - front_overhang: 1.0 # between front wheel center and vehicle front - rear_overhang: 1.03 # between rear wheel center and vehicle rear - left_overhang: 0.1 # between left wheel center and vehicle left - right_overhang: 0.1 # between right wheel center and vehicle right - vehicle_height: 2.5 - max_steer_angle: 0.70 # [rad] diff --git a/planning/autoware_static_centerline_generator/launch/run_planning_server.launch.xml b/planning/autoware_static_centerline_generator/launch/run_planning_server.launch.xml deleted file mode 100644 index cb368ca336316..0000000000000 --- a/planning/autoware_static_centerline_generator/launch/run_planning_server.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml deleted file mode 100644 index 0590009ab378c..0000000000000 --- a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml +++ /dev/null @@ -1,91 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/planning/autoware_static_centerline_generator/media/rviz.png b/planning/autoware_static_centerline_generator/media/rviz.png deleted file mode 100644 index e76ede78294f0..0000000000000 Binary files a/planning/autoware_static_centerline_generator/media/rviz.png and /dev/null differ diff --git a/planning/autoware_static_centerline_generator/media/unsafe_footprints.png b/planning/autoware_static_centerline_generator/media/unsafe_footprints.png deleted file mode 100644 index b4404f4cfa89b..0000000000000 Binary files a/planning/autoware_static_centerline_generator/media/unsafe_footprints.png and /dev/null differ diff --git a/planning/autoware_static_centerline_generator/msg/PointsWithLaneId.msg b/planning/autoware_static_centerline_generator/msg/PointsWithLaneId.msg deleted file mode 100644 index 97cd0355df831..0000000000000 --- a/planning/autoware_static_centerline_generator/msg/PointsWithLaneId.msg +++ /dev/null @@ -1,2 +0,0 @@ -int64 lane_id -geometry_msgs/Point[] points diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml deleted file mode 100644 index cea18e5605921..0000000000000 --- a/planning/autoware_static_centerline_generator/package.xml +++ /dev/null @@ -1,57 +0,0 @@ - - - - autoware_static_centerline_generator - 0.40.0 - The autoware_static_centerline_generator package - Takayuki Murooka - Kosuke Takeuchi - Apache License 2.0 - - Takayuki Murooka - - ament_cmake_auto - autoware_cmake - - rosidl_default_generators - - autoware_behavior_path_planner_common - autoware_geography_utils - autoware_global_parameter_loader - autoware_interpolation - autoware_lanelet2_extension - autoware_lanelet2_map_visualizer - autoware_map_loader - autoware_map_msgs - autoware_map_projection_loader - autoware_map_tf_generator - autoware_mission_planner - autoware_motion_utils - autoware_osqp_interface - autoware_path_optimizer - autoware_path_smoother - autoware_perception_msgs - autoware_planning_msgs - autoware_route_handler - autoware_universe_utils - autoware_vehicle_info_utils - geometry_msgs - rclcpp - rclcpp_components - - python3-flask-cors - rosidl_default_runtime - - ament_cmake_gmock - ament_lint_auto - autoware_behavior_path_planner - autoware_behavior_velocity_planner - autoware_lint_common - ros_testing - - rosidl_interface_packages - - - ament_cmake - - diff --git a/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz b/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz deleted file mode 100644 index f1bd110783009..0000000000000 --- a/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz +++ /dev/null @@ -1,459 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.550000011920929 - Tree Height: 386 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Dummy Car1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" - - Class: rviz_plugins::AutowareStatePanel - Name: AutowareStatePanel -Visualization Manager: - Class: "" - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: false - Enabled: false - Name: System - - Class: rviz_common/Group - Displays: - - Alpha: 0.20000000298023224 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 28.71826171875 - Min Value: -7.4224700927734375 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 237 - Min Color: 211; 215; 207 - Min Intensity: 0 - Name: PointCloudMap - Position Transformer: XYZ - Selectable: false - Size (Pixels): 1 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/pointcloud_map - Use Fixed Frame: true - Use rainbow: false - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Lanelet2VectorMap - Namespaces: - center_lane_line: true - center_line_arrows: true - lane_start_bound: false - lanelet_id: false - left_lane_bound: true - right_lane_bound: true - road_lanelets: false - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/vector_map_marker - Value: true - Enabled: true - Name: Map - - Class: rviz_plugins/PathWithLaneId - Color Border Vel Max: 3 - Enabled: false - Name: Raw Centerline - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_generator/input_centerline - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: true - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View LaneId: - Scale: 0.10000000149011612 - Value: false - View Path: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Value: false - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: Optimized Centerline - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_generator/output/centerline - Value: true - View Footprint: - Alpha: 0.5 - Color: 0; 255; 0 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: true - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Value: false - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: Raw Centerline (Path type) - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_generator/debug/raw_centerline - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: true - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MarkerArray - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_generator/output/validation_results - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Debug Markers - Namespaces: - curvature: false - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_generator/debug/markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_generator/debug/ego_footprint_bounds - Value: true - Enabled: true - Name: debug - Enabled: true - Global Options: - Background Color: 10; 10; 10 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/goal - - Acceleration: 0 - Class: rviz_plugins/PedestrianInitialPoseTool - Interactive: false - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 1 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/CarInitialPoseTool - H vehicle height: 2 - Interactive: false - L vehicle length: 4 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 8 - W vehicle width: 1.7999999523162842 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/BusInitialPoseTool - H vehicle height: 3.5 - Interactive: false - L vehicle length: 10.5 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - W vehicle width: 2.5 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Class: rviz_plugins/MissionCheckpointTool - Pose Topic: /planning/mission_planning/checkpoint - Theta std deviation: 0.2617993950843811 - X std deviation: 0.5 - Y std deviation: 0.5 - Z position: 0 - - Class: rviz_plugins/DeleteAllObjectsTool - Pose Topic: /simulation/dummy_perception_publisher/object_info - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz_default_plugins) - X: 0 - Y: 0 - Saved: - - Class: rviz_default_plugins/ThirdPersonFollower - Distance: 18 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: ThirdPersonFollower - Near Clip Distance: 0.009999999776482582 - Pitch: 0.20000000298023224 - Target Frame: base_link - Value: ThirdPersonFollower (rviz) - Yaw: 3.141592025756836 - - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: TopDownOrtho - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 -Window Geometry: - AutowareStatePanel: - collapsed: false - Displays: - collapsed: false - Height: 1043 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000019200000357fc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e000001c0000000ca00fffffffc0000020400000191000000c40100001dfa000000000100000002fb0000000a0056006900650077007301000000000000033c0000010000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000000ffffffff0000008a00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000682000000eb0000000000000000fb0000000a0049006d0061006700650100000505000002680000000000000000fb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007a00fffffffb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000000000000000fb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d00610067006500000001d3000000af0000000000000000fb00000024004100750074006f00770061007200650053007400610074006500500061006e0065006c000000029b000000fb0000017c00ffffff000000010000015f000006fffc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000e7a0000005afc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000005afc0100000002fb0000000800540069006d00650100000000000007800000027100fffffffb0000000800540069006d00650100000000000004500000000000000000000005e80000035700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1920 - X: 0 - Y: 0 diff --git a/planning/autoware_static_centerline_generator/scripts/app.py b/planning/autoware_static_centerline_generator/scripts/app.py deleted file mode 100755 index 08bff8b80dcb7..0000000000000 --- a/planning/autoware_static_centerline_generator/scripts/app.py +++ /dev/null @@ -1,178 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2022 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. - -import json -import uuid - -from autoware_static_centerline_generator.srv import LoadMap -from autoware_static_centerline_generator.srv import PlanPath -from autoware_static_centerline_generator.srv import PlanRoute -from flask import Flask -from flask import jsonify -from flask import request -from flask_cors import CORS -import rclpy -from rclpy.node import Node - -rclpy.init() -node = Node("static_centerline_generator_http_server") - -app = Flask(__name__) -CORS(app) - - -def create_client(service_type, server_name): - # create client - cli = node.create_client(service_type, server_name) - while not cli.wait_for_service(timeout_sec=1.0): - print("{} service not available, waiting again...".format(server_name)) - return cli - - -@app.route("/map", methods=["POST"]) -def get_map(): - data = request.get_json() - - map_uuid = str(uuid.uuid4()) - global map_id - map_id = map_uuid - - # create client - cli = create_client(LoadMap, "/planning/static_centerline_generator/load_map") - - # request map loading - req = LoadMap.Request(map=data["map"]) - future = cli.call_async(req) - - # get result - rclpy.spin_until_future_complete(node, future) - res = future.result() - - # error handling - if res.message == "InvalidMapFormat": - return jsonify(code=res.message, message="Map format is invalid."), 400 - elif res.message != "": - return ( - jsonify( - code="InternalServerError", - message="Error occurred on the server. Please check the terminal.", - ), - 500, - ) - - return map_uuid - - -@app.route("/planned_route", methods=["GET"]) -def post_planned_route(): - args = request.args.to_dict() - global map_id - if map_id != args.get("map_id"): - # TODO(murooka) error handling for map_id mismatch - print("map_id is not correct.") - - # create client - cli = create_client(PlanRoute, "/planning/static_centerline_generator/plan_route") - - # request route planning - req = PlanRoute.Request( - start_lane_id=int(args.get("start_lane_id")), end_lane_id=int(args.get("end_lane_id")) - ) - future = cli.call_async(req) - - # get result - rclpy.spin_until_future_complete(node, future) - res = future.result() - - # error handling - if res.message == "MapNotFound": - return jsonify(code=res.message, message="Map is missing."), 404 - elif res.message == "RouteNotFound": - return jsonify(code=res.message, message="Planning route failed."), 404 - elif res.message != "": - return ( - jsonify( - code="InternalServerError", - message="Error occurred on the server. Please check the terminal.", - ), - 500, - ) - - return json.dumps(tuple(res.lane_ids)) - - -@app.route("/planned_path", methods=["GET"]) -def post_planned_path(): - args = request.args.to_dict() - global map_id - if map_id != args.get("map_id"): - # TODO(murooka) error handling for map_id mismatch - print("map_id is not correct.") - - # create client - cli = create_client(PlanPath, "/planning/static_centerline_generator/plan_path") - - # request path planning - route_lane_ids = [eval(i) for i in request.args.getlist("route[]")] - req = PlanPath.Request(route=route_lane_ids) - future = cli.call_async(req) - - # get result - rclpy.spin_until_future_complete(node, future) - res = future.result() - - # error handling - if res.message == "MapNotFound": - return jsonify(code=res.message, message="Map is missing."), 404 - elif res.message == "LaneletsNotConnected": - return ( - jsonify( - code=res.message, - message="Lanelets are not connected.", - object_ids=tuple(res.unconnected_lane_ids), - ), - 400, - ) - elif res.message != "": - return ( - jsonify( - code="InternalServerError", - message="Error occurred on the server. Please check the terminal.", - ), - 500, - ) - - # create output json - result_json = [] - for points_with_lane_id in res.points_with_lane_ids: - current_lane_points = [] - for geom_point in points_with_lane_id.points: - point = {"x": geom_point.x, "y": geom_point.y, "z": geom_point.z} - current_lane_points.append(point) - - current_result_json = {} - current_result_json["lane_id"] = int(points_with_lane_id.lane_id) - current_result_json["points"] = current_lane_points - - result_json.append(current_result_json) - - return json.dumps(tuple(result_json)) - - -if __name__ == "__main__": - app.debug = True - app.secret_key = "tmp_secret_key" - app.run(host="localhost", port=4010) diff --git a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py deleted file mode 100755 index f3d908713361d..0000000000000 --- a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py +++ /dev/null @@ -1,207 +0,0 @@ -#!/bin/env python3 - -# 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. - - -import sys -import time - -from PyQt5 import QtCore -from PyQt5.QtWidgets import QApplication -from PyQt5.QtWidgets import QGroupBox -from PyQt5.QtWidgets import QMainWindow -from PyQt5.QtWidgets import QPushButton -from PyQt5.QtWidgets import QSizePolicy -from PyQt5.QtWidgets import QSlider -from PyQt5.QtWidgets import QVBoxLayout -from PyQt5.QtWidgets import QWidget -from autoware_planning_msgs.msg import Trajectory -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSProfile -from std_msgs.msg import Empty -from std_msgs.msg import Float32 -from std_msgs.msg import Int32 - - -class CenterlineUpdaterWidget(QMainWindow): - def __init__(self): - super(self.__class__, self).__init__() - self.setupUI() - - def setupUI(self): - self.setObjectName("MainWindow") - self.resize(480, 120) - self.setWindowFlags(QtCore.Qt.WindowStaysOnTopHint) - - central_widget = QWidget(self) - self.grid_layout = QVBoxLayout(central_widget) - self.grid_layout.setContentsMargins(10, 10, 10, 10) - - # slide of the trajectory's start and end index - self.traj_start_index_slider = QSlider(QtCore.Qt.Horizontal) - self.traj_end_index_slider = QSlider(QtCore.Qt.Horizontal) - self.min_traj_start_index = 0 - self.max_traj_end_index = None - - # Layout: Range of Centerline - centerline_vertical_box = QVBoxLayout(self) - centerline_vertical_box.addWidget(self.traj_start_index_slider) - centerline_vertical_box.addWidget(self.traj_end_index_slider) - centerline_group = QGroupBox("Centerline") - centerline_group.setLayout(centerline_vertical_box) - self.grid_layout.addWidget(centerline_group) - - """ - # 2. Road Boundary - road_boundary_group = QGroupBox("Road Boundary") - road_boundary_vertical_box = QVBoxLayout(self) - road_boundary_group.setLayout(road_boundary_vertical_box) - self.grid_layout.addWidget(road_boundary_group) - - # 2.1. Slider - self.road_boundary_lateral_margin_slider = QSlider(QtCore.Qt.Horizontal) - road_boundary_vertical_box.addWidget(self.road_boundary_lateral_margin_slider) - self.road_boundary_lateral_margin_ratio = 10 - self.road_boundary_lateral_margin_slider.setMinimum(0) - self.road_boundary_lateral_margin_slider.setMaximum( - 5 * self.road_boundary_lateral_margin_ratio - ) - road_boundary_vertical_box.addWidget(QPushButton("reset")) - """ - - # 3. General - general_group = QGroupBox("General") - general_vertical_box = QVBoxLayout(self) - general_group.setLayout(general_vertical_box) - self.grid_layout.addWidget(general_group) - - # 3.1. Validate Centerline - self.validate_button = QPushButton("validate") - self.validate_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) - general_vertical_box.addWidget(self.validate_button) - - # 3.2. Save Map - self.save_map_button = QPushButton("save map") - self.save_map_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) - general_vertical_box.addWidget(self.save_map_button) - - self.setCentralWidget(central_widget) - - def initWithEndIndex(self, max_traj_end_index): - self.max_traj_end_index = max_traj_end_index - - # initialize sliders - self.traj_start_index_slider.setMinimum(self.min_traj_start_index) - self.traj_start_index_slider.setMaximum(self.max_traj_end_index) - self.traj_start_index_slider.setValue(self.min_traj_start_index) - - self.traj_end_index_slider.setMinimum(self.min_traj_start_index) - self.traj_end_index_slider.setMaximum(self.max_traj_end_index) - self.traj_end_index_slider.setValue(self.max_traj_end_index) - - -class CenterlineUpdaterHelper(Node): - def __init__(self): - super().__init__("centerline_updater_helper") - # Qt - self.widget = CenterlineUpdaterWidget() - self.widget.show() - self.widget.save_map_button.clicked.connect(self.onSaveMapButtonPushed) - self.widget.validate_button.clicked.connect(self.onValidateButtonPushed) - self.widget.traj_start_index_slider.valueChanged.connect(self.onStartIndexChanged) - self.widget.traj_end_index_slider.valueChanged.connect(self.onEndIndexChanged) - """ - self.widget.road_boundary_lateral_margin_slider.valueChanged.connect( - self.onRoadBoundaryLateralMargin - ) - """ - - # ROS - self.pub_save_map = self.create_publisher(Empty, "/static_centerline_generator/save_map", 1) - self.pub_validate = self.create_publisher(Empty, "/static_centerline_generator/validate", 1) - self.pub_traj_start_index = self.create_publisher( - Int32, "/static_centerline_generator/traj_start_index", 1 - ) - self.pub_traj_end_index = self.create_publisher( - Int32, "/static_centerline_generator/traj_end_index", 1 - ) - self.pub_road_boundary_lateral_margin = self.create_publisher( - Float32, "/static_centerline_generator/road_boundary_lateral_margin", 1 - ) - - transient_local_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) - self.sub_whole_centerline = self.create_subscription( - Trajectory, - "/static_centerline_generator/output/whole_centerline", - self.onWholeCenterline, - transient_local_qos, - ) - - while self.widget.max_traj_end_index is None: - rclpy.spin_once(self, timeout_sec=0.01) - time.sleep(0.1) - - def onWholeCenterline(self, whole_centerline): - self.widget.initWithEndIndex(len(whole_centerline.points) - 1) - - def onSaveMapButtonPushed(self, event): - msg = Empty() - self.pub_save_map.publish(msg) - - # NOTE: After saving the map, the generated centerline is written - # in original_map_ptr_ in static_centerline_generator_node. - # When saving the map again, another centerline is written without - # removing the previous centerline. - # Therefore, saving the map can be called only once. - self.widget.save_map_button.setEnabled(False) - - def onValidateButtonPushed(self, event): - msg = Empty() - self.pub_validate.publish(msg) - - def onStartIndexChanged(self, event): - msg = Int32() - msg.data = self.widget.traj_start_index_slider.value() - self.pub_traj_start_index.publish(msg) - - def onEndIndexChanged(self, event): - msg = Int32() - msg.data = self.widget.traj_end_index_slider.value() - self.pub_traj_end_index.publish(msg) - - def onRoadBoundaryLateralMargin(self, event): - msg = Float32() - msg.data = ( - self.widget.road_boundary_lateral_margin_slider.value() - / self.widget.road_boundary_lateral_margin_ratio - ) - self.pub_road_boundary_lateral_margin.publish(msg) - - -def main(args=None): - app = QApplication(sys.argv) - - rclpy.init(args=args) - node = CenterlineUpdaterHelper() - - while True: - app.processEvents() - rclpy.spin_once(node, timeout_sec=0.01) - - -if __name__ == "__main__": - main() diff --git a/planning/autoware_static_centerline_generator/scripts/show_lanelet2_map_diff.py b/planning/autoware_static_centerline_generator/scripts/show_lanelet2_map_diff.py deleted file mode 100755 index 00d06ca2213d1..0000000000000 --- a/planning/autoware_static_centerline_generator/scripts/show_lanelet2_map_diff.py +++ /dev/null @@ -1,139 +0,0 @@ -#!/bin/env python3 - -# 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. - -import argparse -from decimal import Decimal -import os -import subprocess -import xml.etree.ElementTree as ET - - -def sort_attributes(root): - for shallow_element in root: - # sort nodes - attrib = shallow_element.attrib - if len(attrib) > 1: - attributes = sorted(attrib.items()) - attrib.clear() - attrib.update(attributes) - if shallow_element.tag == "relation": - pass - - # sort the element in the tag - for deep_element in shallow_element: - attrib = deep_element.attrib - if len(attrib) > 1: - # adjust attribute order, e.g. by sorting - attributes = sorted(attrib.items()) - attrib.clear() - attrib.update(attributes) - - # sort tags - sorted_shallow_element = sorted(shallow_element, key=lambda x: x.items()) - if len(shallow_element) != 0: - # NOTE: This "tail" is related to the indent of the next tag - first_tail = shallow_element[0].tail - last_tail = shallow_element[-1].tail - for idx, val_shallow_element in enumerate(sorted_shallow_element): - shallow_element[idx] = val_shallow_element - if idx == len(shallow_element) - 1: - shallow_element[idx].tail = last_tail - else: - shallow_element[idx].tail = first_tail - - -def remove_diff_to_ignore(osm_root): - decimal_precision = 11 # for lat/lon values - - # remove attributes of the osm tag - osm_root.attrib.clear() - - # remove the MetaInfo tag generated by Vector Map Builder - if osm_root[0].tag == "MetaInfo": - osm_root.remove(osm_root[0]) - - # remove unnecessary attributes for diff - for osm_child in osm_root: - if osm_child.tag == "osm": - osm_child.attrib.pop("osm") - if "visible" in osm_child.attrib and osm_child.attrib["visible"]: - osm_child.attrib.pop("visible") - if "version" in osm_child.attrib and osm_child.attrib["version"]: - osm_child.attrib.pop("version") - if "action" in osm_child.attrib and osm_child.attrib["action"] == "modify": - osm_child.attrib.pop("action") - if "lat" in osm_child.attrib: - osm_child.attrib["lat"] = str( - Decimal(float(osm_child.attrib["lat"])).quantize(Decimal(f"1e-{decimal_precision}")) - ) - if "lon" in osm_child.attrib: - osm_child.attrib["lon"] = str( - Decimal(float(osm_child.attrib["lon"])).quantize(Decimal(f"1e-{decimal_precision}")) - ) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument( - "-s", "--sort-attributes", action="store_true", help="Sort attributes of LL2 maps" - ) - parser.add_argument( - "-i", - "--ignore-minor-attributes", - action="store_true", - help="Ignore minor attributes of LL2 maps which does not change the map's behavior", - ) - args = parser.parse_args() - - original_osm_file_name = "/tmp/autoware_static_centerline_generator/input/lanelet2_map.osm" - modified_osm_file_name = "/tmp/autoware_static_centerline_generator/output/lanelet2_map.osm" - - # load LL2 maps - original_osm_tree = ET.parse(original_osm_file_name) - original_osm_root = original_osm_tree.getroot() - modified_osm_tree = ET.parse(modified_osm_file_name) - modified_osm_root = modified_osm_tree.getroot() - - # sort attributes - if args.sort_attributes: - sort_attributes(modified_osm_root) - sort_attributes(original_osm_root) - - # ignore minor attributes - if args.ignore_minor_attributes: - remove_diff_to_ignore(original_osm_root) - remove_diff_to_ignore(modified_osm_root) - - # write LL2 maps - output_dir_path = "/tmp/autoware_static_centerline_generator/show_lanelet2_map_diff/" - os.makedirs(output_dir_path + "original/", exist_ok=True) - os.makedirs(output_dir_path + "modified/", exist_ok=True) - - original_osm_tree.write(output_dir_path + "original/lanelet2_map.osm") - modified_osm_tree.write(output_dir_path + "modified/lanelet2_map.osm") - - # show diff - print("[INFO] Show diff of following LL2 maps") - print(f" {output_dir_path + 'original/lanelet2_map.osm'}") - print(f" {output_dir_path + 'modified/lanelet2_map.osm'}") - subprocess.run( - [ - "diff", - output_dir_path + "original/lanelet2_map.osm", - output_dir_path + "modified/lanelet2_map.osm", - "--color", - ] - ) diff --git a/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh b/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh deleted file mode 100755 index c170e24e475d9..0000000000000 --- a/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh +++ /dev/null @@ -1,5 +0,0 @@ -#!/bin/bash - -ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" mode:="GUI" lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory_turn-right.db3" vehicle_model:=lexus - -# ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" mode:="GUI" lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory_turn-left-right.db3" vehicle_model:=lexus diff --git a/planning/autoware_static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh b/planning/autoware_static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh deleted file mode 100755 index 488091989d1fa..0000000000000 --- a/planning/autoware_static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="optimization_trajectory_base" run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" start_lanelet_id:=125 end_lanelet_id:=132 vehicle_model:=lexus diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp deleted file mode 100644 index dc950f190652b..0000000000000 --- a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp +++ /dev/null @@ -1,86 +0,0 @@ -// 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 "centerline_source/bag_ego_trajectory_based_centerline.hpp" - -#include "rclcpp/serialization.hpp" -#include "rosbag2_cpp/reader.hpp" -#include "static_centerline_generator_node.hpp" - -#include - -#include -#include -#include - -namespace autoware::static_centerline_generator -{ -std::vector generate_centerline_with_bag(rclcpp::Node & node) -{ - const auto bag_filename = node.declare_parameter("bag_filename"); - - // open rosbag - rosbag2_cpp::Reader bag_reader; - bag_reader.open(bag_filename); - - // extract 2D position of ego's trajectory from rosbag - rclcpp::Serialization bag_serialization; - std::vector centerline_traj_points; - while (bag_reader.has_next()) { - const rosbag2_storage::SerializedBagMessageSharedPtr msg = bag_reader.read_next(); - - if (msg->topic_name != "/localization/kinematic_state") { - continue; - } - - rclcpp::SerializedMessage serialized_msg(*msg->serialized_data); - const auto ros_msg = std::make_shared(); - - bag_serialization.deserialize_message(&serialized_msg, ros_msg.get()); - - if (!centerline_traj_points.empty()) { - constexpr double epsilon = 1e-1; - if ( - std::abs(centerline_traj_points.back().pose.position.x - ros_msg->pose.pose.position.x) < - epsilon && - std::abs(centerline_traj_points.back().pose.position.y - ros_msg->pose.pose.position.y) < - epsilon) { - continue; - } - } - TrajectoryPoint centerline_traj_point; - centerline_traj_point.pose.position = ros_msg->pose.pose.position; - centerline_traj_points.push_back(centerline_traj_point); - } - - RCLCPP_INFO(node.get_logger(), "Extracted centerline from the bag."); - - // calculate rough orientation of centerline trajectory points - for (size_t i = 0; i < centerline_traj_points.size(); ++i) { - if (i == centerline_traj_points.size() - 1) { - if (i != 0) { - centerline_traj_points.at(i).pose.orientation = - centerline_traj_points.at(i - 1).pose.orientation; - } - } else { - const double yaw_angle = autoware::universe_utils::calcAzimuthAngle( - centerline_traj_points.at(i).pose.position, centerline_traj_points.at(i + 1).pose.position); - centerline_traj_points.at(i).pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(yaw_angle); - } - } - - return centerline_traj_points; -} -} // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp deleted file mode 100644 index 2275f88184c6f..0000000000000 --- a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp +++ /dev/null @@ -1,27 +0,0 @@ -// 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 CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ -#define CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ - -#include "rclcpp/rclcpp.hpp" -#include "type_alias.hpp" - -#include - -namespace autoware::static_centerline_generator -{ -std::vector generate_centerline_with_bag(rclcpp::Node & node); -} // namespace autoware::static_centerline_generator -#endif // CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp deleted file mode 100644 index 51725fb8a3799..0000000000000 --- a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ /dev/null @@ -1,187 +0,0 @@ -// 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 "centerline_source/optimization_trajectory_based_centerline.hpp" - -#include "autoware/motion_utils/resample/resample.hpp" -#include "autoware/motion_utils/trajectory/conversion.hpp" -#include "autoware/path_optimizer/node.hpp" -#include "autoware/path_smoother/elastic_band_smoother.hpp" -#include "autoware/universe_utils/ros/parameter.hpp" -#include "static_centerline_generator_node.hpp" -#include "utils.hpp" - -#include -#include - -namespace autoware::static_centerline_generator -{ -namespace -{ -rclcpp::NodeOptions create_node_options() -{ - return rclcpp::NodeOptions{}; -} - -Path convert_to_path(const PathWithLaneId & path_with_lane_id) -{ - Path path; - path.header = path_with_lane_id.header; - path.left_bound = path_with_lane_id.left_bound; - path.right_bound = path_with_lane_id.right_bound; - for (const auto & point : path_with_lane_id.points) { - path.points.push_back(point.point); - } - - return path; -} - -Trajectory convert_to_trajectory(const Path & path) -{ - Trajectory traj; - for (const auto & point : path.points) { - TrajectoryPoint traj_point; - traj_point.pose = point.pose; - traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; - traj_point.lateral_velocity_mps = point.lateral_velocity_mps; - traj_point.heading_rate_rps = point.heading_rate_rps; - - traj.points.push_back(traj_point); - } - return traj; -} -} // namespace - -OptimizationTrajectoryBasedCenterline::OptimizationTrajectoryBasedCenterline(rclcpp::Node & node) -{ - pub_raw_path_with_lane_id_ = - node.create_publisher("input_centerline", utils::create_transient_local_qos()); - pub_raw_path_ = - node.create_publisher("debug/raw_centerline", utils::create_transient_local_qos()); -} - -std::vector -OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( - rclcpp::Node & node, const RouteHandler & route_handler, - const std::vector & route_lane_ids) -{ - const auto route_lanelets = utils::get_lanelets_from_ids(route_handler, route_lane_ids); - - // optimize centerline inside the lane - const auto start_center_pose = utils::get_center_pose(route_handler, route_lane_ids.front()); - - // get ego nearest search parameters and resample interval in behavior_path_planner - const double ego_nearest_dist_threshold = - autoware::universe_utils::getOrDeclareParameter(node, "ego_nearest_dist_threshold"); - const double ego_nearest_yaw_threshold = - autoware::universe_utils::getOrDeclareParameter(node, "ego_nearest_yaw_threshold"); - const double behavior_path_interval = - autoware::universe_utils::getOrDeclareParameter(node, "output_path_interval"); - const double behavior_vel_interval = - autoware::universe_utils::getOrDeclareParameter(node, "behavior_output_path_interval"); - - // extract path with lane id from lanelets - const auto raw_path_with_lane_id = [&]() { - const auto non_resampled_path_with_lane_id = utils::get_path_with_lane_id( - route_handler, route_lanelets, start_center_pose, ego_nearest_dist_threshold, - ego_nearest_yaw_threshold); - return autoware::motion_utils::resamplePath( - non_resampled_path_with_lane_id, behavior_path_interval); - }(); - pub_raw_path_with_lane_id_->publish(raw_path_with_lane_id); - RCLCPP_INFO(node.get_logger(), "Calculated raw path with lane id and published."); - - // convert path with lane id to path - const auto raw_path = [&]() { - const auto non_resampled_path = convert_to_path(raw_path_with_lane_id); - return autoware::motion_utils::resamplePath(non_resampled_path, behavior_vel_interval); - }(); - pub_raw_path_->publish(raw_path); - RCLCPP_INFO(node.get_logger(), "Converted to path and published."); - - // smooth trajectory and road collision avoidance - const auto optimized_traj_points = optimize_trajectory(raw_path); - RCLCPP_INFO( - node.get_logger(), - "Smoothed trajectory and made it collision free with the road and published."); - - return optimized_traj_points; -} - -std::vector OptimizationTrajectoryBasedCenterline::optimize_trajectory( - const Path & raw_path) const -{ - // convert to trajectory points - const auto raw_traj_points = [&]() { - const auto raw_traj = convert_to_trajectory(raw_path); - return autoware::motion_utils::convertToTrajectoryPointArray(raw_traj); - }(); - - // create an instance of elastic band and model predictive trajectory. - const auto eb_path_smoother_ptr = - autoware::path_smoother::ElasticBandSmoother(create_node_options()).getElasticBandSmoother(); - const auto mpt_optimizer_ptr = - autoware::path_optimizer::PathOptimizer(create_node_options()).getMPTOptimizer(); - - // NOTE: The optimization is executed every valid_optimized_traj_points_num points. - constexpr int valid_optimized_traj_points_num = 10; - const int traj_segment_num = raw_traj_points.size() / valid_optimized_traj_points_num; - - // NOTE: num_initial_optimization exists to make the both optimizations stable since they may use - // warm start. - constexpr int num_initial_optimization = 2; - - std::vector whole_optimized_traj_points; - for (int virtual_ego_pose_idx = -num_initial_optimization; - virtual_ego_pose_idx < traj_segment_num; ++virtual_ego_pose_idx) { - // calculate virtual ego pose for the optimization - constexpr int virtual_ego_pose_offset_idx = 1; - const auto virtual_ego_pose = - raw_traj_points - .at( - valid_optimized_traj_points_num * std::max(virtual_ego_pose_idx, 0) + - virtual_ego_pose_offset_idx) - .pose; - - // smooth trajectory by elastic band in the autoware_path_smoother package - const auto smoothed_traj_points = - eb_path_smoother_ptr->smoothTrajectory(raw_traj_points, virtual_ego_pose); - - // road collision avoidance by model predictive trajectory in the autoware_path_optimizer - // package - const autoware::path_optimizer::PlannerData planner_data{ - raw_path.header, smoothed_traj_points, raw_path.left_bound, raw_path.right_bound, - virtual_ego_pose}; - const auto optimized_traj_points = mpt_optimizer_ptr->optimizeTrajectory(planner_data); - - // connect the previously and currently optimized trajectory points - for (size_t j = 0; j < whole_optimized_traj_points.size(); ++j) { - const double dist = autoware::universe_utils::calcDistance2d( - whole_optimized_traj_points.at(j), optimized_traj_points.front()); - if (dist < 0.5) { - const std::vector extracted_whole_optimized_traj_points{ - whole_optimized_traj_points.begin(), - whole_optimized_traj_points.begin() + std::max(j, 1UL) - 1}; - whole_optimized_traj_points = extracted_whole_optimized_traj_points; - break; - } - } - for (size_t j = 0; j < optimized_traj_points.size(); ++j) { - whole_optimized_traj_points.push_back(optimized_traj_points.at(j)); - } - } - - return whole_optimized_traj_points; -} -} // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp deleted file mode 100644 index 88def6fa7bd4c..0000000000000 --- a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// 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 CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT -#define CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT - -#include "rclcpp/rclcpp.hpp" -#include "type_alias.hpp" - -#include - -namespace autoware::static_centerline_generator -{ -class OptimizationTrajectoryBasedCenterline -{ -public: - OptimizationTrajectoryBasedCenterline() = default; - explicit OptimizationTrajectoryBasedCenterline(rclcpp::Node & node); - std::vector generate_centerline_with_optimization( - rclcpp::Node & node, const RouteHandler & route_handler, - const std::vector & route_lane_ids); - -private: - std::vector optimize_trajectory(const Path & raw_path) const; - - rclcpp::Publisher::SharedPtr pub_raw_path_with_lane_id_{nullptr}; - rclcpp::Publisher::SharedPtr pub_raw_path_{nullptr}; -}; -} // namespace autoware::static_centerline_generator -// clang-format off -#endif // CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT -// clang-format on diff --git a/planning/autoware_static_centerline_generator/src/main.cpp b/planning/autoware_static_centerline_generator/src/main.cpp deleted file mode 100644 index 811d57c6036ef..0000000000000 --- a/planning/autoware_static_centerline_generator/src/main.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright 2022 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 "static_centerline_generator_node.hpp" - -#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."); - } - - // 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_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp deleted file mode 100644 index f172e3e0cb1cd..0000000000000 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ /dev/null @@ -1,774 +0,0 @@ -// Copyright 2022 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 "static_centerline_generator_node.hpp" - -#include "autoware/map_loader/lanelet2_map_loader_node.hpp" -#include "autoware/map_projection_loader/load_info_from_lanelet2_map.hpp" -#include "autoware/map_projection_loader/map_projection_loader.hpp" -#include "autoware/motion_utils/resample/resample.hpp" -#include "autoware/motion_utils/trajectory/conversion.hpp" -#include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/parameter.hpp" -#include "autoware_lanelet2_extension/utility/message_conversion.hpp" -#include "autoware_lanelet2_extension/utility/query.hpp" -#include "autoware_lanelet2_extension/utility/utilities.hpp" -#include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" -#include "centerline_source/bag_ego_trajectory_based_centerline.hpp" -#include "type_alias.hpp" -#include "utils.hpp" - -#include -#include -#include -#include - -#include "std_msgs/msg/empty.hpp" -#include "std_msgs/msg/float32.hpp" -#include "std_msgs/msg/int32.hpp" - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define RESET_TEXT "\x1B[0m" -#define RED_TEXT "\x1B[31m" -#define YELLOW_TEXT "\x1b[33m" -#define BOLD_TEXT "\x1B[1m" - -namespace autoware::static_centerline_generator -{ -namespace -{ -std::vector get_lane_ids_from_route(const LaneletRoute & route) -{ - std::vector lane_ids; - for (const auto & segment : route.segments) { - const auto & target_lanelet_id = segment.preferred_primitive.id; - lane_ids.push_back(target_lanelet_id); - } - - return lane_ids; -} - -lanelet::BasicPoint2d convert_to_lanelet_point(const geometry_msgs::msg::Point & geom_point) -{ - lanelet::BasicPoint2d point(geom_point.x, geom_point.y); - return point; -} - -LinearRing2d create_vehicle_footprint( - const geometry_msgs::msg::Pose & pose, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0) -{ - const auto & i = vehicle_info; - - const double x_front = i.front_overhang_m + i.wheel_base_m + margin; - const double x_rear = -(i.rear_overhang_m + margin); - const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + margin; - const double y_right = -(i.wheel_tread_m / 2.0 + i.right_overhang_m + margin); - - std::vector geom_points; - geom_points.push_back( - autoware::universe_utils::calcOffsetPose(pose, x_front, y_left, 0.0).position); - geom_points.push_back( - autoware::universe_utils::calcOffsetPose(pose, x_front, y_right, 0.0).position); - geom_points.push_back( - autoware::universe_utils::calcOffsetPose(pose, x_rear, y_right, 0.0).position); - geom_points.push_back( - autoware::universe_utils::calcOffsetPose(pose, x_rear, y_left, 0.0).position); - - LinearRing2d footprint; - for (const auto & geom_point : geom_points) { - footprint.push_back(Point2d{geom_point.x, geom_point.y}); - } - footprint.push_back(footprint.back()); - - boost::geometry::correct(footprint); - - return footprint; -} - -geometry_msgs::msg::Pose get_text_pose( - const geometry_msgs::msg::Pose & pose, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) -{ - const auto & i = vehicle_info; - - const double x_front = i.front_overhang_m + i.wheel_base_m; - const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + 0.5; - - return autoware::universe_utils::calcOffsetPose(pose, x_front, y_left, 0.0); -} - -std::array convert_hex_string_to_decimal(const std::string & hex_str_color) -{ - unsigned int hex_int_color; - std::istringstream iss(hex_str_color); - iss >> std::hex >> hex_int_color; - - unsigned int unit = 16 * 16; - unsigned int b = hex_int_color % unit; - unsigned int g = (hex_int_color - b) / unit % unit; - unsigned int r = (hex_int_color - g * unit - b) / unit / unit; - - return std::array{r / 255.0, g / 255.0, b / 255.0}; -} - -std::vector check_lanelet_connection( - const RouteHandler & route_handler, const lanelet::ConstLanelets & route_lanelets) -{ - std::vector unconnected_lane_ids; - - for (size_t i = 0; i < route_lanelets.size() - 1; ++i) { - const auto next_lanelets = route_handler.getNextLanelets(route_lanelets.at(i)); - - const bool is_connected = - std::find_if(next_lanelets.begin(), next_lanelets.end(), [&](const auto & next_lanelet) { - return next_lanelet.id() == route_lanelets.at(i + 1).id(); - }) != next_lanelets.end(); - if (!is_connected) { - unconnected_lane_ids.push_back(route_lanelets.at(i).id()); - } - } - - return unconnected_lane_ids; -} - -std_msgs::msg::Header create_header(const rclcpp::Time & now) -{ - std_msgs::msg::Header header; - header.frame_id = "map"; - header.stamp = now; - return header; -} - -std::vector resample_trajectory_points( - const std::vector & input_traj_points, const double resample_interval) -{ - // resample and calculate trajectory points' orientation - const auto input_traj = autoware::motion_utils::convertToTrajectory(input_traj_points); - auto resampled_input_traj = - autoware::motion_utils::resampleTrajectory(input_traj, resample_interval); - return autoware::motion_utils::convertToTrajectoryPointArray(resampled_input_traj); -} - -std::vector convertToGeometryPoints(const LineString2d & lanelet_points) -{ - std::vector points; - for (const auto & lanelet_point : lanelet_points) { - geometry_msgs::msg::Point point; - point.x = lanelet_point.x(); - point.y = lanelet_point.y(); - points.push_back(point); - } - return points; -} -} // namespace - -StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( - const rclcpp::NodeOptions & node_options) -: Node("static_centerline_generator", node_options) -{ - // publishers - pub_map_bin_ = - create_publisher("lanelet2_map_topic", utils::create_transient_local_qos()); - pub_whole_centerline_ = - create_publisher("~/output/whole_centerline", utils::create_transient_local_qos()); - pub_centerline_ = - create_publisher("~/output/centerline", utils::create_transient_local_qos()); - - // debug publishers - pub_validation_results_ = - create_publisher("~/validation_results", utils::create_transient_local_qos()); - pub_debug_markers_ = - create_publisher("~/debug/markers", utils::create_transient_local_qos()); - - pub_debug_ego_footprint_bounds_ = create_publisher( - "~/debug/ego_footprint_bounds", utils::create_transient_local_qos()); - - // subscribers - sub_footprint_margin_for_road_bound_ = create_subscription( - "/static_centerline_generator/road_boundary_lateral_margin", rclcpp::QoS{1}, - [this](const std_msgs::msg::Float32 & msg) { footprint_margin_for_road_bound_ = msg.data; }); - sub_traj_start_index_ = create_subscription( - "/static_centerline_generator/traj_start_index", rclcpp::QoS{1}, - [this](const std_msgs::msg::Int32 & msg) { - if (centerline_handler_.update_start_index(msg.data)) { - visualize_selected_centerline(); - } - }); - sub_traj_end_index_ = create_subscription( - "/static_centerline_generator/traj_end_index", rclcpp::QoS{1}, - [this](const std_msgs::msg::Int32 & msg) { - if (centerline_handler_.update_end_index(msg.data)) { - visualize_selected_centerline(); - } - }); - sub_save_map_ = create_subscription( - "/static_centerline_generator/save_map", rclcpp::QoS{1}, - [this]([[maybe_unused]] const std_msgs::msg::Empty & msg) { - if (!centerline_handler_.is_valid()) { - return; - } - save_map(); - }); - sub_validate_ = create_subscription( - "/static_centerline_generator/validate", rclcpp::QoS{1}, - [this]([[maybe_unused]] const std_msgs::msg::Empty & msg) { validate(); }); - - // services - callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - srv_load_map_ = create_service( - "/planning/static_centerline_generator/load_map", - std::bind( - &StaticCenterlineGeneratorNode::on_load_map, this, std::placeholders::_1, - std::placeholders::_2), - rmw_qos_profile_services_default, callback_group_); - srv_plan_route_ = create_service( - "/planning/static_centerline_generator/plan_route", - std::bind( - &StaticCenterlineGeneratorNode::on_plan_route, this, std::placeholders::_1, - std::placeholders::_2), - rmw_qos_profile_services_default, callback_group_); - srv_plan_path_ = create_service( - "/planning/static_centerline_generator/plan_path", - std::bind( - &StaticCenterlineGeneratorNode::on_plan_path, this, std::placeholders::_1, - std::placeholders::_2), - rmw_qos_profile_services_default, callback_group_); - - // vehicle info - vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); - - centerline_source_ = [&]() { - const auto centerline_source_param = declare_parameter("centerline_source"); - if (centerline_source_param == "optimization_trajectory_base") { - optimization_trajectory_based_centerline_ = OptimizationTrajectoryBasedCenterline(*this); - return CenterlineSource::OptimizationTrajectoryBase; - } else if (centerline_source_param == "bag_ego_trajectory_base") { - return CenterlineSource::BagEgoTrajectoryBase; - } - throw std::logic_error( - "The centerline source is not supported in autoware_static_centerline_generator."); - }(); -} - -void StaticCenterlineGeneratorNode::visualize_selected_centerline() -{ - // publish selected centerline - const auto selected_centerline = centerline_handler_.get_selected_centerline(); - pub_centerline_->publish( - autoware::motion_utils::convertToTrajectory(selected_centerline, create_header(this->now()))); - - // delete markers for validation - pub_validation_results_->publish(utils::create_delete_all_marker_array({}, now())); - pub_debug_markers_->publish(utils::create_delete_all_marker_array( - {"unsafe_footprints", "unsafe_footprints_distance"}, now())); - pub_debug_ego_footprint_bounds_->publish( - utils::create_delete_all_marker_array({"road_bounds"}, now())); -} - -void StaticCenterlineGeneratorNode::generate_centerline() -{ - // declare planning setting parameters - const auto lanelet2_input_file_path = declare_parameter("lanelet2_input_file_path"); - - // process - load_map(lanelet2_input_file_path); - const auto whole_centerline_with_route = generate_whole_centerline_with_route(); - centerline_handler_ = CenterlineHandler(whole_centerline_with_route); - - visualize_selected_centerline(); -} - -CenterlineWithRoute StaticCenterlineGeneratorNode::generate_whole_centerline_with_route() -{ - if (!route_handler_ptr_) { - RCLCPP_ERROR(get_logger(), "Route handler is not ready. Return empty trajectory."); - return CenterlineWithRoute{}; - } - - // generate centerline with route - auto centerline_with_route = [&]() { - if (centerline_source_ == CenterlineSource::OptimizationTrajectoryBase) { - const lanelet::Id start_lanelet_id = declare_parameter("start_lanelet_id"); - const lanelet::Id end_lanelet_id = declare_parameter("end_lanelet_id"); - const auto route_lane_ids = plan_route_by_lane_ids(start_lanelet_id, end_lanelet_id); - const auto optimized_centerline = - optimization_trajectory_based_centerline_.generate_centerline_with_optimization( - *this, *route_handler_ptr_, route_lane_ids); - return CenterlineWithRoute{optimized_centerline, route_lane_ids}; - } else if (centerline_source_ == CenterlineSource::BagEgoTrajectoryBase) { - const auto bag_centerline = generate_centerline_with_bag(*this); - const auto route_lane_ids = - plan_route(bag_centerline.front().pose, bag_centerline.back().pose); - return CenterlineWithRoute{bag_centerline, route_lane_ids}; - } - throw std::logic_error( - "The centerline source is not supported in autoware_static_centerline_generator."); - }(); - - // resample - const double output_trajectory_interval = declare_parameter("output_trajectory_interval"); - centerline_with_route.centerline = - resample_trajectory_points(centerline_with_route.centerline, output_trajectory_interval); - - pub_whole_centerline_->publish(autoware::motion_utils::convertToTrajectory( - centerline_with_route.centerline, create_header(this->now()))); - - return centerline_with_route; -} - -void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_file_path) -{ - // copy the input LL2 map to the temporary file for debugging - const std::string debug_input_file_dir{"/tmp/autoware_static_centerline_generator/input/"}; - std::filesystem::create_directories(debug_input_file_dir); - std::filesystem::copy( - lanelet2_input_file_path, debug_input_file_dir + "lanelet2_map.osm", - std::filesystem::copy_options::overwrite_existing); - - // load map by the map_loader package - map_bin_ptr_ = [&]() -> LaneletMapBin::ConstSharedPtr { - // load map - map_projector_info_ = std::make_unique( - autoware::map_projection_loader::load_info_from_lanelet2_map(lanelet2_input_file_path)); - const auto map_ptr = autoware::map_loader::Lanelet2MapLoaderNode::load_map( - lanelet2_input_file_path, *map_projector_info_); - if (!map_ptr) { - return nullptr; - } - - // NOTE: The original map is stored here since the centerline will be added to all the - // lanelet when lanelet::utils::overwriteLaneletCenterline is called. - original_map_ptr_ = autoware::map_loader::Lanelet2MapLoaderNode::load_map( - lanelet2_input_file_path, *map_projector_info_); - - // overwrite more dense centerline - // NOTE: overwriteLaneletsCenterlineWithWaypoints is used only in real time calculation. - lanelet::utils::overwriteLaneletsCenterline(map_ptr, 5.0, false); - - // create map bin msg - const auto map_bin_msg = autoware::map_loader::Lanelet2MapLoaderNode::create_map_bin_msg( - map_ptr, lanelet2_input_file_path, now()); - - return std::make_shared(map_bin_msg); - }(); - - // check if map_bin_ptr_ is not null pointer - if (!map_bin_ptr_) { - RCLCPP_ERROR(get_logger(), "Loading map failed"); - return; - } - RCLCPP_INFO(get_logger(), "Loaded map."); - - // publish map bin msg - pub_map_bin_->publish(*map_bin_ptr_); - RCLCPP_INFO(get_logger(), "Published map."); - - // create route_handler - route_handler_ptr_ = std::make_shared(); - route_handler_ptr_->setMap(*map_bin_ptr_); -} - -void StaticCenterlineGeneratorNode::on_load_map( - const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response) -{ - const std::string tmp_lanelet2_input_file_path = "/tmp/input_lanelet2_map.osm"; - - // save map file temporarily since load map's input must be a file - std::ofstream map_writer; - map_writer.open(tmp_lanelet2_input_file_path, std::ios::out); - map_writer << request->map; - map_writer.close(); - - // load map from the saved map file - load_map(tmp_lanelet2_input_file_path); - - if (map_bin_ptr_) { - return; - } - - response->message = "InvalidMapFormat"; -} - -std::vector StaticCenterlineGeneratorNode::plan_route_by_lane_ids( - const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id) -{ - if (!route_handler_ptr_) { - RCLCPP_ERROR(get_logger(), "Map or route handler is not ready. Return empty lane ids."); - return std::vector{}; - } - - const auto start_center_pose = utils::get_center_pose(*route_handler_ptr_, start_lanelet_id); - const auto end_center_pose = utils::get_center_pose(*route_handler_ptr_, end_lanelet_id); - return plan_route(start_center_pose, end_center_pose); -} - -std::vector StaticCenterlineGeneratorNode::plan_route( - const geometry_msgs::msg::Pose & start_center_pose, - const geometry_msgs::msg::Pose & end_center_pose) -{ - if (!map_bin_ptr_) { - RCLCPP_ERROR(get_logger(), "Map or route handler is not ready. Return empty lane ids."); - return std::vector{}; - } - - // plan route by the mission_planner package - const auto route = [&]() { - // calculate check points - RCLCPP_INFO(get_logger(), "Calculated check points."); - const auto check_points = - std::vector{start_center_pose, end_center_pose}; - - // create mission_planner plugin - auto plugin_loader = pluginlib::ClassLoader( - "autoware_mission_planner", "autoware::mission_planner::PlannerPlugin"); - auto mission_planner = - plugin_loader.createSharedInstance("autoware::mission_planner::lanelet2::DefaultPlanner"); - - // initialize mission_planner - auto node = rclcpp::Node("mission_planner"); - mission_planner->initialize(&node, map_bin_ptr_); - - // plan route - const auto route = mission_planner->plan(check_points); - - return route; - }(); - - // get lanelets - const auto route_lane_ids = get_lane_ids_from_route(route); - - std::string route_lane_ids_str = ""; - for (const lanelet::Id route_lane_id : route_lane_ids) { - route_lane_ids_str += std::to_string(route_lane_id) + ","; - } - RCLCPP_INFO_STREAM(get_logger(), "Planned route. (" << route_lane_ids_str << ")"); - - return route_lane_ids; -} - -void StaticCenterlineGeneratorNode::on_plan_route( - const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response) -{ - if (!map_bin_ptr_ || !route_handler_ptr_) { - response->message = "MapNotFound"; - RCLCPP_ERROR(get_logger(), "Map is not ready."); - return; - } - - const lanelet::Id start_lanelet_id = request->start_lane_id; - const lanelet::Id end_lanelet_id = request->end_lane_id; - - // plan route - const auto route_lane_ids = plan_route_by_lane_ids(start_lanelet_id, end_lanelet_id); - const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - - // extract lane ids - std::vector lane_ids; - for (const auto & lanelet : route_lanelets) { - lane_ids.push_back(lanelet.id()); - } - - // check calculation result - if (lane_ids.empty()) { - response->message = "RouteNotFound"; - RCLCPP_ERROR(get_logger(), "Route planning failed."); - return; - } - - // set response - response->lane_ids = lane_ids; -} - -void StaticCenterlineGeneratorNode::on_plan_path( - const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response) -{ - if (!route_handler_ptr_) { - response->message = "MapNotFound"; - RCLCPP_ERROR(get_logger(), "Route handler is not ready."); - return; - } - - // get lanelets from route lane ids - const auto route_lane_ids = request->route; - const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - - // check if input route lanelets are connected to each other. - const auto unconnected_lane_ids = check_lanelet_connection(*route_handler_ptr_, route_lanelets); - if (!unconnected_lane_ids.empty()) { - response->message = "LaneletsNotConnected"; - response->unconnected_lane_ids = unconnected_lane_ids; - RCLCPP_ERROR(get_logger(), "Lanelets are not connected."); - return; - } - - // plan path - const auto optimized_traj_points = - optimization_trajectory_based_centerline_.generate_centerline_with_optimization( - *this, *route_handler_ptr_, route_lane_ids); - - // check calculation result - if (optimized_traj_points.empty()) { - response->message = "PathNotFound"; - RCLCPP_ERROR(get_logger(), "Path planning failed."); - return; - } - - centerline_handler_ = - CenterlineHandler(CenterlineWithRoute{optimized_traj_points, route_lane_ids}); - - // publish unsafe_footprints - validate(); - - // create output data - auto target_traj_point = optimized_traj_points.cbegin(); - bool is_end_lanelet = false; - for (const auto & lanelet : route_lanelets) { - std::vector current_lanelet_points; - - // check if target point is inside the lanelet - while (lanelet::geometry::inside( - lanelet, convert_to_lanelet_point(target_traj_point->pose.position))) { - // memorize points inside the lanelet - current_lanelet_points.push_back(target_traj_point->pose.position); - target_traj_point++; - - if (target_traj_point == optimized_traj_points.cend()) { - is_end_lanelet = true; - break; - } - } - - if (!current_lanelet_points.empty()) { - // register points with lane_id - autoware_static_centerline_generator::msg::PointsWithLaneId points_with_lane_id; - points_with_lane_id.lane_id = lanelet.id(); - points_with_lane_id.points = current_lanelet_points; - response->points_with_lane_ids.push_back(points_with_lane_id); - } - - if (is_end_lanelet) { - break; - } - } - - // empty string if error did not occur - response->message = ""; -} - -void StaticCenterlineGeneratorNode::validate() -{ - std::cerr << std::endl - << "############################################## Validation Results " - "##############################################" - << std::endl; - - const auto & centerline = centerline_handler_.get_selected_centerline(); - const auto & route_lane_ids = centerline_handler_.get_route_lane_ids(); - const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - - const double dist_thresh_to_road_border = - getRosParameter("validation.dist_threshold_to_road_border"); - const double max_steer_angle_margin = - getRosParameter("validation.max_steer_angle_margin"); - - // calculate color for distance to road border - const auto dist_thresh_vec = getRosParameter>("marker_color_dist_thresh"); - const auto marker_color_vec = getRosParameter>("marker_color"); - const auto get_marker_color = [&](const double dist) -> boost::optional> { - for (size_t i = 0; i < dist_thresh_vec.size(); ++i) { - const double dist_thresh = dist_thresh_vec.at(i); - if (dist < dist_thresh) { - return convert_hex_string_to_decimal(marker_color_vec.at(i)); - } - } - return boost::none; - }; - - // create right/left bound - LineString2d lanelet_right_bound; - LineString2d lanelet_left_bound; - for (const auto & lanelet : route_lanelets) { - for (const auto & point : lanelet.rightBound()) { - boost::geometry::append(lanelet_right_bound, Point2d(point.x(), point.y())); - } - for (const auto & point : lanelet.leftBound()) { - boost::geometry::append(lanelet_left_bound, Point2d(point.x(), point.y())); - } - } - - // calculate curvature - const auto curvature_vec = autoware::motion_utils::calcCurvature(centerline); - const double steer_angle_threshold = vehicle_info_.max_steer_angle_rad - max_steer_angle_margin; - - // calculate the distance between footprint and right/left bounds - MarkerArray marker_array; - double min_dist = std::numeric_limits::max(); - double max_curvature = std::numeric_limits::min(); - for (size_t i = 0; i < centerline.size(); ++i) { - const auto & traj_point = centerline.at(i); - - const auto footprint_poly = create_vehicle_footprint(traj_point.pose, vehicle_info_); - - const double dist_to_right = boost::geometry::distance(footprint_poly, lanelet_right_bound); - const double dist_to_left = boost::geometry::distance(footprint_poly, lanelet_left_bound); - const double min_dist_to_bound = std::min(dist_to_right, dist_to_left); - - if (min_dist_to_bound < min_dist) { - min_dist = min_dist_to_bound; - } - - // create marker - const auto marker_color_opt = get_marker_color(min_dist_to_bound); - const auto text_pose = get_text_pose(traj_point.pose, vehicle_info_); - if (marker_color_opt) { - const auto & marker_color = marker_color_opt.get(); - - // add footprint marker - const auto footprint_marker = utils::create_footprint_marker( - footprint_poly, 0.05, marker_color.at(0), marker_color.at(1), marker_color.at(2), 0.7, - now(), i); - marker_array.markers.push_back(footprint_marker); - - // add text of distance to bounds marker - const auto text_marker = utils::create_text_marker( - "unsafe_footprints_distance", text_pose, min_dist_to_bound, marker_color.at(0), - marker_color.at(1), marker_color.at(2), 0.999, now(), i); - marker_array.markers.push_back(text_marker); - } - - const double curvature = curvature_vec.at(i); - const auto text_marker = - utils::create_text_marker("curvature", text_pose, curvature, 0.05, 0.05, 0.0, 0.9, now(), i); - marker_array.markers.push_back(text_marker); - - if (max_curvature < std::abs(curvature)) { - max_curvature = std::abs(curvature); - } - } - const double max_steer_angle = vehicle_info_.calcSteerAngleFromCurvature(max_curvature); - - // publish road boundaries - const auto left_bound = convertToGeometryPoints(lanelet_left_bound); - const auto right_bound = convertToGeometryPoints(lanelet_right_bound); - - marker_array.markers.push_back( - utils::create_points_marker("left_bound", left_bound, 0.05, 0.0, 0.6, 0.8, 0.8, now())); - marker_array.markers.push_back( - utils::create_points_marker("right_bound", right_bound, 0.05, 0.0, 0.6, 0.8, 0.8, now())); - pub_debug_markers_->publish(marker_array); - - // show the validation results - // 1. distance from footprints to road boundaries - const bool are_footprints_inside_lanelets = [&]() { - std::cerr << "1. Footprints inside Lanelets:" << std::endl; - if (dist_thresh_to_road_border < min_dist) { - std::cerr << " The generated centerline is inside the lanelet. (threshold:" - << dist_thresh_to_road_border << "[m] < actual:" << min_dist << "[m])" << std::endl - << " Passed." << std::endl; - return true; - } - std::cerr << RED_TEXT - << " The generated centerline is outside the lanelet. (actual:" << min_dist - << "[m] <= threshold:" << dist_thresh_to_road_border << "[m])" << std::endl - << " Failed." << RESET_TEXT << std::endl; - return false; - }(); - // 2. centerline's curvature - std::cerr << "2. Curvature:" << std::endl; - const bool is_curvature_low = - true; // always tre for now since the curvature is just estimated and not enough precise. - if (max_steer_angle < steer_angle_threshold) { - std::cerr << " The generated centerline has no high steer angle. (estimated:" - << autoware::universe_utils::rad2deg(max_steer_angle) - << "[deg] < threshold:" << autoware::universe_utils::rad2deg(steer_angle_threshold) - << "[deg])" << std::endl - << " Passed." << std::endl; - } else { - std::cerr << YELLOW_TEXT << " The generated centerline has a too high steer angle. (threshold:" - << autoware::universe_utils::rad2deg(steer_angle_threshold) - << "[deg] <= estimated:" << autoware::universe_utils::rad2deg(max_steer_angle) - << "[deg])" << std::endl - << " However, the estimated steer angle is not enough precise, so the result is " - "conditional pass." - << std::endl - << " Conditionally Passed." << RESET_TEXT << std::endl; - } - // 3. result - std::cerr << std::endl << BOLD_TEXT << "Result:" << RESET_TEXT << std::endl; - if (are_footprints_inside_lanelets && is_curvature_low) { - std::cerr << BOLD_TEXT << " Passed!" << RESET_TEXT << std::endl; - } else { - std::cerr << BOLD_TEXT << RED_TEXT << " Failed!" << RESET_TEXT << std::endl; - } - - std::cerr << "###################################################################################" - "#############################" - << std::endl - << std::endl; - RCLCPP_INFO(get_logger(), "Validated the generated centerline."); -} - -void StaticCenterlineGeneratorNode::save_map() -{ - if (!route_handler_ptr_) { - return; - } - - const auto & centerline = centerline_handler_.get_selected_centerline(); - const auto & route_lane_ids = centerline_handler_.get_route_lane_ids(); - - const auto lanelet2_output_file_path = getRosParameter("lanelet2_output_file_path"); - - const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - - // update centerline in map - utils::update_centerline(original_map_ptr_, route_lanelets, centerline); - RCLCPP_INFO(get_logger(), "Updated centerline in map."); - - // save map with modified center line - std::filesystem::create_directory("/tmp/autoware_static_centerline_generator"); - const auto map_projector = - autoware::geography_utils::get_lanelet2_projector(*map_projector_info_); - lanelet::write(lanelet2_output_file_path, *original_map_ptr_, *map_projector); - RCLCPP_INFO( - get_logger(), "Saved map in %s", "/tmp/autoware_static_centerline_generator/lanelet2_map.osm"); - - // copy the output LL2 map to the temporary file for debugging - const std::string debug_output_file_dir{"/tmp/autoware_static_centerline_generator/output/"}; - std::filesystem::create_directories(debug_output_file_dir); - std::filesystem::copy( - lanelet2_output_file_path, debug_output_file_dir + "lanelet2_map.osm", - std::filesystem::copy_options::overwrite_existing); -} -} // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp deleted file mode 100644 index c591dcfc73bd8..0000000000000 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp +++ /dev/null @@ -1,187 +0,0 @@ -// Copyright 2022 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 STATIC_CENTERLINE_GENERATOR_NODE_HPP_ -#define STATIC_CENTERLINE_GENERATOR_NODE_HPP_ - -#include "autoware/universe_utils/ros/parameter.hpp" -#include "autoware_static_centerline_generator/srv/load_map.hpp" -#include "autoware_static_centerline_generator/srv/plan_path.hpp" -#include "autoware_static_centerline_generator/srv/plan_route.hpp" -#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "centerline_source/optimization_trajectory_based_centerline.hpp" -#include "rclcpp/rclcpp.hpp" -#include "type_alias.hpp" - -#include "autoware_map_msgs/msg/map_projector_info.hpp" -#include "std_msgs/msg/empty.hpp" -#include "std_msgs/msg/float32.hpp" -#include "std_msgs/msg/int32.hpp" - -#include -#include -#include -#include - -namespace autoware::static_centerline_generator -{ -using autoware_map_msgs::msg::MapProjectorInfo; -using autoware_static_centerline_generator::srv::LoadMap; -using autoware_static_centerline_generator::srv::PlanPath; -using autoware_static_centerline_generator::srv::PlanRoute; - -struct CenterlineWithRoute -{ - std::vector centerline{}; - std::vector route_lane_ids{}; -}; -struct CenterlineHandler -{ - CenterlineHandler() = default; - explicit CenterlineHandler(const CenterlineWithRoute & centerline_with_route) - : whole_centerline_with_route(centerline_with_route), - start_index(0), - end_index(centerline_with_route.centerline.size() - 1) - { - } - std::vector get_selected_centerline() const - { - if (!whole_centerline_with_route) { - return std::vector{}; - } - const auto & centerline_begin = whole_centerline_with_route->centerline.begin(); - return std::vector( - centerline_begin + start_index, centerline_begin + end_index + 1); - } - std::vector get_route_lane_ids() const - { - if (!whole_centerline_with_route) { - return std::vector{}; - } - return whole_centerline_with_route->route_lane_ids; - } - bool is_valid() const { return whole_centerline_with_route && start_index < end_index; } - bool update_start_index(const int arg_start_index) - { - if (whole_centerline_with_route && arg_start_index < end_index) { - start_index = arg_start_index; - return true; - } - return false; - } - bool update_end_index(const int arg_end_index) - { - if (whole_centerline_with_route && start_index < arg_end_index) { - end_index = arg_end_index; - return true; - } - return false; - } - - std::optional whole_centerline_with_route{std::nullopt}; - int start_index{}; - int end_index{}; -}; - -struct RoadBounds -{ - std::vector left_bound; - std::vector right_bound; -}; - -class StaticCenterlineGeneratorNode : public rclcpp::Node -{ -public: - explicit StaticCenterlineGeneratorNode(const rclcpp::NodeOptions & node_options); - void generate_centerline(); - void validate(); - void save_map(); - -private: - // load map - void load_map(const std::string & lanelet2_input_file_path); - void on_load_map( - const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response); - - // plan route - std::vector plan_route_by_lane_ids( - const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id); - std::vector plan_route( - const geometry_msgs::msg::Pose & start_center_pose, - const geometry_msgs::msg::Pose & end_center_pose); - - void on_plan_route( - const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response); - - // plan centerline - CenterlineWithRoute generate_whole_centerline_with_route(); - std::vector get_route_lane_ids_from_points( - const std::vector & points); - void on_plan_path( - const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response); - - void visualize_selected_centerline(); - - // parameter - template - T getRosParameter(const std::string & param_name) - { - return autoware::universe_utils::getOrDeclareParameter(*this, param_name); - } - - lanelet::LaneletMapPtr original_map_ptr_{nullptr}; - LaneletMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; - std::shared_ptr route_handler_ptr_{nullptr}; - std::unique_ptr map_projector_info_{nullptr}; - - CenterlineHandler centerline_handler_; - - float footprint_margin_for_road_bound_{0.0}; - - enum class CenterlineSource { - OptimizationTrajectoryBase = 0, - BagEgoTrajectoryBase, - }; - CenterlineSource centerline_source_; - OptimizationTrajectoryBasedCenterline optimization_trajectory_based_centerline_; - - // publisher - rclcpp::Publisher::SharedPtr pub_map_bin_{nullptr}; - rclcpp::Publisher::SharedPtr pub_whole_centerline_{nullptr}; - rclcpp::Publisher::SharedPtr pub_centerline_{nullptr}; - rclcpp::Publisher::SharedPtr pub_validation_results_{nullptr}; - rclcpp::Publisher::SharedPtr pub_debug_ego_footprint_bounds_{nullptr}; - rclcpp::Publisher::SharedPtr pub_debug_markers_{nullptr}; - - // subscriber - rclcpp::Subscription::SharedPtr sub_traj_start_index_; - rclcpp::Subscription::SharedPtr sub_traj_end_index_; - rclcpp::Subscription::SharedPtr sub_save_map_; - rclcpp::Subscription::SharedPtr sub_validate_; - rclcpp::Subscription::SharedPtr sub_traj_resample_interval_; - rclcpp::Subscription::SharedPtr sub_footprint_margin_for_road_bound_; - - // service - rclcpp::Service::SharedPtr srv_load_map_; - rclcpp::Service::SharedPtr srv_plan_route_; - rclcpp::Service::SharedPtr srv_plan_path_; - - // callback group for service - rclcpp::CallbackGroup::SharedPtr callback_group_; - - // vehicle info - autoware::vehicle_info_utils::VehicleInfo vehicle_info_; -}; -} // namespace autoware::static_centerline_generator -#endif // STATIC_CENTERLINE_GENERATOR_NODE_HPP_ diff --git a/planning/autoware_static_centerline_generator/src/type_alias.hpp b/planning/autoware_static_centerline_generator/src/type_alias.hpp deleted file mode 100644 index 2b7b99bfe81be..0000000000000 --- a/planning/autoware_static_centerline_generator/src/type_alias.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2022 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 TYPE_ALIAS_HPP_ -#define TYPE_ALIAS_HPP_ - -#include "autoware/route_handler/route_handler.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" - -#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" -#include "autoware_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_planning_msgs/msg/lanelet_route.hpp" -#include "autoware_planning_msgs/msg/path.hpp" -#include "autoware_planning_msgs/msg/path_point.hpp" -#include "autoware_planning_msgs/msg/trajectory.hpp" -#include "autoware_planning_msgs/msg/trajectory_point.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - -namespace autoware::static_centerline_generator -{ -using autoware::route_handler::RouteHandler; -using autoware::universe_utils::LinearRing2d; -using autoware::universe_utils::LineString2d; -using autoware::universe_utils::Point2d; -using autoware_map_msgs::msg::LaneletMapBin; -using autoware_perception_msgs::msg::PredictedObjects; -using autoware_planning_msgs::msg::LaneletRoute; -using autoware_planning_msgs::msg::Path; -using autoware_planning_msgs::msg::PathPoint; -using autoware_planning_msgs::msg::Trajectory; -using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_planning_msgs::msg::PathWithLaneId; -using visualization_msgs::msg::Marker; -using visualization_msgs::msg::MarkerArray; -} // namespace autoware::static_centerline_generator - -#endif // TYPE_ALIAS_HPP_ diff --git a/planning/autoware_static_centerline_generator/src/utils.cpp b/planning/autoware_static_centerline_generator/src/utils.cpp deleted file mode 100644 index f84fe79cec288..0000000000000 --- a/planning/autoware_static_centerline_generator/src/utils.cpp +++ /dev/null @@ -1,263 +0,0 @@ -// Copyright 2022 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 "utils.hpp" - -#include "autoware/behavior_path_planner_common/data_manager.hpp" -#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/ros/marker_helper.hpp" - -#include -#include - -#include -#include -#include -#include -#include - -namespace autoware::static_centerline_generator -{ -namespace -{ -nav_msgs::msg::Odometry::ConstSharedPtr convert_to_odometry(const geometry_msgs::msg::Pose & pose) -{ - auto odometry_ptr = std::make_shared(); - odometry_ptr->pose.pose = pose; - return odometry_ptr; -} - -lanelet::Point3d createPoint3d(const double x, const double y, const double z) -{ - lanelet::Point3d point(lanelet::utils::getId()); - - // position - point.x() = x; - point.y() = y; - point.z() = z; - - // attributes - point.setAttribute("local_x", x); - point.setAttribute("local_y", y); - // NOTE: It seems that the attribute "ele" is assigned automatically. - - return point; -} -} // namespace - -namespace utils -{ -rclcpp::QoS create_transient_local_qos() -{ - return rclcpp::QoS{1}.transient_local(); -} - -lanelet::ConstLanelets get_lanelets_from_ids( - const RouteHandler & route_handler, const std::vector & lane_ids) -{ - lanelet::ConstLanelets lanelets; - for (const lanelet::Id lane_id : lane_ids) { - const auto lanelet = route_handler.getLaneletsFromId(lane_id); - lanelets.push_back(lanelet); - } - return lanelets; -} - -geometry_msgs::msg::Pose get_center_pose( - const RouteHandler & route_handler, const size_t lanelet_id) -{ - // get middle idx of the lanelet - const auto lanelet = route_handler.getLaneletsFromId(lanelet_id); - const auto center_line = lanelet.centerline(); - const size_t middle_point_idx = std::floor(center_line.size() / 2.0); - - // get middle position of the lanelet - geometry_msgs::msg::Point middle_pos; - middle_pos.x = center_line[middle_point_idx].x(); - middle_pos.y = center_line[middle_point_idx].y(); - middle_pos.z = center_line[middle_point_idx].z(); - - // get next middle position of the lanelet - geometry_msgs::msg::Point next_middle_pos; - next_middle_pos.x = center_line[middle_point_idx + 1].x(); - next_middle_pos.y = center_line[middle_point_idx + 1].y(); - next_middle_pos.z = center_line[middle_point_idx + 1].z(); - - // calculate middle pose - geometry_msgs::msg::Pose middle_pose; - middle_pose.position = middle_pos; - const double yaw = autoware::universe_utils::calcAzimuthAngle(middle_pos, next_middle_pos); - middle_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); - - return middle_pose; -} - -PathWithLaneId get_path_with_lane_id( - const RouteHandler & route_handler, const lanelet::ConstLanelets lanelets, - const geometry_msgs::msg::Pose & start_pose, const double ego_nearest_dist_threshold, - const double ego_nearest_yaw_threshold) -{ - // get center line - constexpr double s_start = 0.0; - constexpr double s_end = std::numeric_limits::max(); - auto path_with_lane_id = route_handler.getCenterLinePath(lanelets, s_start, s_end); - path_with_lane_id.header.frame_id = "map"; - - // create planner data - auto planner_data = std::make_shared(); - planner_data->route_handler = std::make_shared(route_handler); - planner_data->self_odometry = convert_to_odometry(start_pose); - planner_data->parameters.ego_nearest_dist_threshold = ego_nearest_dist_threshold; - planner_data->parameters.ego_nearest_yaw_threshold = ego_nearest_yaw_threshold; - - // generate drivable area and store it in path with lane id - constexpr double vehicle_length = 0.0; - const auto drivable_lanes = behavior_path_planner::utils::generateDrivableLanes(lanelets); - behavior_path_planner::utils::generateDrivableArea( - path_with_lane_id, drivable_lanes, false, false, vehicle_length, planner_data); - - return path_with_lane_id; -} - -void update_centerline( - lanelet::LaneletMapPtr lanelet_map_ptr, const lanelet::ConstLanelets & lanelets, - const std::vector & new_centerline) -{ - // get lanelet as reference to update centerline - lanelet::Lanelets lanelets_ref; - for (const auto & lanelet : lanelets) { - for (auto & lanelet_ref : lanelet_map_ptr->laneletLayer) { - if (lanelet_ref.id() == lanelet.id()) { - lanelets_ref.push_back(lanelet_ref); - } - } - } - - // store new centerline in lanelets - size_t lanelet_idx = 0; - lanelet::LineString3d centerline(lanelet::utils::getId()); - for (size_t traj_idx = 0; traj_idx < new_centerline.size(); ++traj_idx) { - const auto & traj_pos = new_centerline.at(traj_idx).pose.position; - - for (; lanelet_idx < lanelets_ref.size(); ++lanelet_idx) { - auto & lanelet_ref = lanelets_ref.at(lanelet_idx); - - const lanelet::BasicPoint2d point(traj_pos.x, traj_pos.y); - // TODO(murooka) This does not work with L-crank map. - const bool is_inside = lanelet::geometry::inside(lanelet_ref, point); - if (is_inside) { - const auto center_point = createPoint3d(traj_pos.x, traj_pos.y, traj_pos.z); - - // set center point - centerline.push_back(center_point); - lanelet_map_ptr->add(center_point); - break; - } - - if (!centerline.empty()) { - // set centerline - lanelet_map_ptr->add(centerline); - lanelet_ref.setCenterline(centerline); - - // prepare new centerline - centerline = lanelet::LineString3d(lanelet::utils::getId()); - } - } - - if (traj_idx == new_centerline.size() - 1 && !centerline.empty()) { - auto & lanelet_ref = lanelets_ref.at(lanelet_idx); - - // set centerline - lanelet_map_ptr->add(centerline); - lanelet_ref.setCenterline(centerline); - } - } -} - -Marker create_footprint_marker( - const LinearRing2d & footprint_poly, const double width, const double r, const double g, - const double b, const double a, const rclcpp::Time & now, const size_t idx) -{ - auto marker = autoware::universe_utils::createDefaultMarker( - "map", rclcpp::Clock().now(), "unsafe_footprints", idx, - visualization_msgs::msg::Marker::LINE_STRIP, - autoware::universe_utils::createMarkerScale(width, 0.0, 0.0), - autoware::universe_utils::createMarkerColor(r, g, b, a)); - marker.header.stamp = now; - // TODO(murooka) Ideally, the following is unnecessary for the topic of transient local. - marker.lifetime = rclcpp::Duration(0, 0); - - for (const auto & point : footprint_poly) { - geometry_msgs::msg::Point geom_point; - geom_point.x = point.x(); - geom_point.y = point.y(); - // geom_point.z = point.z(); - - marker.points.push_back(geom_point); - } - marker.points.push_back(marker.points.front()); - return marker; -} - -Marker create_text_marker( - const std::string & ns, const geometry_msgs::msg::Pose & pose, const double value, const double r, - const double g, const double b, const double a, const rclcpp::Time & now, const size_t idx) -{ - auto marker = autoware::universe_utils::createDefaultMarker( - "map", rclcpp::Clock().now(), ns, idx, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - autoware::universe_utils::createMarkerScale(0.5, 0.5, 0.5), - autoware::universe_utils::createMarkerColor(r, g, b, a)); - marker.pose = pose; - marker.header.stamp = now; - marker.lifetime = rclcpp::Duration(0, 0); - - std::stringstream ss; - ss << std::setprecision(2) << value; - marker.text = ss.str(); - - return marker; -} - -Marker create_points_marker( - const std::string & ns, const std::vector & points, const double width, - const double r, const double g, const double b, const double a, const rclcpp::Time & now) -{ - auto marker = autoware::universe_utils::createDefaultMarker( - "map", now, ns, 1, Marker::LINE_STRIP, - autoware::universe_utils::createMarkerScale(width, 0.0, 0.0), - autoware::universe_utils::createMarkerColor(r, g, b, a)); - marker.lifetime = rclcpp::Duration(0, 0); - marker.points = points; - return marker; -} - -MarkerArray create_delete_all_marker_array( - const std::vector & ns_vec, const rclcpp::Time & now) -{ - Marker marker; - marker.header.stamp = now; - marker.action = visualization_msgs::msg::Marker::DELETEALL; - - MarkerArray marker_array; - for (const auto & ns : ns_vec) { - marker.ns = ns; - marker_array.markers.push_back(marker); - } - - return marker_array; -} - -} // namespace utils -} // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/utils.hpp b/planning/autoware_static_centerline_generator/src/utils.hpp deleted file mode 100644 index f4d15d3dcfd4f..0000000000000 --- a/planning/autoware_static_centerline_generator/src/utils.hpp +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright 2022 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 UTILS_HPP_ -#define UTILS_HPP_ - -#include "autoware/route_handler/route_handler.hpp" -#include "type_alias.hpp" - -#include - -#include -#include -#include -#include - -namespace autoware::static_centerline_generator -{ -namespace utils -{ -rclcpp::QoS create_transient_local_qos(); - -lanelet::ConstLanelets get_lanelets_from_ids( - const RouteHandler & route_handler, const std::vector & lane_ids); - -geometry_msgs::msg::Pose get_center_pose( - const RouteHandler & route_handler, const size_t lanelet_id); - -PathWithLaneId get_path_with_lane_id( - const RouteHandler & route_handler, const lanelet::ConstLanelets lanelets, - const geometry_msgs::msg::Pose & start_pose, const double ego_nearest_dist_threshold, - const double ego_nearest_yaw_threshold); - -void update_centerline( - lanelet::LaneletMapPtr lanelet_map_ptr, const lanelet::ConstLanelets & lanelets, - const std::vector & new_centerline); - -Marker create_footprint_marker( - const LinearRing2d & footprint_poly, const double width, const double r, const double g, - const double b, const double a, const rclcpp::Time & now, const size_t idx); - -Marker create_text_marker( - const std::string & ns, const geometry_msgs::msg::Pose & pose, const double value, const double r, - const double g, const double b, const double a, const rclcpp::Time & now, const size_t idx); - -Marker create_points_marker( - const std::string & ns, const std::vector & points, const double width, - const double r, const double g, const double b, const double a, const rclcpp::Time & now); - -MarkerArray create_delete_all_marker_array( - const std::vector & ns_vec, const rclcpp::Time & now); - -} // namespace utils -} // namespace autoware::static_centerline_generator - -#endif // UTILS_HPP_ diff --git a/planning/autoware_static_centerline_generator/srv/LoadMap.srv b/planning/autoware_static_centerline_generator/srv/LoadMap.srv deleted file mode 100644 index 02142e60c0035..0000000000000 --- a/planning/autoware_static_centerline_generator/srv/LoadMap.srv +++ /dev/null @@ -1,3 +0,0 @@ -string map ---- -string message diff --git a/planning/autoware_static_centerline_generator/srv/PlanPath.srv b/planning/autoware_static_centerline_generator/srv/PlanPath.srv deleted file mode 100644 index 3a8d0321ffb9a..0000000000000 --- a/planning/autoware_static_centerline_generator/srv/PlanPath.srv +++ /dev/null @@ -1,6 +0,0 @@ -uint32 map_id -int64[] route ---- -autoware_static_centerline_generator/PointsWithLaneId[] points_with_lane_ids -string message -int64[] unconnected_lane_ids diff --git a/planning/autoware_static_centerline_generator/srv/PlanRoute.srv b/planning/autoware_static_centerline_generator/srv/PlanRoute.srv deleted file mode 100644 index fb50c04b0ff26..0000000000000 --- a/planning/autoware_static_centerline_generator/srv/PlanRoute.srv +++ /dev/null @@ -1,5 +0,0 @@ -int64 start_lane_id -int64 end_lane_id ---- -int64[] lane_ids -string message diff --git a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 deleted file mode 100644 index ed3448772b794..0000000000000 Binary files a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 and /dev/null differ diff --git a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-right.db3 b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-right.db3 deleted file mode 100644 index 0883307acbae0..0000000000000 Binary files a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-right.db3 and /dev/null differ diff --git a/planning/autoware_static_centerline_generator/test/data/lanelet2_map.osm b/planning/autoware_static_centerline_generator/test/data/lanelet2_map.osm deleted file mode 100644 index 406cd85c151ea..0000000000000 --- a/planning/autoware_static_centerline_generator/test/data/lanelet2_map.osm +++ /dev/null @@ -1,146 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py deleted file mode 100644 index ca2621212ef83..0000000000000 --- a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py +++ /dev/null @@ -1,112 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2022 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. - -import os -import unittest - -from ament_index_python import get_package_share_directory -import launch -from launch import LaunchDescription -from launch_ros.actions import Node -import launch_testing -import pytest - - -@pytest.mark.launch_test -def generate_test_description(): - lanelet2_map_path = os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), - "test/data/lanelet2_map.osm", - ) - - static_centerline_generator_node = Node( - package="autoware_static_centerline_generator", - executable="main", - output="screen", - parameters=[ - {"lanelet2_map_path": lanelet2_map_path}, - {"mode": "AUTO"}, - {"rviz": False}, - {"centerline_source": "optimization_trajectory_base"}, - {"lanelet2_input_file_path": lanelet2_map_path}, - {"lanelet2_output_file_path": "/tmp/lanelet2_map.osm"}, - {"start_lanelet_id": 215}, - {"end_lanelet_id": 216}, - os.path.join( - get_package_share_directory("autoware_mission_planner"), - "config", - "mission_planner.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), - "config/static_centerline_generator.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_behavior_path_planner"), - "config/behavior_path_planner.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_behavior_velocity_planner"), - "config/behavior_velocity_planner.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_path_smoother"), - "config/elastic_band_smoother.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_path_optimizer"), - "config/path_optimizer.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_map_loader"), - "config/lanelet2_map_loader.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), - "config/common.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), - "config/nearest_search.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), - "config/vehicle_info.param.yaml", - ), - ], - ) - - context = {} - - return ( - LaunchDescription( - [ - static_centerline_generator_node, - # Start test after 1s - gives time for the autoware_static_centerline_generator to finish initialization - launch.actions.TimerAction( - period=1.0, actions=[launch_testing.actions.ReadyToTest()] - ), - ] - ), - context, - ) - - -@launch_testing.post_shutdown_test() -class TestProcessOutput(unittest.TestCase): - def test_exit_code(self, proc_info): - # Check that process exits with code 0: no error - launch_testing.asserts.assertExitCodes(proc_info) 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/examples/plot_map_case1.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp index 56e207c160873..46d3b97b71e58 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp @@ -288,7 +288,7 @@ bool hasEnoughDistance( std::vector selectPullOverPaths( const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates, const std::shared_ptr planner_data, - const GoalPlannerParameters & parameters, const BehaviorModuleOutput & previous_module_output) + const GoalPlannerParameters & parameters, const BehaviorModuleOutput & upstream_module_output) { using autoware::behavior_path_planner::utils::getExtendedCurrentLanesFromPath; using autoware::motion_utils::calcSignedArcLength; @@ -313,15 +313,15 @@ std::vector selectPullOverPaths( } const double prev_path_front_to_goal_dist = calcSignedArcLength( - previous_module_output.path.points, - previous_module_output.path.points.front().point.pose.position, goal_pose.position); + upstream_module_output.path.points, + upstream_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 previous_module_output.path; + return upstream_module_output.path; } // get road lanes which is at least backward_length[m] behind the goal const auto road_lanes = getExtendedCurrentLanesFromPath( - previous_module_output.path, planner_data, backward_length, 0.0, false); + upstream_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), diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp index 102e5ef53b164..3518b5041be53 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp @@ -287,7 +287,7 @@ bool hasEnoughDistance( std::vector selectPullOverPaths( const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates, const std::shared_ptr planner_data, - const GoalPlannerParameters & parameters, const BehaviorModuleOutput & previous_module_output) + const GoalPlannerParameters & parameters, const BehaviorModuleOutput & upstream_module_output) { using autoware::behavior_path_planner::utils::getExtendedCurrentLanesFromPath; using autoware::motion_utils::calcSignedArcLength; @@ -312,15 +312,15 @@ std::vector selectPullOverPaths( } const double prev_path_front_to_goal_dist = calcSignedArcLength( - previous_module_output.path.points, - previous_module_output.path.points.front().point.pose.position, goal_pose.position); + upstream_module_output.path.points, + upstream_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 previous_module_output.path; + return upstream_module_output.path; } // get road lanes which is at least backward_length[m] behind the goal const auto road_lanes = getExtendedCurrentLanesFromPath( - previous_module_output.path, planner_data, backward_length, 0.0, false); + upstream_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), diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp index f670e3b05fa77..201b7c2d33bf6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp @@ -38,15 +38,15 @@ class FixedGoalPlannerBase virtual BehaviorModuleOutput plan( const std::shared_ptr & planner_data) const = 0; - void setPreviousModuleOutput(const BehaviorModuleOutput & previous_module_output) + void setPreviousModuleOutput(const BehaviorModuleOutput & upstream_module_output) { - previous_module_output_ = previous_module_output; + upstream_module_output_ = upstream_module_output; } - BehaviorModuleOutput getPreviousModuleOutput() const { return previous_module_output_; } + BehaviorModuleOutput getPreviousModuleOutput() const { return upstream_module_output_; } protected: - BehaviorModuleOutput previous_module_output_; + BehaviorModuleOutput upstream_module_output_; }; } // namespace autoware::behavior_path_planner 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 3a1773324fe24..0765147cbeb80 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 @@ -133,12 +133,12 @@ bool isOnModifiedGoal( const GoalPlannerParameters & parameters); bool hasPreviousModulePathShapeChanged( - const BehaviorModuleOutput & previous_module_output, - const BehaviorModuleOutput & last_previous_module_output); + const BehaviorModuleOutput & upstream_module_output, + const BehaviorModuleOutput & last_upstream_module_output); bool hasDeviatedFromLastPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & last_previous_module_output); + const PlannerData & planner_data, const BehaviorModuleOutput & last_upstream_module_output); bool hasDeviatedFromCurrentPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & previous_module_output); + const PlannerData & planner_data, const BehaviorModuleOutput & upstream_module_output); bool needPathUpdate( const Pose & current_pose, const double path_update_duration, const rclcpp::Time & now, @@ -189,6 +189,7 @@ class LaneParkingPlanner void onTimer(); private: + const GoalPlannerParameters parameters_; std::mutex & mutex_; const std::optional & request_; LaneParkingResponse & response_; @@ -196,6 +197,10 @@ class LaneParkingPlanner rclcpp::Logger logger_; std::vector> pull_over_planners_; + BehaviorModuleOutput + original_upstream_module_output_; // plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) override; + const BehaviorModuleOutput & upstream_module_output) override; std::vector plans( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output); + const BehaviorModuleOutput & upstream_module_output); private: const LaneDepartureChecker lane_departure_checker_; @@ -50,7 +50,7 @@ class BezierPullOver : public PullOverPlannerBase std::vector generateBezierPath( const GoalCandidate & goal_candidate, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, + const BehaviorModuleOutput & upstream_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const; PathWithLaneId generateReferencePath( 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 e32488965f69a..37c82ea904a55 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 @@ -41,7 +41,7 @@ class FreespacePullOver : public PullOverPlannerBase std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) override; + const BehaviorModuleOutput & upstream_module_output) override; protected: const double velocity_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp index 89181b258fbea..d15c1e796bbe7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp @@ -45,7 +45,7 @@ class GeometricPullOver : public PullOverPlannerBase std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) override; + const BehaviorModuleOutput & upstream_module_output) override; std::vector generatePullOverPaths( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp index 5b336a7de6acc..5af69894c0a2e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp @@ -130,7 +130,7 @@ class PullOverPlannerBase virtual std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) = 0; + const BehaviorModuleOutput & upstream_module_output) = 0; protected: const autoware::vehicle_info_utils::VehicleInfo vehicle_info_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp index 08e0b8508c991..2c6aec919bfbb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp @@ -38,7 +38,7 @@ class ShiftPullOver : public PullOverPlannerBase std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) override; + const BehaviorModuleOutput & upstream_module_output) override; protected: PathWithLaneId generateReferencePath( @@ -49,7 +49,7 @@ class ShiftPullOver : public PullOverPlannerBase std::optional generatePullOverPath( const GoalCandidate & goal_candidate, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, + const BehaviorModuleOutput & upstream_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const; static double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double after_shifted_arc_length, const double dr); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp index bfef4226b2661..7173b275e26fb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp @@ -32,35 +32,27 @@ class LaneParkingRequest { public: LaneParkingRequest( - const GoalPlannerParameters & parameters, const autoware::universe_utils::LinearRing2d & vehicle_footprint, - const GoalCandidates & goal_candidates, const BehaviorModuleOutput & previous_module_output) - : parameters_(parameters), - vehicle_footprint_(vehicle_footprint), + const GoalCandidates & goal_candidates, const BehaviorModuleOutput & upstream_module_output) + : vehicle_footprint_(vehicle_footprint), goal_candidates_(goal_candidates), - previous_module_output_(previous_module_output), - last_previous_module_output_(previous_module_output) + upstream_module_output_(upstream_module_output) { } void update( const PlannerData & planner_data, const ModuleStatus & current_status, - const BehaviorModuleOutput & previous_module_output, + const BehaviorModuleOutput & upstream_module_output, const std::optional & pull_over_path, const PathDecisionState & prev_data); - const GoalPlannerParameters parameters_; const autoware::universe_utils::LinearRing2d vehicle_footprint_; const GoalCandidates goal_candidates_; const std::shared_ptr & get_planner_data() const { return planner_data_; } const ModuleStatus & get_current_status() const { return current_status_; } - const BehaviorModuleOutput & get_previous_module_output() const - { - return previous_module_output_; - } - const BehaviorModuleOutput & get_last_previous_module_output() const + const BehaviorModuleOutput & get_upstream_module_output() const { - return last_previous_module_output_; + return upstream_module_output_; } const std::optional & get_pull_over_path() const { return pull_over_path_; } const PathDecisionState & get_prev_data() const { return prev_data_; } @@ -68,8 +60,7 @@ class LaneParkingRequest private: std::shared_ptr planner_data_; ModuleStatus current_status_; - BehaviorModuleOutput previous_module_output_; - BehaviorModuleOutput last_previous_module_output_; + BehaviorModuleOutput upstream_module_output_; std::optional pull_over_path_; //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 60c056280e4db..8272ddde0189e 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 @@ -150,17 +150,17 @@ bool isOnModifiedGoal( } bool hasPreviousModulePathShapeChanged( - const BehaviorModuleOutput & previous_module_output, - const BehaviorModuleOutput & last_previous_module_output) + const BehaviorModuleOutput & upstream_module_output, + const BehaviorModuleOutput & last_upstream_module_output) { // Calculate the lateral distance between each point of the current path and the nearest point of // the last path constexpr double LATERAL_DEVIATION_THRESH = 0.3; - for (const auto & p : previous_module_output.path.points) { + for (const auto & p : upstream_module_output.path.points) { const size_t nearest_seg_idx = autoware::motion_utils::findNearestSegmentIndex( - last_previous_module_output.path.points, p.point.pose.position); - const auto seg_front = last_previous_module_output.path.points.at(nearest_seg_idx); - const auto seg_back = last_previous_module_output.path.points.at(nearest_seg_idx + 1); + last_upstream_module_output.path.points, p.point.pose.position); + const auto seg_front = last_upstream_module_output.path.points.at(nearest_seg_idx); + const auto seg_back = last_upstream_module_output.path.points.at(nearest_seg_idx + 1); // Check if the target point is within the segment const Eigen::Vector3d segment_vec{ seg_back.point.pose.position.x - seg_front.point.pose.position.x, @@ -176,7 +176,7 @@ bool hasPreviousModulePathShapeChanged( continue; } const double lateral_distance = std::abs(autoware::motion_utils::calcLateralOffset( - last_previous_module_output.path.points, p.point.pose.position, nearest_seg_idx)); + last_upstream_module_output.path.points, p.point.pose.position, nearest_seg_idx)); if (lateral_distance > LATERAL_DEVIATION_THRESH) { return true; } @@ -185,19 +185,19 @@ bool hasPreviousModulePathShapeChanged( } bool hasDeviatedFromLastPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & last_previous_module_output) + const PlannerData & planner_data, const BehaviorModuleOutput & last_upstream_module_output) { return std::abs(autoware::motion_utils::calcLateralOffset( - last_previous_module_output.path.points, + last_upstream_module_output.path.points, planner_data.self_odometry->pose.pose.position)) > 0.3; } bool hasDeviatedFromCurrentPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & previous_module_output) + const PlannerData & planner_data, const BehaviorModuleOutput & upstream_module_output) { constexpr double LATERAL_DEVIATION_THRESH = 0.3; return std::abs(autoware::motion_utils::calcLateralOffset( - previous_module_output.path.points, planner_data.self_odometry->pose.pose.position)) > + upstream_module_output.path.points, planner_data.self_odometry->pose.pose.position)) > LATERAL_DEVIATION_THRESH; } @@ -282,7 +282,8 @@ LaneParkingPlanner::LaneParkingPlanner( const std::optional & request, LaneParkingResponse & response, std::atomic & is_lane_parking_cb_running, const rclcpp::Logger & logger, const GoalPlannerParameters & parameters) -: mutex_(lane_parking_mutex), +: parameters_(parameters), + mutex_(lane_parking_mutex), request_(request), response_(response), is_lane_parking_cb_running_(is_lane_parking_cb_running), @@ -335,12 +336,10 @@ void LaneParkingPlanner::onTimer() return; } const auto & local_request = local_request_opt.value(); - const auto & parameters = local_request.parameters_; const auto & goal_candidates = local_request.goal_candidates_; const auto & local_planner_data = local_request.get_planner_data(); const auto & current_status = local_request.get_current_status(); - const auto & previous_module_output = local_request.get_previous_module_output(); - const auto & last_previous_module_output = local_request.get_last_previous_module_output(); + const auto & upstream_module_output = local_request.get_upstream_module_output(); const auto & pull_over_path_opt = local_request.get_pull_over_path(); const auto & prev_data = local_request.get_prev_data(); @@ -371,19 +370,21 @@ void LaneParkingPlanner::onTimer() ? std::make_optional(pull_over_path_opt.value().modified_goal()) : std::nullopt; if (isOnModifiedGoal( - local_planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) { + local_planner_data->self_odometry->pose.pose, modified_goal_opt, parameters_)) { return false; } - if (hasDeviatedFromCurrentPreviousModulePath(*local_planner_data, previous_module_output)) { + if (hasDeviatedFromCurrentPreviousModulePath(*local_planner_data, upstream_module_output)) { RCLCPP_DEBUG(getLogger(), "has deviated from current previous module path"); return false; } - if (hasPreviousModulePathShapeChanged(previous_module_output, last_previous_module_output)) { + if (hasPreviousModulePathShapeChanged( + upstream_module_output, original_upstream_module_output_)) { RCLCPP_DEBUG(getLogger(), "has previous module path shape changed"); return true; } if ( - hasDeviatedFromLastPreviousModulePath(*local_planner_data, last_previous_module_output) && + hasDeviatedFromLastPreviousModulePath( + *local_planner_data, original_upstream_module_output_) && current_state != PathDecisionState::DecisionKind::DECIDED) { RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path"); return true; @@ -400,8 +401,8 @@ void LaneParkingPlanner::onTimer() // generate valid pull over path candidates and calculate closest start pose const auto current_lanes = utils::getExtendedCurrentLanes( - local_planner_data, parameters.backward_goal_search_length, - parameters.forward_goal_search_length, + local_planner_data, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); std::vector path_candidates{}; std::optional closest_start_pose{}; @@ -410,7 +411,7 @@ void LaneParkingPlanner::onTimer() const std::shared_ptr & planner, const GoalCandidate & goal_candidate) { const auto pull_over_path = planner->plan( - goal_candidate, path_candidates.size(), local_planner_data, previous_module_output); + goal_candidate, path_candidates.size(), local_planner_data, upstream_module_output); if (pull_over_path) { // calculate absolute maximum curvature of parking path(start pose to end pose) for path // priority @@ -428,13 +429,13 @@ void LaneParkingPlanner::onTimer() // todo: currently non centerline input path is supported only by shift pull over const bool is_center_line_input_path = goal_planner_utils::isReferencePath( - previous_module_output.reference_path, previous_module_output.path, 0.1); + upstream_module_output.reference_path, upstream_module_output.path, 0.1); RCLCPP_DEBUG( getLogger(), "the input path of pull over planner is center line: %d", is_center_line_input_path); // plan candidate paths and set them to the member variable - if (parameters.path_priority == "efficient_path") { + if (parameters_.path_priority == "efficient_path") { for (const auto & planner : pull_over_planners_) { // todo: temporary skip NON SHIFT planner when input path is not center line if (!is_center_line_input_path && planner->getPlannerType() != PullOverPlannerType::SHIFT) { @@ -444,7 +445,7 @@ void LaneParkingPlanner::onTimer() planCandidatePaths(planner, goal_candidate); } } - } else if (parameters.path_priority == "close_goal") { + } else if (parameters_.path_priority == "close_goal") { for (const auto & goal_candidate : goal_candidates) { for (const auto & planner : pull_over_planners_) { // todo: temporary skip NON SHIFT planner when input path is not center line @@ -457,14 +458,15 @@ void LaneParkingPlanner::onTimer() } else { RCLCPP_ERROR( getLogger(), "path_priority should be efficient_path or close_goal, but %s is given.", - parameters.path_priority.c_str()); + parameters_.path_priority.c_str()); throw std::domain_error("[pull_over] invalid path_priority"); } // set response { + original_upstream_module_output_ = upstream_module_output; std::lock_guard guard(mutex_); - response_.pull_over_path_candidates = path_candidates; + response_.pull_over_path_candidates = std::move(path_candidates); response_.closest_start_pose = closest_start_pose; RCLCPP_INFO( getLogger(), "generated %lu pull over path candidates", @@ -562,7 +564,7 @@ std::pair GoalPlannerModule::sync // In PlannerManager::run(), it calls SceneModuleInterface::setData and // SceneModuleInterface::setPreviousModuleOutput before module_ptr->run(). // Then module_ptr->run() invokes GoalPlannerModule::updateData and then - // planWaitingApproval()/plan(), so we can copy latest current_status/previous_module_output to + // planWaitingApproval()/plan(), so we can copy latest current_status/upstream_module_output to // lane_parking_request/freespace_parking_request std::optional pull_over_path = @@ -578,7 +580,7 @@ std::pair GoalPlannerModule::sync std::lock_guard guard(lane_parking_mutex_); if (!lane_parking_request_) { lane_parking_request_.emplace( - *parameters_, vehicle_footprint_, goal_candidates_, getPreviousModuleOutput()); + vehicle_footprint_, goal_candidates_, getPreviousModuleOutput()); } // NOTE: for the above reasons, PlannerManager/behavior_path_planner_node ensure that // planner_data_ is not nullptr, so it is OK to copy as value diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp index 0da5ab5dae38b..ad42ccf1582ab 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp @@ -43,7 +43,7 @@ BezierPullOver::BezierPullOver( std::optional BezierPullOver::plan( [[maybe_unused]] const GoalCandidate & modified_goal_pose, [[maybe_unused]] const size_t id, [[maybe_unused]] const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) + [[maybe_unused]] const BehaviorModuleOutput & upstream_module_output) { const auto & route_handler = planner_data->route_handler; const double min_jerk = parameters_.minimum_lateral_jerk; @@ -55,7 +55,7 @@ std::optional BezierPullOver::plan( std::abs(max_jerk - min_jerk) / shift_sampling_num; const auto road_lanes = utils::getExtendedCurrentLanesFromPath( - previous_module_output.path, planner_data, backward_search_length, forward_search_length, + upstream_module_output.path, planner_data, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( @@ -67,7 +67,7 @@ std::optional BezierPullOver::plan( // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { const auto pull_over_path = generateBezierPath( - modified_goal_pose, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, + modified_goal_pose, id, planner_data, upstream_module_output, road_lanes, pull_over_lanes, lateral_jerk); if (!pull_over_path.empty()) { return pull_over_path.at(0); @@ -80,7 +80,7 @@ std::optional BezierPullOver::plan( std::vector BezierPullOver::plans( [[maybe_unused]] const GoalCandidate & modified_goal_pose, [[maybe_unused]] const size_t id, [[maybe_unused]] const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) + [[maybe_unused]] const BehaviorModuleOutput & upstream_module_output) { const auto & route_handler = planner_data->route_handler; const double min_jerk = parameters_.minimum_lateral_jerk; @@ -92,7 +92,7 @@ std::vector BezierPullOver::plans( std::abs(max_jerk - min_jerk) / shift_sampling_num; const auto road_lanes = utils::getExtendedCurrentLanesFromPath( - previous_module_output.path, planner_data, backward_search_length, forward_search_length, + upstream_module_output.path, planner_data, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( @@ -105,7 +105,7 @@ std::vector BezierPullOver::plans( std::vector paths; for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { auto pull_over_paths = generateBezierPath( - modified_goal_pose, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, + modified_goal_pose, id, planner_data, upstream_module_output, road_lanes, pull_over_lanes, lateral_jerk); std::copy( std::make_move_iterator(pull_over_paths.begin()), @@ -118,7 +118,7 @@ std::vector BezierPullOver::plans( std::vector BezierPullOver::generateBezierPath( const GoalCandidate & goal_candidate, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, + const BehaviorModuleOutput & upstream_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const { const double pull_over_velocity = parameters_.pull_over_velocity; @@ -133,7 +133,7 @@ std::vector BezierPullOver::generateBezierPath( generateReferencePath(planner_data, road_lanes, shift_end_pose), parameters_.center_line_path_interval); const auto prev_module_path = utils::resamplePathWithSpline( - previous_module_output.path, parameters_.center_line_path_interval); + upstream_module_output.path, parameters_.center_line_path_interval); const auto prev_module_path_terminal_pose = prev_module_path.points.back().point.pose; // process previous module path for path shifter input path @@ -318,7 +318,7 @@ std::vector BezierPullOver::generateBezierPath( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); const auto combined_drivable = utils::combineDrivableLanes( - expanded_lanes, previous_module_output.drivable_area_info.drivable_lanes); + expanded_lanes, upstream_module_output.drivable_area_info.drivable_lanes); return !lane_departure_checker_.checkPathWillLeaveLane( utils::transformToLanelets(combined_drivable), pull_over_path.parking_path()); }); 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 5b11d1b170ce1..473f76b5904d6 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 @@ -60,7 +60,7 @@ FreespacePullOver::FreespacePullOver( std::optional FreespacePullOver::plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) + [[maybe_unused]] const BehaviorModuleOutput & upstream_module_output) { const Pose & current_pose = planner_data->self_odometry->pose.pose; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp index c0bac5c5ce2a8..74b3fe149fd1d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -40,7 +40,7 @@ GeometricPullOver::GeometricPullOver( std::optional GeometricPullOver::plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) + [[maybe_unused]] const BehaviorModuleOutput & upstream_module_output) { const auto & route_handler = planner_data->route_handler; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index edecfd733d3be..9a3e2d6b13db9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -41,7 +41,7 @@ ShiftPullOver::ShiftPullOver( std::optional ShiftPullOver::plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) + const BehaviorModuleOutput & upstream_module_output) { const auto & route_handler = planner_data->route_handler; const double min_jerk = parameters_.minimum_lateral_jerk; @@ -52,7 +52,7 @@ std::optional ShiftPullOver::plan( const double jerk_resolution = std::abs(max_jerk - min_jerk) / shift_sampling_num; const auto road_lanes = utils::getExtendedCurrentLanesFromPath( - previous_module_output.path, planner_data, backward_search_length, forward_search_length, + upstream_module_output.path, planner_data, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( @@ -64,7 +64,7 @@ std::optional ShiftPullOver::plan( // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { const auto pull_over_path = generatePullOverPath( - modified_goal_pose, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, + modified_goal_pose, id, planner_data, upstream_module_output, road_lanes, pull_over_lanes, lateral_jerk); if (!pull_over_path) continue; return pull_over_path; @@ -134,7 +134,7 @@ std::optional ShiftPullOver::cropPrevModulePath( std::optional ShiftPullOver::generatePullOverPath( const GoalCandidate & goal_candidate, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, + const BehaviorModuleOutput & upstream_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const { const double pull_over_velocity = parameters_.pull_over_velocity; @@ -151,7 +151,7 @@ std::optional ShiftPullOver::generatePullOverPath( generateReferencePath(planner_data, road_lanes, shift_end_pose), parameters_.center_line_path_interval); const auto prev_module_path = utils::resamplePathWithSpline( - previous_module_output.path, parameters_.center_line_path_interval); + upstream_module_output.path, parameters_.center_line_path_interval); const auto prev_module_path_terminal_pose = prev_module_path.points.back().point.pose; // process previous module path for path shifter input path diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp index f12d510e23030..89458c199c130 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp @@ -21,14 +21,13 @@ namespace autoware::behavior_path_planner void LaneParkingRequest::update( const PlannerData & planner_data, const ModuleStatus & current_status, - const BehaviorModuleOutput & previous_module_output, + const BehaviorModuleOutput & upstream_module_output, const std::optional & pull_over_path, const PathDecisionState & prev_data) { planner_data_ = std::make_shared(planner_data); planner_data_->route_handler = std::make_shared(*(planner_data.route_handler)); current_status_ = current_status; - last_previous_module_output_ = previous_module_output_; - previous_module_output_ = previous_module_output; + upstream_module_output_ = upstream_module_output; pull_over_path_ = pull_over_path; prev_data_ = prev_data; } 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..0508dc753a2e8 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 @@ -874,6 +874,16 @@ endif @enduml ``` +## Terminal Lane Change Path + +Depending on the space configuration around the Ego vehicle, it is possible that a valid LC path cannot be generated. If that happens, then Ego will get stuck at `terminal_start` and will be not be able to proceed. Therefore we introduced the terminal LC path feature; when ego gets near to the terminal point (dist to `terminal_start` is less than the maximum lane change length) a terminal lane changing path will be computed starting from the terminal start point on the current lane and connects to the target lane. The terminal path only needs to be computed once in each execution of LC module. If no valid candidate paths are found in the path generation process, then the terminal path will be used as a fallback candidate path, the safety of the terminal path is not ensured and therefore it can only be force approved. The following images illustrate the expected behavior without and with the terminal path feature respectively: + +![no terminal path](./images/lane_change-no_terminal_path.png) + +![terminal path](./images/lane_change-terminal_path.png) + +Additionally if terminal path feature is enabled and path is computed, stop point placement can be configured to be at the edge of the current lane instead of at the `terminal_start` position, as indicated by the dashed red line in the image above. + ## Parameters ### Essential lane change parameters @@ -886,6 +896,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 | @@ -934,6 +945,16 @@ The following parameters are used to judge lane change completion. | `delay_lane_change.min_road_shoulder_width` | [m] | double | Width considered as road shoulder if lane doesn't have road shoulder when checking for parked vehicle | 0.5 | | `delay_lane_change.th_parked_vehicle_shift_ratio` | [-] | double | Stopped vehicles beyond this distance ratio from center line will be considered as parked | 0.6 | +### Terminal Lane Change Path + +The following parameters are used to configure terminal lane change path feature. + +| Name | Unit | Type | Description | Default value | +| :-------------------------------- | ---- | ---- | ------------------------------------------------------------------------- | ------------- | +| `terminal_path.enable` | [-] | bool | Flag to enable/disable terminal path feature | true | +| `terminal_path.disable_near_goal` | [-] | bool | Flag to disable terminal path feature if ego is near goal | true | +| `terminal_path.stop_at_boundary` | [-] | bool | If true, ego will stop at current lane boundary instead of middle of lane | false | + ### Collision checks #### Target Objects 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..91b296f8bd40f 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,10 +5,17 @@ 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] + # terminal path + terminal_path: + enable: true + disable_near_goal: true + stop_at_boundary: false + # trajectory generation trajectory: max_prepare_duration: 4.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-no_terminal_path.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-no_terminal_path.png new file mode 100644 index 0000000000000..2c7b76e174997 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-no_terminal_path.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-terminal_path.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-terminal_path.png new file mode 100644 index 0000000000000..83e9d377e8f34 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-terminal_path.png differ 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..43a929591dad1 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; @@ -130,7 +132,7 @@ class LaneChangeBase virtual void insert_stop_point( [[maybe_unused]] const lanelet::ConstLanelets & lanelets, - [[maybe_unused]] PathWithLaneId & path) + [[maybe_unused]] PathWithLaneId & path, [[maybe_unused]] const bool is_waiting_approval = false) { } @@ -224,7 +226,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 +237,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,9 +286,8 @@ class LaneChangeBase lane_change::CommonDataPtr common_data_ptr_; FilteredLanesObjects filtered_objects_{}; BehaviorModuleOutput prev_module_output_{}; - std::optional lane_change_stop_pose_{std::nullopt}; - - PathWithLaneId prev_approved_path_; + PoseWithDetailOpt lane_change_stop_pose_{std::nullopt}; + mutable std::optional terminal_lane_change_path_{std::nullopt}; int unsafe_hysteresis_count_{0}; bool is_abort_path_approved_{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index 9d6fbd65acfd8..a69ae0d92647a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -129,8 +129,6 @@ class LaneChangeInterface : public SceneModuleInterface mutable MarkerArray virtual_wall_marker_; - std::unique_ptr prev_approved_path_; - void clearAbortApproval() { is_abort_path_approved_ = false; } bool is_abort_path_approved_{false}; 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..fe1d18ea6ea0c 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 @@ -70,9 +71,12 @@ class NormalLaneChange : public LaneChangeBase void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; - void insert_stop_point(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; + void insert_stop_point( + const lanelet::ConstLanelets & lanelets, PathWithLaneId & path, + const bool is_waiting_approval = false) override; - void insert_stop_point_on_current_lanes(PathWithLaneId & path); + void insert_stop_point_on_current_lanes( + PathWithLaneId & path, const bool is_waiting_approval = false); PathWithLaneId getReferencePath() const override; @@ -128,14 +132,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 +139,21 @@ 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; + std::optional compute_terminal_lane_change_path() 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 +163,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 +181,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..d5bbfe25fe1e9 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 @@ -128,6 +128,22 @@ struct Info terminal_lane_changing_velocity = _lc_metrics.velocity; shift_line = _shift_line; } + + void set_prepare(const PhaseMetrics & _prep_metrics) + { + longitudinal_acceleration.prepare = _prep_metrics.actual_lon_accel; + velocity.prepare = _prep_metrics.velocity; + duration.prepare = _prep_metrics.duration; + length.prepare = _prep_metrics.length; + } + + void set_lane_changing(const PhaseMetrics & _lc_metrics) + { + longitudinal_acceleration.lane_changing = _lc_metrics.actual_lon_accel; + velocity.lane_changing = _lc_metrics.velocity; + duration.lane_changing = _lc_metrics.duration; + length.lane_changing = _lc_metrics.length; + } }; struct TargetLaneLeadingObjects @@ -160,6 +176,8 @@ struct TargetObjects : leading(std::move(leading)), trailing(std::move(trailing)) { } + + [[nodiscard]] bool empty() const { return leading.empty() && trailing.empty(); } }; enum class ModuleType { @@ -217,6 +235,8 @@ struct TransientData double target_lane_length{std::numeric_limits::min()}; + double dist_to_target_end{std::numeric_limits::max()}; + lanelet::ArcCoordinates current_lanes_ego_arc; // arc coordinates of ego pose along current lanes lanelet::ArcCoordinates target_lanes_ego_arc; // arc coordinates of ego pose along target lanes 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..b25dbc99189e8 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 @@ -137,18 +137,27 @@ struct DelayParameters double th_parked_vehicle_shift_ratio{0.6}; }; +struct TerminalPathParameters +{ + bool enable{false}; + bool disable_near_goal{false}; + bool stop_at_boundary{false}; +}; + struct Parameters { TrajectoryParameters trajectory{}; SafetyParameters safety{}; CancelParameters cancel{}; DelayParameters delay{}; + TerminalPathParameters terminal_path{}; // lane change parameters double backward_lane_length{200.0}; 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/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index eced227a5be32..29a9f258545a2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -67,6 +67,9 @@ double calc_dist_to_last_fit_width( const lanelet::ConstLanelets & lanelets, const Pose & src_pose, const BehaviorPathPlannerParameters & bpp_param, const double margin = 0.1); +double calc_dist_to_last_fit_width( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const double margin = 0.1); + /** * @brief Calculates the maximum preparation longitudinal distance for lane change. * 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..77ba8fe68a653 --- /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 double prep_length, 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..9f3c6c9ef48bf 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 @@ -43,8 +43,7 @@ LaneChangeInterface::LaneChangeInterface( std::unique_ptr && module_type) : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{std::move(parameters)}, - module_type_{std::move(module_type)}, - prev_approved_path_{std::make_unique()} + module_type_{std::move(module_type)} { module_type_->setTimeKeeper(getTimeKeeper()); logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr()); @@ -109,11 +108,8 @@ BehaviorModuleOutput LaneChangeInterface::plan() auto output = module_type_->generateOutput(); 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) { @@ -157,11 +153,9 @@ BehaviorModuleOutput LaneChangeInterface::plan() BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { - BehaviorModuleOutput out = getPreviousModuleOutput(); + BehaviorModuleOutput out = module_type_->getTerminalLaneChangePath(); - *prev_approved_path_ = out.path; - - module_type_->insert_stop_point(module_type_->get_current_lanes(), out.path); + module_type_->insert_stop_point(module_type_->get_current_lanes(), out.path, true); out.turn_signal_info = module_type_->get_current_turn_signal_info(); const auto & lane_change_debug = module_type_->getDebugData(); @@ -171,9 +165,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..ec000b8fee97c 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( @@ -240,6 +242,13 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s // debug marker p.publish_debug_marker = getOrDeclareParameter(*node, parameter("publish_debug_marker")); + // terminal lane change path + p.terminal_path.enable = getOrDeclareParameter(*node, parameter("terminal_path.enable")); + p.terminal_path.disable_near_goal = + getOrDeclareParameter(*node, parameter("terminal_path.disable_near_goal")); + p.terminal_path.stop_at_boundary = + getOrDeclareParameter(*node, parameter("terminal_path.stop_at_boundary")); + // validation of safety check parameters // if loose check is not enabled, lane change module will keep on chattering and canceling, and // false positive situation might occur @@ -305,6 +314,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..2719239baaed8 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 @@ -42,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -168,6 +170,9 @@ void NormalLaneChange::update_transient_data(const bool is_approved) transient_data.dist_to_terminal_start = transient_data.dist_to_terminal_end - transient_data.current_dist_buffer.min; + transient_data.dist_to_target_end = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->target, common_data_ptr_->get_ego_pose()); + transient_data.max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); transient_data.target_lane_length = @@ -245,6 +250,10 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) bool found_safe_path = get_lane_change_paths(valid_paths); // if no safe path is found and ego is stuck, try to find a path with a small margin + if (valid_paths.empty() && terminal_lane_change_path_) { + valid_paths.push_back(terminal_lane_change_path_.value()); + } + lane_change_debug_.valid_paths = valid_paths; if (valid_paths.empty()) { @@ -381,17 +390,18 @@ LaneChangePath NormalLaneChange::getLaneChangePath() const BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const { - auto output = prev_module_output_; + if ( + !lane_change_parameters_->terminal_path.enable || + !common_data_ptr_->transient_data.is_ego_near_current_terminal_start) { + return 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 is_near_goal = lane_change_parameters_->terminal_path.disable_near_goal && + common_data_ptr_->lanes_ptr->target_lane_in_goal_section && + common_data_ptr_->transient_data.dist_to_target_end < + common_data_ptr_->transient_data.lane_changing_length.max; + if (is_near_goal) { + return prev_module_output_; } const auto & current_lanes = get_current_lanes(); @@ -400,23 +410,19 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const return prev_module_output_; } - const auto terminal_path = - calcTerminalLaneChangePath(current_lanes, get_lane_change_lanes(current_lanes)); - if (!terminal_path) { + const auto terminal_lc_path = compute_terminal_lane_change_path(); + + if (!terminal_lc_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(); + auto output = prev_module_output_; - extendOutputDrivableArea(output); + output.path = *terminal_lc_path; + output.turn_signal_info = get_current_turn_signal_info(); - 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); + extendOutputDrivableArea(output); return output; } @@ -449,7 +455,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); } @@ -491,7 +497,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) c } void NormalLaneChange::insert_stop_point( - const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) + const lanelet::ConstLanelets & lanelets, PathWithLaneId & path, const bool is_waiting_approval) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (lanelets.empty()) { @@ -512,59 +518,62 @@ 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; } - insert_stop_point_on_current_lanes(path); + insert_stop_point_on_current_lanes(path, is_waiting_approval); } -void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) +void NormalLaneChange::insert_stop_point_on_current_lanes( + PathWithLaneId & path, const bool is_waiting_approval) { const auto & path_front_pose = path.points.front().point.pose; - const auto & center_line = common_data_ptr_->current_lanes_path.points; - const auto get_arc_length_along_lanelet = [&](const geometry_msgs::msg::Pose & target) { - return motion_utils::calcSignedArcLength( - center_line, path_front_pose.position, target.position); - }; + const auto & ego_pose = common_data_ptr_->get_ego_pose(); + const auto dist_from_path_front = + motion_utils::calcSignedArcLength(path.points, path_front_pose.position, ego_pose.position); const auto & transient_data = common_data_ptr_->transient_data; const auto & lanes_ptr = common_data_ptr_->lanes_ptr; const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; - const auto dist_to_terminal = std::invoke([&]() -> double { - const auto target_pose = (lanes_ptr->current_lane_in_goal_section) - ? common_data_ptr_->route_handler_ptr->getGoalPose() - : center_line.back().point.pose; - return get_arc_length_along_lanelet(target_pose); - }); - - const auto & bpp_param_ptr = common_data_ptr_->bpp_param_ptr; - const auto min_dist_buffer = transient_data.current_dist_buffer.min; const auto dist_to_terminal_start = - dist_to_terminal - min_dist_buffer - calculation::calc_stopping_distance(lc_param_ptr); + transient_data.dist_to_terminal_start - calculation::calc_stopping_distance(lc_param_ptr); - const auto distance_to_last_fit_width = std::invoke([&]() -> double { + const auto & bpp_param_ptr = common_data_ptr_->bpp_param_ptr; + const auto dist_to_terminal_stop = std::invoke([&]() -> double { const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current; - if (utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), *bpp_param_ptr)) { - return utils::lane_change::calculation::calc_dist_to_last_fit_width( - lanes_ptr->current, path.points.front().point.pose, *bpp_param_ptr); + if (!utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), *bpp_param_ptr)) { + return dist_from_path_front + dist_to_terminal_start; } - return std::numeric_limits::max(); - }); - const auto dist_to_terminal_stop = std::min(dist_to_terminal_start, distance_to_last_fit_width); + if ( + terminal_lane_change_path_ && is_waiting_approval && + lc_param_ptr->terminal_path.stop_at_boundary) { + return calculation::calc_dist_to_last_fit_width(common_data_ptr_, path); + } + + const auto dist_to_last_fit_width = calculation::calc_dist_to_last_fit_width( + lanes_ptr->current, path.points.front().point.pose, *bpp_param_ptr); - if (filtered_objects_.current_lane.empty()) { - set_stop_pose(dist_to_terminal_stop, path); + return std::min(dist_from_path_front + dist_to_terminal_start, dist_to_last_fit_width); + }); + + 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; } + const auto & center_line = common_data_ptr_->current_lanes_path.points; const auto dist_to_target_lane_start = std::invoke([&]() -> double { const auto & front_lane = lanes_ptr->target_neighbor.front(); const auto target_front = utils::to_geom_msg_pose(front_lane.centerline2d().front(), front_lane); - return get_arc_length_along_lanelet(target_front); + return motion_utils::calcSignedArcLength( + center_line, path_front_pose.position, target_front.position); }); const auto arc_length_to_current_obj = utils::lane_change::get_min_dist_to_current_lanes_obj( @@ -582,12 +591,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 +612,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 +931,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 +997,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 +1015,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 +1073,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 +1108,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 +1136,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 +1175,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.length, prepare_segment)) { debug_print("Reject: failed to get valid prepare segment!"); continue; } @@ -1258,7 +1195,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 +1214,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 +1241,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,145 +1270,113 @@ 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 +std::optional NormalLaneChange::compute_terminal_lane_change_path() 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)); + const auto & transient_data = common_data_ptr_->transient_data; + const auto dist_to_terminal_start = transient_data.dist_to_terminal_start; + const auto min_lc_velocity = lane_change_parameters_->trajectory.min_lane_changing_velocity; + const auto current_velocity = getEgoVelocity(); - if (!lane_changing_start_pose) { - RCLCPP_DEBUG(logger_, "Reject: lane changing start pose not found!!!"); - return {}; + PathWithLaneId prepare_segment; + try { + if (!utils::lane_change::get_prepare_segment( + common_data_ptr_, prev_module_output_.path, dist_to_terminal_start, prepare_segment)) { + RCLCPP_DEBUG(logger_, "failed to get valid prepare segment for terminal LC path"); + return std::nullopt; + } + } catch (const std::exception & e) { + RCLCPP_DEBUG(logger_, "failed to get terminal LC path: %s", e.what()); + return std::nullopt; } - 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 {}; + // t = 2 * d / (v1 + v2) + const auto duration_to_lc_start = + 2.0 * dist_to_terminal_start / (current_velocity + min_lc_velocity); + const auto lon_accel = std::invoke([&]() -> double { + if (duration_to_lc_start < calculation::eps) { + return 0.0; + } + return std::clamp( + (min_lc_velocity - current_velocity) / duration_to_lc_start, + lane_change_parameters_->trajectory.min_longitudinal_acc, + lane_change_parameters_->trajectory.max_longitudinal_acc); + }); + const auto vel_on_prep = current_velocity + lon_accel * duration_to_lc_start; + const LaneChangePhaseMetrics prep_metric( + duration_to_lc_start, dist_to_terminal_start, vel_on_prep, lon_accel, lon_accel, 0.0); + + if (terminal_lane_change_path_) { + terminal_lane_change_path_->info.set_prepare(prep_metric); + if (prepare_segment.points.empty()) { + terminal_lane_change_path_->path = terminal_lane_change_path_->shifted_path.path; + return terminal_lane_change_path_->path; + } + prepare_segment.points.pop_back(); + terminal_lane_change_path_->path = + utils::combinePath(prepare_segment, terminal_lane_change_path_->shifted_path.path); + return terminal_lane_change_path_->path; } - 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); + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto shift_length = + lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); - if (target_segment.points.empty()) { - RCLCPP_DEBUG(logger_, "Reject: target segment is empty!! something wrong..."); - return {}; - } + const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, + prepare_segment.points.back().point.pose); - 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; + const auto max_lane_changing_length = std::invoke([&]() { + double max_length = transient_data.dist_to_terminal_end - prep_metric.length; + return std::min( + max_length, dist_lc_start_to_end_of_lanes - transient_data.next_dist_buffer.min); + }); - 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 max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; + constexpr double lane_changing_lon_accel{0.0}; + const auto lane_changing_metrics = calculation::calc_shift_phase_metrics( + common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, + lane_changing_lon_accel, max_lane_changing_length); - 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); + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); - if (target_lane_reference_path.points.empty()) { - RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); - return {}; + LaneChangePath candidate_path; + for (const auto & lc_metric : lane_changing_metrics) { + try { + 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) { + continue; + } + terminal_lane_change_path_ = candidate_path; + return candidate_path.path; } - 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; + return std::nullopt; } PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const @@ -1545,9 +1406,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 +1644,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 +1702,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 +1753,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 +1823,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..a0130fcd27041 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 @@ -58,22 +58,10 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr) } double calc_dist_to_last_fit_width( - const lanelet::ConstLanelets & lanelets, const Pose & src_pose, + const lanelet::ConstLanelets & lanelets, const lanelet::BasicPolygon2d & lanelet_polygon, + const universe_utils::LineString2d & line_string, const Pose & src_pose, const BehaviorPathPlannerParameters & bpp_param, const double margin) { - if (lanelets.empty()) return 0.0; - - const auto lane_polygon = lanelets.back().polygon2d().basicPolygon(); - const auto center_line = lanelet::utils::generateFineCenterline(lanelets.back(), 1.0); - - if (center_line.size() <= 1) return 0.0; - - universe_utils::LineString2d line_string; - line_string.reserve(center_line.size() - 1); - std::for_each(center_line.begin() + 1, center_line.end(), [&line_string](const auto & point) { - boost::geometry::append(line_string, universe_utils::Point2d(point.x(), point.y())); - }); - const double buffer_distance = 0.5 * bpp_param.vehicle_width + margin; universe_utils::MultiPolygon2d center_line_polygon; namespace strategy = boost::geometry::strategy::buffer; @@ -85,7 +73,7 @@ double calc_dist_to_last_fit_width( if (center_line_polygon.empty()) return 0.0; std::vector intersection_points; - boost::geometry::intersection(lane_polygon, center_line_polygon, intersection_points); + boost::geometry::intersection(lanelet_polygon, center_line_polygon, intersection_points); if (intersection_points.empty()) { return utils::getDistanceToEndOfLane(src_pose, lanelets); @@ -102,6 +90,46 @@ double calc_dist_to_last_fit_width( return std::max(distance - (bpp_param.base_link2front + margin), 0.0); } +double calc_dist_to_last_fit_width( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const double margin) +{ + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & current_lanes_polygon = common_data_ptr->lanes_polygon_ptr->current; + const auto & bpp_param = *common_data_ptr->bpp_param_ptr; + + universe_utils::LineString2d line_string; + line_string.reserve(path.points.size() - 1); + std::for_each(path.points.begin() + 1, path.points.end(), [&line_string](const auto & point) { + const auto & position = point.point.pose.position; + boost::geometry::append(line_string, universe_utils::Point2d(position.x, position.y)); + }); + + const auto & src_pose = path.points.front().point.pose; + return calc_dist_to_last_fit_width( + current_lanes, current_lanes_polygon, line_string, src_pose, bpp_param, margin); +} + +double calc_dist_to_last_fit_width( + const lanelet::ConstLanelets & lanelets, const Pose & src_pose, + const BehaviorPathPlannerParameters & bpp_param, const double margin) +{ + if (lanelets.empty()) return 0.0; + + const auto lane_polygon = lanelets.back().polygon2d().basicPolygon(); + const auto center_line = lanelet::utils::generateFineCenterline(lanelets.back(), 1.0); + + if (center_line.size() <= 1) return 0.0; + + universe_utils::LineString2d line_string; + line_string.reserve(center_line.size() - 1); + std::for_each(center_line.begin() + 1, center_line.end(), [&line_string](const auto & point) { + boost::geometry::append(line_string, universe_utils::Point2d(point.x(), point.y())); + }); + + return calc_dist_to_last_fit_width( + lanelets, lane_polygon, line_string, src_pose, bpp_param, margin); +} + double calc_maximum_prepare_length(const CommonDataPtr & common_data_ptr) { const auto max_prepare_duration = common_data_ptr->lc_param_ptr->trajectory.max_prepare_duration; @@ -417,7 +445,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..44ee1624f0f51 --- /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 double prep_length, 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_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/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md index 1e8532eba2f3a..3b787303d8d20 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md @@ -533,9 +533,9 @@ If this case happened in the slot, `is_upstream_waiting_approved` is set to true ### Failure modules -The failure modules return the status `ModuleStatus::FAILURE`. The manager removes the module from approved modules stack as well as waiting approval modules, but the failure module is not moved to candidate modules stack. +If a module returns `ModuleStatus::FAILURE`, the manager removes the failed module. Additionally, all modules after the failed module are removed, even if they did not return `ModuleStatus::FAILURE`. These modules are not added back to the candidate modules stack and will instead run again from the beginning. Once these modules are removed, the output of the module prior to the failed module will be used as the planner's output. -As a result, the module A's output is used as approved modules stack. +As shown in the example below, modules B, A, and C are running. When module A returns `ModuleStatus::FAILURE`, both module A and C are removed from the approved modules stack. Module B's output is then used as the final output of the planner. ![failure_modules](../image/manager/failure_modules.drawio.svg) 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..f9c736b9d5dde 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml @@ -21,6 +21,7 @@ Kyoichi Sugahara Takayuki Murooka Go Sakayori + Mamoru Sobue Apache License 2.0 @@ -29,7 +30,6 @@ Fumiya Watanabe Zulfaqar Azmi Kosuke Takeuchi - Yutaka Shimizu Takayuki Murooka Ryohsuke Mitsudome @@ -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_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/freespace_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp index e358e35ed7794..364d2d31a2577 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp @@ -35,10 +35,7 @@ using autoware::freespace_planning_algorithms::RRTStar; class FreespacePullOut : public PullOutPlannerBase { public: - FreespacePullOut( - rclcpp::Node & node, const StartPlannerParameters & parameters, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, - std::shared_ptr time_keeper); + FreespacePullOut(rclcpp::Node & node, const StartPlannerParameters & parameters); PlannerType getPlannerType() const override { return PlannerType::FREESPACE; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp index 18d1f3c3b9b81..78eb72183cdf5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -35,7 +35,8 @@ class GeometricPullOut : public PullOutPlannerBase rclcpp::Node & node, const StartPlannerParameters & parameters, const std::shared_ptr lane_departure_checker, - std::shared_ptr time_keeper); + std::shared_ptr time_keeper = + std::make_shared()); PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; }; std::optional plan( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp index f606679e7f2da..dfd972aff9be0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -40,12 +40,13 @@ class PullOutPlannerBase public: explicit PullOutPlannerBase( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr time_keeper) - : time_keeper_(time_keeper) + std::shared_ptr time_keeper = + std::make_shared()) + : parameters_(parameters), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()), + vehicle_footprint_(vehicle_info_.createFootprint()), + time_keeper_(time_keeper) { - vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); - vehicle_footprint_ = vehicle_info_.createFootprint(); - parameters_ = parameters; } virtual ~PullOutPlannerBase() = default; @@ -68,9 +69,9 @@ class PullOutPlannerBase double collision_check_distance_from_end) const; std::shared_ptr planner_data_; + StartPlannerParameters parameters_; autoware::vehicle_info_utils::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; - StartPlannerParameters parameters_; double collision_check_margin_; mutable std::shared_ptr time_keeper_; 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..8da104940d327 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 @@ -36,7 +36,8 @@ class ShiftPullOut : public PullOutPlannerBase explicit ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, std::shared_ptr & lane_departure_checker, - std::shared_ptr time_keeper); + std::shared_ptr time_keeper = + std::make_shared()); PlannerType getPlannerType() const override { return PlannerType::SHIFT; }; std::optional plan( @@ -56,6 +57,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/freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp index 42698f799c6b3..a089f6a8a83fc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -28,15 +28,11 @@ namespace autoware::behavior_path_planner { -FreespacePullOut::FreespacePullOut( - rclcpp::Node & node, const StartPlannerParameters & parameters, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, - std::shared_ptr time_keeper) -: PullOutPlannerBase{node, parameters, time_keeper}, - velocity_{parameters.freespace_planner_velocity} +FreespacePullOut::FreespacePullOut(rclcpp::Node & node, const StartPlannerParameters & parameters) +: PullOutPlannerBase{node, parameters}, velocity_{parameters.freespace_planner_velocity} { autoware::freespace_planning_algorithms::VehicleShape vehicle_shape( - vehicle_info, parameters.vehicle_shape_margin); + vehicle_info_, parameters.vehicle_shape_margin); if (parameters.freespace_planner_algorithm == "astar") { use_back_ = parameters.astar_parameters.use_back; planner_ = std::make_unique( 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/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index c223390e454d1..f45924b9542dc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -90,8 +90,7 @@ StartPlannerModule::StartPlannerModule( } if (parameters_->enable_freespace_planner) { - freespace_planner_ = - std::make_unique(node, *parameters, vehicle_info_, time_keeper_); + freespace_planner_ = std::make_unique(node, *parameters); const auto freespace_planner_period_ns = rclcpp::Rate(1.0).period(); freespace_planner_timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp index fb39e186afd4e..dd8bb02c97dea 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp @@ -156,9 +156,8 @@ class TestGeometricPullOut : public ::testing::Test parameters->th_moving_object_velocity = th_moving_object_velocity_; parameters->divide_pull_out_path = divide_pull_out_path_; - auto time_keeper = std::make_shared(); geometric_pull_out_ = - std::make_shared(*node_, *parameters, lane_departure_checker_, time_keeper); + std::make_shared(*node_, *parameters, lane_departure_checker_); } void initialize_planner_data() 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..474da055b4de7 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp @@ -0,0 +1,177 @@ +// 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_); + + shift_pull_out_ = std::make_shared(*node_, parameters, lane_departure_checker_); + } +}; + +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/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt index b4a688d711221..220c380eb0fd7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.14) project(autoware_behavior_velocity_blind_spot_module) +option(EXPORT_TEST_PLOT_FIGURE "Export plot figures in test" OFF) + find_package(autoware_cmake REQUIRED) autoware_package() pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) @@ -11,6 +13,20 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene.cpp src/decisions.cpp src/util.cpp + src/parameter.cpp ) -ament_auto_package(INSTALL_TO_SHARE config) +if(BUILD_TESTING) + if(EXPORT_TEST_PLOT_FIGURE) + add_definitions(-DEXPORT_TEST_PLOT_FIGURE "-Wno-attributes") # // cspell: ignore DEXPORT + endif() + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + # NOTE(soblin): pybind11::scoped_interpreter needs to be initialized globally, not in the FixtureClass instantiated for each test suite + ament_add_gtest(test_${PROJECT_NAME}_util + test/test_util.cpp + ) + target_link_libraries(test_${PROJECT_NAME}_util ${PROJECT_NAME}) +endif() + +ament_auto_package(INSTALL_TO_SHARE config test_data) 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..41330e459a524 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 @@ -15,11 +15,12 @@ #ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__MANAGER_HPP_ #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__MANAGER_HPP_ +#include "autoware/behavior_velocity_blind_spot_module/parameter.hpp" #include "autoware/behavior_velocity_blind_spot_module/scene.hpp" #include #include -#include +#include #include #include @@ -38,12 +39,12 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC const char * getModuleName() override { return "blind_spot"; } private: - BlindSpotModule::PlannerParam planner_param_; + PlannerParam planner_param_; 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/parameter.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/parameter.hpp new file mode 100644 index 0000000000000..59322caecb62f --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/parameter.hpp @@ -0,0 +1,40 @@ +// 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_BLIND_SPOT_MODULE__PARAMETER_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__PARAMETER_HPP_ + +#include + +#include + +namespace autoware::behavior_velocity_planner +{ +struct PlannerParam +{ + static PlannerParam init(rclcpp::Node & node, const std::string & ns); + bool use_pass_judge_line{}; + double stop_line_margin{}; + double backward_detection_length{}; + double ignore_width_from_center_line{}; + double adjacent_extend_width{}; + double opposite_adjacent_extend_width{}; + double max_future_movement_time{}; + double ttc_min{}; + double ttc_max{}; + double ttc_ego_minimal_velocity{}; +}; +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__PARAMETER_HPP_ 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..5e8ef9fc8a063 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 @@ -15,9 +15,10 @@ #ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_ #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_ +#include #include -#include #include +#include #include #include @@ -59,7 +60,7 @@ struct Safe using BlindSpotDecision = std::variant; -class BlindSpotModule : public SceneModuleInterface +class BlindSpotModule : public SceneModuleInterfaceWithRTC { public: struct DebugData @@ -70,20 +71,6 @@ class BlindSpotModule : public SceneModuleInterface }; public: - struct PlannerParam - { - bool use_pass_judge_line; - double stop_line_margin; - double backward_detection_length; - double ignore_width_from_center_line; - double adjacent_extend_width; - double opposite_adjacent_extend_width; - double max_future_movement_time; - double ttc_min; - double ttc_max; - double ttc_ego_minimal_velocity; - }; - 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, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp index 9d908414b6d95..e18d96709ef92 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__UTIL_HPP_ #include +#include #include @@ -25,6 +26,7 @@ #include #include #include +#include namespace autoware::behavior_velocity_planner { @@ -50,31 +52,49 @@ std::optional generateInterpolatedPathInfo( const lanelet::Id lane_id, const tier4_planning_msgs::msg::PathWithLaneId & input_path, rclcpp::Logger logger); +std::optional getFirstPointIntersectsLineByFootprint( + const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length); + std::optional getSiblingStraightLanelet( const lanelet::Lanelet assigned_lane, const lanelet::routing::RoutingGraphConstPtr routing_graph_ptr); /** - * @brief Create half lanelet - * @param lanelet input lanelet - * @return Half lanelet + * @brief generate a new lanelet object on the `turn_direction` side of `lanelet` which is offset + * from `ignore_width_from_centerline` from the centerline of `lanelet` + * @return new lanelet object */ lanelet::ConstLanelet generateHalfLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection & turn_direction, const double ignore_width_from_centerline); +/** + * @brief generate a new lanelet object from the `turn_direction` side neighboring lanelet of the + * input `lanelet` by the width of `adjacent_extend_width` + * @param new lanelet object + */ lanelet::ConstLanelet generateExtendedAdjacentLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection direction, const double adjacent_extend_width); + +/** + * @brief generate a new lanelet object from the `turn_direction` side neighboring opposite lanelet + * of the input `lanelet` by the width of `opposite_adjacent_extend_width` + * @param new lanelet object + */ lanelet::ConstLanelet generateExtendedOppositeAdjacentLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection direction, const double opposite_adjacent_extend_width); +std::vector find_lane_ids_upto( + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::Id lane_id); + lanelet::ConstLanelets generateBlindSpotLanelets( const std::shared_ptr route_handler, - const TurnDirection turn_direction, const lanelet::Id lane_id, - const tier4_planning_msgs::msg::PathWithLaneId & path, const double ignore_width_from_centerline, - const double adjacent_extend_width, const double opposite_adjacent_extend_width); + const TurnDirection turn_direction, const std::vector & lane_ids_upto_intersection, + const double ignore_width_from_centerline, const double adjacent_extend_width, + const double opposite_adjacent_extend_width); /** * @brief Make blind spot areas. Narrow area is made from closest path point to stop line index. 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..dabd3045b31d2 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 @@ -17,12 +17,15 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_test_utils autoware_universe_utils geometry_msgs pluginlib @@ -31,6 +34,7 @@ tier4_planning_msgs visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common 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..d6cae9004600a 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 @@ -34,23 +34,7 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(BlindSpotModuleManager::getModuleName()); - planner_param_.use_pass_judge_line = - getOrDeclareParameter(node, ns + ".use_pass_judge_line"); - planner_param_.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); - planner_param_.backward_detection_length = - getOrDeclareParameter(node, ns + ".backward_detection_length"); - planner_param_.ignore_width_from_center_line = - getOrDeclareParameter(node, ns + ".ignore_width_from_center_line"); - planner_param_.adjacent_extend_width = - getOrDeclareParameter(node, ns + ".adjacent_extend_width"); - planner_param_.opposite_adjacent_extend_width = - getOrDeclareParameter(node, ns + ".opposite_adjacent_extend_width"); - planner_param_.max_future_movement_time = - getOrDeclareParameter(node, ns + ".max_future_movement_time"); - planner_param_.ttc_min = getOrDeclareParameter(node, ns + ".ttc_min"); - planner_param_.ttc_max = getOrDeclareParameter(node, ns + ".ttc_max"); - planner_param_.ttc_ego_minimal_velocity = - getOrDeclareParameter(node, ns + ".ttc_ego_minimal_velocity"); + planner_param_ = PlannerParam::init(node, ns); } void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) @@ -83,7 +67,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/parameter.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp new file mode 100644 index 0000000000000..0984de6f73262 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp @@ -0,0 +1,45 @@ +// 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_velocity_blind_spot_module/parameter.hpp" + +#include + +#include + +namespace autoware::behavior_velocity_planner +{ + +PlannerParam PlannerParam::init(rclcpp::Node & node, const std::string & ns) +{ + using autoware::universe_utils::getOrDeclareParameter; + PlannerParam param; + param.use_pass_judge_line = getOrDeclareParameter(node, ns + ".use_pass_judge_line"); + param.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); + param.backward_detection_length = + getOrDeclareParameter(node, ns + ".backward_detection_length"); + param.ignore_width_from_center_line = + getOrDeclareParameter(node, ns + ".ignore_width_from_center_line"); + param.adjacent_extend_width = getOrDeclareParameter(node, ns + ".adjacent_extend_width"); + param.opposite_adjacent_extend_width = + getOrDeclareParameter(node, ns + ".opposite_adjacent_extend_width"); + param.max_future_movement_time = + getOrDeclareParameter(node, ns + ".max_future_movement_time"); + param.ttc_min = getOrDeclareParameter(node, ns + ".ttc_min"); + param.ttc_max = getOrDeclareParameter(node, ns + ".ttc_max"); + param.ttc_ego_minimal_velocity = + getOrDeclareParameter(node, ns + ".ttc_ego_minimal_velocity"); + return param; +} +} // namespace autoware::behavior_velocity_planner 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..8a9401646aaea 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), @@ -101,8 +101,9 @@ BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(PathWithLaneId * pat } if (!blind_spot_lanelets_) { + const auto lane_ids_upto_intersection = find_lane_ids_upto(input_path, lane_id_); const auto blind_spot_lanelets = generateBlindSpotLanelets( - planner_data_->route_handler_, turn_direction_, lane_id_, input_path, + planner_data_->route_handler_, turn_direction_, lane_ids_upto_intersection, planner_param_.ignore_width_from_center_line, planner_param_.adjacent_extend_width, planner_param_.opposite_adjacent_extend_width); if (!blind_spot_lanelets.empty()) { @@ -179,27 +180,6 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path) return true; } -static std::optional getFirstPointIntersectsLineByFootprint( - const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, - const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) -{ - const auto & path_ip = interpolated_path_info.path; - const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); - const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); - const size_t start = - static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); - const auto line2d = line.basicLineString(); - for (auto i = start; i <= lane_end; ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = autoware::universe_utils::transformVector( - footprint, autoware::universe_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, line2d)) { - return std::make_optional(i); - } - } - return std::nullopt; -} - static std::optional getDuplicatedPointIdx( const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp index 5c5aa0a26b3b4..8451661b2b71f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp @@ -88,6 +88,27 @@ std::optional generateInterpolatedPathInfo( return interpolated_path_info; } +std::optional getFirstPointIntersectsLineByFootprint( + const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); + const auto line2d = line.basicLineString(); + for (auto i = start; i <= lane_end; ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = autoware::universe_utils::transformVector( + footprint, autoware::universe_utils::pose2transform(base_pose)); + if (boost::geometry::intersects(path_footprint, line2d)) { + return std::make_optional(i); + } + } + return std::nullopt; +} + std::optional getSiblingStraightLanelet( const lanelet::Lanelet assigned_lane, const lanelet::routing::RoutingGraphConstPtr routing_graph_ptr) @@ -200,15 +221,9 @@ static lanelet::LineString3d removeConst(lanelet::ConstLineString3d line) return lanelet::LineString3d(lanelet::InvalId, pts); } -lanelet::ConstLanelets generateBlindSpotLanelets( - const std::shared_ptr route_handler, - const TurnDirection turn_direction, const lanelet::Id lane_id, - const tier4_planning_msgs::msg::PathWithLaneId & path, const double ignore_width_from_centerline, - const double adjacent_extend_width, const double opposite_adjacent_extend_width) +std::vector find_lane_ids_upto( + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::Id lane_id) { - const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); - const auto routing_graph_ptr = route_handler->getRoutingGraphPtr(); - std::vector lane_ids; /* get lane ids until intersection */ for (const auto & point : path.points) { @@ -216,19 +231,29 @@ lanelet::ConstLanelets generateBlindSpotLanelets( for (const auto id : point.lane_ids) { if (id == lane_id) { found_intersection_lane = true; - lane_ids.push_back(lane_id); break; } // make lane_ids unique - if (std::find(lane_ids.begin(), lane_ids.end(), lane_id) == lane_ids.end()) { - lane_ids.push_back(lane_id); + if (std::find(lane_ids.begin(), lane_ids.end(), id) == lane_ids.end()) { + lane_ids.push_back(id); } } if (found_intersection_lane) break; } + return lane_ids; +} + +lanelet::ConstLanelets generateBlindSpotLanelets( + const std::shared_ptr route_handler, + const TurnDirection turn_direction, const std::vector & lane_ids_upto_intersection, + const double ignore_width_from_centerline, const double adjacent_extend_width, + const double opposite_adjacent_extend_width) +{ + const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); + const auto routing_graph_ptr = route_handler->getRoutingGraphPtr(); lanelet::ConstLanelets blind_spot_lanelets; - for (const auto i : lane_ids) { + for (const auto i : lane_ids_upto_intersection) { const auto lane = lanelet_map_ptr->laneletLayer.get(i); const auto ego_half_lanelet = generateHalfLanelet(lane, turn_direction, ignore_width_from_centerline); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..e0ceca4e12951 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// 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 + +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); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "blind_spot", "autoware::behavior_velocity_planner::BlindSpotModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + 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_blind_spot_module/test/test_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp new file mode 100644 index 0000000000000..5c2a239e4142f --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp @@ -0,0 +1,448 @@ +// 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 + +#ifdef EXPORT_TEST_PLOT_FIGURE +#include +#include +#include +#include + +#include +#include // needed for passing std::string to Args + +#endif + +using autoware::test_utils::parse; + +class TestWithAdjLaneData : public ::testing::Test +{ +protected: + void SetUp() override + { + const auto test_data_file = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/object_on_adj_lane.yaml"; + const auto config = YAML::LoadFile(test_data_file); + const auto route = parse(config["route"]); + const auto map_path = + autoware::test_utils::resolve_pkg_share_uri(config["map_path_uri"].as()); + if (!map_path) { + ASSERT_DEATH({ assert(false); }, "invalid map path"); + } + const auto intersection_map = autoware::test_utils::make_map_bin_msg(map_path.value()); + route_handler = std::make_shared(); + route_handler->setMap(intersection_map); + route_handler->setRoute(route); + self_odometry = autoware::test_utils::create_const_shared_ptr( + parse(config["self_odometry"])); + dynamic_object = autoware::test_utils::create_const_shared_ptr( + parse(config["dynamic_object"])); + path_with_lane_id = + parse(config["path_with_lane_id"]); + + // parameter + auto node_options = rclcpp::NodeOptions{}; + node_options.arguments(std::vector{ + "--ros-args", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/config/blind_spot.param.yaml", + }); + + auto node = rclcpp::Node::make_shared("blind_spot_test", node_options); + param = autoware::behavior_velocity_planner::PlannerParam::init(*node, "blind_spot"); + } + + std::shared_ptr route_handler{}; + std::shared_ptr self_odometry{}; + std::shared_ptr dynamic_object{}; + const lanelet::Id lane_id_{2200}; + tier4_planning_msgs::msg::PathWithLaneId path_with_lane_id; + autoware::behavior_velocity_planner::PlannerParam param; +}; + +TEST_F(TestWithAdjLaneData, getSiblingStraightLanelet) +{ + const auto sibling_straight_lanelet_opt = + autoware::behavior_velocity_planner::getSiblingStraightLanelet( + route_handler->getLaneletMapPtr()->laneletLayer.get(lane_id_), + route_handler->getRoutingGraphPtr()); + ASSERT_NO_FATAL_FAILURE({ ASSERT_TRUE(sibling_straight_lanelet_opt.has_value()); }); + EXPECT_EQ(sibling_straight_lanelet_opt.value().id(), 2100); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(lane_id_), ax, + autoware::test_utils::LaneConfig{"intersection turning lanes", LineConfig{"blue"}}); + + // for illustration + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(3010933), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"green"}}); + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(2201), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"blue"}}); + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(2202), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"blue"}}); + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(2010), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"green"}}); + + const auto [x0, x1] = ax.get_xlim(); + const auto [y0, y1] = ax.get_ylim(); + const double width = x1 - x0; + const double height = y1 - y0; + ax.set_xlim(Args(x0, x0 + width * 1.3)); + ax.set_ylim(Args(y0, y0 + height * 1.3)); + autoware::test_utils::plot_lanelet2_object( + sibling_straight_lanelet_opt.value(), ax, + LaneConfig{"sibling_straight_lanelet", LineConfig{"red"}}); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateHalfLanelet) +{ + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(2010); + + const auto half_lanelet = autoware::behavior_velocity_planner::generateHalfLanelet( + lanelet, autoware::behavior_velocity_planner::TurnDirection::LEFT, + param.ignore_width_from_center_line); + + /* + TODO(soblin): how to check if they overlap only on the left line string + EXPECT_EQ( + boost::geometry::within( + half_lanelet.polygon2d().basicPolygon(), lanelet.polygon2d().basicPolygon()), + true); + */ + EXPECT_EQ( + boost::geometry::area(lanelet.polygon2d().basicPolygon()) / 2.0 > + boost::geometry::area(half_lanelet.polygon2d().basicPolygon()), + true); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, autoware::test_utils::LaneConfig{"original", LineConfig{"blue", 1.5}}); + autoware::test_utils::plot_lanelet2_object( + half_lanelet, ax, LaneConfig{"half lanelet", LineConfig{"red", 0.75}}); + + const auto [x0, x1] = ax.get_xlim(); + const auto [y0, y1] = ax.get_ylim(); + const double width = x1 - x0; + // const double height = y1 - y0; + ax.set_xlim(Args(x0, x0 + width * 1.3)); + ax.set_ylim(Args(y0, 650)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateExtendedAdjacentLanelet) +{ + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(2010); + const auto adj_lanelet = *(route_handler->getRoutingGraphPtr()->adjacentLeft(lanelet)); + + const auto extended_adj_lanelet = + autoware::behavior_velocity_planner::generateExtendedAdjacentLanelet( + adj_lanelet, autoware::behavior_velocity_planner::TurnDirection::LEFT, + param.adjacent_extend_width); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, autoware::test_utils::LaneConfig{"given", LineConfig{"blue", 1.0}}); + autoware::test_utils::plot_lanelet2_object( + adj_lanelet, ax, autoware::test_utils::LaneConfig{"adjacent", LineConfig{"green", 1.0}}); + autoware::test_utils::plot_lanelet2_object( + extended_adj_lanelet, ax, LaneConfig{"generated", LineConfig{"red", 2.0}}); + + const auto [x0, x1] = ax.get_xlim(); + const auto [y0, y1] = ax.get_ylim(); + const double width = x1 - x0; + const double height = y1 - y0; + ax.set_xlim(Args(x0, x0 + width * 1.3)); + ax.set_ylim(Args(y0, y0 + height * 1.3)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateBlindSpotLanelets_left) +{ + const auto blind_spot_lanelets = autoware::behavior_velocity_planner::generateBlindSpotLanelets( + route_handler, autoware::behavior_velocity_planner::TurnDirection::LEFT, {2000, 2010}, + param.ignore_width_from_center_line, param.adjacent_extend_width, + param.opposite_adjacent_extend_width); + EXPECT_EQ(blind_spot_lanelets.size(), 2); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {2010, 3010933, 2200, 3010920, 3010933, 2000}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + for (const auto & blind_spot_lanelet : blind_spot_lanelets) { + autoware::test_utils::plot_lanelet2_object( + blind_spot_lanelet, ax, LaneConfig{"blind_spot_lanelet", LineConfig{"blue", 2.0}}); + } + const auto [y0, y1] = ax.get_ylim(); + const double height = y1 - y0; + ax.set_xlim(Args(300, 365)); + ax.set_ylim(Args(y0, y0 + height * 1.3)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateBlindSpotLanelets_right) +{ + const auto blind_spot_lanelets = autoware::behavior_velocity_planner::generateBlindSpotLanelets( + route_handler, autoware::behavior_velocity_planner::TurnDirection::RIGHT, {3008067}, + param.ignore_width_from_center_line, param.adjacent_extend_width, + param.opposite_adjacent_extend_width); + EXPECT_EQ(blind_spot_lanelets.size(), 1); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {3008057, 3008054, 3008056, 3008061, 3008062, 3008059, 3008067, 3008066}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + for (const auto & blind_spot_lanelet : blind_spot_lanelets) { + autoware::test_utils::plot_lanelet2_object( + blind_spot_lanelet, ax, LaneConfig{"blind_spot_lanelet", LineConfig{"blue", 2.0}}); + } + ax.set_xlim(Args(905, 920)); + ax.set_ylim(Args(650, 950)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateInterpolatedPathInfo) +{ + const auto interpolated_path_info_opt = + autoware::behavior_velocity_planner::generateInterpolatedPathInfo( + lane_id_, path_with_lane_id, rclcpp::get_logger("test")); + EXPECT_EQ(interpolated_path_info_opt.has_value(), true); + const auto & interpolated_path_info = interpolated_path_info_opt.value(); + EXPECT_EQ(interpolated_path_info.lane_id_interval.has_value(), true); + const auto [start, end] = interpolated_path_info.lane_id_interval.value(); + tier4_planning_msgs::msg::PathWithLaneId interpolated_path; + for (auto i = start; i <= end; ++i) { + interpolated_path.points.push_back(interpolated_path_info.path.points.at(i)); + } +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + using autoware::test_utils::PathWithLaneIdConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {2010, 3010933, 2200, 3010920, 3010933, 2000, 3500}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + autoware::test_utils::plot_autoware_object( + path_with_lane_id, ax, PathWithLaneIdConfig{"original path", "red", 0.75}); + autoware::test_utils::plot_autoware_object( + interpolated_path, ax, PathWithLaneIdConfig{"interpolated path on lane 2010", "blue", 1.0}); + const auto [x0, x1] = ax.get_xlim(); + ax.set_xlim(Args(335, x1)); + ax.set_ylim(Args(640, 670)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +class TestWithShoulderData : public ::testing::Test +{ +protected: + void SetUp() override + { + const auto test_data_file = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/object_on_shoulder.yaml"; + const auto config = YAML::LoadFile(test_data_file); + const auto route = parse(config["route"]); + const auto map_path = + autoware::test_utils::resolve_pkg_share_uri(config["map_path_uri"].as()); + if (!map_path) { + ASSERT_DEATH({ assert(false); }, "invalid map path"); + } + const auto intersection_map = autoware::test_utils::make_map_bin_msg(map_path.value()); + route_handler = std::make_shared(); + route_handler->setMap(intersection_map); + route_handler->setRoute(route); + self_odometry = autoware::test_utils::create_const_shared_ptr( + parse(config["self_odometry"])); + dynamic_object = autoware::test_utils::create_const_shared_ptr( + parse(config["dynamic_object"])); + path_with_lane_id = + parse(config["path_with_lane_id"]); + + // parameter + auto node_options = rclcpp::NodeOptions{}; + node_options.arguments(std::vector{ + "--ros-args", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/config/blind_spot.param.yaml", + }); + + auto node = rclcpp::Node::make_shared("blind_spot_test", node_options); + param = autoware::behavior_velocity_planner::PlannerParam::init(*node, "blind_spot"); + } + + std::shared_ptr route_handler{}; + std::shared_ptr self_odometry{}; + std::shared_ptr dynamic_object{}; + const lanelet::Id lane_id_{1010}; + tier4_planning_msgs::msg::PathWithLaneId path_with_lane_id; + autoware::behavior_velocity_planner::PlannerParam param; +}; + +TEST_F(TestWithShoulderData, generateBlindSpotLaneletsShoulder_left) +{ + const auto blind_spot_lanelets = autoware::behavior_velocity_planner::generateBlindSpotLanelets( + route_handler, autoware::behavior_velocity_planner::TurnDirection::LEFT, {1000, 1010}, + param.ignore_width_from_center_line, param.adjacent_extend_width, + param.opposite_adjacent_extend_width); + EXPECT_EQ(blind_spot_lanelets.size(), 2); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {1000, 1010, 3010907, 3010879, 1200, 1100}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + for (const auto & blind_spot_lanelet : blind_spot_lanelets) { + autoware::test_utils::plot_lanelet2_object( + blind_spot_lanelet, ax, LaneConfig{"blind_spot_lanelet", LineConfig{"blue", 2.0}}); + } + ax.set_xlim(Args(330, 365)); + ax.set_ylim(Args(580, 625)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper left"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +int main(int argc, char ** argv) +{ +#ifdef EXPORT_TEST_PLOT_FIGURE + py::scoped_interpreter guard{}; +#endif + rclcpp::init(0, nullptr); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_adj_lane.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_adj_lane.yaml new file mode 100644 index 0000000000000..97068d69cd296 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_adj_lane.yaml @@ -0,0 +1,2698 @@ +# +# AUTO GENERATED by autoware_test_utils::topic_snapshot_saver +# format1: +# +# format_version: +# map_path_uri: package:/// +# fields(this is array) +# - name: +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | PathWithLaneId | TBD} +# topic: +# +format_version: 1 +map_path_uri: package://autoware_test_utils/test_map/intersection/lanelet2_map.osm +path_with_lane_id: + header: + stamp: + sec: 563 + nanosec: 125840339 + frame_id: map + points: + - point: + pose: + position: + x: 330.010 + y: 643.206 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00655991 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 332.010 + y: 643.233 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00656125 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 334.009 + y: 643.259 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00658745 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 336.009 + y: 643.285 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00660718 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 338.009 + y: 643.312 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00658719 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 340.009 + y: 643.338 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00656661 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 342.009 + y: 643.364 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00657433 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 342.809 + y: 643.375 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0162350 + w: 0.999868 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - 2200 + - point: + pose: + position: + x: 344.008 + y: 643.414 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0355542 + w: 0.999368 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 346.003 + y: 643.556 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0600763 + w: 0.998194 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 347.986 + y: 643.796 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.134724 + w: 0.990883 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 349.912 + y: 644.329 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.199426 + w: 0.979913 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 351.776 + y: 645.121 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.281519 + w: 0.959556 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 353.448 + y: 646.194 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.345347 + w: 0.938475 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 354.997 + y: 647.513 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.407454 + w: 0.913226 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 356.322 + y: 648.989 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.480104 + w: 0.877212 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 357.418 + y: 650.701 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.544895 + w: 0.838504 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 358.228 + y: 652.525 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.610262 + w: 0.792199 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 358.741 + y: 654.467 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.646506 + w: 0.762909 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.070 + y: 656.445 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.697457 + w: 0.716627 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.124 + y: 658.438 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.713869 + w: 0.700280 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.085 + y: 660.438 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714886 + w: 0.699241 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.052 + y: 661.935 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - 3500 + - point: + pose: + position: + x: 359.040 + y: 662.438 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.993 + y: 664.437 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.945 + y: 666.436 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715334 + w: 0.698782 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.900 + y: 668.374 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714518 + w: 0.699617 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.858 + y: 670.373 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712894 + w: 0.701272 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.825 + y: 672.373 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710606 + w: 0.703590 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.807 + y: 674.174 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705159 + w: 0.709049 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.812 + y: 675.174 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705159 + w: 0.709049 + longitudinal_velocity_mps: 0.00000 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + left_bound: + - x: 329.488 + y: 644.822 + z: 100.000 + - x: 332.672 + y: 644.864 + z: 100.000 + - x: 334.352 + y: 644.886 + z: 100.000 + - x: 336.351 + y: 644.913 + z: 100.000 + - x: 337.719 + y: 644.931 + z: 100.000 + - x: 340.352 + y: 644.965 + z: 100.000 + - x: 342.813 + y: 645.020 + z: 100.000 + - x: 344.277 + y: 645.102 + z: 100.000 + - x: 346.200 + y: 645.256 + z: 100.000 + - x: 347.374 + y: 645.428 + z: 100.000 + - x: 348.470 + y: 645.697 + z: 100.000 + - x: 349.568 + y: 646.035 + z: 100.000 + - x: 350.592 + y: 646.513 + z: 100.000 + - x: 351.572 + y: 647.043 + z: 100.000 + - x: 352.507 + y: 647.713 + z: 100.000 + - x: 353.327 + y: 648.476 + z: 100.000 + - x: 354.109 + y: 649.196 + z: 100.000 + - x: 354.661 + y: 649.830 + z: 100.000 + - x: 355.275 + y: 650.523 + z: 100.000 + - x: 356.177 + y: 652.020 + z: 100.000 + - x: 356.615 + y: 653.084 + z: 100.000 + - x: 356.929 + y: 654.139 + z: 100.000 + - x: 357.146 + y: 655.241 + z: 100.000 + - x: 357.284 + y: 656.160 + z: 100.000 + - x: 357.312 + y: 657.420 + z: 100.000 + - x: 357.425 + y: 658.703 + z: 100.000 + - x: 357.383 + y: 660.465 + z: 100.000 + - x: 357.349 + y: 661.894 + z: 100.000 + - x: 357.386 + y: 664.700 + z: 100.000 + - x: 357.338 + y: 666.699 + z: 100.000 + - x: 357.291 + y: 668.699 + z: 100.000 + - x: 357.243 + y: 670.698 + z: 100.000 + - x: 357.195 + y: 672.698 + z: 100.000 + - x: 357.148 + y: 674.697 + z: 100.000 + - x: 357.021 + y: 680.023 + z: 100.000 + right_bound: + - x: 329.531 + y: 641.578 + z: 100.000 + - x: 332.717 + y: 641.619 + z: 100.000 + - x: 334.395 + y: 641.641 + z: 100.000 + - x: 336.394 + y: 641.668 + z: 100.000 + - x: 337.761 + y: 641.686 + z: 100.000 + - x: 340.394 + y: 641.721 + z: 100.000 + - x: 342.805 + y: 641.730 + z: 100.000 + - x: 344.462 + y: 641.823 + z: 100.000 + - x: 346.283 + y: 641.878 + z: 100.000 + - x: 347.708 + y: 641.985 + z: 100.000 + - x: 349.158 + y: 642.284 + z: 100.000 + - x: 350.570 + y: 642.681 + z: 100.000 + - x: 351.954 + y: 643.219 + z: 100.000 + - x: 353.269 + y: 643.933 + z: 100.000 + - x: 354.499 + y: 644.727 + z: 100.000 + - x: 355.646 + y: 645.678 + z: 100.000 + - x: 356.516 + y: 646.483 + z: 100.000 + - x: 357.282 + y: 647.228 + z: 100.000 + - x: 357.690 + y: 647.819 + z: 100.000 + - x: 358.527 + y: 649.081 + z: 100.000 + - x: 359.237 + y: 650.418 + z: 100.000 + - x: 359.789 + y: 651.774 + z: 100.000 + - x: 360.225 + y: 653.220 + z: 100.000 + - x: 360.523 + y: 654.391 + z: 100.000 + - x: 360.701 + y: 655.780 + z: 100.000 + - x: 360.976 + y: 657.487 + z: 100.000 + - x: 360.804 + y: 658.842 + z: 100.000 + - x: 360.774 + y: 660.766 + z: 100.000 + - x: 360.755 + y: 661.975 + z: 100.000 + - x: 360.585 + y: 664.776 + z: 100.000 + - x: 360.537 + y: 666.776 + z: 100.000 + - x: 360.490 + y: 668.775 + z: 100.000 + - x: 360.442 + y: 670.774 + z: 100.000 + - x: 360.394 + y: 672.774 + z: 100.000 + - x: 360.347 + y: 674.773 + z: 100.000 + - x: 360.220 + y: 680.099 + z: 100.000 +dynamic_object: + header: + stamp: + sec: 563 + nanosec: 160706650 + frame_id: map + objects: + - object_id: + uuid: + - 6 + - 144 + - 167 + - 221 + - 26 + - 196 + - 145 + - 186 + - 207 + - 95 + - 21 + - 211 + - 96 + - 13 + - 92 + - 5 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 404.688 + y: 647.065 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + covariance: + - 0.415785 + - -0.000621846 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000621846 + - 0.388031 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0108039 + initial_twist_with_covariance: + twist: + linear: + x: 9.96635 + y: 3.24754e-05 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 5.69744e-05 + covariance: + - 1.01780 + - -2.02150e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - -3.54650e-05 + - -2.02150e-05 + - 0.00403854 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00708516 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - -3.54650e-05 + - 0.00708516 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0124301 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 404.688 + y: 647.065 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 409.670 + y: 647.155 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 414.653 + y: 647.245 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 419.635 + y: 647.335 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 424.617 + y: 647.425 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 429.600 + y: 647.515 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 434.582 + y: 647.605 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 439.565 + y: 647.694 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 444.547 + y: 647.784 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 449.529 + y: 647.874 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 454.512 + y: 647.964 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 459.494 + y: 648.054 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 464.476 + y: 648.144 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 469.459 + y: 648.233 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 474.441 + y: 648.323 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 479.423 + y: 648.413 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 484.406 + y: 648.503 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 489.388 + y: 648.593 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 494.371 + y: 648.683 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 499.353 + y: 648.773 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 504.335 + y: 648.862 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 1.00000 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.90000 + y: 0.501209 + z: 0.690476 + - object_id: + uuid: + - 32 + - 119 + - 141 + - 16 + - 72 + - 85 + - 32 + - 153 + - 66 + - 136 + - 88 + - 177 + - 247 + - 166 + - 151 + - 80 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + covariance: + - 0.0657687 + - -0.000366900 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000366900 + - 0.0451891 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00300423 + initial_twist_with_covariance: + twist: + linear: + x: 10.0048 + y: -0.00124906 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: -0.00219133 + covariance: + - 0.522271 + - -6.09993e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000107016 + - -6.09993e-05 + - 0.00263830 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00462859 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - -0.000107016 + - 0.00462859 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00812034 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.785 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00453640 + w: 0.999990 + - position: + x: 333.933 + y: 644.723 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00622365 + w: 0.999981 + - position: + x: 338.935 + y: 644.640 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00828910 + w: 0.999966 + - position: + x: 343.877 + y: 644.600 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00409926 + w: 0.999992 + - position: + x: 348.624 + y: 645.122 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0547342 + w: 0.998501 + - position: + x: 352.898 + y: 646.993 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.204867 + w: 0.978790 + - position: + x: 356.221 + y: 650.176 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.372718 + w: 0.927945 + - position: + x: 358.029 + y: 654.521 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.554937 + w: 0.831892 + - position: + x: 358.484 + y: 659.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.673390 + w: 0.739287 + - position: + x: 358.380 + y: 664.391 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714418 + w: 0.699720 + - position: + x: 358.261 + y: 669.391 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.142 + y: 674.391 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.023 + y: 679.392 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.904 + y: 684.392 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.784 + y: 689.392 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.665 + y: 694.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.546 + y: 699.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.427 + y: 704.394 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.308 + y: 709.394 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.188 + y: 714.395 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.069 + y: 719.395 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.950 + y: 724.396 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.831 + y: 729.397 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.712 + y: 734.398 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.593 + y: 739.399 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.473 + y: 744.400 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.354 + y: 749.400 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.235 + y: 754.401 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.116 + y: 759.402 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.997 + y: 764.403 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.786 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00451016 + w: 0.999990 + - position: + x: 333.933 + y: 644.725 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00607237 + w: 0.999982 + - position: + x: 338.935 + y: 644.645 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00796688 + w: 0.999968 + - position: + x: 343.880 + y: 644.606 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00392303 + w: 0.999992 + - position: + x: 348.786 + y: 644.906 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0304731 + w: 0.999536 + - position: + x: 353.463 + y: 645.966 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.111241 + w: 0.993793 + - position: + x: 357.696 + y: 648.148 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.235723 + w: 0.971820 + - position: + x: 361.186 + y: 651.334 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.361617 + w: 0.932327 + - position: + x: 363.716 + y: 655.456 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.488304 + w: 0.872674 + - position: + x: 364.853 + y: 660.052 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.616332 + w: 0.787487 + - position: + x: 364.887 + y: 665.003 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.704715 + w: 0.709490 + - position: + x: 364.768 + y: 670.003 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.648 + y: 675.003 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.529 + y: 680.004 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.410 + y: 685.004 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.291 + y: 690.005 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.172 + y: 695.005 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.053 + y: 700.006 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.933 + y: 705.006 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.814 + y: 710.007 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.695 + y: 715.007 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.576 + y: 720.008 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.457 + y: 725.009 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.338 + y: 730.010 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.218 + y: 735.011 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.099 + y: 740.011 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.980 + y: 745.012 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.861 + y: 750.013 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.742 + y: 755.014 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.622 + y: 760.015 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.786 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00451016 + w: 0.999990 + - position: + x: 333.933 + y: 644.725 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00607237 + w: 0.999982 + - position: + x: 338.935 + y: 644.645 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00796688 + w: 0.999968 + - position: + x: 343.873 + y: 644.613 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00329511 + w: 0.999995 + - position: + x: 348.698 + y: 645.015 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0415760 + w: 0.999135 + - position: + x: 353.223 + y: 646.395 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.147464 + w: 0.989067 + - position: + x: 357.115 + y: 649.075 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.296991 + w: 0.954880 + - position: + x: 359.997 + y: 652.871 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.444541 + w: 0.895759 + - position: + x: 361.525 + y: 657.337 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.581548 + w: 0.813512 + - position: + x: 361.651 + y: 662.207 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.697878 + w: 0.716217 + - position: + x: 361.533 + y: 667.205 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715386 + w: 0.698729 + - position: + x: 361.414 + y: 672.205 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 361.295 + y: 677.206 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 361.176 + y: 682.206 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 361.057 + y: 687.206 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.938 + y: 692.207 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.818 + y: 697.207 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.699 + y: 702.208 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.580 + y: 707.208 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.461 + y: 712.209 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.342 + y: 717.209 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.222 + y: 722.210 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.103 + y: 727.211 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.984 + y: 732.212 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.865 + y: 737.213 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.746 + y: 742.214 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.626 + y: 747.214 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.507 + y: 752.215 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.388 + y: 757.216 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.269 + y: 762.217 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.785 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00457230 + w: 0.999990 + - position: + x: 333.933 + y: 644.721 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00644958 + w: 0.999979 + - position: + x: 338.935 + y: 644.632 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00883007 + w: 0.999961 + - position: + x: 343.938 + y: 644.526 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.0105871 + w: 0.999944 + - position: + x: 348.941 + y: 644.416 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.0110042 + w: 0.999939 + - position: + x: 353.943 + y: 644.319 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00972075 + w: 0.999953 + - position: + x: 358.946 + y: 644.250 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00691289 + w: 0.999976 + - position: + x: 363.948 + y: 644.219 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00309863 + w: 0.999995 + - position: + x: 368.950 + y: 644.228 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.000886733 + w: 1.00000 + - position: + x: 373.952 + y: 644.264 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00363761 + w: 0.999993 + - position: + x: 378.954 + y: 644.305 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00413557 + w: 0.999991 + - position: + x: 383.955 + y: 644.347 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00420318 + w: 0.999991 + - position: + x: 388.957 + y: 644.388 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00411113 + w: 0.999992 + - position: + x: 393.958 + y: 644.432 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00439488 + w: 0.999990 + - position: + x: 398.956 + y: 644.454 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00215849 + w: 0.999998 + - position: + x: 403.941 + y: 644.630 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0176845 + w: 0.999844 + - position: + x: 408.939 + y: 644.821 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 413.938 + y: 645.012 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 418.936 + y: 645.202 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 423.935 + y: 645.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 428.933 + y: 645.584 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 433.932 + y: 645.774 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 438.931 + y: 645.965 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 443.929 + y: 646.156 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 448.928 + y: 646.346 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 453.927 + y: 646.537 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 458.925 + y: 646.728 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 463.924 + y: 646.918 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 468.923 + y: 647.109 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 473.922 + y: 647.300 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.90000 + y: 0.604998 + z: 0.767451 +traffic_signal: + stamp: + sec: 0 + nanosec: 0 + traffic_light_groups: [] +route: + header: + stamp: + sec: 21 + nanosec: 17351334 + frame_id: map + start_pose: + position: + x: 301.475 + y: 642.905 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00500123 + w: 0.999987 + goal_pose: + position: + x: 358.812 + y: 675.174 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705159 + w: 0.709049 + segments: + - preferred_primitive: + id: 2000 + primitive_type: "" + primitives: + - id: 2000 + primitive_type: lane + - id: 2001 + primitive_type: lane + - id: 2002 + primitive_type: lane + - preferred_primitive: + id: 2010 + primitive_type: "" + primitives: + - id: 2010 + primitive_type: lane + - preferred_primitive: + id: 2200 + primitive_type: "" + primitives: + - id: 2200 + primitive_type: lane + - preferred_primitive: + id: 3500 + primitive_type: "" + primitives: + - id: 3500 + primitive_type: lane + - id: 3501 + primitive_type: lane + - id: 3502 + primitive_type: lane + uuid: + uuid: + - 195 + - 84 + - 241 + - 105 + - 133 + - 218 + - 136 + - 83 + - 104 + - 191 + - 48 + - 109 + - 85 + - 60 + - 177 + - 115 + allow_modification: false +operation_mode: + stamp: + sec: 550 + nanosec: 913829317 + mode: 2 + is_autoware_control_enabled: true + is_in_transition: false + is_stop_mode_available: true + is_autonomous_mode_available: true + is_local_mode_available: true + is_remote_mode_available: true +self_acceleration: + header: + stamp: + sec: 563 + nanosec: 188542779 + frame_id: /base_link + accel: + accel: + linear: + x: -0.770147 + y: 0.0409905 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 +self_odometry: + header: + stamp: + sec: 563 + nanosec: 188542779 + frame_id: map + child_frame_id: base_link + pose: + pose: + position: + x: 337.371 + y: 643.219 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00401753 + w: 0.999992 + covariance: + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + twist: + twist: + linear: + x: 4.13155 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00992133 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_shoulder.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_shoulder.yaml new file mode 100644 index 0000000000000..9a20cbee9c1ff --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_shoulder.yaml @@ -0,0 +1,2512 @@ +# +# AUTO GENERATED by autoware_test_utils::topic_snapshot_saver +# format1: +# +# format_version: +# map_path_uri: package:/// +# fields(this is array) +# - name: +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | PathWithLaneId | TBD} +# topic: +# +format_version: 1 +map_path_uri: package://autoware_test_utils/test_map/intersection/lanelet2_map.osm +path_with_lane_id: + header: + stamp: + sec: 74 + nanosec: 11949715 + frame_id: map + points: + - point: + pose: + position: + x: 360.306 + y: 597.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710800 + w: 0.703394 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.285 + y: 599.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710829 + w: 0.703365 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.264 + y: 601.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710857 + w: 0.703336 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.243 + y: 603.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710881 + w: 0.703312 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.222 + y: 605.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710951 + w: 0.703242 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.200 + y: 607.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711036 + w: 0.703156 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.177 + y: 609.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711601 + w: 0.702584 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.152 + y: 611.245 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711932 + w: 0.702248 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.133 + y: 612.628 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.716985 + w: 0.697089 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - 1200 + - point: + pose: + position: + x: 360.116 + y: 613.245 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.732064 + w: 0.681236 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.972 + y: 615.239 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.740537 + w: 0.672016 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.778 + y: 617.231 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.759018 + w: 0.651069 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.474 + y: 619.204 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.784627 + w: 0.619968 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.010 + y: 621.159 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.835622 + w: 0.549305 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 358.221 + y: 622.986 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.876871 + w: 0.480726 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 357.127 + y: 624.700 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.906015 + w: 0.423245 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 355.847 + y: 626.230 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.934815 + w: 0.355135 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 354.341 + y: 627.567 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.956562 + w: 0.291529 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 352.674 + y: 628.688 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.976933 + w: 0.213544 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 350.854 + y: 629.523 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.989029 + w: 0.147720 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 348.920 + y: 630.114 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.997418 + w: 0.0718077 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 346.951 + y: 630.399 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.999926 + w: 0.0121684 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 344.951 + y: 630.448 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 1.00000 + w: 0.000253960 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 342.951 + y: 630.449 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999959 + w: 0.00906744 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 342.741 + y: 630.445 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - 2500 + - point: + pose: + position: + x: 340.951 + y: 630.409 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999893 + w: 0.0146423 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 339.014 + y: 630.352 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999803 + w: 0.0198557 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 337.016 + y: 630.273 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999732 + w: 0.0231414 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 335.018 + y: 630.180 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999702 + w: 0.0243973 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 333.020 + y: 630.083 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999705 + w: 0.0242858 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 332.731 + y: 630.068 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999615 + w: 0.0277448 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 331.733 + y: 630.013 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999615 + w: 0.0277448 + longitudinal_velocity_mps: 0.00000 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + left_bound: + - x: 358.794 + y: 596.731 + z: 100.000 + - x: 358.788 + y: 597.342 + z: 100.000 + - x: 358.758 + y: 600.198 + z: 100.000 + - x: 358.736 + y: 602.392 + z: 100.000 + - x: 358.718 + y: 604.199 + z: 100.000 + - x: 358.698 + y: 606.199 + z: 100.000 + - x: 358.686 + y: 607.440 + z: 100.000 + - x: 358.610 + y: 610.191 + z: 100.000 + - x: 358.512 + y: 612.597 + z: 100.000 + - x: 358.405 + y: 614.096 + z: 100.000 + - x: 358.218 + y: 616.079 + z: 100.000 + - x: 358.086 + y: 617.465 + z: 100.000 + - x: 357.913 + y: 618.622 + z: 100.000 + - x: 357.666 + y: 619.746 + z: 100.000 + - x: 357.285 + y: 620.810 + z: 100.000 + - x: 356.832 + y: 621.812 + z: 100.000 + - x: 356.284 + y: 622.823 + z: 100.000 + - x: 355.663 + y: 623.752 + z: 100.000 + - x: 354.964 + y: 624.613 + z: 100.000 + - x: 354.158 + y: 625.437 + z: 100.000 + - x: 353.306 + y: 626.152 + z: 100.000 + - x: 352.395 + y: 626.788 + z: 100.000 + - x: 351.400 + y: 627.366 + z: 100.000 + - x: 350.389 + y: 627.820 + z: 100.000 + - x: 349.337 + y: 628.212 + z: 100.000 + - x: 348.219 + y: 628.479 + z: 100.000 + - x: 347.083 + y: 628.681 + z: 100.000 + - x: 346.042 + y: 628.729 + z: 100.000 + - x: 344.050 + y: 628.720 + z: 100.000 + - x: 342.764 + y: 628.777 + z: 100.000 + - x: 340.080 + y: 628.723 + z: 100.000 + - x: 338.078 + y: 628.751 + z: 100.000 + - x: 336.079 + y: 628.710 + z: 100.000 + - x: 334.080 + y: 628.670 + z: 100.000 + - x: 332.082 + y: 628.629 + z: 100.000 + - x: 326.872 + y: 628.524 + z: 100.000 + right_bound: + - x: 361.830 + y: 596.762 + z: 100.000 + - x: 361.823 + y: 597.400 + z: 100.000 + - x: 361.792 + y: 600.231 + z: 100.000 + - x: 361.767 + y: 602.446 + z: 100.000 + - x: 361.748 + y: 604.230 + z: 100.000 + - x: 361.726 + y: 606.230 + z: 100.000 + - x: 361.712 + y: 607.493 + z: 100.000 + - x: 361.718 + y: 610.230 + z: 100.000 + - x: 361.754 + y: 612.657 + z: 100.000 + - x: 361.627 + y: 614.331 + z: 100.000 + - x: 361.519 + y: 616.330 + z: 100.000 + - x: 361.452 + y: 617.634 + z: 100.000 + - x: 361.277 + y: 619.056 + z: 100.000 + - x: 360.998 + y: 620.486 + z: 100.000 + - x: 360.581 + y: 621.905 + z: 100.000 + - x: 359.991 + y: 623.293 + z: 100.000 + - x: 359.319 + y: 624.583 + z: 100.000 + - x: 358.490 + y: 625.825 + z: 100.000 + - x: 357.546 + y: 626.984 + z: 100.000 + - x: 356.525 + y: 628.025 + z: 100.000 + - x: 355.377 + y: 628.981 + z: 100.000 + - x: 354.158 + y: 629.845 + z: 100.000 + - x: 352.881 + y: 630.552 + z: 100.000 + - x: 351.515 + y: 631.167 + z: 100.000 + - x: 350.098 + y: 631.615 + z: 100.000 + - x: 348.672 + y: 631.919 + z: 100.000 + - x: 347.236 + y: 632.124 + z: 100.000 + - x: 346.083 + y: 632.098 + z: 100.000 + - x: 344.082 + y: 632.081 + z: 100.000 + - x: 342.719 + y: 632.113 + z: 100.000 + - x: 340.012 + y: 632.058 + z: 100.000 + - x: 338.014 + y: 631.950 + z: 100.000 + - x: 336.015 + y: 631.909 + z: 100.000 + - x: 334.016 + y: 631.869 + z: 100.000 + - x: 332.017 + y: 631.829 + z: 100.000 + - x: 326.807 + y: 631.724 + z: 100.000 +dynamic_object: + header: + stamp: + sec: 73 + nanosec: 980215670 + frame_id: map + objects: + - object_id: + uuid: + - 251 + - 180 + - 23 + - 85 + - 112 + - 118 + - 142 + - 88 + - 241 + - 29 + - 194 + - 27 + - 222 + - 164 + - 130 + - 251 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 357.408 + y: 667.312 + z: 100.774 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712257 + w: 0.701919 + covariance: + - 0.299126 + - -8.23923e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -8.23923e-05 + - 0.349251 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00879510 + initial_twist_with_covariance: + twist: + linear: + x: 9.98083 + y: 0.00185652 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00325706 + covariance: + - 0.970951 + - 0.000139981 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.000245582 + - 0.000139981 + - 0.00384142 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00673936 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.000245582 + - 0.00673936 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0118235 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 357.408 + y: 667.312 + z: 100.774 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712257 + w: 0.701919 + - position: + x: 357.373 + y: 672.303 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.709614 + w: 0.704590 + - position: + x: 357.520 + y: 677.298 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.696604 + w: 0.717456 + - position: + x: 357.917 + y: 682.299 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.678600 + w: 0.734508 + - position: + x: 358.531 + y: 687.305 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.662668 + w: 0.748913 + - position: + x: 359.266 + y: 692.315 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.653723 + w: 0.756734 + - position: + x: 359.995 + y: 697.324 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.654217 + w: 0.756307 + - position: + x: 360.590 + y: 702.329 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.664107 + w: 0.747638 + - position: + x: 360.955 + y: 707.330 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.680844 + w: 0.732428 + - position: + x: 361.063 + y: 712.324 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.699424 + w: 0.714707 + - position: + x: 360.983 + y: 717.314 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712725 + w: 0.701443 + - position: + x: 360.864 + y: 722.303 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.746 + y: 727.292 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.627 + y: 732.280 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.508 + y: 737.269 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.389 + y: 742.258 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.270 + y: 747.247 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.151 + y: 752.236 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.032 + y: 757.225 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.913 + y: 762.214 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.794 + y: 767.203 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.675 + y: 772.192 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.557 + y: 777.181 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.438 + y: 782.170 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.319 + y: 787.159 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.200 + y: 792.148 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.081 + y: 797.137 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.962 + y: 802.126 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.843 + y: 807.115 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.724 + y: 812.104 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.605 + y: 817.093 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.230769 + - path: + - position: + x: 357.408 + y: 667.312 + z: 100.774 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712257 + w: 0.701919 + - position: + x: 357.340 + y: 672.302 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711946 + w: 0.702234 + - position: + x: 357.296 + y: 677.293 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710176 + w: 0.704024 + - position: + x: 357.286 + y: 682.284 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707820 + w: 0.706393 + - position: + x: 357.304 + y: 687.276 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705868 + w: 0.708343 + - position: + x: 357.334 + y: 692.269 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.704976 + w: 0.709232 + - position: + x: 357.357 + y: 697.261 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705452 + w: 0.708758 + - position: + x: 357.355 + y: 702.252 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707256 + w: 0.706957 + - position: + x: 357.314 + y: 707.243 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.709999 + w: 0.704203 + - position: + x: 357.231 + y: 712.233 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712949 + w: 0.701216 + - position: + x: 357.119 + y: 717.222 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715047 + w: 0.699076 + - position: + x: 357.000 + y: 722.211 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.881 + y: 727.199 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.762 + y: 732.188 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.643 + y: 737.177 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.524 + y: 742.166 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.405 + y: 747.155 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.286 + y: 752.144 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.167 + y: 757.133 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.049 + y: 762.122 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.930 + y: 767.111 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.811 + y: 772.100 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.692 + y: 777.089 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.573 + y: 782.078 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.454 + y: 787.067 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.335 + y: 792.056 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.216 + y: 797.045 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.097 + y: 802.033 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 354.978 + y: 807.022 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 354.859 + y: 812.011 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 354.741 + y: 817.000 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.769231 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.89999 + y: 0.500442 + z: 0.612431 + - object_id: + uuid: + - 218 + - 141 + - 152 + - 171 + - 185 + - 127 + - 84 + - 175 + - 69 + - 40 + - 199 + - 89 + - 96 + - 107 + - 61 + - 243 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 358.218 + y: 603.622 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707443 + w: 0.706770 + covariance: + - 0.0636258 + - -0.000170219 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000170219 + - 0.0826995 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00394708 + initial_twist_with_covariance: + twist: + linear: + x: 10.0303 + y: -0.00247386 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: -0.00434011 + covariance: + - 0.579437 + - -9.81341e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000172165 + - -9.81341e-05 + - 0.00311875 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00547149 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - -0.000172165 + - 0.00547149 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00959910 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 358.218 + y: 603.622 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707443 + w: 0.706770 + - position: + x: 358.224 + y: 608.637 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.706633 + w: 0.707581 + - position: + x: 358.279 + y: 613.653 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.703235 + w: 0.710958 + - position: + x: 358.397 + y: 618.671 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.698737 + w: 0.715379 + - position: + x: 358.567 + y: 623.690 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.695068 + w: 0.718944 + - position: + x: 358.758 + y: 628.710 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.693532 + w: 0.720426 + - position: + x: 358.932 + y: 633.729 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.694735 + w: 0.719266 + - position: + x: 359.054 + y: 638.747 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.698453 + w: 0.715656 + - position: + x: 359.097 + y: 643.763 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.704045 + w: 0.710155 + - position: + x: 359.057 + y: 648.778 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.709938 + w: 0.704265 + - position: + x: 358.958 + y: 653.791 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714096 + w: 0.700048 + - position: + x: 358.849 + y: 658.805 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714730 + w: 0.699401 + - position: + x: 358.740 + y: 663.817 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714768 + w: 0.699361 + - position: + x: 358.620 + y: 668.830 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.501 + y: 673.843 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.381 + y: 678.856 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.262 + y: 683.870 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.142 + y: 688.883 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.023 + y: 693.896 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.903 + y: 698.909 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.784 + y: 703.923 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.664 + y: 708.936 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.545 + y: 713.950 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.425 + y: 718.963 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.306 + y: 723.977 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.186 + y: 728.991 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.067 + y: 734.004 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.947 + y: 739.018 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.828 + y: 744.032 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.708 + y: 749.045 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.589 + y: 754.059 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.500000 + - path: + - position: + x: 358.218 + y: 603.622 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707443 + w: 0.706770 + - position: + x: 358.206 + y: 608.635 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707930 + w: 0.706283 + - position: + x: 358.149 + y: 613.642 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711130 + w: 0.703060 + - position: + x: 358.010 + y: 618.635 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.716853 + w: 0.697224 + - position: + x: 357.024 + y: 623.117 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.779378 + w: 0.626554 + - position: + x: 354.441 + y: 627.006 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.881248 + w: 0.472653 + - position: + x: 350.340 + y: 629.665 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.958935 + w: 0.283625 + - position: + x: 345.469 + y: 630.694 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.994592 + w: 0.103864 + - position: + x: 340.508 + y: 630.814 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.999926 + w: 0.0121264 + - position: + x: 335.554 + y: 630.768 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999989 + w: 0.00460609 + - position: + x: 330.597 + y: 630.677 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999957 + w: 0.00925679 + - position: + x: 325.638 + y: 630.576 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 320.674 + y: 630.476 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 315.708 + y: 630.376 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 310.737 + y: 630.276 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 305.763 + y: 630.175 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 300.785 + y: 630.075 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 295.803 + y: 629.974 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 290.817 + y: 629.874 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 285.828 + y: 629.773 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 280.835 + y: 629.672 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 275.839 + y: 629.571 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 270.840 + y: 629.470 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 265.838 + y: 629.370 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 260.833 + y: 629.268 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 255.825 + y: 629.167 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 250.816 + y: 629.066 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 245.805 + y: 628.965 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 240.792 + y: 628.864 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 235.778 + y: 628.763 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 230.764 + y: 628.662 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.500000 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.90000 + y: 0.644288 + z: 0.991445 +traffic_signal: + stamp: + sec: 0 + nanosec: 0 + traffic_light_groups: [] +route: + header: + stamp: + sec: 19 + nanosec: 549630704 + frame_id: map + start_pose: + position: + x: 360.481 + y: 591.844 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.721675 + w: 0.692232 + goal_pose: + position: + x: 331.733 + y: 630.013 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.999615 + w: -0.0277448 + segments: + - preferred_primitive: + id: 1010 + primitive_type: "" + primitives: + - id: 1010 + primitive_type: lane + - preferred_primitive: + id: 1200 + primitive_type: "" + primitives: + - id: 1200 + primitive_type: lane + - preferred_primitive: + id: 2500 + primitive_type: "" + primitives: + - id: 2500 + primitive_type: lane + - id: 2501 + primitive_type: lane + uuid: + uuid: + - 83 + - 230 + - 40 + - 201 + - 182 + - 232 + - 31 + - 237 + - 204 + - 91 + - 39 + - 90 + - 153 + - 179 + - 6 + - 123 + allow_modification: false +operation_mode: + stamp: + sec: 65 + nanosec: 819714871 + mode: 2 + is_autoware_control_enabled: true + is_in_transition: false + is_stop_mode_available: true + is_autonomous_mode_available: true + is_local_mode_available: true + is_remote_mode_available: true +self_acceleration: + header: + stamp: + sec: 74 + nanosec: 42641063 + frame_id: /base_link + accel: + accel: + linear: + x: -1.01094 + y: 0.00400509 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 +self_odometry: + header: + stamp: + sec: 74 + nanosec: 42641063 + frame_id: map + child_frame_id: base_link + pose: + pose: + position: + x: 360.302 + y: 604.493 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707950 + w: 0.706263 + covariance: + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + twist: + twist: + linear: + x: 2.31221 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00173214 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt index 10b694ce22cbd..3a429008c1b8e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt @@ -15,6 +15,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_crosswalk.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) endif() 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 699f80ec8356e..1f0e9e68a65c6 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 @@ -21,8 +21,11 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner 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,7 +44,6 @@ pluginlib rclcpp sensor_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 d6f6ddfb7b6ad..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,7 +19,7 @@ #include #include -#include +#include #include #include @@ -48,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 9b771c0d503a4..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 @@ -112,7 +112,7 @@ double InterpolateMap( } } // namespace -class CrosswalkModule : public SceneModuleInterface +class CrosswalkModule : public SceneModuleInterfaceWithRTC { public: struct PlannerParam @@ -456,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_crosswalk_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..7a875327d4dde --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// 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 + +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); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "crosswalk", "autoware::behavior_velocity_planner::CrosswalkModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + 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_detection_area_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt index 3aff4a524ffdd..53eafaffbba6c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt @@ -14,6 +14,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_utils.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) endif() diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml index b49b43794685c..9294795274066 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml @@ -8,5 +8,4 @@ state_clear_time: 2.0 hold_stop_margin_distance: 0.0 distance_to_judge_over_stop_line: 0.5 - enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval suppress_pass_judge_when_stopping: false 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..4ae1d20991078 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 @@ -17,6 +17,7 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_lanelet2_extension autoware_motion_utils 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..fb2c17d9e3745 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 @@ -34,8 +34,7 @@ using autoware::universe_utils::getOrDeclareParameter; using lanelet::autoware::DetectionArea; DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) +: SceneModuleManagerInterface(node, getModuleName()) { const std::string ns(DetectionAreaModuleManager::getModuleName()); planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); @@ -66,10 +65,6 @@ void DetectionAreaModuleManager::launchNewModules( registerModule(std::make_shared( module_id, lane_id, *detection_area_with_lane_id.first, planner_param_, logger_.get_child("detection_area_module"), clock_)); - generateUUID(module_id); - updateRTCStatus( - getUUID(module_id), true, State::WAITING_FOR_EXECUTION, - std::numeric_limits::lowest(), path.header.stamp); } } } 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..4b34ac4a45298 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 @@ -29,7 +29,7 @@ namespace autoware::behavior_velocity_planner { -class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC +class DetectionAreaModuleManager : public SceneModuleManagerInterface<> { public: explicit DetectionAreaModuleManager(rclcpp::Node & node); 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..031c45753022e 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 @@ -105,12 +105,10 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) modified_stop_line_seg_idx = current_seg_idx; } - setDistance(stop_dist); - // Check state - setSafe(detection_area::can_clear_stop_state( - last_obstacle_found_time_, clock_->now(), planner_param_.state_clear_time)); - if (isActivated()) { + const bool is_safe = detection_area::can_clear_stop_state( + last_obstacle_found_time_, clock_->now(), planner_param_.state_clear_time); + if (is_safe) { last_obstacle_found_time_ = {}; if (!planner_param_.suppress_pass_judge_when_stopping || !is_stopped) { state_ = State::GO; @@ -139,7 +137,6 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) dead_line_seg_idx); if (dist_from_ego_to_dead_line < 0.0) { RCLCPP_WARN(logger_, "[detection_area] vehicle is over dead line"); - setSafe(true); return true; } } @@ -152,7 +149,6 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) if ( state_ != State::STOP && dist_from_ego_to_stop < -planner_param_.distance_to_judge_over_stop_line) { - setSafe(true); return true; } @@ -169,7 +165,6 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) RCLCPP_WARN_THROTTLE( logger_, *clock_, std::chrono::milliseconds(1000).count(), "[detection_area] vehicle is over stop border"); - setSafe(true); return true; } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..f84d22debea8e --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// 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 + +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); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "detection_area", "autoware::behavior_velocity_planner::DetectionAreaModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + 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_intersection_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml index b056f71614da3..4f7c3aeea7618 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 @@ -19,7 +19,10 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner 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 80c87e55c6696..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,6 +21,7 @@ #include #include #include +#include #include #include @@ -47,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; @@ -63,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..93a13a4d62737 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); @@ -353,9 +354,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); const bool over_stopline = (dist_stopline < 0.0); const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - state_machine.setState(StateMachine::State::GO); - } else if (is_stopped_duration && approached_dist_stopline) { + if (over_stopline || (is_stopped_duration && approached_dist_stopline)) { state_machine.setState(StateMachine::State::GO); } return state_machine.getState() == StateMachine::State::GO; @@ -366,12 +365,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin); const bool is_stopped = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - return true; - } else if (is_stopped && approached_dist_stopline) { - return true; - } - return false; + return over_stopline || (is_stopped && approached_dist_stopline); }; const auto occlusion_wo_tl_pass_judge_line_idx = 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_intersection_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..b515107e0ae8e --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// 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 + +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); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "intersection", "autoware::behavior_velocity_planner::IntersectionModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + 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_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/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt index b710924410549..e6b362271afb3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt @@ -14,6 +14,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_utils.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) endif() 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..28677bfb80b15 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 @@ -17,7 +17,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner 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_no_stopping_area_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..875e757cbfcec --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// 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 + +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); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "no_stopping_area", "autoware::behavior_velocity_planner::NoStoppingAreaModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + 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_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_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index 522e83390b0f1..c5d1cf61614b2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -36,7 +36,7 @@ // turn on only when debugging. #define DEBUG_PRINT(enable, n, x) \ if (enable) { \ - const std::string time_msg = n + std::to_string(x); \ + const std::string time_msg = (n) + std::to_string(x); \ RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, time_msg); \ } 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..a520b02a43c05 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt @@ -2,20 +2,13 @@ cmake_minimum_required(VERSION 3.14) project(autoware_behavior_velocity_planner) find_package(autoware_cmake REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -rosidl_generate_interfaces( - ${PROJECT_NAME} - "srv/LoadPlugin.srv" - "srv/UnloadPlugin.srv" - DEPENDENCIES -) 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 @@ -23,18 +16,9 @@ rclcpp_components_register_node(${PROJECT_NAME}_lib EXECUTABLE ${PROJECT_NAME}_node ) -if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(${PROJECT_NAME}_lib - ${PROJECT_NAME} "rosidl_typesupport_cpp") -else() - rosidl_get_typesupport_target( - cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") - target_link_libraries(${PROJECT_NAME}_lib "${cpp_typesupport_target}") -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 85% 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 4efb58e38b74f..472538406c382 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,19 +12,18 @@ // 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 -#include -#include #include +#include #include #include #include @@ -45,8 +44,6 @@ namespace autoware::behavior_velocity_planner { -using autoware_behavior_velocity_planner::srv::LoadPlugin; -using autoware_behavior_velocity_planner::srv::UnloadPlugin; using autoware_map_msgs::msg::LaneletMapBin; using tier4_planning_msgs::msg::VelocityLimit; @@ -84,10 +81,6 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node 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"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_occupancy_grid_{this, "~/input/occupancy_grid"}; @@ -125,13 +118,14 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node BehaviorVelocityPlannerManager planner_manager_; bool is_driving_forward_{true}; - rclcpp::Service::SharedPtr srv_load_plugin_; - rclcpp::Service::SharedPtr srv_unload_plugin_; + rclcpp::Service::SharedPtr srv_load_plugin_; + rclcpp::Service::SharedPtr srv_unload_plugin_; void onUnloadPlugin( - const UnloadPlugin::Request::SharedPtr request, - const UnloadPlugin::Response::SharedPtr response); + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response); void onLoadPlugin( - const LoadPlugin::Request::SharedPtr request, const LoadPlugin::Response::SharedPtr response); + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response); // mutex for planner_data_ std::mutex mutex_; @@ -150,4 +144,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 e051374ed3dda..c40d80c2bf998 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml @@ -32,9 +32,8 @@ autoware_cmake eigen3_cmake_module - rosidl_default_generators - autoware_behavior_velocity_planner_common + autoware_internal_debug_msgs autoware_lanelet2_extension autoware_map_msgs autoware_motion_utils @@ -57,31 +56,13 @@ tf2_geometry_msgs tf2_ros tier4_planning_msgs - tier4_v2x_msgs visualization_msgs - rosidl_default_runtime - 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 - rosidl_interface_packages - ament_cmake 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..443bfebe2a3eb 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 @@ -70,9 +70,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio this->create_subscription( "~/input/path_with_lane_id", 1, std::bind(&BehaviorVelocityPlannerNode::onTrigger, this, _1)); - srv_load_plugin_ = create_service( + srv_load_plugin_ = create_service( "~/service/load_plugin", std::bind(&BehaviorVelocityPlannerNode::onLoadPlugin, this, _1, _2)); - srv_unload_plugin_ = create_service( + srv_unload_plugin_ = create_service( "~/service/unload_plugin", std::bind(&BehaviorVelocityPlannerNode::onUnloadPlugin, this, _1, _2)); @@ -112,19 +112,19 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio } void BehaviorVelocityPlannerNode::onLoadPlugin( - const LoadPlugin::Request::SharedPtr request, - [[maybe_unused]] const LoadPlugin::Response::SharedPtr response) + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + [[maybe_unused]] const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response) { std::unique_lock lk(mutex_); - planner_manager_.launchScenePlugin(*this, request->plugin_name); + planner_manager_.launchScenePlugin(*this, request->data); } void BehaviorVelocityPlannerNode::onUnloadPlugin( - const UnloadPlugin::Request::SharedPtr request, - [[maybe_unused]] const UnloadPlugin::Response::SharedPtr response) + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + [[maybe_unused]] const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response) { std::unique_lock lk(mutex_); - planner_manager_.removeScenePlugin(*this, request->plugin_name); + planner_manager_.removeScenePlugin(*this, request->data); } void BehaviorVelocityPlannerNode::onParam() @@ -271,9 +271,6 @@ bool BehaviorVelocityPlannerNode::processData(rclcpp::Clock clock) planner_data_.route_handler_ = std::make_shared(*map_data); } - // optional data - getData(planner_data_.virtual_traffic_light_states, sub_virtual_traffic_light_states_); - // planner_data_.external_velocity_limit is std::optional type variable. const auto external_velocity_limit = sub_external_velocity_limit_.takeData(); if (external_velocity_limit) { 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/src/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp new file mode 100644 index 0000000000000..8b9f39e97d22b --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp @@ -0,0 +1,119 @@ +// 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_velocity_planner/test_utils.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_velocity_planner → test_node_ + test_manager->setPathSubscriber("behavior_velocity_planner_node/output/path"); + + // set behavior_velocity_planner node's input topic name(this topic is changed to test node) + test_manager->setPathWithLaneIdTopicName( + "behavior_velocity_planner_node/input/path_with_lane_id"); + + test_manager->setInitialPoseTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); + test_manager->setOdometryTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); + + return test_manager; +} + +std::shared_ptr generateNode( + const std::vector & plugin_info_vec) +{ + auto node_options = rclcpp::NodeOptions{}; + + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); + const auto behavior_velocity_planner_common_dir = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner_common"); + const auto behavior_velocity_planner_dir = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner"); + const auto velocity_smoother_dir = + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); + + const auto get_behavior_velocity_module_config = [](const std::string & module) { + const auto package_name = "autoware_behavior_velocity_" + module + "_module"; + const auto package_path = ament_index_cpp::get_package_share_directory(package_name); + return package_path + "/config/" + module + ".param.yaml"; + }; + + 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", plugin_names); + params.emplace_back("is_simulation", false); + node_options.parameter_overrides(params); + + 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 + // TODO(Kyoichi Sugahara) set to true launch_occlusion_spot + // TODO(Kyoichi Sugahara) set to true launch_run_out + // TODO(Kyoichi Sugahara) set to true launch_speed_bump + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishTF(test_target_node, "/tf"); + test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); + test_manager->publishPredictedObjects( + test_target_node, "behavior_velocity_planner_node/input/dynamic_objects"); + test_manager->publishPointCloud( + test_target_node, "behavior_velocity_planner_node/input/no_ground_pointcloud"); + test_manager->publishOdometry( + test_target_node, "behavior_velocity_planner_node/input/vehicle_odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); + test_manager->publishMap(test_target_node, "behavior_velocity_planner_node/input/vector_map"); + test_manager->publishTrafficSignals( + test_target_node, "behavior_velocity_planner_node/input/traffic_signals"); + test_manager->publishMaxVelocity( + test_target_node, "behavior_velocity_planner_node/input/external_velocity_limit_mps"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_velocity_planner_node/input/occupancy_grid"); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/LoadPlugin.srv b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/LoadPlugin.srv deleted file mode 100644 index 7b9f08ab0ded8..0000000000000 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/LoadPlugin.srv +++ /dev/null @@ -1,3 +0,0 @@ -string plugin_name ---- -bool success diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv deleted file mode 100644 index 7b9f08ab0ded8..0000000000000 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv +++ /dev/null @@ -1,3 +0,0 @@ -string plugin_name ---- -bool success 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/test/src/test_node_interface.cpp deleted file mode 100644 index fe79450d0def8..0000000000000 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ /dev/null @@ -1,179 +0,0 @@ -// 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 "node.hpp" - -#include -#include -#include - -#include - -#include -#include -#include -#include - -using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; -using autoware::planning_test_manager::PlanningInterfaceTestManager; - -std::shared_ptr generateTestManager() -{ - auto test_manager = std::make_shared(); - - // set subscriber with topic name: behavior_velocity_planner → test_node_ - test_manager->setPathSubscriber("behavior_velocity_planner_node/output/path"); - - // set behavior_velocity_planner node's input topic name(this topic is changed to test node) - test_manager->setPathWithLaneIdTopicName( - "behavior_velocity_planner_node/input/path_with_lane_id"); - - test_manager->setInitialPoseTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); - test_manager->setOdometryTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); - - return test_manager; -} - -std::shared_ptr generateNode() -{ - auto node_options = rclcpp::NodeOptions{}; - - const auto autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_test_utils"); - const auto behavior_velocity_planner_common_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner_common"); - const auto behavior_velocity_planner_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner"); - const auto velocity_smoother_dir = - ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); - - const auto get_behavior_velocity_module_config = [](const std::string & module) { - const auto package_name = "autoware_behavior_velocity_" + module + "_module"; - const auto package_path = ament_index_cpp::get_package_share_directory(package_name); - 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 params; - params.emplace_back("launch_modules", module_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")}); - - // TODO(Takagi, Isamu): set launch_modules - // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light - // TODO(Kyoichi Sugahara) set to true launch_occlusion_spot - // TODO(Kyoichi Sugahara) set to true launch_run_out - // TODO(Kyoichi Sugahara) set to true launch_speed_bump - - return std::make_shared(node_options); -} - -void publishMandatoryTopics( - std::shared_ptr test_manager, - std::shared_ptr test_target_node) -{ - // publish necessary topics from test_manager - test_manager->publishTF(test_target_node, "/tf"); - test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); - test_manager->publishPredictedObjects( - test_target_node, "behavior_velocity_planner_node/input/dynamic_objects"); - test_manager->publishPointCloud( - test_target_node, "behavior_velocity_planner_node/input/no_ground_pointcloud"); - test_manager->publishOdometry( - test_target_node, "behavior_velocity_planner_node/input/vehicle_odometry"); - test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); - test_manager->publishMap(test_target_node, "behavior_velocity_planner_node/input/vector_map"); - test_manager->publishTrafficSignals( - test_target_node, "behavior_velocity_planner_node/input/traffic_signals"); - test_manager->publishMaxVelocity( - test_target_node, "behavior_velocity_planner_node/input/external_velocity_limit_mps"); - test_manager->publishVirtualTrafficLightState( - test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states"); - 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(); -} 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 983aaec2acf4f..f8b37999cb6bf 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 @@ -30,7 +30,6 @@ #include #include #include -#include #include #include @@ -64,7 +63,6 @@ struct PlannerData 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::ConstSharedPtr virtual_traffic_light_states; bool is_simulation = false; 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..d8d640a078267 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,22 +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 #include @@ -54,14 +54,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 @@ -96,17 +94,6 @@ class SceneModuleInterface planner_data_ = planner_data; } - std::optional getInfrastructureCommand() - { - return infrastructure_command_; - } - - void setInfrastructureCommand( - const std::optional & command) - { - infrastructure_command_ = command; - } - void setTimeKeeper(const std::shared_ptr & time_keeper) { time_keeper_ = time_keeper; @@ -114,12 +101,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,28 +108,13 @@ 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_; - std::optional infrastructure_command_; autoware::motion_utils::VelocityFactorInterface velocity_factor_; 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 +126,36 @@ 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); + + 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 +163,103 @@ 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(); + + 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); + } + + 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_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_; @@ -211,8 +275,6 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_debug_path_; rclcpp::Publisher::SharedPtr pub_velocity_factor_; - rclcpp::Publisher::SharedPtr - pub_infrastructure_commands_; std::shared_ptr processing_time_publisher_; @@ -220,66 +282,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 6d94e20cfdb1b..9f920dff8f166 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 @@ -45,7 +45,6 @@ tf2_geometry_msgs tf2_ros tier4_planning_msgs - tier4_v2x_msgs visualization_msgs ament_cmake_ros 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/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CMakeLists.txt index da2f4ce33ff60..a2f35ada11b5e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CMakeLists.txt @@ -17,10 +17,11 @@ ament_auto_add_library(${PROJECT_NAME} SHARED if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - tests/test_dynamic_obstacle.cpp - tests/test_path_utils.cpp - tests/test_utils.cpp - tests/test_state_machine.cpp + test/test_dynamic_obstacle.cpp + test/test_path_utils.cpp + test/test_utils.cpp + test/test_state_machine.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} autoware_behavior_velocity_run_out_module 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..18db8281356f8 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 @@ -20,7 +20,9 @@ eigen3_cmake_module autoware_behavior_velocity_crosswalk_module + autoware_behavior_velocity_planner 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_run_out_module/tests/test_dynamic_obstacle.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_dynamic_obstacle.cpp similarity index 100% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_dynamic_obstacle.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_dynamic_obstacle.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..75bf59751ed44 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// 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 + +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); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "run_out", "autoware::behavior_velocity_planner::RunOutModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + 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_run_out_module/tests/test_path_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_path_utils.cpp similarity index 100% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_path_utils.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_path_utils.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_state_machine.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_state_machine.cpp similarity index 100% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_state_machine.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_state_machine.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_utils.cpp similarity index 100% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_utils.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_utils.cpp 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/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt index f4528f0d13cf4..a187b4cda9459 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt @@ -14,6 +14,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_scene.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} gtest_main diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md index f373afc2351bf..efe4a9a353ecc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md @@ -1,33 +1,33 @@ -## Stop Line +# Stop Line -### Role +## Role -This module plans velocity so that the vehicle can stop right before stop lines and restart driving after stopped. +This module plans the vehicle's velocity to ensure it stops just before stop lines and can resume movement after stopping. ![stop line](docs/stop_line.svg) -### Activation Timing +## Activation Timing This module is activated when there is a stop line in a target lane. -### Module Parameters +## Module Parameters | Parameter | Type | Description | | -------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `stop_margin` | double | a margin that the vehicle tries to stop before stop_line | -| `stop_duration_sec` | double | [s] time parameter for the ego vehicle to stop in front of a stop line | -| `hold_stop_margin_distance` | double | [m] parameter for restart prevention (See Algorithm section). Also, when the ego vehicle is within this distance from a stop line, the ego state becomes STOPPED from APPROACHING | -| `use_initialization_stop_state` | bool | A flag to determine whether to return to the approaching state when the vehicle moves away from a stop line. | -| `show_stop_line_collision_check` | bool | A flag to determine whether to show the debug information of collision check with a stop line | +| `stop_margin` | double | Margin that the vehicle tries to stop in before stop_line | +| `stop_duration_sec` | double | [s] Time parameter for the ego vehicle to stop before stop line | +| `hold_stop_margin_distance` | double | [m] Parameter for restart prevention (See Algorithm section). Also, when the ego vehicle is within this distance from a stop line, the ego state becomes STOPPED from APPROACHING | +| `use_initialization_stop_state` | bool | Flag to determine whether to return to the approaching state when the vehicle moves away from a stop line. | +| `show_stop_line_collision_check` | bool | Flag to determine whether to show the debug information of collision check with a stop line | -### Inner-workings / Algorithms +## Inner-workings / Algorithms - Gets a stop line from map information. -- insert a stop point on the path from the stop line defined in the map and the ego vehicle length. +- Inserts a stop point on the path from the stop line defined in the map and the ego vehicle length. - Sets velocities of the path after the stop point to 0[m/s]. -- Release the inserted stop velocity when the vehicle stops at the stop point for `stop_duration_sec` seconds. +- Releases the inserted stop velocity when the vehicle halts at the stop point for `stop_duration_sec` seconds. -#### Flowchart +### Flowchart ```plantuml @startuml @@ -85,15 +85,15 @@ Then, we can get `offset segment` and `offset from segment start`. ![find_offset_segment](docs/./find_offset_segment.drawio.svg) -After that, we can calculate a offset point from `offset segment` and `offset`. This will be `stop_pose`. +After that, we can calculate an offset point from `offset segment` and `offset`. This will be `stop_pose`. ![calculate_stop_pose](docs/./calculate_stop_pose.drawio.svg) -#### Restart prevention +### Restart Prevention -If it needs X meters (e.g. 0.5 meters) to stop once the vehicle starts moving due to the poor vehicle control performance, the vehicle goes over the stopping position that should be strictly observed when the vehicle starts to moving in order to approach the near stop point (e.g. 0.3 meters away). +If the vehicle requires X meters (e.g. 0.5 meters) to stop once it starts moving due to poor vehicle control performance, it may overshoot the stopping position, which must be strictly observed. This happens when the vehicle begins to move in order to approach a nearby stop point (e.g. 0.3 meters away). -This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle is stopped within `hold_stop_margin_distance` meters from stop point of the module (\_front_to_stop_line < hold_stop_margin_distance), the module judges that the vehicle has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is stopped due to other factors. +This module includes the parameter `hold_stop_margin_distance` to prevent redundant restarts in such a situation. If the vehicle is stopped within `hold_stop_margin_distance` meters of the stop point (\_front_to_stop_line < hold_stop_margin_distance), the module determines that the vehicle has already stopped at the stop point and will maintain the current stopping position, even if the vehicle has come to a stop due to other factors.
![example](docs/restart_prevention.svg){width=1000} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg index 182c941907f57..6cd47ffc14a56 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg @@ -233,7 +233,7 @@
= node(i) + node(i+1)
- = all segment has two points + = all segments have two points diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml index fd4b9f83ae998..849ab3915c649 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml @@ -20,6 +20,7 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_lanelet2_extension autoware_motion_utils 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_stop_line_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..c6d6ff638cfbb --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// 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 + +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); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "stop_line", "autoware::behavior_velocity_planner::StopLineModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + 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_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/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CMakeLists.txt index 6370dd5e6c21d..02ed192459970 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CMakeLists.txt @@ -15,6 +15,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_utils.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} 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..4bc7a15cddc48 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 @@ -19,7 +19,9 @@ eigen3_cmake_module autoware_adapi_v1_msgs + autoware_behavior_velocity_planner 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_traffic_light_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..e24c2dab5dab9 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// 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 + +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); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "traffic_light", "autoware::behavior_velocity_planner::TrafficLightModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + 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_virtual_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml index 6d3bc68242d7c..056f84a970c58 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp index a51cf9211c21f..3815c83d3b6ab 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -21,6 +21,8 @@ #include #include +#include + #include #include @@ -53,6 +55,14 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node p.check_timeout_after_stop_line = getOrDeclareParameter(node, ns + ".check_timeout_after_stop_line"); } + + sub_virtual_traffic_light_states_ = autoware::universe_utils::InterProcessPollingSubscriber< + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>:: + create_subscription(&node, "~/input/virtual_traffic_light_states"); + + pub_infrastructure_commands_ = + node.create_publisher( + "~/output/infrastructure_commands", 1); } void VirtualTrafficLightModuleManager::launchNewModules( @@ -89,17 +99,43 @@ void VirtualTrafficLightModuleManager::launchNewModules( } } -std::function &)> +std::function &)> VirtualTrafficLightModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [id_set](const std::shared_ptr & scene_module) { + return [id_set](const std::shared_ptr & scene_module) { return id_set.count(scene_module->getModuleId()) == 0; }; } + +void VirtualTrafficLightModuleManager::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path) +{ + // NOTE: virtual traffic light specific implementation + // Since the argument of modifyPathVelocity cannot be changed, the specific information + // of virtual traffic light states is set here. + const auto virtual_traffic_light_states = sub_virtual_traffic_light_states_->takeData(); + for (const auto & scene_module : scene_modules_) { + scene_module->setVirtualTrafficLightStates(virtual_traffic_light_states); + } + + SceneModuleManagerInterface::modifyPathVelocity(path); + + // NOTE: virtual traffic light specific implementation + // publish infrastructure_command_array + tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; + infrastructure_command_array.stamp = clock_->now(); + + for (const auto & scene_module : scene_modules_) { + if (const auto command = scene_module->getInfrastructureCommand()) { + infrastructure_command_array.commands.push_back(*command); + } + } + pub_infrastructure_commands_->publish(infrastructure_command_array); +} } // namespace autoware::behavior_velocity_planner #include 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..aecef0851d8ab 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 @@ -20,16 +20,20 @@ #include #include #include +#include #include #include +#include +#include #include #include namespace autoware::behavior_velocity_planner { -class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface +class VirtualTrafficLightModuleManager +: public SceneModuleManagerInterface { public: explicit VirtualTrafficLightModuleManager(rclcpp::Node & node); @@ -38,10 +42,20 @@ class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface private: VirtualTrafficLightModule::PlannerParam planner_param_; + + void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( + std::function &)> getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) override; + + autoware::universe_utils::InterProcessPollingSubscriber< + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>::SharedPtr + sub_virtual_traffic_light_states_; + + rclcpp::Publisher::SharedPtr + pub_infrastructure_commands_; }; class VirtualTrafficLightModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 206fb14b6d41c..86239816ed6f2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -333,12 +333,12 @@ bool VirtualTrafficLightModule::isNearAnyEndLine(const size_t end_line_idx) std::optional VirtualTrafficLightModule::findCorrespondingState() { - // No message - if (!planner_data_->virtual_traffic_light_states) { + // Note: This variable is set by virtual traffic light's manager. + if (!virtual_traffic_light_states_) { return {}; } - for (const auto & state : planner_data_->virtual_traffic_light_states->states) { + for (const auto & state : virtual_traffic_light_states_->states) { if (state.id == map_data_.instrument_id) { return state; } @@ -457,4 +457,23 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( module_data_.stop_head_pose_at_end_line = calcHeadPose(stop_pose, planner_data_->vehicle_info_.max_longitudinal_offset_m); } + +std::optional +VirtualTrafficLightModule::getInfrastructureCommand() const +{ + return infrastructure_command_; +} + +void VirtualTrafficLightModule::setInfrastructureCommand( + const std::optional & command) +{ + infrastructure_command_ = command; +} + +void VirtualTrafficLightModule::setVirtualTrafficLightStates( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr + virtual_traffic_light_states) +{ + virtual_traffic_light_states_ = virtual_traffic_light_states; +} } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index b031ba5b2bb2b..7f37e7078a431 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -24,6 +25,9 @@ #include #include +#include +#include + #include #include @@ -84,13 +88,23 @@ class VirtualTrafficLightModule : public SceneModuleInterface visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; + std::optional getInfrastructureCommand() const; + void setInfrastructureCommand( + const std::optional & command); + + void setVirtualTrafficLightStates( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr + virtual_traffic_light_states); + private: const int64_t lane_id_; const lanelet::autoware::VirtualTrafficLight & reg_elem_; const lanelet::ConstLanelet lane_; const PlannerParam planner_param_; + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states_; State state_{State::NONE}; tier4_v2x_msgs::msg::InfrastructureCommand command_; + std::optional infrastructure_command_; MapData map_data_; ModuleData module_data_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..ab2fd5847c5be --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp @@ -0,0 +1,74 @@ +// 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 + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "virtual_traffic_light", + "autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + test_manager->publishVirtualTrafficLightState( + test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states"); + + // 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); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "virtual_traffic_light", + "autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + test_manager->publishVirtualTrafficLightState( + test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states"); + + // 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_walkway_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/CMakeLists.txt index 11504d9c8999c..8b11fdce7283e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/CMakeLists.txt @@ -11,4 +11,14 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene_walkway.cpp ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + file(GLOB_RECURSE TEST_SOURCES test/*.cpp) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + ${TEST_SOURCES} + ) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) +endif() + ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml index 5f1aea22855a4..857d9b524beb0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_behavior_velocity_crosswalk_module + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_lanelet2_extension autoware_motion_utils @@ -31,6 +32,7 @@ rclcpp visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common 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/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..e0b7fa5c31965 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// 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 + +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); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "walkway", "autoware::behavior_velocity_planner::WalkwayModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + 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/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..7fd5834ba2cfa 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_; @@ -130,7 +131,7 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( ? 0.0 : params_.hysteresis; const auto dynamic_obstacles = dynamic_obstacle_stop::filter_predicted_objects( - planner_data->predicted_objects, ego_data, params_, hysteresis); + planner_data->objects, ego_data, params_, hysteresis); const auto preprocessing_duration_us = stopwatch.toc("preprocessing"); @@ -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_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp index 2ab3c09e64edd..0726fdba02de0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -96,20 +96,22 @@ bool is_unavoidable( }; std::vector filter_predicted_objects( - const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const std::vector & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis) { std::vector filtered_objects; - for (const auto & object : objects.objects) { - const auto is_not_too_slow = object.kinematics.initial_twist_with_covariance.twist.linear.x >= - params.minimum_object_velocity; + for (const auto & object : objects) { + const auto & predicted_object = object.predicted_object; + const auto is_not_too_slow = + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x >= + params.minimum_object_velocity; if ( - is_vehicle(object) && is_not_too_slow && - is_in_range(object, ego_data.trajectory, params, hysteresis) && - is_not_too_close(object, ego_data, params.ego_longitudinal_offset) && + is_vehicle(predicted_object) && is_not_too_slow && + is_in_range(predicted_object, ego_data.trajectory, params, hysteresis) && + is_not_too_close(predicted_object, ego_data, params.ego_longitudinal_offset) && (!params.ignore_unavoidable_collisions || - !is_unavoidable(object, ego_data.pose, ego_data.earliest_stop_pose, params))) - filtered_objects.push_back(object); + !is_unavoidable(predicted_object, ego_data.pose, ego_data.earliest_stop_pose, params))) + filtered_objects.push_back(predicted_object); } return filtered_objects; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp index d20d6354066c6..6024cde019489 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp @@ -17,6 +17,8 @@ #include "types.hpp" +#include + #include #include @@ -74,7 +76,7 @@ bool is_unavoidable( /// @param hysteresis [m] extra distance threshold used for filtering /// @return filtered predicted objects std::vector filter_predicted_objects( - const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const std::vector & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis); } // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp index 3fa179903c23f..5993f81c7a8f1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp @@ -14,6 +14,7 @@ #include "../src/object_filtering.hpp" +#include #include #include @@ -26,6 +27,8 @@ #include #include +#include + TEST(TestObjectFiltering, isVehicle) { using autoware::motion_velocity_planner::dynamic_obstacle_stop::is_vehicle; @@ -202,8 +205,7 @@ TEST(TestObjectFiltering, isUnavoidable) TEST(TestObjectFiltering, filterPredictedObjects) { using autoware::motion_velocity_planner::dynamic_obstacle_stop::filter_predicted_objects; - autoware_perception_msgs::msg::PredictedObjects objects; - autoware_perception_msgs::msg::PredictedObject object; + std::vector objects; autoware::motion_velocity_planner::dynamic_obstacle_stop::EgoData ego_data; autoware::motion_velocity_planner::dynamic_obstacle_stop::PlannerParam params; double hysteresis{}; 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..c8aa9895a0451 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,51 @@ 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) + 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) + 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.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp index e501b756af6a1..9be7f52bca99a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp @@ -29,7 +29,7 @@ namespace autoware::motion_velocity_planner::obstacle_velocity_limiter { multi_polygon_t createPolygonMasks( - const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const double buffer, + const std::vector & dynamic_obstacles, const double buffer, const double min_vel) { return createObjectPolygons(dynamic_obstacles, buffer, min_vel); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp index d7d2de63b33f8..b0cdb1f802e0f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp @@ -56,7 +56,7 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b /// @param[in] min_vel minimum velocity for an object to be masked /// @return polygon masks around dynamic objects multi_polygon_t createPolygonMasks( - const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const double buffer, + const std::vector & dynamic_obstacles, const double buffer, const double min_vel); /// @brief create footprint polygons from projection lines 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..1923f023552e3 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); @@ -169,7 +170,7 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( const auto preprocessing_us = stopwatch.toc("preprocessing"); stopwatch.tic("obstacles"); obstacle_masks.negative_masks = obstacle_velocity_limiter::createPolygonMasks( - planner_data->predicted_objects, obstacle_params_.dynamic_obstacles_buffer, + planner_data->objects, obstacle_params_.dynamic_obstacles_buffer, obstacle_params_.dynamic_obstacles_min_vel); if (obstacle_params_.ignore_on_path) obstacle_masks.negative_masks.push_back(obstacle_velocity_limiter::createTrajectoryFootprint( @@ -188,8 +189,8 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( obstacle_masks.positive_mask = obstacle_velocity_limiter::createEnvelopePolygon(footprint_polygons); obstacle_velocity_limiter::addSensorObstacles( - obstacles, planner_data->occupancy_grid, planner_data->no_ground_pointcloud, obstacle_masks, - obstacle_params_); + obstacles, planner_data->occupancy_grid, planner_data->no_ground_pointcloud.pointcloud, + obstacle_masks, obstacle_params_); } const auto obstacles_us = stopwatch.toc("obstacles"); autoware::motion_utils::VirtualWalls virtual_walls; @@ -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_obstacle_velocity_limiter_module/src/obstacles.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp index 47215ee5cb0c4..187f028dc5969 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp @@ -25,6 +25,7 @@ #include #include +#include namespace autoware::motion_velocity_planner::obstacle_velocity_limiter { @@ -58,17 +59,18 @@ polygon_t createObjectPolygon( } multi_polygon_t createObjectPolygons( - const autoware_perception_msgs::msg::PredictedObjects & objects, const double buffer, - const double min_velocity) + const std::vector & objects, const double buffer, const double min_velocity) { multi_polygon_t polygons; - for (const auto & object : objects.objects) { + for (const auto & object : objects) { + const auto & predicted_object = object.predicted_object; const double obj_vel_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); if (min_velocity <= obj_vel_norm) { polygons.push_back(createObjectPolygon( - object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions, buffer)); + predicted_object.kinematics.initial_pose_with_covariance.pose, + predicted_object.shape.dimensions, buffer)); } } return polygons; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp index 7e4704ba6818c..a98d92994f497 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp @@ -18,6 +18,7 @@ #include "parameters.hpp" #include "types.hpp" +#include #include #include @@ -163,8 +164,7 @@ polygon_t createObjectPolygon( /// @param [in] min_velocity objects with velocity lower will be ignored /// @return polygons of the objects multi_polygon_t createObjectPolygons( - const autoware_perception_msgs::msg::PredictedObjects & objects, const double buffer, - const double min_velocity); + const std::vector & objects, const double buffer, const double min_velocity); /// @brief add obstacles obtained from sensors to the given Obstacles object /// @param[out] obstacles Obstacles object in which to add the sensor obstacles diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp index ac07c62f62cf7..023ae83774917 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp @@ -15,6 +15,7 @@ #include "../src/distance.hpp" #include "../src/obstacles.hpp" #include "../src/types.hpp" +#include "autoware/motion_velocity_planner_common/planner_data.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -27,6 +28,7 @@ #include #include +#include const auto point_in_polygon = [](const auto x, const auto y, const auto & polygon) { return std::find_if(polygon.outer().begin(), polygon.outer().end(), [=](const auto & pt) { @@ -387,11 +389,12 @@ TEST(TestCollisionDistance, arcDistance) TEST(TestCollisionDistance, createObjPolygons) { + using autoware::motion_velocity_planner::PlannerData; using autoware::motion_velocity_planner::obstacle_velocity_limiter::createObjectPolygons; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; - PredictedObjects objects; + std::vector objects; auto polygons = createObjectPolygons(objects, 0.0, 0.0); EXPECT_TRUE(polygons.empty()); @@ -404,7 +407,7 @@ TEST(TestCollisionDistance, createObjPolygons) object1.kinematics.initial_twist_with_covariance.twist.linear.x = 0.0; object1.shape.dimensions.x = 1.0; object1.shape.dimensions.y = 1.0; - objects.objects.push_back(object1); + objects.push_back(PlannerData::Object(object1)); polygons = createObjectPolygons(objects, 0.0, 1.0); EXPECT_TRUE(polygons.empty()); @@ -431,7 +434,7 @@ TEST(TestCollisionDistance, createObjPolygons) object2.kinematics.initial_twist_with_covariance.twist.linear.x = 2.0; object2.shape.dimensions.x = 2.0; object2.shape.dimensions.y = 1.0; - objects.objects.push_back(object2); + objects.push_back(PlannerData::Object(object2)); polygons = createObjectPolygons(objects, 0.0, 2.0); ASSERT_EQ(polygons.size(), 1ul); 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/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index f9ba7f4af9877..a6c8368c20571 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -106,23 +106,26 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params) { autoware_perception_msgs::msg::PredictedObjects filtered_objects; - filtered_objects.header = planner_data.predicted_objects.header; - for (const auto & object : planner_data.predicted_objects.objects) { + filtered_objects.header = planner_data.predicted_objects_header; + for (const auto & object : planner_data.objects) { + const auto & predicted_object = object.predicted_object; const auto is_pedestrian = - std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { - return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; - }) != object.classification.end(); + std::find_if( + predicted_object.classification.begin(), predicted_object.classification.end(), + [](const auto & c) { + return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + }) != predicted_object.classification.end(); if (is_pedestrian) continue; const auto is_coming_from_behind = motion_utils::calcSignedArcLength( ego_data.trajectory_points, 0UL, - object.kinematics.initial_pose_with_covariance.pose.position) < 0.0; + predicted_object.kinematics.initial_pose_with_covariance.pose.position) < 0.0; if (params.objects_ignore_behind_ego && is_coming_from_behind) { continue; } - auto filtered_object = object; + auto filtered_object = predicted_object; const auto is_invalid_predicted_path = [&](const auto & predicted_path) { const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); @@ -130,10 +133,10 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const auto lat_offset_to_current_ego = std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); const auto is_crossing_ego = - lat_offset_to_current_ego <= - object.shape.dimensions.y / 2.0 + std::max( - params.left_offset + params.extra_left_offset, - params.right_offset + params.extra_right_offset); + lat_offset_to_current_ego <= predicted_object.shape.dimensions.y / 2.0 + + std::max( + params.left_offset + params.extra_left_offset, + params.right_offset + params.extra_right_offset); return is_low_confidence || is_crossing_ego; }; auto & predicted_paths = filtered_object.kinematics.predicted_paths; 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 a5f9badd9f4f7..07c8e1580e4f0 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,7 +31,6 @@ #include #include #include -#include #include #include @@ -40,6 +39,7 @@ #include #include #include +#include namespace autoware::motion_velocity_planner { @@ -55,11 +55,72 @@ struct PlannerData { } + struct Object + { + public: + Object() = default; + explicit Object(const autoware_perception_msgs::msg::PredictedObject & arg_predicted_object) + : predicted_object(arg_predicted_object) + { + } + + autoware_perception_msgs::msg::PredictedObject predicted_object; + // double get_lon_vel_relative_to_traj() + // { + // if (!lon_vel_relative_to_traj) { + // lon_vel_relative_to_traj = 0.0; + // } + // return *lon_vel_relative_to_traj; + // } + // double get_lat_vel_relative_to_traj() + // { + // if (!lat_vel_relative_to_traj) { + // lat_vel_relative_to_traj = 0.0; + // } + // return *lat_vel_relative_to_traj; + // } + + private: + // TODO(murooka) implement the following variables and their functions. + // std::optional dist_to_traj_poly{std::nullopt}; + // std::optional dist_to_traj_lateral{std::nullopt}; + // std::optional dist_from_ego_longitudinal{std::nullopt}; + // std::optional lon_vel_relative_to_traj{std::nullopt}; + // std::optional lat_vel_relative_to_traj{std::nullopt}; + }; + + struct Pointcloud + { + public: + Pointcloud() = default; + explicit Pointcloud(const pcl::PointCloud & arg_pointcloud) + : pointcloud(arg_pointcloud) + { + } + + pcl::PointCloud pointcloud; + + private: + // NOTE: clustered result will be added. + }; + + void process_predicted_objects( + const autoware_perception_msgs::msg::PredictedObjects & predicted_objects) + { + predicted_objects_header = predicted_objects.header; + + objects.clear(); + for (const auto & predicted_object : predicted_objects.objects) { + objects.push_back(Object(predicted_object)); + } + } + // msgs from callbacks that are used for data-ready nav_msgs::msg::Odometry current_odometry; geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration; - autoware_perception_msgs::msg::PredictedObjects predicted_objects; - pcl::PointCloud no_ground_pointcloud; + std_msgs::msg::Header predicted_objects_header; + std::vector objects; + Pointcloud no_ground_pointcloud; nav_msgs::msg::OccupancyGrid occupancy_grid; std::shared_ptr route_handler; @@ -72,7 +133,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; // 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/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 586d27f2c9614..a78ab1489e080 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); @@ -160,13 +161,14 @@ bool MotionVelocityPlannerNode::update_planner_data( const auto predicted_objects_ptr = sub_predicted_objects_.takeData(); if (check_with_log(predicted_objects_ptr, "Waiting for predicted objects")) - planner_data_.predicted_objects = *predicted_objects_ptr; + planner_data_.process_predicted_objects(*predicted_objects_ptr); processing_times["update_planner_data.pred_obj"] = sw.toc(true); const auto no_ground_pointcloud_ptr = sub_no_ground_pointcloud_.takeData(); if (check_with_log(no_ground_pointcloud_ptr, "Waiting for pointcloud")) { const auto no_ground_pointcloud = process_no_ground_pointcloud(no_ground_pointcloud_ptr); - if (no_ground_pointcloud) planner_data_.no_ground_pointcloud = *no_ground_pointcloud; + if (no_ground_pointcloud) + planner_data_.no_ground_pointcloud = PlannerData::Pointcloud(*no_ground_pointcloud); } processing_times["update_planner_data.pcd"] = sw.toc(true); @@ -299,7 +301,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); @@ -356,7 +358,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 = @@ -381,10 +382,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 6e33b89e026c3..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 @@ -97,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/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/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/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py b/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py index a4f8dee7af1a0..9b978b66feefb 100644 --- a/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py +++ b/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py @@ -147,7 +147,7 @@ def __init__(self): elif sensor["type"] == "sensor.lidar.ray_cast": if sensor["id"] in self.sensor_frequencies: self.pub_lidar[sensor["id"]] = self.ros2_node.create_publisher( - PointCloud2, f'/sensing/lidar/{sensor["id"]}/pointcloud', 10 + PointCloud2, f'/sensing/lidar/{sensor["id"]}/pointcloud_before_sync', 10 ) else: self.ros2_node.get_logger().info( diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index d61654c32af02..8c95e144b3f8d 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -585,9 +585,7 @@ void SimplePlanningSimulator::set_input(const InputCommand & cmd, const double a std::visit( [this, acc_by_slope](auto && arg) { using T = std::decay_t; - if constexpr (std::is_same_v) { - set_input(arg, acc_by_slope); - } else if constexpr (std::is_same_v) { + if constexpr (std::is_same_v || std::is_same_v) { set_input(arg, acc_by_slope); } else { throw std::invalid_argument("Invalid input command type"); @@ -640,7 +638,7 @@ void SimplePlanningSimulator::set_input(const Control & cmd, const double acc_by input << vel, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC || - vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { // NOLINT input << combined_acc, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || 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/system_monitor/reader/hdd_reader/hdd_reader.cpp b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp index 740f841382f47..098f4240782ae 100644 --- a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp +++ b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -655,48 +656,56 @@ void run(int port) int main(int argc, char ** argv) { - static struct option long_options[] = { - {"help", no_argument, nullptr, 'h'}, - {"port", required_argument, nullptr, 'p'}, - {nullptr, 0, nullptr, 0}}; - - // Parse command-line options - int c = 0; - int option_index = 0; - int port = PORT; - while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { - switch (c) { - case 'h': - usage(); - return EXIT_SUCCESS; - - case 'p': - try { - port = boost::lexical_cast(optarg); - } catch (const boost::bad_lexical_cast & e) { - printf("Error: %s\n", e.what()); - return EXIT_FAILURE; - } - break; - - default: - break; + try { + static struct option long_options[] = { + {"help", no_argument, nullptr, 'h'}, + {"port", required_argument, nullptr, 'p'}, + {nullptr, 0, nullptr, 0}}; + + // Parse command-line options + int c = 0; + int option_index = 0; + int port = PORT; + while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { + switch (c) { + case 'h': + usage(); + return EXIT_SUCCESS; + + case 'p': + try { + port = boost::lexical_cast(optarg); + } catch (const boost::bad_lexical_cast & e) { + printf("Error: %s\n", e.what()); + return EXIT_FAILURE; + } + break; + + default: + break; + } } - } - // Put the program in the background - if (daemon(0, 0) < 0) { - printf("Failed to put the program in the background. %s\n", strerror(errno)); - return errno; - } + // Put the program in the background + if (daemon(0, 0) < 0) { + printf("Failed to put the program in the background. %s\n", strerror(errno)); + return errno; + } - // Open connection to system logger - openlog(nullptr, LOG_PID, LOG_DAEMON); + // Open connection to system logger + openlog(nullptr, LOG_PID, LOG_DAEMON); - run(port); + run(port); - // Close descriptor used to write to system logger - closelog(); + // Close descriptor used to write to system logger + closelog(); + } catch (const std::exception & e) { + std::cerr << "Exception in main(): " << e.what() << std::endl; + return EXIT_FAILURE; + } catch (...) { + std::cerr << "Unknown exception in main()" << std::endl; + return EXIT_FAILURE; + } return EXIT_SUCCESS; } diff --git a/system/system_monitor/reader/msr_reader/msr_reader.cpp b/system/system_monitor/reader/msr_reader/msr_reader.cpp index fc7bcab795be0..89f95d05c282e 100644 --- a/system/system_monitor/reader/msr_reader/msr_reader.cpp +++ b/system/system_monitor/reader/msr_reader/msr_reader.cpp @@ -212,90 +212,98 @@ void run(int port, const std::vector & list) int main(int argc, char ** argv) { - static struct option long_options[] = { - {"help", no_argument, 0, 'h'}, {"port", required_argument, 0, 'p'}, {0, 0, 0, 0}}; - - // Parse command-line options - int c = 0; - int option_index = 0; - int port = PORT; - while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { - switch (c) { - case 'h': - usage(); - return EXIT_SUCCESS; - - case 'p': - try { - port = boost::lexical_cast(optarg); - } catch (const boost::bad_lexical_cast & e) { - printf("Error: %s\n", e.what()); - return EXIT_FAILURE; - } - break; + try { + static struct option long_options[] = { + {"help", no_argument, 0, 'h'}, {"port", required_argument, 0, 'p'}, {0, 0, 0, 0}}; + + // Parse command-line options + int c = 0; + int option_index = 0; + int port = PORT; + while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { + switch (c) { + case 'h': + usage(); + return EXIT_SUCCESS; + + case 'p': + try { + port = boost::lexical_cast(optarg); + } catch (const boost::bad_lexical_cast & e) { + printf("Error: %s\n", e.what()); + return EXIT_FAILURE; + } + break; + + default: + break; + } + } - default: - break; + if (!fs::exists("/dev/cpu")) { + printf("Failed to access /dev/cpu.\n"); + return EXIT_FAILURE; } - } - if (!fs::exists("/dev/cpu")) { - printf("Failed to access /dev/cpu.\n"); - return EXIT_FAILURE; - } + std::vector list; + const fs::path root("/dev/cpu"); - std::vector list; - const fs::path root("/dev/cpu"); + for (const fs::path & path : boost::make_iterator_range( + fs::recursive_directory_iterator(root), fs::recursive_directory_iterator())) { + if (fs::is_directory(path)) { + continue; + } - for (const fs::path & path : boost::make_iterator_range( - fs::recursive_directory_iterator(root), fs::recursive_directory_iterator())) { - if (fs::is_directory(path)) { - continue; - } + std::cmatch match; + const char * msr = path.generic_string().c_str(); - std::cmatch match; - const char * msr = path.generic_string().c_str(); + // /dev/cpu/[0-9]/msr ? + if (!std::regex_match(msr, match, std::regex(".*msr"))) { + continue; + } - // /dev/cpu/[0-9]/msr ? - if (!std::regex_match(msr, match, std::regex(".*msr"))) { - continue; + list.push_back(path.generic_string()); } - list.push_back(path.generic_string()); - } + std::sort(list.begin(), list.end(), [](const std::string & c1, const std::string & c2) { + std::cmatch match; + const std::regex filter(".*/(\\d+)/msr"); + int n1 = 0; + int n2 = 0; + if (std::regex_match(c1.c_str(), match, filter)) { + n1 = std::stoi(match[1].str()); + } + if (std::regex_match(c2.c_str(), match, filter)) { + n2 = std::stoi(match[1].str()); + } + return n1 < n2; + }); // NOLINT - std::sort(list.begin(), list.end(), [](const std::string & c1, const std::string & c2) { - std::cmatch match; - const std::regex filter(".*/(\\d+)/msr"); - int n1 = 0; - int n2 = 0; - if (std::regex_match(c1.c_str(), match, filter)) { - n1 = std::stoi(match[1].str()); - } - if (std::regex_match(c2.c_str(), match, filter)) { - n2 = std::stoi(match[1].str()); + if (list.empty()) { + printf("No msr found in /dev/cpu.\n"); + return EXIT_FAILURE; } - return n1 < n2; - }); // NOLINT - - if (list.empty()) { - printf("No msr found in /dev/cpu.\n"); - return EXIT_FAILURE; - } - // Put the program in the background - if (daemon(0, 0) < 0) { - printf("Failed to put the program in the background. %s\n", strerror(errno)); - return errno; - } + // Put the program in the background + if (daemon(0, 0) < 0) { + printf("Failed to put the program in the background. %s\n", strerror(errno)); + return errno; + } - // Open connection to system logger - openlog(nullptr, LOG_PID, LOG_DAEMON); + // Open connection to system logger + openlog(nullptr, LOG_PID, LOG_DAEMON); - run(port, list); + run(port, list); - // Close descriptor used to write to system logger - closelog(); + // Close descriptor used to write to system logger + closelog(); + } catch (const std::exception & e) { + std::cerr << "Exception in main(): " << e.what() << std::endl; + return EXIT_FAILURE; + } catch (...) { + std::cerr << "Unknown exception in main()" << std::endl; + return EXIT_FAILURE; + } return EXIT_SUCCESS; } diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt b/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt index 909dd336bd282..0d5b6734418c4 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt +++ b/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt @@ -21,7 +21,10 @@ 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 vehicle_adaptor) +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" @@ -34,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 vehicle_adaptor 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/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..faef2f674832e --- /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.40.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_ diff --git a/visualization/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp b/visualization/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp index b7d754b02d4bd..386c8cdfb0b13 100644 --- a/visualization/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp +++ b/visualization/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp @@ -184,7 +184,7 @@ void ThirdPersonViewController::handleMouseEvent(rviz_common::ViewportMouseEvent void ThirdPersonViewController::mimic(rviz_common::ViewController * source_view) { - FramePositionTrackingViewController::mimic(source_view); + FramePositionTrackingViewController::mimic(source_view); // NOLINT target_frame_property_->setValue(TARGET_FRAME_START); getNewTransform();