diff --git a/.github/actions/build-and-test-differential/action.yaml b/.github/actions/build-and-test-differential/action.yaml deleted file mode 100644 index 89893e9f55fb5..0000000000000 --- a/.github/actions/build-and-test-differential/action.yaml +++ /dev/null @@ -1,111 +0,0 @@ -name: build-and-test-differential -description: "" - -inputs: - rosdistro: - description: "" - required: true - container: - description: "" - required: true - container-suffix: - description: "" - required: true - runner: - description: "" - required: true - build-depends-repos: - description: "" - required: true - build-pre-command: - description: "" - required: true - codecov-token: - description: "" - required: true - -runs: - using: composite - steps: - - name: Show disk space before the tasks - run: df -h - shell: bash - - - name: Show machine specs - run: lscpu && free -h - shell: bash - - - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - - - name: Get modified packages - id: get-modified-packages - uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 - - - name: Create ccache directory - run: | - mkdir -p ${CCACHE_DIR} - du -sh ${CCACHE_DIR} && ccache -s - shell: bash - - - name: Attempt to restore ccache - uses: actions/cache/restore@v4 - with: - path: | - /root/.ccache - key: ccache-main-${{ runner.arch }}-${{ inputs.rosdistro }}-${{ github.event.pull_request.base.sha }} - restore-keys: | - ccache-main-${{ runner.arch }}-${{ inputs.rosdistro }}- - - - name: Show ccache stats before build and reset stats - run: | - du -sh ${CCACHE_DIR} && ccache -s - ccache --zero-stats - shell: bash - - - name: Export CUDA state as a variable for adding to cache key - run: | - build_type_cuda_state=nocuda - if [[ "${{ inputs.container-suffix }}" == "-cuda" ]]; then - build_type_cuda_state=cuda - fi - echo "BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" >> "${GITHUB_ENV}" - echo "::notice::BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" - shell: bash - - - name: Build - if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-build@v1 - with: - rosdistro: ${{ inputs.rosdistro }} - target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - build-depends-repos: ${{ inputs.build-depends-repos }} - cache-key-element: ${{ env.BUILD_TYPE_CUDA_STATE }} - build-pre-command: ${{ inputs.build-pre-command }} - - - name: Show ccache stats after build - run: du -sh ${CCACHE_DIR} && ccache -s - shell: bash - - - name: Test - id: test - if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-test@v1 - with: - rosdistro: ${{ inputs.rosdistro }} - target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - build-depends-repos: ${{ inputs.build-depends-repos }} - - - name: Upload coverage to CodeCov - if: ${{ steps.test.outputs.coverage-report-files != '' }} - uses: codecov/codecov-action@v4 - with: - files: ${{ steps.test.outputs.coverage-report-files }} - fail_ci_if_error: false - verbose: true - flags: differential - token: ${{ inputs.codecov-token }} - - - name: Show disk space after the tasks - run: df -h - shell: bash diff --git a/.github/actions/evaluate-job-result/action.yaml b/.github/actions/evaluate-job-result/action.yaml new file mode 100644 index 0000000000000..c5c013080fd27 --- /dev/null +++ b/.github/actions/evaluate-job-result/action.yaml @@ -0,0 +1,45 @@ +name: Evaluate Job Result +description: Evaluates the result of a job and updates the summary. +inputs: + job_result: + description: Result of the job to evaluate (e.g., success, failure, skipped) + required: true + type: string + job_name: + description: Name of the job to evaluate + required: true + type: string + expected_results: + description: Comma-separated list of acceptable results (e.g., success,skipped) + required: true + type: string +outputs: + failed: + description: Indicates if the job failed + value: ${{ steps.evaluate.outputs.failed }} + +runs: + using: composite + steps: + - name: Evaluate Job Result + id: evaluate + run: | + JOB_RESULT="${{ inputs.job_result }}" + IFS=',' read -ra EXPECTED <<< "${{ inputs.expected_results }}" + FAILED=false + + for RESULT in "${EXPECTED[@]}"; do + if [[ "$JOB_RESULT" == "$RESULT" ]]; then + echo "- **${{ inputs.job_name }}:** "$JOB_RESULT" ✅" >> "$GITHUB_STEP_SUMMARY" + echo "failed=false" >> "$GITHUB_OUTPUT" + exit 0 + fi + done + + # If no expected result matched + echo "::error::${{ inputs.job_name }} failed. ❌" + echo "- **${{ inputs.job_name }}:** failed ❌" >> "$GITHUB_STEP_SUMMARY" + echo "failed=true" >> "$GITHUB_OUTPUT" + + exit 1 + shell: bash diff --git a/.github/labeler.yaml b/.github/labeler.yaml index 115f75197f41a..47f8737ebbf39 100644 --- a/.github/labeler.yaml +++ b/.github/labeler.yaml @@ -39,6 +39,3 @@ - tools/**/* "component:vehicle": - vehicle/**/* -"tag:require-cuda-build-and-test": - - perception/**/* - - sensing/**/* diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index f62904b03c6e4..0925cb0f53930 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -1,57 +1,35 @@ name: build-and-test-differential on: - pull_request: - types: - - opened - - synchronize - - reopened - - labeled - -concurrency: - group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.run_id }} - cancel-in-progress: true + workflow_call: + inputs: + container: + required: true + type: string + runner: + default: ubuntu-24.04 + required: false + type: string + rosdistro: + default: humble + required: false + type: string + container-suffix: + required: false + default: "" + type: string + secrets: + codecov-token: + required: true env: CC: /usr/lib/ccache/gcc CXX: /usr/lib/ccache/g++ jobs: - make-sure-label-is-present: - uses: autowarefoundation/autoware-github-actions/.github/workflows/make-sure-label-is-present.yaml@v1 - with: - label: tag:run-build-and-test-differential - - make-sure-require-cuda-label-is-present: - uses: autowarefoundation/autoware-github-actions/.github/workflows/make-sure-label-is-present.yaml@v1 - with: - label: tag:require-cuda-build-and-test - - prepare-build-and-test-differential: - runs-on: ubuntu-latest - needs: [make-sure-label-is-present, make-sure-require-cuda-label-is-present] - outputs: - cuda_build: ${{ steps.check-if-cuda-build-is-required.outputs.cuda_build }} - steps: - - name: Check if cuda-build is required - id: check-if-cuda-build-is-required - run: | - if ${{ needs.make-sure-require-cuda-label-is-present.outputs.result == 'true' }}; then - echo "cuda-build is required" - echo "cuda_build=true" >> $GITHUB_OUTPUT - else - echo "cuda-build is not required" - echo "cuda_build=false" >> $GITHUB_OUTPUT - fi - shell: bash - - name: Fail if the tag:run-build-and-test-differential is missing - if: ${{ needs.make-sure-label-is-present.outputs.result != 'true' }} - run: exit 1 - build-and-test-differential: - runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware:universe-devel - needs: prepare-build-and-test-differential + runs-on: ${{ inputs.runner }} + container: ${{ inputs.container }}${{ inputs.container-suffix }} steps: - name: Set PR fetch depth run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" @@ -63,61 +41,13 @@ jobs: ref: ${{ github.event.pull_request.head.sha }} fetch-depth: ${{ env.PR_FETCH_DEPTH }} - - name: Run build-and-test-differential action - uses: ./.github/actions/build-and-test-differential - with: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware:universe-devel - container-suffix: "" - runner: ubuntu-latest - build-depends-repos: build_depends.repos - build-pre-command: "" - codecov-token: ${{ secrets.CODECOV_TOKEN }} - - build-and-test-differential-cuda: - runs-on: codebuild-autoware-us-east-1-${{ github.run_id }}-${{ github.run_attempt }}-ubuntu-7.0-large - container: ghcr.io/autowarefoundation/autoware:universe-devel-cuda - needs: prepare-build-and-test-differential - if: ${{ needs.prepare-build-and-test-differential.outputs.cuda_build == 'true' }} - steps: - - name: Set PR fetch depth - run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" - shell: bash - - - name: Checkout PR branch and all PR commits - uses: actions/checkout@v4 - with: - ref: ${{ github.event.pull_request.head.sha }} - fetch-depth: ${{ env.PR_FETCH_DEPTH }} - - - name: Run build-and-test-differential action - uses: ./.github/actions/build-and-test-differential - with: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware:universe-devel - container-suffix: -cuda - runner: codebuild-autoware-us-east-1-${{ github.run_id }}-${{ github.run_attempt }}-ubuntu-7.0-large - build-depends-repos: build_depends.repos - build-pre-command: taskset --cpu-list 0-5 - codecov-token: ${{ secrets.CODECOV_TOKEN }} - - clang-tidy-differential: - needs: [build-and-test-differential, prepare-build-and-test-differential] - if: ${{ needs.prepare-build-and-test-differential.outputs.cuda_build == 'false' }} - runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware:universe-devel - steps: - - name: Set PR fetch depth - run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" - - - name: Checkout PR branch and all PR commits - uses: actions/checkout@v4 - with: - ref: ${{ github.event.pull_request.head.sha }} - fetch-depth: ${{ env.PR_FETCH_DEPTH }} - - name: Show disk space before the tasks run: df -h + shell: bash + + - name: Show machine specs + run: lscpu && free -h + shell: bash - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 @@ -126,66 +56,69 @@ jobs: id: get-modified-packages uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 - - name: Get changed files (existing files only) - id: get-changed-files + - name: Create ccache directory run: | - echo "changed-files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | while read -r file; do [ -e "$file" ] && echo -n "$file "; done)" >> $GITHUB_OUTPUT + mkdir -p ${CCACHE_DIR} + du -sh ${CCACHE_DIR} && ccache -s shell: bash - - name: Run clang-tidy - if: ${{ steps.get-changed-files.outputs.changed-files != '' }} - uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 + - name: Attempt to restore ccache + uses: actions/cache/restore@v4 with: - rosdistro: humble - target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy-ci - clang-tidy-ignore-path: .clang-tidy-ignore - build-depends-repos: build_depends.repos - cache-key-element: cuda + path: | + /root/.ccache + key: ccache-main-${{ runner.arch }}-${{ inputs.rosdistro }}-${{ github.event.pull_request.base.sha }} + restore-keys: | + ccache-main-${{ runner.arch }}-${{ inputs.rosdistro }}- - - name: Show disk space after the tasks - run: df -h + - name: Show ccache stats before build and reset stats + run: | + du -sh ${CCACHE_DIR} && ccache -s + ccache --zero-stats + shell: bash - clang-tidy-differential-cuda: - needs: build-and-test-differential-cuda - runs-on: codebuild-autoware-us-east-1-${{ github.run_id }}-${{ github.run_attempt }}-ubuntu-7.0-large - container: ghcr.io/autowarefoundation/autoware:universe-devel-cuda - steps: - - name: Set PR fetch depth - run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" + - name: Export CUDA state as a variable for adding to cache key + run: | + build_type_cuda_state=nocuda + if [[ "${{ inputs.container-suffix }}" == "-cuda" ]]; then + build_type_cuda_state=cuda + fi + echo "BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" >> "${GITHUB_ENV}" + echo "::notice::BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" + shell: bash - - name: Checkout PR branch and all PR commits - uses: actions/checkout@v4 + - name: Build + if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} + uses: autowarefoundation/autoware-github-actions/colcon-build@v1 with: - ref: ${{ github.event.pull_request.head.sha }} - fetch-depth: ${{ env.PR_FETCH_DEPTH }} - - - name: Show disk space before the tasks - run: df -h - - - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - - - name: Get modified packages - id: get-modified-packages - uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 + rosdistro: ${{ inputs.rosdistro }} + target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} + build-depends-repos: build_depends.repos + cache-key-element: ${{ env.BUILD_TYPE_CUDA_STATE }} - - name: Get changed files (existing files only) - id: get-changed-files - run: | - echo "changed-files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | while read -r file; do [ -e "$file" ] && echo -n "$file "; done)" >> $GITHUB_OUTPUT + - name: Show ccache stats after build + run: du -sh ${CCACHE_DIR} && ccache -s shell: bash - - name: Run clang-tidy - if: ${{ steps.get-changed-files.outputs.changed-files != '' }} - uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 + - name: Test + id: test + if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} + uses: autowarefoundation/autoware-github-actions/colcon-test@v1 with: - rosdistro: humble + rosdistro: ${{ inputs.rosdistro }} target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy-ci - clang-tidy-ignore-path: .clang-tidy-ignore build-depends-repos: build_depends.repos - cache-key-element: cuda + + - name: Upload coverage to CodeCov + if: ${{ steps.test.outputs.coverage-report-files != '' }} + uses: codecov/codecov-action@v4 + with: + files: ${{ steps.test.outputs.coverage-report-files }} + fail_ci_if_error: false + verbose: true + flags: differential${{ inputs.container-suffix }} + token: ${{ secrets.codecov-token }} - name: Show disk space after the tasks run: df -h + shell: bash diff --git a/.github/workflows/build-test-tidy-pr.yaml b/.github/workflows/build-test-tidy-pr.yaml new file mode 100644 index 0000000000000..d1ea9b2d0f79d --- /dev/null +++ b/.github/workflows/build-test-tidy-pr.yaml @@ -0,0 +1,137 @@ +name: build-test-tidy-pr + +on: + pull_request: + types: + - opened + - synchronize + - reopened + - labeled + +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.run_id }} + cancel-in-progress: true + +jobs: + require-label: + uses: autowarefoundation/autoware-github-actions/.github/workflows/require-label.yaml@v1 + with: + label: run:build-and-test-differential + + check-if-cuda-job-is-needed: + needs: require-label + runs-on: ubuntu-latest + outputs: + cuda_job_is_needed: ${{ steps.check.outputs.any_changed }} + steps: + - uses: actions/checkout@v4 + + - name: Check if relevant files changed + id: check + uses: tj-actions/changed-files@v45 + with: + files: | + perception/** + sensing/** + + - name: Output result + run: | + echo "CUDA job needed: ${{ steps.check.outputs.any_changed }}" + shell: bash + + build-and-test-differential: + needs: + - require-label + uses: ./.github/workflows/build-and-test-differential.yaml + with: + container: ghcr.io/autowarefoundation/autoware:universe-devel + secrets: + codecov-token: ${{ secrets.CODECOV_TOKEN }} + + build-and-test-differential-cuda: + needs: check-if-cuda-job-is-needed + if: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'true' }} + uses: ./.github/workflows/build-and-test-differential.yaml + with: + container: ghcr.io/autowarefoundation/autoware:universe-devel + container-suffix: -cuda + secrets: + codecov-token: ${{ secrets.CODECOV_TOKEN }} + + build-test-pr: + needs: + - build-and-test-differential + - build-and-test-differential-cuda + if: ${{ always() }} + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + + - name: Initialize Summary + run: echo "### Build Test PR Results" > $GITHUB_STEP_SUMMARY + shell: bash + + - name: Evaluate build-and-test-differential + uses: ./.github/actions/evaluate-job-result + with: + job_result: ${{ needs.build-and-test-differential.result }} + job_name: build-and-test-differential + expected_results: success + + - name: Evaluate build-and-test-differential-cuda + if: ${{ always() }} + uses: ./.github/actions/evaluate-job-result + with: + job_result: ${{ needs.build-and-test-differential-cuda.result }} + job_name: build-and-test-differential-cuda + expected_results: success,skipped + + clang-tidy-differential: + needs: + - check-if-cuda-job-is-needed + - build-and-test-differential + if: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'false' }} + uses: ./.github/workflows/clang-tidy-differential.yaml + with: + container: ghcr.io/autowarefoundation/autoware:universe-devel + + clang-tidy-differential-cuda: + needs: + - build-and-test-differential-cuda + uses: ./.github/workflows/clang-tidy-differential.yaml + with: + container: ghcr.io/autowarefoundation/autoware:universe-devel + container-suffix: -cuda + + clang-tidy-pr: + needs: + - clang-tidy-differential + - clang-tidy-differential-cuda + if: ${{ always() }} + runs-on: ubuntu-latest + steps: + - name: Initialize Summary + run: echo "### Clang Tidy PR Results" > $GITHUB_STEP_SUMMARY + shell: bash + + - name: Check clang-tidy success + if: ${{ needs.clang-tidy-differential.result == 'success' || needs.clang-tidy-differential-cuda.result == 'success' }} + run: | + echo "✅ Either one of the following has succeeded:" >> $GITHUB_STEP_SUMMARY + + - name: Fail if conditions not met + if: ${{ !(needs.clang-tidy-differential.result == 'success' || needs.clang-tidy-differential-cuda.result == 'success') }} + run: | + echo "::error::❌ Either one of the following should have succeeded:" + echo "::error::clang-tidy-differential: ${{ needs.clang-tidy-differential.result }}" + echo "::error::clang-tidy-differential-cuda: ${{ needs.clang-tidy-differential-cuda.result }}" + + echo "❌ Either one of the following should have succeeded:" >> $GITHUB_STEP_SUMMARY + + exit 1 + + - name: Print the results + if: ${{ always() }} + run: | + echo "- **clang-tidy-differential:** ${{ needs.clang-tidy-differential.result }}" >> $GITHUB_STEP_SUMMARY + echo "- **clang-tidy-differential-cuda:** ${{ needs.clang-tidy-differential-cuda.result }}" >> $GITHUB_STEP_SUMMARY diff --git a/.github/workflows/clang-tidy-differential.yaml b/.github/workflows/clang-tidy-differential.yaml new file mode 100644 index 0000000000000..51e0a8408468c --- /dev/null +++ b/.github/workflows/clang-tidy-differential.yaml @@ -0,0 +1,75 @@ +name: clang-tidy-differential + +on: + workflow_call: + inputs: + container: + required: true + type: string + container-suffix: + required: false + default: "" + type: string + runner: + default: ubuntu-24.04 + required: false + type: string + +jobs: + clang-tidy-differential: + runs-on: ${{ inputs.runner }} + container: ${{ inputs.container }}${{ inputs.container-suffix }} + steps: + - name: Set PR fetch depth + run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" + + - name: Checkout PR branch and all PR commits + uses: actions/checkout@v4 + with: + ref: ${{ github.event.pull_request.head.sha }} + fetch-depth: ${{ env.PR_FETCH_DEPTH }} + + - name: Show machine specs + run: lscpu && free -h + shell: bash + + - name: Show disk space before the tasks + run: df -h + shell: bash + + - name: Remove exec_depend + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 + + - name: Get modified packages + id: get-modified-packages + uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 + + - name: Get changed files (existing files only) + id: get-changed-files + run: | + echo "changed-files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | while read -r file; do [ -e "$file" ] && echo -n "$file "; done)" >> $GITHUB_OUTPUT + shell: bash + + - name: Export CUDA state as a variable for adding to cache key + run: | + build_type_cuda_state=nocuda + if [[ "${{ inputs.container-suffix }}" == "-cuda" ]]; then + build_type_cuda_state=cuda + fi + echo "BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" >> "${GITHUB_ENV}" + echo "::notice::BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" + shell: bash + + - name: Run clang-tidy + if: ${{ steps.get-changed-files.outputs.changed-files != '' }} + uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 + with: + rosdistro: humble + target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} + clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy-ci + clang-tidy-ignore-path: .clang-tidy-ignore + build-depends-repos: build_depends.repos + cache-key-element: ${{ env.BUILD_TYPE_CUDA_STATE }} + + - name: Show disk space after the tasks + run: df -h diff --git a/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_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..120aed7c7548e 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 DiagnosticInterface { public: - DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name); + DiagnosticInterface(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 DiagnosticInterface::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..978af27f202d4 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) +DiagnosticInterface::DiagnosticInterface(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 DiagnosticInterface::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 DiagnosticInterface::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 DiagnosticInterface::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 DiagnosticInterface::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 DiagnosticInterface::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 DiagnosticInterface::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 DiagnosticInterface::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/ros/test_diagnostics_interface.cpp b/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp new file mode 100644 index 0000000000000..a0683694cc2b7 --- /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::DiagnosticInterface; + +class TestDiagnosticInterface : 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(TestDiagnosticInterface, ClearTest) +{ + DiagnosticInterface 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(TestDiagnosticInterface, AddKeyValueTest) +{ + DiagnosticInterface 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(TestDiagnosticInterface, UpdateLevelAndMessageTest) +{ + DiagnosticInterface 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/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml index fcfa9baf4ae20..236fea11d9dda 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml @@ -120,6 +120,7 @@ + @@ -132,6 +133,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml index c289a81c906fe..5da942e8ff8f3 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml @@ -143,6 +143,7 @@ + @@ -155,6 +156,7 @@ + @@ -178,6 +180,7 @@ Control parameter 'use_radar_tracking_fusion' should defined in perception.launch.xml --> + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml index a492e7667c347..4242903c1082d 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml @@ -58,6 +58,7 @@ + @@ -70,6 +71,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index ce13a742c6202..1d34dd279b5fa 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -57,6 +57,7 @@ + diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp index 9511f168f346f..d52fb5e798b58 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..70334738e9ce3 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..bc7dbbcfc9ca1 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::DiagnosticInterface(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..1b5672cff9d04 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..5ebcd105d57ba 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..b38958c420b19 100644 --- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp +++ b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp @@ -16,7 +16,7 @@ #define LOCALIZATION_ERROR_MONITOR_HPP_ #include "autoware/localization_util/covariance_ellipse.hpp" -#include "autoware/localization_util/diagnostics_module.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include #include @@ -39,7 +39,7 @@ class LocalizationErrorMonitor : public rclcpp::Node std::unique_ptr logger_configure_; - std::unique_ptr diagnostics_error_monitor_; + std::unique_ptr diagnostics_error_monitor_; double scale_; double error_ellipse_size_; diff --git a/localization/autoware_localization_util/CMakeLists.txt b/localization/autoware_localization_util/CMakeLists.txt index dd18f3cbad932..de62633124f3d 100644 --- a/localization/autoware_localization_util/CMakeLists.txt +++ b/localization/autoware_localization_util/CMakeLists.txt @@ -6,7 +6,6 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/util_func.cpp - src/diagnostics_module.cpp src/smart_pose_buffer.cpp src/tree_structured_parzen_estimator.cpp src/covariance_ellipse.cpp diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp index d2dce42e3923e..bf8fce2b302c0 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 DiagnosticInterface = autoware::universe_utils::DiagnosticInterface; 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..25b7417ffbe3c 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -17,8 +17,8 @@ #define FMT_HEADER_ONLY -#include "autoware/localization_util/diagnostics_module.hpp" #include "autoware/localization_util/smart_pose_buffer.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include "hyper_parameters.hpp" #include "map_update_module.hpp" #include "ndt_omp/multigrid_ndt_omp.h" @@ -211,12 +211,12 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr regularization_pose_buffer_; std::atomic is_activated_; - std::unique_ptr diagnostics_scan_points_; - std::unique_ptr diagnostics_initial_pose_; - std::unique_ptr diagnostics_regularization_pose_; - std::unique_ptr diagnostics_map_update_; - std::unique_ptr diagnostics_ndt_align_; - std::unique_ptr diagnostics_trigger_node_; + std::unique_ptr diagnostics_scan_points_; + std::unique_ptr diagnostics_initial_pose_; + std::unique_ptr diagnostics_regularization_pose_; + std::unique_ptr diagnostics_map_update_; + std::unique_ptr diagnostics_ndt_align_; + std::unique_ptr diagnostics_trigger_node_; std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; diff --git a/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp index 25b51b55aebd7..eea0b8f3d06c4 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..cef8724423bed 100644 --- a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -49,9 +49,9 @@ using autoware::localization_util::matrix4f_to_pose; using autoware::localization_util::point_to_vector3d; using autoware::localization_util::pose_to_matrix4f; -using autoware::localization_util::DiagnosticsModule; using autoware::localization_util::SmartPoseBuffer; using autoware::localization_util::TreeStructuredParzenEstimator; +using autoware::universe_utils::DiagnosticInterface; tier4_debug_msgs::msg::Float32Stamped make_float32_stamped( const builtin_interfaces::msg::Time & stamp, const float data) @@ -141,7 +141,7 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) std::make_unique(this->get_logger(), value_as_unlimited, value_as_unlimited); diagnostics_regularization_pose_ = - std::make_unique(this, "regularization_pose_subscriber_status"); + std::make_unique(this, "regularization_pose_subscriber_status"); } sensor_aligned_pose_pub_ = @@ -209,13 +209,13 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, param_.dynamic_map_loading); - diagnostics_scan_points_ = std::make_unique(this, "scan_matching_status"); + diagnostics_scan_points_ = std::make_unique(this, "scan_matching_status"); diagnostics_initial_pose_ = - std::make_unique(this, "initial_pose_subscriber_status"); - diagnostics_map_update_ = std::make_unique(this, "map_update_status"); - diagnostics_ndt_align_ = std::make_unique(this, "ndt_align_service_status"); + std::make_unique(this, "initial_pose_subscriber_status"); + diagnostics_map_update_ = std::make_unique(this, "map_update_status"); + diagnostics_ndt_align_ = std::make_unique(this, "ndt_align_service_status"); diagnostics_trigger_node_ = - std::make_unique(this, "trigger_node_service_status"); + std::make_unique(this, "trigger_node_service_status"); logger_configure_ = std::make_unique(this); } diff --git a/localization/autoware_pose_initializer/package.xml b/localization/autoware_pose_initializer/package.xml index eeeb46a8db3e5..de3ecd880ea45 100644 --- a/localization/autoware_pose_initializer/package.xml +++ b/localization/autoware_pose_initializer/package.xml @@ -21,7 +21,6 @@ autoware_component_interface_specs autoware_component_interface_utils - autoware_localization_util autoware_map_height_fitter autoware_motion_utils autoware_universe_utils diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp index 5e9c68d2acc80..67d5c447c09b6 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..5305bcc3ad347 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp @@ -15,10 +15,9 @@ #ifndef POSE_INITIALIZER_CORE_HPP_ #define POSE_INITIALIZER_CORE_HPP_ -#include "autoware/localization_util/diagnostics_module.hpp" - #include #include +#include #include #include @@ -61,7 +60,7 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; std::unique_ptr logger_configure_; - std::unique_ptr diagnostics_pose_reliable_; + std::unique_ptr diagnostics_pose_reliable_; double stop_check_duration_; void change_node_trigger(bool flag, bool need_spin = false); diff --git a/perception/autoware_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..239d406650ce8 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp @@ -15,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 @@ -55,6 +56,7 @@ using sensor_msgs::msg::PointCloud2; using tier4_perception_msgs::msg::DetectedObjectsWithFeature; using tier4_perception_msgs::msg::DetectedObjectWithFeature; using PointCloud = pcl::PointCloud; +using autoware::image_projection_based_fusion::CameraProjection; using autoware_perception_msgs::msg::ObjectClassification; template @@ -86,7 +88,7 @@ class FusionNode : public rclcpp::Node virtual void fuseOnSingleImage( const TargetMsg3D & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, TargetMsg3D & output_msg) = 0; + TargetMsg3D & output_msg) = 0; // set args if you need virtual void postprocess(TargetMsg3D & output_msg); @@ -100,11 +102,16 @@ class FusionNode : public rclcpp::Node tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - bool point_project_to_unrectified_image_{false}; + std::vector point_project_to_unrectified_image_; // camera_info std::map camera_info_map_; std::vector::SharedPtr> camera_info_subs_; + // camera projection + std::vector camera_projectors_; + std::vector approx_camera_projection_; + float approx_grid_cell_w_size_; + float approx_grid_cell_h_size_; rclcpp::TimerBase::SharedPtr timer_; double timeout_ms_{}; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp index 8c0e2ed78fc6c..cd6cd87976522 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -53,15 +53,12 @@ class PointPaintingFusionNode void fuseOnSingleImage( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const std::size_t image_id, const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override; void postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override; rclcpp::Publisher::SharedPtr obj_pub_ptr_; - std::vector tan_h_; // horizontal field of view - int omp_num_threads_{1}; float score_threshold_{0.0}; std::vector class_names_; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp index be05c0a1c4bc6..903b153af0681 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -38,7 +38,6 @@ class RoiClusterFusionNode void fuseOnSingleImage( const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id, const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg) override; std::string trust_object_iou_mode_{"iou"}; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index 7103537ec852d..43ec134168ef9 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -42,13 +42,11 @@ class RoiDetectedObjectFusionNode void fuseOnSingleImage( const DetectedObjects & input_object_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjects & output_object_msg) override; + const DetectedObjectsWithFeature & input_roi_msg, DetectedObjects & output_object_msg) override; std::map generateDetectedObjectRoIs( - const DetectedObjects & input_object_msg, const double image_width, const double image_height, - const Eigen::Affine3d & object2camera_affine, - const image_geometry::PinholeCameraModel & pinhole_camera_model); + const DetectedObjects & input_object_msg, const std::size_t & image_id, + const Eigen::Affine3d & object2camera_affine); void fuseObjectsOnImage( const DetectedObjects & input_object_msg, diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp index 9baf754e224a7..2f2c8222e196f 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -49,8 +49,7 @@ class RoiPointCloudFusionNode void fuseOnSingleImage( const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override; + const DetectedObjectsWithFeature & input_roi_msg, PointCloud2 & output_pointcloud_msg) override; bool out_of_scope(const DetectedObjectWithFeature & obj) override; }; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index 0bec3195bb402..7454cb7bcd173 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -61,7 +61,7 @@ class SegmentPointCloudFusionNode : public FusionNode getTransformStamped( const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, const std::string & source_frame_id, const rclcpp::Time & time); diff --git a/perception/autoware_image_projection_based_fusion/schema/fusion_common.schema.json b/perception/autoware_image_projection_based_fusion/schema/fusion_common.schema.json index 73ee1661adaea..8077f3e2f3e30 100644 --- a/perception/autoware_image_projection_based_fusion/schema/fusion_common.schema.json +++ b/perception/autoware_image_projection_based_fusion/schema/fusion_common.schema.json @@ -31,6 +31,11 @@ "default": 15, "minimum": 1 }, + "point_project_to_unrectified_image": { + "type": "array", + "description": "An array of options indicating whether to project point to unrectified image or not.", + "default": [false, false, false, false, false, false] + }, "debug_mode": { "type": "boolean", "description": "Whether to debug or not.", @@ -65,6 +70,21 @@ "type": "number", "description": "Maximum z position [m].", "default": 100.0 + }, + "approximate_camera_projection": { + "type": "array", + "description": "An array of options indicating whether to use approximated projection for each camera.", + "default": [true, true, true, true, true, true] + }, + "approximation_grid_cell_width": { + "type": "number", + "description": "The width of grid cell used in approximated projection [pixel].", + "default": 1.0 + }, + "approximation_grid_cell_height": { + "type": "number", + "description": "The height of grid cell used in approximated projection [pixel].", + "default": 1.0 } } } diff --git a/perception/autoware_image_projection_based_fusion/src/camera_projection.cpp b/perception/autoware_image_projection_based_fusion/src/camera_projection.cpp new file mode 100644 index 0000000000000..c0a820609a0a7 --- /dev/null +++ b/perception/autoware_image_projection_based_fusion/src/camera_projection.cpp @@ -0,0 +1,252 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/image_projection_based_fusion/camera_projection.hpp" + +#include + +namespace autoware::image_projection_based_fusion +{ +CameraProjection::CameraProjection( + const sensor_msgs::msg::CameraInfo & camera_info, const float grid_cell_width, + const float grid_cell_height, const bool unrectify, const bool use_approximation = false) +: camera_info_(camera_info), + cell_width_(grid_cell_width), + cell_height_(grid_cell_height), + unrectify_(unrectify), + use_approximation_(use_approximation) +{ + if (grid_cell_width <= 0.0 || grid_cell_height <= 0.0) { + throw std::runtime_error("Both grid_cell_width and grid_cell_height must be > 0.0"); + } + + image_width_ = camera_info.width; + image_height_ = camera_info.height; + + // prepare camera model + camera_model_.fromCameraInfo(camera_info); + + // cache settings + inv_cell_width_ = 1 / cell_width_; + inv_cell_height_ = 1 / cell_height_; + grid_width_ = static_cast(std::ceil(image_width_ / cell_width_)); + grid_height_ = static_cast(std::ceil(image_height_ / cell_height_)); + cache_size_ = grid_width_ * grid_height_; + + // compute 3D rays for the image corners and pixels related to optical center + // to find the actual FOV size + // optical centers + const double ocx = static_cast(camera_info.k.at(2)); + const double ocy = static_cast(camera_info.k.at(5)); + // for checking pincushion shape case + const cv::Point3d ray_top_left = camera_model_.projectPixelTo3dRay(cv::Point(0, 0)); + const cv::Point3d ray_top_right = + camera_model_.projectPixelTo3dRay(cv::Point(image_width_ - 1, 0)); + const cv::Point3d ray_bottom_left = + camera_model_.projectPixelTo3dRay(cv::Point(0, image_height_ - 1)); + const cv::Point3d ray_bottom_right = + camera_model_.projectPixelTo3dRay(cv::Point(image_width_ - 1, image_height_ - 1)); + // for checking barrel shape case + const cv::Point3d ray_mid_top = camera_model_.projectPixelTo3dRay(cv::Point(ocx, 0)); + const cv::Point3d ray_mid_left = camera_model_.projectPixelTo3dRay(cv::Point(0, ocy)); + const cv::Point3d ray_mid_right = + camera_model_.projectPixelTo3dRay(cv::Point(image_width_ - 1, ocy)); + const cv::Point3d ray_mid_bottom = + camera_model_.projectPixelTo3dRay(cv::Point(ocx, image_height_ - 1)); + + cv::Point3d x_left = ray_top_left; + cv::Point3d x_right = ray_top_right; + cv::Point3d y_top = ray_top_left; + cv::Point3d y_bottom = ray_bottom_left; + + // check each side of the view + if (ray_bottom_left.x < x_left.x) x_left = ray_bottom_left; + if (ray_mid_left.x < x_left.x) x_left = ray_mid_left; + + if (x_right.x < ray_bottom_right.x) x_right = ray_bottom_right; + if (x_right.x < ray_mid_right.x) x_right = ray_mid_right; + + if (y_top.y < ray_top_right.y) y_top = ray_top_right; + if (y_top.y < ray_mid_top.y) y_top = ray_mid_top; + + if (ray_bottom_left.y < y_bottom.y) y_bottom = ray_bottom_left; + if (ray_mid_bottom.y < y_bottom.y) y_bottom = ray_mid_bottom; + + // set FOV at z = 1.0 + fov_left_ = x_left.x / x_left.z; + fov_right_ = x_right.x / x_right.z; + fov_top_ = y_top.y / y_top.z; + fov_bottom_ = y_bottom.y / y_bottom.z; +} + +void CameraProjection::initialize() +{ + if (unrectify_) { + if (use_approximation_) { + // create the cache with size of grid center + // store only xy position in float to reduce memory consumption + projection_cache_ = std::make_unique(cache_size_); + initializeCache(); + + calcImageProjectedPoint = [this]( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point) { + return this->calcRawImageProjectedPointWithApproximation(point3d, projected_point); + }; + } else { + calcImageProjectedPoint = [this]( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point) { + return this->calcRawImageProjectedPoint(point3d, projected_point); + }; + } + } else { + calcImageProjectedPoint = [this]( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point) { + return this->calcRectifiedImageProjectedPoint(point3d, projected_point); + }; + } +} + +void CameraProjection::initializeCache() +{ + // sample grid cell centers till the camera height, width to precompute the projections + // + // grid cell size + // / + // v + // |---| w + // 0-----------------> + // 0 | . | . | . | + // |___|___|___| + // | . | . | . | + // | ^ + // h | | + // v grid cell center + // + // each pixel will be rounded in these grid cell centers + // edge pixels in right and bottom side in the image will be assign to these centers + // that is the outside of the image + + for (int idx_y = 0; idx_y < grid_height_; idx_y++) { + for (int idx_x = 0; idx_x < grid_width_; idx_x++) { + const uint32_t grid_index = idx_y * grid_width_ + idx_x; + const float px = (idx_x + 0.5f) * cell_width_; + const float py = (idx_y + 0.5f) * cell_height_; + + // precompute projected point + cv::Point2d raw_image_point = camera_model_.unrectifyPoint(cv::Point2d(px, py)); + projection_cache_[grid_index] = + PixelPos{static_cast(raw_image_point.x), static_cast(raw_image_point.y)}; + } + } +} + +/** + * @brief Calculate a projection of 3D point to rectified image plane 2D point. + * @return Return a boolean indicating whether the projected point is on the image plane. + */ +bool CameraProjection::calcRectifiedImageProjectedPoint( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point) +{ + const cv::Point2d rectified_image_point = camera_model_.project3dToPixel(point3d); + + if ( + rectified_image_point.x < 0.0 || rectified_image_point.x >= image_width_ || + rectified_image_point.y < 0.0 || rectified_image_point.y >= image_height_) { + return false; + } else { + projected_point << rectified_image_point.x, rectified_image_point.y; + return true; + } +} + +/** + * @brief Calculate a projection of 3D point to raw image plane 2D point. + * @return Return a boolean indicating whether the projected point is on the image plane. + */ +bool CameraProjection::calcRawImageProjectedPoint( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point) +{ + const cv::Point2d rectified_image_point = camera_model_.project3dToPixel(point3d); + const cv::Point2d raw_image_point = camera_model_.unrectifyPoint(rectified_image_point); + + if ( + rectified_image_point.x < 0.0 || rectified_image_point.x >= image_width_ || + rectified_image_point.y < 0.0 || rectified_image_point.y >= image_height_) { + return false; + } else { + projected_point << raw_image_point.x, raw_image_point.y; + return true; + } +} + +/** + * @brief Calculate a projection of 3D point to raw image plane 2D point with approximation. + * @return Return a boolean indicating whether the projected point is on the image plane. + */ +bool CameraProjection::calcRawImageProjectedPointWithApproximation( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point) +{ + const cv::Point2d rectified_image_point = camera_model_.project3dToPixel(point3d); + + // round to a near grid cell + const int grid_idx_x = static_cast(std::floor(rectified_image_point.x * inv_cell_width_)); + const int grid_idx_y = static_cast(std::floor(rectified_image_point.y * inv_cell_height_)); + + if (grid_idx_x < 0.0 || grid_idx_x >= grid_width_) return false; + if (grid_idx_y < 0.0 || grid_idx_y >= grid_height_) return false; + + const uint32_t grid_index = grid_idx_y * grid_width_ + grid_idx_x; + projected_point << projection_cache_[grid_index].x, projection_cache_[grid_index].y; + + return true; +} + +sensor_msgs::msg::CameraInfo CameraProjection::getCameraInfo() +{ + return camera_info_; +} + +bool CameraProjection::isOutsideHorizontalView(const float px, const float pz) +{ + // assuming the points' origin is centered on the camera + if (pz <= 0.0) return true; + if (px < fov_left_ * pz) return true; + if (px > fov_right_ * pz) return true; + + // inside of the horizontal view + return false; +} + +bool CameraProjection::isOutsideVerticalView(const float py, const float pz) +{ + // assuming the points' origin is centered on the camera + if (pz <= 0.0) return true; + // up in the camera coordinate is negative + if (py < fov_top_ * pz) return true; + if (py > fov_bottom_ * pz) return true; + + // inside of the vertical view + return false; +} + +bool CameraProjection::isOutsideFOV(const float px, const float py, const float pz) +{ + if (isOutsideHorizontalView(px, pz)) return true; + if (isOutsideVerticalView(py, pz)) return true; + + // inside of the FOV + return false; +} + +} // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index bd4d57e45c582..ea0904215cb86 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -18,6 +18,7 @@ #include #include +#include #include @@ -124,6 +125,34 @@ FusionNode::FusionNode( timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&FusionNode::timer_callback, this)); + // camera projection settings + camera_projectors_.resize(rois_number_); + point_project_to_unrectified_image_ = + declare_parameter>("point_project_to_unrectified_image"); + + if (rois_number_ > point_project_to_unrectified_image_.size()) { + throw std::runtime_error( + "The number of point_project_to_unrectified_image does not match the number of rois topics."); + } + approx_camera_projection_ = declare_parameter>("approximate_camera_projection"); + if (rois_number_ != approx_camera_projection_.size()) { + const std::size_t current_size = approx_camera_projection_.size(); + RCLCPP_WARN( + this->get_logger(), + "The number of elements in approximate_camera_projection should be the same as in " + "rois_number. " + "It has %zu elements.", + current_size); + if (current_size < rois_number_) { + approx_camera_projection_.resize(rois_number_); + for (std::size_t i = current_size; i < rois_number_; i++) { + approx_camera_projection_.at(i) = true; + } + } + } + approx_grid_cell_w_size_ = declare_parameter("approximation_grid_cell_width"); + approx_grid_cell_h_size_ = declare_parameter("approximation_grid_cell_height"); + // debugger if (declare_parameter("debug_mode", false)) { std::size_t image_buffer_size = @@ -142,9 +171,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; @@ -168,7 +194,18 @@ void FusionNode::cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id) { - camera_info_map_[camera_id] = *input_camera_info_msg; + // create the CameraProjection when the camera info arrives for the first time + // assuming the camera info does not change while the node is running + if ( + camera_info_map_.find(camera_id) == camera_info_map_.end() && + checkCameraInfo(*input_camera_info_msg)) { + camera_projectors_.at(camera_id) = CameraProjection( + *input_camera_info_msg, approx_grid_cell_w_size_, approx_grid_cell_h_size_, + point_project_to_unrectified_image_.at(camera_id), approx_camera_projection_.at(camera_id)); + camera_projectors_.at(camera_id).initialize(); + + camera_info_map_[camera_id] = *input_camera_info_msg; + } } template @@ -273,8 +310,7 @@ void FusionNode::subCallback( } fuseOnSingleImage( - *input_msg, roi_i, *((cached_roi_msgs_.at(roi_i))[matched_stamp]), - camera_info_map_.at(roi_i), *output_msg); + *input_msg, roi_i, *((cached_roi_msgs_.at(roi_i))[matched_stamp]), *output_msg); (cached_roi_msgs_.at(roi_i)).erase(matched_stamp); is_fused_.at(roi_i) = true; @@ -346,9 +382,7 @@ void FusionNode::roiCallback( debugger_->clear(); } - fuseOnSingleImage( - *(cached_msg_.second), roi_i, *input_roi_msg, camera_info_map_.at(roi_i), - *(cached_msg_.second)); + fuseOnSingleImage(*(cached_msg_.second), roi_i, *input_roi_msg, *(cached_msg_.second)); is_fused_.at(roi_i) = true; if (debug_publisher_) { 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..2837bac458541 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -161,13 +161,6 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS().keep_last(3), sub_callback); - tan_h_.resize(rois_number_); - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - auto fx = camera_info_map_[roi_i].k.at(0); - auto x0 = camera_info_map_[roi_i].k.at(2); - tan_h_[roi_i] = x0 / fx; - } - detection_class_remapper_.setParameters( allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); @@ -260,9 +253,7 @@ void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted void PointPaintingFusionNode::fuseOnSingleImage( __attribute__((unused)) const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, - __attribute__((unused)) const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, + const std::size_t image_id, const DetectedObjectsWithFeature & input_roi_msg, sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { @@ -276,8 +267,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_); @@ -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 @@ -346,24 +332,27 @@ dc | dc dc dc dc ||zc| p_y = point_camera.y(); p_z = point_camera.z(); - if (p_z <= 0.0 || p_x > (tan_h_.at(image_id) * p_z) || p_x < (-tan_h_.at(image_id) * p_z)) { + if (camera_projectors_[image_id].isOutsideHorizontalView(p_x, p_z)) { continue; } + // project - Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(p_x, p_y, p_z), point_project_to_unrectified_image_); - - // iterate 2d bbox - for (const auto & feature_object : objects) { - sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; - // paint current point if it is inside bbox - int label2d = feature_object.object.classification.front().label; - if (!isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) { - // cppcheck-suppress invalidPointerCast - auto p_class = reinterpret_cast(&output[stride + class_offset]); - for (const auto & cls : isClassTable_) { - // add up the class values if the point belongs to multiple classes - *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; + Eigen::Vector2d projected_point; + if (camera_projectors_[image_id].calcImageProjectedPoint( + cv::Point3d(p_x, p_y, p_z), projected_point)) { + // iterate 2d bbox + for (const auto & feature_object : objects) { + sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; + // paint current point if it is inside bbox + int label2d = feature_object.object.classification.front().label; + if ( + !isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) { + // cppcheck-suppress invalidPointerCast + auto p_class = reinterpret_cast(&output[stride + class_offset]); + for (const auto & cls : isClassTable_) { + // add up the class values if the point belongs to multiple classes + *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; + } } } #if 0 @@ -375,15 +364,15 @@ dc | dc dc dc dc ||zc| } } - for (const auto & feature_object : input_roi_msg.feature_objects) { - debug_image_rois.push_back(feature_object.feature.roi); - } - if (debugger_) { std::unique_ptr inner_st_ptr; if (time_keeper_) inner_st_ptr = std::make_unique("publish debug message", *time_keeper_); + for (const auto & feature_object : input_roi_msg.feature_objects) { + debug_image_rois.push_back(feature_object.feature.roi); + } + debugger_->image_rois_ = debug_image_rois; debugger_->obstacle_points_ = debug_image_points; debugger_->publishImage(image_id, input_roi_msg.header.stamp); 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..32db5319bb487 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -95,16 +95,12 @@ void RoiClusterFusionNode::postprocess(DetectedObjectsWithFeature & output_clust void RoiClusterFusionNode::fuseOnSingleImage( const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg) + const DetectedObjectsWithFeature & input_roi_msg, DetectedObjectsWithFeature & output_cluster_msg) { - if (!checkCameraInfo(camera_info)) return; - std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - image_geometry::PinholeCameraModel pinhole_camera_model; - pinhole_camera_model.fromCameraInfo(camera_info); + const sensor_msgs::msg::CameraInfo & camera_info = camera_projectors_[image_id].getCameraInfo(); // get transform from cluster frame id to camera optical frame id geometry_msgs::msg::TransformStamped transform_stamped; @@ -152,18 +148,17 @@ void RoiClusterFusionNode::fuseOnSingleImage( continue; } - Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(*iter_x, *iter_y, *iter_z), - point_project_to_unrectified_image_); - if ( - 0 <= static_cast(projected_point.x()) && - static_cast(projected_point.x()) <= static_cast(camera_info.width) - 1 && - 0 <= static_cast(projected_point.y()) && - static_cast(projected_point.y()) <= static_cast(camera_info.height) - 1) { - min_x = std::min(static_cast(projected_point.x()), min_x); - min_y = std::min(static_cast(projected_point.y()), min_y); - max_x = std::max(static_cast(projected_point.x()), max_x); - max_y = std::max(static_cast(projected_point.y()), max_y); + Eigen::Vector2d projected_point; + if (camera_projectors_[image_id].calcImageProjectedPoint( + cv::Point3d(*iter_x, *iter_y, *iter_z), projected_point)) { + const int px = static_cast(projected_point.x()); + const int py = static_cast(projected_point.y()); + + min_x = std::min(px, min_x); + min_y = std::min(py, min_y); + max_x = std::max(px, max_x); + max_y = std::max(py, max_y); + projected_points.push_back(projected_point); if (debugger_) debugger_->obstacle_points_.push_back(projected_point); } diff --git a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 035bc259ab73c..7be18d59fdbf1 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -84,14 +84,11 @@ 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))) { 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 +100,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( object2camera_affine = transformToEigen(transform_stamped_optional.value().transform); } - image_geometry::PinholeCameraModel pinhole_camera_model; - pinhole_camera_model.fromCameraInfo(camera_info); - - const auto object_roi_map = generateDetectedObjectRoIs( - input_object_msg, static_cast(camera_info.width), - static_cast(camera_info.height), object2camera_affine, pinhole_camera_model); + const auto object_roi_map = + generateDetectedObjectRoIs(input_object_msg, image_id, object2camera_affine); fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map); if (debugger_) { @@ -122,9 +115,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( std::map RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( - const DetectedObjects & input_object_msg, const double image_width, const double image_height, - const Eigen::Affine3d & object2camera_affine, - const image_geometry::PinholeCameraModel & pinhole_camera_model) + const DetectedObjects & input_object_msg, const std::size_t & image_id, + const Eigen::Affine3d & object2camera_affine) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -139,6 +131,10 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( return object_roi_map; } const auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); + const sensor_msgs::msg::CameraInfo & camera_info = camera_projectors_[image_id].getCameraInfo(); + const double image_width = static_cast(camera_info.width); + const double image_height = static_cast(camera_info.height); + for (std::size_t obj_i = 0; obj_i < input_object_msg.objects.size(); ++obj_i) { std::vector vertices_camera_coord; const auto & object = input_object_msg.objects.at(obj_i); @@ -165,18 +161,17 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( continue; } - Eigen::Vector2d proj_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(point.x(), point.y(), point.z()), - point_project_to_unrectified_image_); + Eigen::Vector2d proj_point; + if (camera_projectors_[image_id].calcImageProjectedPoint( + cv::Point3d(point.x(), point.y(), point.z()), proj_point)) { + const double px = proj_point.x(); + const double py = proj_point.y(); - min_x = std::min(proj_point.x(), min_x); - min_y = std::min(proj_point.y(), min_y); - max_x = std::max(proj_point.x(), max_x); - max_y = std::max(proj_point.y(), max_y); + min_x = std::min(px, min_x); + min_y = std::min(py, min_y); + max_x = std::max(px, max_x); + max_y = std::max(py, max_y); - if ( - proj_point.x() >= 0 && proj_point.x() < image_width && proj_point.y() >= 0 && - proj_point.y() < image_height) { point_on_image_cnt++; if (debugger_) { 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..998072d87774e 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -86,10 +86,8 @@ void RoiPointCloudFusionNode::postprocess( } } void RoiPointCloudFusionNode::fuseOnSingleImage( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, - __attribute__((unused)) const std::size_t image_id, + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const std::size_t image_id, const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, __attribute__((unused)) sensor_msgs::msg::PointCloud2 & output_pointcloud_msg) { std::unique_ptr st_ptr; @@ -98,7 +96,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 +119,6 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( return; } - // transform pointcloud to camera optical frame id - image_geometry::PinholeCameraModel pinhole_camera_model; - pinhole_camera_model.fromCameraInfo(camera_info); - geometry_msgs::msg::TransformStamped transform_stamped; { const auto transform_stamped_optional = getTransformStamped( @@ -136,10 +129,13 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( } transform_stamped = transform_stamped_optional.value(); } - int point_step = input_pointcloud_msg.point_step; - int x_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "x")].offset; - int y_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "y")].offset; - int z_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "z")].offset; + const int point_step = input_pointcloud_msg.point_step; + const int x_offset = + input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "x")].offset; + const int y_offset = + input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "y")].offset; + const int z_offset = + input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "z")].offset; sensor_msgs::msg::PointCloud2 transformed_cloud; tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); @@ -164,33 +160,36 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( if (transformed_z <= 0.0) { continue; } - Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z), - point_project_to_unrectified_image_); - for (std::size_t i = 0; i < output_objs.size(); ++i) { - auto & feature_obj = output_objs.at(i); - const auto & check_roi = feature_obj.feature.roi; - auto & cluster = clusters.at(i); - - if ( - clusters_data_size.at(i) >= - static_cast(max_cluster_size_) * static_cast(point_step)) { - continue; - } - if ( - check_roi.x_offset <= projected_point.x() && check_roi.y_offset <= projected_point.y() && - check_roi.x_offset + check_roi.width >= projected_point.x() && - check_roi.y_offset + check_roi.height >= projected_point.y()) { - std::memcpy( - &cluster.data[clusters_data_size.at(i)], &input_pointcloud_msg.data[offset], point_step); - clusters_data_size.at(i) += point_step; + + Eigen::Vector2d projected_point; + if (camera_projectors_[image_id].calcImageProjectedPoint( + cv::Point3d(transformed_x, transformed_y, transformed_z), projected_point)) { + for (std::size_t i = 0; i < output_objs.size(); ++i) { + auto & feature_obj = output_objs.at(i); + const auto & check_roi = feature_obj.feature.roi; + auto & cluster = clusters.at(i); + + const double px = projected_point.x(); + const double py = projected_point.y(); + + if ( + clusters_data_size.at(i) >= + static_cast(max_cluster_size_) * static_cast(point_step)) { + continue; + } + if ( + check_roi.x_offset <= px && check_roi.y_offset <= py && + check_roi.x_offset + check_roi.width >= px && + check_roi.y_offset + check_roi.height >= py) { + std::memcpy( + &cluster.data[clusters_data_size.at(i)], &input_pointcloud_msg.data[offset], + point_step); + clusters_data_size.at(i) += point_step; + } } - } - if (debugger_) { - // add all points inside image to debug - if ( - projected_point.x() > 0 && projected_point.x() < camera_info.width && - projected_point.y() > 0 && projected_point.y() < camera_info.height) { + + if (debugger_) { + // add all points inside image to debug debug_image_points.push_back(projected_point); } } diff --git a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index afeff213c0afe..486ae291abc6a 100644 --- a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -87,9 +87,8 @@ void SegmentPointCloudFusionNode::postprocess(PointCloud2 & pointcloud_msg) } void SegmentPointCloudFusionNode::fuseOnSingleImage( - const PointCloud2 & input_pointcloud_msg, __attribute__((unused)) const std::size_t image_id, - [[maybe_unused]] const Image & input_mask, __attribute__((unused)) const CameraInfo & camera_info, - __attribute__((unused)) PointCloud2 & output_cloud) + const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, + [[maybe_unused]] const Image & input_mask, __attribute__((unused)) PointCloud2 & output_cloud) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -97,10 +96,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 = camera_projectors_[image_id].getCameraInfo(); std::vector mask_data(input_mask.data.begin(), input_mask.data.end()); cv::Mat mask = perception_utils::runLengthDecoder(mask_data, input_mask.height, input_mask.width); @@ -115,8 +115,6 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( const int orig_height = camera_info.height; // resize mask to the same size as the camera image cv::resize(mask, mask, cv::Size(orig_width, orig_height), 0, 0, cv::INTER_NEAREST); - image_geometry::PinholeCameraModel pinhole_camera_model; - pinhole_camera_model.fromCameraInfo(camera_info); geometry_msgs::msg::TransformStamped transform_stamped; // transform pointcloud from frame id to camera optical frame id @@ -151,13 +149,9 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( continue; } - Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z), - point_project_to_unrectified_image_); - - bool is_inside_image = projected_point.x() > 0 && projected_point.x() < camera_info.width && - projected_point.y() > 0 && projected_point.y() < camera_info.height; - if (!is_inside_image) { + Eigen::Vector2d projected_point; + if (!camera_projectors_[image_id].calcImageProjectedPoint( + cv::Point3d(transformed_x, transformed_y, transformed_z), projected_point)) { continue; } diff --git a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp index 338a5605e5a79..4456d25b18167 100644 --- a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp @@ -44,20 +44,6 @@ bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info) return true; } -Eigen::Vector2d calcRawImageProjectedPoint( - const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d, - const bool & unrectify) -{ - const cv::Point2d rectified_image_point = pinhole_camera_model.project3dToPixel(point3d); - - if (!unrectify) { - return Eigen::Vector2d(rectified_image_point.x, rectified_image_point.y); - } - const cv::Point2d raw_image_point = pinhole_camera_model.unrectifyPoint(rectified_image_point); - - return Eigen::Vector2d(raw_image_point.x, raw_image_point.y); -} - std::optional getTransformStamped( const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, const std::string & source_frame_id, const rclcpp::Time & time) diff --git a/perception/autoware_multi_object_tracker/CMakeLists.txt b/perception/autoware_multi_object_tracker/CMakeLists.txt index 298b6605192a5..fcea50235bf0d 100644 --- a/perception/autoware_multi_object_tracker/CMakeLists.txt +++ b/perception/autoware_multi_object_tracker/CMakeLists.txt @@ -30,6 +30,8 @@ set(${PROJECT_NAME}_src set(${PROJECT_NAME}_lib lib/association/association.cpp lib/association/mu_successive_shortest_path/mu_ssp.cpp + lib/object_model/types.cpp + lib/object_model/shapes.cpp lib/tracker/motion_model/motion_model_base.cpp lib/tracker/motion_model/bicycle_motion_model.cpp # cspell: ignore ctrv diff --git a/perception/autoware_multi_object_tracker/README.md b/perception/autoware_multi_object_tracker/README.md index afb1598645733..3f83817f6509f 100644 --- a/perception/autoware_multi_object_tracker/README.md +++ b/perception/autoware_multi_object_tracker/README.md @@ -69,7 +69,12 @@ Multiple inputs are pre-defined in the input channel parameters (described below ### Core Parameters +- Node + {{ json_to_markdown("perception/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json") }} + +- Association + {{ json_to_markdown("perception/autoware_multi_object_tracker/schema/data_association_matrix.schema.json") }} #### Simulation parameters diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp index 2c12341d0aa67..b92e17987eb5f 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp @@ -27,7 +27,7 @@ #include #include -#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include #include #include @@ -58,7 +58,7 @@ class DataAssociation const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, std::unordered_map & reverse_assignment); Eigen::MatrixXd calcScoreMatrix( - const autoware_perception_msgs::msg::DetectedObjects & measurements, + const types::DynamicObjectList & measurements, const std::list> & trackers); virtual ~DataAssociation() {} }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp new file mode 100644 index 0000000000000..d399f356136fc --- /dev/null +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp @@ -0,0 +1,57 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee + +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_ + +#include "autoware/multi_object_tracker/object_model/types.hpp" + +#include + +#include + +#include + +namespace autoware::multi_object_tracker +{ +namespace shapes +{ +bool transformObjects( + const types::DynamicObjectList & input_msg, const std::string & target_frame_id, + const tf2_ros::Buffer & tf_buffer, types::DynamicObjectList & output_msg); + +double get2dIoU( + const types::DynamicObject & source_object, const types::DynamicObject & target_object, + const double min_union_area = 0.01); + +bool convertConvexHullToBoundingBox( + const types::DynamicObject & input_object, types::DynamicObject & output_object); + +bool getMeasurementYaw( + const types::DynamicObject & object, const double & predicted_yaw, double & measurement_yaw); + +int getNearestCornerOrSurface( + const double x, const double y, const double yaw, const double width, const double length, + const geometry_msgs::msg::Transform & self_transform); + +void calcAnchorPointOffset( + const double w, const double l, const int indx, const types::DynamicObject & input_object, + const double & yaw, types::DynamicObject & offset_object, Eigen::Vector2d & tracking_offset); +} // namespace shapes +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_ diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp new file mode 100644 index 0000000000000..7dab840aac481 --- /dev/null +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp @@ -0,0 +1,88 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee + +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace autoware::multi_object_tracker +{ +namespace types +{ +enum OrientationAvailability : uint8_t { + UNAVAILABLE = 0, + SIGN_UNKNOWN = 1, + AVAILABLE = 2, +}; + +struct ObjectKinematics +{ + geometry_msgs::msg::PoseWithCovariance pose_with_covariance; + geometry_msgs::msg::TwistWithCovariance twist_with_covariance; + bool has_position_covariance = false; + OrientationAvailability orientation_availability; + bool has_twist = false; + bool has_twist_covariance = false; +}; + +struct DynamicObject +{ + unique_identifier_msgs::msg::UUID object_id = unique_identifier_msgs::msg::UUID(); + uint channel_index; + float existence_probability; + std::vector classification; + ObjectKinematics kinematics; + autoware_perception_msgs::msg::Shape shape; +}; + +struct DynamicObjectList +{ + std_msgs::msg::Header header; + uint channel_index; + std::vector objects; +}; + +DynamicObject toDynamicObject( + const autoware_perception_msgs::msg::DetectedObject & det_object, const uint channel_index = 0); + +DynamicObjectList toDynamicObjectList( + const autoware_perception_msgs::msg::DetectedObjects & det_objects, const uint channel_index = 0); + +autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObject & dyn_object); + +} // namespace types + +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_ diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 8501c68b0cf23..ad3667eb240c0 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -19,18 +19,20 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include + namespace autoware::multi_object_tracker { class BicycleTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; + types::DynamicObject object_; rclcpp::Logger logger_; object_model::ObjectModel object_model_ = object_model::bicycle; @@ -50,23 +52,19 @@ class BicycleTracker : public Tracker public: BicycleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool measureWithPose(const types::DynamicObject & object); + bool measureWithShape(const types::DynamicObject & object); + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; private: - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, + types::DynamicObject getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform) const; }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index 8b4fa1babf652..b9e026bf3c009 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -19,10 +19,11 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp" +#include #include namespace autoware::multi_object_tracker @@ -36,17 +37,13 @@ class MultipleVehicleTracker : public Tracker public: MultipleVehicleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; virtual ~MultipleVehicleTracker() {} }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp index 45cd0f31a4e85..6230ba6e3b0f4 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -19,32 +19,30 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "tracker_base.hpp" +#include + namespace autoware::multi_object_tracker { class PassThroughTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; - autoware_perception_msgs::msg::DetectedObject prev_observed_object_; + types::DynamicObject object_; + types::DynamicObject prev_observed_object_; rclcpp::Logger logger_; rclcpp::Time last_update_time_; public: PassThroughTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index 4287e0f99d5ee..8a4bfc59d37ac 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -19,11 +19,13 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" #include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include + namespace autoware::multi_object_tracker { @@ -35,17 +37,13 @@ class PedestrianAndBicycleTracker : public Tracker public: PedestrianAndBicycleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; virtual ~PedestrianAndBicycleTracker() {} }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index f20b38f73e95f..5a2acc50a8249 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -19,18 +19,20 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" +#include + namespace autoware::multi_object_tracker { class PedestrianTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; + types::DynamicObject object_; rclcpp::Logger logger_; object_model::ObjectModel object_model_ = object_model::pedestrian; @@ -56,23 +58,19 @@ class PedestrianTracker : public Tracker public: PedestrianTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool measureWithPose(const types::DynamicObject & object); + bool measureWithShape(const types::DynamicObject & object); + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; private: - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, + types::DynamicObject getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform) const; }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index 0b9ea9c44a6cf..ac5527fca6400 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp @@ -20,14 +20,14 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ #define EIGEN_MPL2_ONLY -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include +#include #include -#include "autoware_perception_msgs/msg/detected_object.hpp" -#include "autoware_perception_msgs/msg/tracked_object.hpp" +#include +#include #include #include @@ -67,9 +67,8 @@ class Tracker return existence_vector.size() > 0; } bool updateWithMeasurement( - const autoware_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform, - const uint & channel_index); + const types::DynamicObject & object, const rclcpp::Time & measurement_time, + const geometry_msgs::msg::Transform & self_transform); bool updateWithoutMeasurement(const rclcpp::Time & now); // classification @@ -108,12 +107,11 @@ class Tracker protected: virtual bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) = 0; public: - virtual bool getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const = 0; + virtual bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const = 0; virtual bool predict(const rclcpp::Time & time) = 0; }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp index 9f128c864ad6c..075db6b8a9d69 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -19,17 +19,19 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" +#include + namespace autoware::multi_object_tracker { class UnknownTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; + types::DynamicObject object_; rclcpp::Logger logger_; struct EkfParams @@ -47,22 +49,17 @@ class UnknownTracker : public Tracker public: UnknownTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + types::DynamicObject getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform); + bool measureWithPose(const types::DynamicObject & object); + bool measureWithShape(const types::DynamicObject & object); + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp index f3708fd1ff905..f50d117acc081 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp @@ -19,11 +19,13 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__VEHICLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__VEHICLE_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include + namespace autoware::multi_object_tracker { @@ -35,7 +37,7 @@ class VehicleTracker : public Tracker double velocity_deviation_threshold_; - autoware_perception_msgs::msg::DetectedObject object_; + types::DynamicObject object_; double z_; struct BoundingBox @@ -53,24 +55,19 @@ class VehicleTracker : public Tracker public: VehicleTracker( const object_model::ObjectModel & object_model, const rclcpp::Time & time, - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool measureWithPose(const types::DynamicObject & object); + bool measureWithShape(const types::DynamicObject & object); + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; private: - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + types::DynamicObject getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform); }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp index 12bd865795b85..44ad012a5f428 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp @@ -19,6 +19,7 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__UNCERTAINTY__UNCERTAINTY_PROCESSOR_HPP_ #include "autoware/multi_object_tracker/object_model/object_model.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include @@ -32,21 +33,19 @@ namespace uncertainty { using autoware::multi_object_tracker::object_model::ObjectModel; -using autoware_perception_msgs::msg::DetectedObject; -using autoware_perception_msgs::msg::DetectedObjects; using autoware_perception_msgs::msg::ObjectClassification; using nav_msgs::msg::Odometry; ObjectModel decodeObjectModel(const ObjectClassification & object_class); -DetectedObjects modelUncertainty(const DetectedObjects & detected_objects); +types::DynamicObjectList modelUncertainty(const types::DynamicObjectList & detected_objects); object_model::StateCovariance covarianceFromObjectClass( - const DetectedObject & detected_object, const ObjectClassification & object_class); + const types::DynamicObject & detected_object, const ObjectClassification & object_class); -void normalizeUncertainty(DetectedObjects & detected_objects); +void normalizeUncertainty(types::DynamicObjectList & detected_objects); -void addOdometryUncertainty(const Odometry & odometry, DetectedObjects & detected_objects); +void addOdometryUncertainty(const Odometry & odometry, types::DynamicObjectList & detected_objects); } // namespace uncertainty } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/association/association.cpp b/perception/autoware_multi_object_tracker/lib/association/association.cpp index 0c809ee5f086d..d74d87489f8db 100644 --- a/perception/autoware_multi_object_tracker/lib/association/association.cpp +++ b/perception/autoware_multi_object_tracker/lib/association/association.cpp @@ -15,8 +15,10 @@ #include "autoware/multi_object_tracker/association/association.hpp" #include "autoware/multi_object_tracker/association/solver/gnn_solver.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/multi_object_tracker/object_model/shapes.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" + +#include #include #include @@ -150,7 +152,7 @@ void DataAssociation::assign( } Eigen::MatrixXd DataAssociation::calcScoreMatrix( - const autoware_perception_msgs::msg::DetectedObjects & measurements, + const types::DynamicObjectList & measurements, const std::list> & trackers) { Eigen::MatrixXd score_matrix = @@ -162,14 +164,13 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( for (size_t measurement_idx = 0; measurement_idx < measurements.objects.size(); ++measurement_idx) { - const autoware_perception_msgs::msg::DetectedObject & measurement_object = - measurements.objects.at(measurement_idx); + const types::DynamicObject & measurement_object = measurements.objects.at(measurement_idx); const std::uint8_t measurement_label = autoware::object_recognition_utils::getHighestProbLabel(measurement_object.classification); double score = 0.0; if (can_assign_matrix_(tracker_label, measurement_label)) { - autoware_perception_msgs::msg::TrackedObject tracked_object; + types::DynamicObject tracked_object; (*tracker_itr)->getTrackedObject(measurements.header.stamp, tracked_object); const double max_dist = max_dist_matrix_(tracker_label, measurement_label); @@ -210,8 +211,8 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double min_iou = min_iou_matrix_(tracker_label, measurement_label); const double min_union_iou_area = 1e-2; - const double iou = autoware::object_recognition_utils::get2dIoU( - measurement_object, tracked_object, min_union_iou_area); + const double iou = + shapes::get2dIoU(measurement_object, tracked_object, min_union_iou_area); if (iou < min_iou) passed_gate = false; } diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp similarity index 59% rename from perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp rename to perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp index 833f768e171f4..2ce23e5df814e 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,31 +13,198 @@ // limitations under the License. // // -// Author: v1.0 Yukihiro Saito -// +// Author: v1.0 Yukihiro Saito, Taekjin Lee -#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ -#define AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#include "autoware/multi_object_tracker/object_model/shapes.hpp" -#include #include +#include +#include + +#include +#include -#include "autoware_perception_msgs/msg/detected_object.hpp" -#include "autoware_perception_msgs/msg/shape.hpp" -#include "autoware_perception_msgs/msg/tracked_object.hpp" -#include -#include -#include +#include #include +#include #include -#include -#include +#include #include -namespace utils +namespace autoware::multi_object_tracker +{ +namespace shapes +{ +inline boost::optional getTransform( + const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, + const std::string & target_frame_id, const rclcpp::Time & time) +{ + try { + geometry_msgs::msg::TransformStamped self_transform_stamped; + self_transform_stamped = tf_buffer.lookupTransform( + target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + return self_transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("multi_object_tracker"), ex.what()); + return boost::none; + } +} + +bool transformObjects( + const types::DynamicObjectList & input_msg, const std::string & target_frame_id, + const tf2_ros::Buffer & tf_buffer, types::DynamicObjectList & output_msg) +{ + output_msg = input_msg; + + // transform to world coordinate + if (input_msg.header.frame_id != target_frame_id) { + output_msg.header.frame_id = target_frame_id; + tf2::Transform tf_target2objects_world; + tf2::Transform tf_target2objects; + tf2::Transform tf_objects_world2objects; + { + const auto ros_target2objects_world = + getTransform(tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + if (!ros_target2objects_world) { + return false; + } + tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); + } + for (auto & object : output_msg.objects) { + auto & pose_with_cov = object.kinematics.pose_with_covariance; + tf2::fromMsg(pose_with_cov.pose, tf_objects_world2objects); + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; + // transform pose, frame difference and object pose + tf2::toMsg(tf_target2objects, pose_with_cov.pose); + // transform covariance, only the frame difference + pose_with_cov.covariance = + tf2::transformCovariance(pose_with_cov.covariance, tf_target2objects_world); + } + } + return true; +} + +inline double getSumArea(const std::vector & polygons) +{ + return std::accumulate( + polygons.begin(), polygons.end(), 0.0, [](double acc, autoware::universe_utils::Polygon2d p) { + return acc + boost::geometry::area(p); + }); +} + +inline double getIntersectionArea( + const autoware::universe_utils::Polygon2d & source_polygon, + const autoware::universe_utils::Polygon2d & target_polygon) +{ + std::vector intersection_polygons; + boost::geometry::intersection(source_polygon, target_polygon, intersection_polygons); + return getSumArea(intersection_polygons); +} + +inline double getUnionArea( + const autoware::universe_utils::Polygon2d & source_polygon, + const autoware::universe_utils::Polygon2d & target_polygon) +{ + std::vector union_polygons; + boost::geometry::union_(source_polygon, target_polygon, union_polygons); + return getSumArea(union_polygons); +} + +double get2dIoU( + const types::DynamicObject & source_object, const types::DynamicObject & target_object, + const double min_union_area) { + static const double MIN_AREA = 1e-6; + + const auto source_polygon = autoware::universe_utils::toPolygon2d( + source_object.kinematics.pose_with_covariance.pose, source_object.shape); + if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; + const auto target_polygon = autoware::universe_utils::toPolygon2d( + target_object.kinematics.pose_with_covariance.pose, target_object.shape); + if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; + + const double intersection_area = getIntersectionArea(source_polygon, target_polygon); + if (intersection_area < MIN_AREA) return 0.0; + const double union_area = getUnionArea(source_polygon, target_polygon); + + const double iou = + union_area < min_union_area ? 0.0 : std::min(1.0, intersection_area / union_area); + return iou; +} + +/** + * @brief convert convex hull shape object to bounding box object + * @param input_object: input convex hull objects + * @param output_object: output bounding box objects + */ +bool convertConvexHullToBoundingBox( + const types::DynamicObject & input_object, types::DynamicObject & output_object) +{ + // check footprint size + if (input_object.shape.footprint.points.size() < 3) { + return false; + } + + // look for bounding box boundary + float max_x = 0; + float max_y = 0; + float min_x = 0; + float min_y = 0; + float max_z = 0; + for (const auto & point : input_object.shape.footprint.points) { + max_x = std::max(max_x, point.x); + max_y = std::max(max_y, point.y); + min_x = std::min(min_x, point.x); + min_y = std::min(min_y, point.y); + max_z = std::max(max_z, point.z); + } + + // calc new center + const Eigen::Vector2d center{ + input_object.kinematics.pose_with_covariance.pose.position.x, + input_object.kinematics.pose_with_covariance.pose.position.y}; + const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation); + const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); + const Eigen::Vector2d new_local_center{(max_x + min_x) / 2.0, (max_y + min_y) / 2.0}; + const Eigen::Vector2d new_center = center + R_inv.transpose() * new_local_center; + + // set output parameters + output_object = input_object; + output_object.kinematics.pose_with_covariance.pose.position.x = new_center.x(); + output_object.kinematics.pose_with_covariance.pose.position.y = new_center.y(); + + output_object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + output_object.shape.dimensions.x = max_x - min_x; + output_object.shape.dimensions.y = max_y - min_y; + output_object.shape.dimensions.z = max_z; + + return true; +} + +bool getMeasurementYaw( + const types::DynamicObject & object, const double & predicted_yaw, double & measurement_yaw) +{ + measurement_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + + // check orientation sign is known or not, and fix the limiting delta yaw + double limiting_delta_yaw = M_PI_2; + if (object.kinematics.orientation_availability == types::OrientationAvailability::AVAILABLE) { + limiting_delta_yaw = M_PI; + } + // limiting delta yaw, even the availability is unknown + while (std::fabs(predicted_yaw - measurement_yaw) > limiting_delta_yaw) { + if (measurement_yaw < predicted_yaw) { + measurement_yaw += 2 * limiting_delta_yaw; + } else { + measurement_yaw -= 2 * limiting_delta_yaw; + } + } + // return false if the orientation is unknown + return object.kinematics.orientation_availability != types::OrientationAvailability::UNAVAILABLE; +} + enum BBOX_IDX { FRONT_SURFACE = 0, RIGHT_SURFACE = 1, @@ -51,17 +218,6 @@ enum BBOX_IDX { INVALID = -1 }; -/** - * @brief check if object label belongs to "large vehicle" - * @param label: input object label - * @return True if object label means large vehicle - */ -inline bool isLargeVehicleLabel(const uint8_t label) -{ - using Label = autoware_perception_msgs::msg::ObjectClassification; - return label == Label::BUS || label == Label::TRUCK || label == Label::TRAILER; -} - /** * @brief Determine the Nearest Corner or Surface of detected object observed from ego vehicle * @@ -73,7 +229,7 @@ inline bool isLargeVehicleLabel(const uint8_t label) * @param self_transform: Ego vehicle position in map frame * @return int index */ -inline int getNearestCornerOrSurface( +int getNearestCornerOrSurface( const double x, const double y, const double yaw, const double width, const double length, const geometry_msgs::msg::Transform & self_transform) { @@ -171,10 +327,9 @@ inline Eigen::Vector2d calcOffsetVectorFromShapeChange( * @param offset_object: output tracking measurement to feed ekf * @return nearest corner index(int) */ -inline void calcAnchorPointOffset( - const double w, const double l, const int indx, - const autoware_perception_msgs::msg::DetectedObject & input_object, const double & yaw, - autoware_perception_msgs::msg::DetectedObject & offset_object, Eigen::Vector2d & tracking_offset) +void calcAnchorPointOffset( + const double w, const double l, const int indx, const types::DynamicObject & input_object, + const double & yaw, types::DynamicObject & offset_object, Eigen::Vector2d & tracking_offset) { // copy value offset_object = input_object; @@ -198,82 +353,6 @@ inline void calcAnchorPointOffset( offset_object.kinematics.pose_with_covariance.pose.position.y += rotated_offset.y(); } -/** - * @brief convert convex hull shape object to bounding box object - * @param input_object: input convex hull objects - * @param output_object: output bounding box objects - */ -inline bool convertConvexHullToBoundingBox( - const autoware_perception_msgs::msg::DetectedObject & input_object, - autoware_perception_msgs::msg::DetectedObject & output_object) -{ - // check footprint size - if (input_object.shape.footprint.points.size() < 3) { - return false; - } - - // look for bounding box boundary - float max_x = 0; - float max_y = 0; - float min_x = 0; - float min_y = 0; - float max_z = 0; - for (const auto & point : input_object.shape.footprint.points) { - max_x = std::max(max_x, point.x); - max_y = std::max(max_y, point.y); - min_x = std::min(min_x, point.x); - min_y = std::min(min_y, point.y); - max_z = std::max(max_z, point.z); - } - - // calc new center - const Eigen::Vector2d center{ - input_object.kinematics.pose_with_covariance.pose.position.x, - input_object.kinematics.pose_with_covariance.pose.position.y}; - const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation); - const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); - const Eigen::Vector2d new_local_center{(max_x + min_x) / 2.0, (max_y + min_y) / 2.0}; - const Eigen::Vector2d new_center = center + R_inv.transpose() * new_local_center; - - // set output parameters - output_object = input_object; - output_object.kinematics.pose_with_covariance.pose.position.x = new_center.x(); - output_object.kinematics.pose_with_covariance.pose.position.y = new_center.y(); - - output_object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; - output_object.shape.dimensions.x = max_x - min_x; - output_object.shape.dimensions.y = max_y - min_y; - output_object.shape.dimensions.z = max_z; - - return true; -} - -inline bool getMeasurementYaw( - const autoware_perception_msgs::msg::DetectedObject & object, const double & predicted_yaw, - double & measurement_yaw) -{ - measurement_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - - // check orientation sign is known or not, and fix the limiting delta yaw - double limiting_delta_yaw = M_PI_2; - if ( - object.kinematics.orientation_availability == - autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { - limiting_delta_yaw = M_PI; - } - // limiting delta yaw, even the availability is unknown - while (std::fabs(predicted_yaw - measurement_yaw) > limiting_delta_yaw) { - if (measurement_yaw < predicted_yaw) { - measurement_yaw += 2 * limiting_delta_yaw; - } else { - measurement_yaw -= 2 * limiting_delta_yaw; - } - } - // return false if the orientation is unknown - return object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; -} - -} // namespace utils +} // namespace shapes -#endif // AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +} // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/object_model/types.cpp b/perception/autoware_multi_object_tracker/lib/object_model/types.cpp new file mode 100644 index 0000000000000..671d5313d0ff8 --- /dev/null +++ b/perception/autoware_multi_object_tracker/lib/object_model/types.cpp @@ -0,0 +1,101 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee + +#include "autoware/multi_object_tracker/object_model/types.hpp" + +#include + +namespace autoware::multi_object_tracker +{ + +namespace types +{ + +DynamicObject toDynamicObject( + const autoware_perception_msgs::msg::DetectedObject & det_object, const uint channel_index) +{ + DynamicObject dynamic_object; + dynamic_object.channel_index = channel_index; + dynamic_object.existence_probability = det_object.existence_probability; + dynamic_object.classification = det_object.classification; + + dynamic_object.kinematics.pose_with_covariance = det_object.kinematics.pose_with_covariance; + dynamic_object.kinematics.twist_with_covariance = det_object.kinematics.twist_with_covariance; + dynamic_object.kinematics.has_position_covariance = det_object.kinematics.has_position_covariance; + if ( + det_object.kinematics.orientation_availability == + autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE) { + dynamic_object.kinematics.orientation_availability = OrientationAvailability::UNAVAILABLE; + } else if ( + det_object.kinematics.orientation_availability == + autoware_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) { + dynamic_object.kinematics.orientation_availability = OrientationAvailability::SIGN_UNKNOWN; + } else if ( + det_object.kinematics.orientation_availability == + autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { + dynamic_object.kinematics.orientation_availability = OrientationAvailability::AVAILABLE; + } + dynamic_object.kinematics.has_twist = det_object.kinematics.has_twist; + dynamic_object.kinematics.has_twist_covariance = det_object.kinematics.has_twist_covariance; + + dynamic_object.shape = det_object.shape; + + return dynamic_object; +} + +DynamicObjectList toDynamicObjectList( + const autoware_perception_msgs::msg::DetectedObjects & det_objects, const uint channel_index) +{ + DynamicObjectList dynamic_objects; + dynamic_objects.header = det_objects.header; + dynamic_objects.channel_index = channel_index; + dynamic_objects.objects.reserve(det_objects.objects.size()); + for (const auto & det_object : det_objects.objects) { + dynamic_objects.objects.emplace_back(toDynamicObject(det_object, channel_index)); + } + return dynamic_objects; +} + +autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObject & dyn_object) +{ + autoware_perception_msgs::msg::TrackedObject tracked_object; + tracked_object.object_id = dyn_object.object_id; + tracked_object.existence_probability = dyn_object.existence_probability; + tracked_object.classification = dyn_object.classification; + + tracked_object.kinematics.pose_with_covariance = dyn_object.kinematics.pose_with_covariance; + tracked_object.kinematics.twist_with_covariance = dyn_object.kinematics.twist_with_covariance; + if (dyn_object.kinematics.orientation_availability == OrientationAvailability::UNAVAILABLE) { + tracked_object.kinematics.orientation_availability = + autoware_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE; + } else if ( + dyn_object.kinematics.orientation_availability == OrientationAvailability::SIGN_UNKNOWN) { + tracked_object.kinematics.orientation_availability = + autoware_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; + } else if (dyn_object.kinematics.orientation_availability == OrientationAvailability::AVAILABLE) { + tracked_object.kinematics.orientation_availability = + autoware_perception_msgs::msg::TrackedObjectKinematics::AVAILABLE; + } + tracked_object.kinematics.is_stationary = false; + + tracked_object.shape = dyn_object.shape; + + return tracked_object; +} +} // namespace types + +} // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index 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..3eebb3c11ee3e 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp @@ -185,15 +185,13 @@ void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Tim void TrackerDebugger::collectObjectInfo( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const types::DynamicObjectList & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment) { if (!debug_settings_.publish_debug_markers) return; object_debugger_.collect( - message_time, list_tracker, channel_index, detected_objects, direct_assignment, - reverse_assignment); + message_time, list_tracker, detected_objects, direct_assignment, reverse_assignment); } // ObjectDebugger diff --git a/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp index 2c05da0c999e2..3df901a63505c 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp @@ -101,8 +101,7 @@ class TrackerDebugger } void collectObjectInfo( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const types::DynamicObjectList & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment); void publishObjectsMarkers(); diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index d2d2d3e087c4e..333b301ce9fcf 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -17,8 +17,8 @@ #include "multi_object_tracker_node.hpp" +#include "autoware/multi_object_tracker/object_model/shapes.hpp" #include "autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" #include #include @@ -95,7 +95,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) get_parameter("selected_input_channels").as_string_array(); // ROS interface - Publisher - tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); + tracked_objects_pub_ = + create_publisher("output", rclcpp::QoS{1}); // ROS interface - Input channels // Get input channels @@ -239,18 +240,18 @@ void MultiObjectTracker::onTrigger() // process start last_updated_time_ = current_time; - const rclcpp::Time latest_time(objects_list.back().second.header.stamp); + const rclcpp::Time latest_time(objects_list.back().header.stamp); debugger_->startMeasurementTime(this->now(), latest_time); - // run process for each DetectedObjects + // run process for each DynamicObject for (const auto & objects_data : objects_list) { - runProcess(objects_data.second, objects_data.first); + runProcess(objects_data); } // process end debugger_->endMeasurementTime(this->now()); // Publish without delay compensation if (!publish_timer_) { - const auto latest_object_time = rclcpp::Time(objects_list.back().second.header.stamp); + const auto latest_object_time = rclcpp::Time(objects_list.back().header.stamp); checkAndPublish(latest_object_time); } } @@ -278,8 +279,7 @@ void MultiObjectTracker::onTimer() if (should_publish) checkAndPublish(current_time); } -void MultiObjectTracker::runProcess( - const DetectedObjects & input_objects, const uint & channel_index) +void MultiObjectTracker::runProcess(const types::DynamicObjectList & input_objects) { // Get the time of the measurement const rclcpp::Time measurement_time = @@ -293,9 +293,8 @@ void MultiObjectTracker::runProcess( } // Transform the objects to the world frame - DetectedObjects transformed_objects; - if (!autoware::object_recognition_utils::transformObjects( - input_objects, world_frame_id_, tf_buffer_, transformed_objects)) { + types::DynamicObjectList transformed_objects; + if (!shapes::transformObjects(input_objects, world_frame_id_, tf_buffer_, transformed_objects)) { return; } @@ -350,19 +349,19 @@ void MultiObjectTracker::runProcess( // Collect debug information - tracker list, existence probabilities, association results debugger_->collectObjectInfo( - measurement_time, processor_->getListTracker(), channel_index, transformed_objects, - direct_assignment, reverse_assignment); + measurement_time, processor_->getListTracker(), transformed_objects, direct_assignment, + reverse_assignment); } /* tracker update */ - processor_->update(transformed_objects, *self_transform, direct_assignment, channel_index); + processor_->update(transformed_objects, *self_transform, direct_assignment); /* tracker pruning */ processor_->prune(measurement_time); /* spawn new tracker */ - if (input_manager_->isChannelSpawnEnabled(channel_index)) { - processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index); + if (input_manager_->isChannelSpawnEnabled(input_objects.channel_index)) { + processor_->spawn(transformed_objects, reverse_assignment); } } @@ -387,7 +386,7 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const return; } // Create output msg - TrackedObjects output_msg, tentative_objects_msg; + autoware_perception_msgs::msg::TrackedObjects output_msg; output_msg.header.frame_id = world_frame_id_; processor_->getTrackedObjects(time, output_msg); @@ -399,7 +398,7 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const debugger_->endPublishTime(this->now(), time); if (debugger_->shouldPublishTentativeObjects()) { - TrackedObjects tentative_output_msg; + autoware_perception_msgs::msg::TrackedObjects tentative_output_msg; tentative_output_msg.header.frame_id = world_frame_id_; processor_->getTentativeObjects(time, tentative_output_msg); debugger_->publishTentativeObjects(tentative_output_msg); diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp index 79a8c1b98dcca..0472d67617f7f 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp @@ -20,6 +20,7 @@ #define MULTI_OBJECT_TRACKER_NODE_HPP_ #include "autoware/multi_object_tracker/association/association.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "debugger/debugger.hpp" #include "processor/input_manager.hpp" @@ -55,10 +56,6 @@ namespace autoware::multi_object_tracker { -using DetectedObject = autoware_perception_msgs::msg::DetectedObject; -using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects; -using TrackedObjects = autoware_perception_msgs::msg::TrackedObjects; - class MultiObjectTracker : public rclcpp::Node { public: @@ -66,8 +63,9 @@ class MultiObjectTracker : public rclcpp::Node private: // ROS interface - rclcpp::Publisher::SharedPtr tracked_objects_pub_; - rclcpp::Subscription::SharedPtr detected_object_sub_; + rclcpp::Publisher::SharedPtr tracked_objects_pub_; + rclcpp::Subscription::SharedPtr + detected_object_sub_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -100,7 +98,7 @@ class MultiObjectTracker : public rclcpp::Node void onTrigger(); // publish processes - void runProcess(const DetectedObjects & input_objects, const uint & channel_index); + void runProcess(const types::DynamicObjectList & input_objects); void checkAndPublish(const rclcpp::Time & time); void publish(const rclcpp::Time & time) const; inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp index 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/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_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_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/include/autoware/behavior_path_lane_change_module/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp index 5317b94db2d6b..1061e07e516a2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp @@ -18,6 +18,7 @@ #include "autoware/behavior_path_lane_change_module/structs/debug.hpp" #include "autoware/behavior_path_lane_change_module/structs/path.hpp" #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -38,6 +39,7 @@ class TestNormalLaneChange; namespace autoware::behavior_path_planner { +using autoware::behavior_path_planner::PoseWithDetailOpt; using autoware::route_handler::Direction; using autoware::universe_utils::StopWatch; using geometry_msgs::msg::Point; @@ -102,8 +104,6 @@ class LaneChangeBase virtual LaneChangePath getLaneChangePath() const = 0; - virtual BehaviorModuleOutput getTerminalLaneChangePath() const = 0; - virtual bool isEgoOnPreparePhase() const = 0; virtual bool isRequiredStop(const bool is_trailing_object) = 0; @@ -224,7 +224,7 @@ class LaneChangeBase return direction_; } - std::optional getStopPose() const { return lane_change_stop_pose_; } + PoseWithDetailOpt getStopPose() const { return lane_change_stop_pose_; } void resetStopPose() { lane_change_stop_pose_ = std::nullopt; } @@ -235,9 +235,6 @@ class LaneChangeBase protected: virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; - virtual bool get_prepare_segment( - PathWithLaneId & prepare_segment, const double prepare_length) const = 0; - virtual bool isValidPath(const PathWithLaneId & path) const = 0; virtual bool isAbleToStopSafely() const = 0; @@ -287,7 +284,7 @@ class LaneChangeBase lane_change::CommonDataPtr common_data_ptr_; FilteredLanesObjects filtered_objects_{}; BehaviorModuleOutput prev_module_output_{}; - std::optional lane_change_stop_pose_{std::nullopt}; + PoseWithDetailOpt lane_change_stop_pose_{std::nullopt}; PathWithLaneId prev_approved_path_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 9bf66722dcec2..5f1da79bc7ea0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -18,6 +18,7 @@ #include "autoware/behavior_path_lane_change_module/structs/data.hpp" #include +#include #include #include @@ -64,8 +65,6 @@ class NormalLaneChange : public LaneChangeBase LaneChangePath getLaneChangePath() const override; - BehaviorModuleOutput getTerminalLaneChangePath() const override; - BehaviorModuleOutput generateOutput() override; void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; @@ -128,14 +127,6 @@ class NormalLaneChange : public LaneChangeBase void filterOncomingObjects(PredictedObjects & objects) const; - bool get_prepare_segment( - PathWithLaneId & prepare_segment, const double prepare_length) const override; - - PathWithLaneId getTargetSegment( - const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, - const double target_lane_length, const double lane_changing_length, - const double lane_changing_velocity, const double buffer_for_next_lane_change) const; - std::vector get_prepare_metrics() const; std::vector get_lane_changing_metrics( const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metrics, @@ -143,32 +134,19 @@ class NormalLaneChange : public LaneChangeBase bool get_lane_change_paths(LaneChangePaths & candidate_paths) const; - LaneChangePath get_candidate_path( - const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, - const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, - const Pose & lc_start_pose, const double shift_length) const; - bool check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const; - std::optional calcTerminalLaneChangePath( - const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) const; - bool isValidPath(const PathWithLaneId & path) const override; PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, + const std::vector> & ego_predicted_paths, const lane_change::TargetObjects & collision_check_objects, const utils::path_safety_checker::RSSparams & rss_params, - const size_t deceleration_sampling_num, CollisionCheckDebugMap & debug_data) const; - - bool has_collision_with_decel_patterns( - const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects, - const size_t deceleration_sampling_num, const RSSparams & rss_param, - const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const; + CollisionCheckDebugMap & debug_data) const; - bool is_collided( + bool is_colliding( const LaneChangePath & lane_change_path, const ExtendedPredictedObject & obj, const std::vector & ego_predicted_path, const RSSparams & selected_rss_param, const bool check_prepare_phase, @@ -178,27 +156,10 @@ class NormalLaneChange : public LaneChangeBase bool is_ego_stuck() const; - /** - * @brief Checks if the given pose is a valid starting point for a lane change. - * - * This function determines whether the given pose (position) of the vehicle is within - * the area of either the target neighbor lane or the target lane itself. It uses geometric - * checks to see if the start point of the lane change is covered by the polygons representing - * these lanes. - * - * @param common_data_ptr Shared pointer to a CommonData structure, which should include: - * - Non-null `lanes_polygon_ptr` that contains the polygon data for lanes. - * @param pose The current pose of the vehicle - * - * @return `true` if the pose is within either the target neighbor lane or the target lane; - * `false` otherwise. - */ - bool is_valid_start_point( - const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) const; - bool check_prepare_phase() const; - void set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path); + void set_stop_pose( + const double arc_length_to_stop_pose, PathWithLaneId & path, const std::string & reason = ""); void updateStopTime(); @@ -213,7 +174,6 @@ class NormalLaneChange : public LaneChangeBase std::vector path_after_intersection_; double stop_time_{0.0}; - static constexpr double floating_err_th{1e-3}; }; } // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp index 4dcb1c7b685d0..5f36be806bba4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp @@ -160,6 +160,8 @@ struct TargetObjects : leading(std::move(leading)), trailing(std::move(trailing)) { } + + [[nodiscard]] bool empty() const { return leading.empty() && trailing.empty(); } }; enum class ModuleType { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp index ea9807ad1616f..8738b412e18b9 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 @@ -45,7 +45,7 @@ struct Debug 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}; @@ -69,7 +69,7 @@ struct Debug 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/utils/markers.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp index 4c2712f053b13..059db8e38300f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp @@ -35,7 +35,7 @@ using autoware::behavior_path_planner::lane_change::Debug; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using visualization_msgs::msg::MarkerArray; MarkerArray showAllValidLaneChangePath( - const std::vector & lanes, std::string && ns); + const std::vector & lane_change_paths, std::string && ns); MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp new file mode 100644 index 0000000000000..dcc327b4793e1 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp @@ -0,0 +1,102 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ + +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.hpp" + +#include + +#include + +namespace autoware::behavior_path_planner::utils::lane_change +{ +using behavior_path_planner::LaneChangePath; +using behavior_path_planner::lane_change::CommonDataPtr; + +/** + * @brief Generates a prepare segment for a lane change maneuver. + * + * This function generates the "prepare segment" of the path by trimming it to the specified length, + * adjusting longitudinal velocity for acceleration or deceleration, and ensuring the starting point + * meets necessary constraints for a lane change. + * + * @param common_data_ptr Shared pointer to CommonData containing current and target lane + * information, vehicle parameters, and ego state. + * @param prev_module_path The input path from the previous module, which will be used + * as the base for the prepare segment. + * @param prep_metric Preparation metrics containing desired length and velocity for the segment. + * @param prepare_segment Output parameter where the resulting prepare segment path will be stored. + * + * @throws std::logic_error If the lane change start point is behind the target lanelet or + * if lane information is invalid. + * + * @return true if the prepare segment is successfully generated, false otherwise. + */ +bool get_prepare_segment( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, + const LaneChangePhaseMetrics prep_metric, PathWithLaneId & prepare_segment); + +/** + * @brief Generates the candidate path for a lane change maneuver. + * + * This function calculates the candidate lane change path based on the provided preparation + * and lane change metrics. It resamples the target lane reference path, determines the start + * and end poses for the lane change, and constructs the shift line required for the maneuver. + * The function ensures that the resulting path satisfies length and distance constraints. + * + * @param common_data_ptr Shared pointer to CommonData containing route, lane, and transient data. + * @param prep_metric Metrics for the preparation phase, including path length and velocity. + * @param lc_metric Metrics for the lane change phase, including path length and velocity. + * @param prep_segment The path segment prepared before initiating the lane change. + * @param sorted_lane_ids A vector of sorted lane IDs used for updating lane information in the + * path. + * @param lc_start_pose The pose where the lane change begins. + * @param shift_length The lateral distance to shift during the lane change maneuver. + * + * @throws std::logic_error If the target lane reference path is empty, candidate path generation + * fails, or the resulting path length exceeds terminal constraints. + * + * @return LaneChangePath The constructed candidate lane change path. + */ +LaneChangePath get_candidate_path( + const CommonDataPtr & common_data_ptr, const LaneChangePhaseMetrics & prep_metric, + const LaneChangePhaseMetrics & lc_metric, const PathWithLaneId & prep_segment, + const std::vector> & sorted_lane_ids, const double shift_length); + +/** + * @brief Constructs a candidate path for a lane change maneuver. + * + * This function generates a candidate lane change path by shifting the target lane's reference path + * and combining it with the prepare segment. It verifies the path's validity by checking the yaw + * difference, adjusting longitudinal velocity, and updating lane IDs based on the provided lane + * sorting information. + * + * @param lane_change_info Information about the lane change, including shift line and target + * velocity. + * @param prepare_segment The path segment leading up to the lane change. + * @param target_lane_reference_path The reference path of the target lane. + * @param sorted_lane_ids A vector of sorted lane IDs used to update the candidate path's lane IDs. + * + * @return std::optional The constructed candidate path if valid, or std::nullopt + * if the path fails any constraints. + */ +LaneChangePath construct_candidate_path( + const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, + const PathWithLaneId & target_lane_reference_path, + const std::vector> & sorted_lane_ids); +} // namespace autoware::behavior_path_planner::utils::lane_change +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 0adb9409fb4b0..69362994dbbac 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -52,6 +52,7 @@ using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LanesPolygon; +using behavior_path_planner::lane_change::LCParamPtr; using behavior_path_planner::lane_change::ModuleType; using behavior_path_planner::lane_change::PathSafetyStatus; using behavior_path_planner::lane_change::TargetLaneLeadingObjects; @@ -61,12 +62,11 @@ using geometry_msgs::msg::Twist; using path_safety_checker::CollisionCheckDebugMap; using tier4_planning_msgs::msg::PathWithLaneId; -bool is_mandatory_lane_change(const ModuleType lc_type); +rclcpp::Logger get_logger(); -double calcLaneChangeResampleInterval( - const double lane_changing_length, const double lane_changing_velocity); +bool is_mandatory_lane_change(const ModuleType lc_type); -void setPrepareVelocity( +void set_prepare_velocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity); std::vector replaceWithSortedIds( @@ -79,27 +79,10 @@ lanelet::ConstLanelets get_target_neighbor_lanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const LaneChangeModuleType & type); -bool isPathInLanelets( - const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes); - bool path_footprint_exceeds_target_lane_bound( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, const double margin = 0.1); -std::optional construct_candidate_path( - const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path, - const std::vector> & sorted_lane_ids); - -ShiftLine get_lane_changing_shift_line( - const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, - const PathWithLaneId & reference_path, const double shift_length); - -PathWithLaneId get_reference_path_from_target_Lane( - const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, - const double lane_changing_length, const double resample_interval); - std::vector generateDrivableLanes( const std::vector & original_drivable_lanes, const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes); @@ -141,8 +124,8 @@ bool isParkedObject( * If the parameter delay_lc_param.check_only_parked_vehicle is set to True, only objects * which pass isParkedObject() check will be considered. * - * @param common_data_ptr Shared pointer to CommonData that holds necessary lanes info, parameters, - * and transient data. + * @param common_data_ptr Shared pointer to CommonData that holds necessary lanes info, + * parameters, and transient data. * @param lane_change_path Candidate lane change path to apply checks on. * @param target_objects Relevant objects to consider for delay LC checks (assumed to only include * target lane leading static objects). @@ -202,8 +185,8 @@ rclcpp::Logger getLogger(const std::string & type); * The footprint is determined by the vehicle's pose and its dimensions, including the distance * from the base to the front and rear ends of the vehicle, as well as its width. * - * @param common_data_ptr Shared pointer to CommonData that holds necessary ego vehicle's dimensions - * and pose information. + * @param common_data_ptr Shared pointer to CommonData that holds necessary ego vehicle's + * dimensions and pose information. * * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle. */ @@ -233,15 +216,15 @@ bool is_within_intersection( /** * @brief Determines if a polygon is within lanes designated for turning. * - * Checks if a polygon overlaps with lanelets tagged for turning directions (excluding 'straight'). - * It evaluates the lanelet's 'turn_direction' attribute and determines overlap with the lanelet's - * area. + * Checks if a polygon overlaps with lanelets tagged for turning directions (excluding + * 'straight'). It evaluates the lanelet's 'turn_direction' attribute and determines overlap with + * the lanelet's area. * * @param lanelet Lanelet representing the road segment whose turn direction is to be evaluated. * @param polygon The polygon to be checked for its presence within turn direction lanes. * - * @return bool True if the polygon is within a lane designated for turning, false if it is within a - * straight lane or no turn direction is specified. + * @return bool True if the polygon is within a lane designated for turning, false if it is within + * a straight lane or no turn direction is specified. */ bool is_within_turn_direction_lanes( const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); @@ -276,8 +259,8 @@ double get_distance_to_next_regulatory_element( * * @param common_data_ptr Pointer to the common data structure containing parameters for lane * change. - * @param filtered_objects A collection of objects filtered by lanes, including those in the current - * lane. + * @param filtered_objects A collection of objects filtered by lanes, including those in the + * current lane. * @param dist_to_target_lane_start The distance to the start of the target lane from the ego * vehicle. * @param path The current path of the ego vehicle, containing path points and lane information. @@ -297,8 +280,8 @@ double get_min_dist_to_current_lanes_obj( * * @param common_data_ptr Pointer to the common data structure containing parameters for the lane * change. - * @param filtered_objects A collection of objects filtered by lanes, including those in the target - * lane. + * @param filtered_objects A collection of objects filtered by lanes, including those in the + * target lane. * @param stop_arc_length The arc length at which the ego vehicle is expected to stop. * @param path The current path of the ego vehicle, containing path points and lane information. * @return true if there is an object in the target lane that influences the stop point decision; @@ -365,14 +348,15 @@ bool has_overtaking_turn_lane_object( * * @param common_data_ptr Shared pointer to CommonData containing information about current lanes, * vehicle dimensions, lane polygons, and behavior parameters. - * @param object An extended predicted object representing a potential obstacle in the environment. + * @param object An extended predicted object representing a potential obstacle in the + * environment. * @param dist_ego_to_current_lanes_center Distance from the ego vehicle to the center of the * current lanes. * @param ahead_of_ego Boolean flag indicating if the object is ahead of the ego vehicle. - * @param before_terminal Boolean flag indicating if the ego vehicle is before the terminal point of - * the lane. - * @param leading_objects Reference to a structure for storing leading objects (stopped, moving, or - * outside boundaries). + * @param before_terminal Boolean flag indicating if the ego vehicle is before the terminal point + * of the lane. + * @param leading_objects Reference to a structure for storing leading objects (stopped, moving, + * or outside boundaries). * @param trailing_objects Reference to a collection for storing trailing objects. * * @return true if the object is classified as either leading or trailing, false otherwise. @@ -415,5 +399,38 @@ std::vector get_preceding_lanes(const CommonDataPtr & co */ bool object_path_overlaps_lanes( const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon); + +/** + * @brief Converts a lane change path into multiple predicted paths with varying acceleration + * profiles. + * + * This function generates a set of predicted paths for the ego vehicle during a lane change, + * using different acceleration values within the specified range. It accounts for deceleration + * sampling if the global minimum acceleration differs from the lane-changing acceleration. + * + * @param common_data_ptr Shared pointer to CommonData containing parameters and state information. + * @param lane_change_path The lane change path used to generate predicted paths. + * @param deceleration_sampling_num Number of samples for deceleration profiles to generate paths. + * + * @return std::vector> A collection of predicted paths, where + * each path is represented as a series of poses with associated velocity. + */ +std::vector> convert_to_predicted_paths( + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, + const size_t deceleration_sampling_num); + +/** + * @brief Validates whether a given pose is a valid starting point for a lane change. + * + * This function checks if the specified pose lies within the polygons representing + * the target lane or its neighboring areas. This ensures that the starting point is + * appropriately positioned for initiating a lane change, even if previous paths were adjusted. + * + * @param common_data_ptr Shared pointer to CommonData containing lane polygon information. + * @param pose The pose to validate as a potential lane change starting point. + * + * @return true if the pose is within the target or target neighbor polygons, false otherwise. + */ +bool is_valid_start_point(const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose); } // namespace autoware::behavior_path_planner::utils::lane_change #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml index dd4dbe63e41d4..fad98ecf8f8e1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml @@ -26,6 +26,7 @@ autoware_motion_utils autoware_rtc_interface autoware_universe_utils + fmt pluginlib range-v3 rclcpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index d65e51997eb32..f80aad721a07c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -111,9 +111,7 @@ BehaviorModuleOutput LaneChangeInterface::plan() path_reference_ = std::make_shared(output.reference_path); *prev_approved_path_ = getPreviousModuleOutput().path; - const auto stop_pose_opt = module_type_->getStopPose(); - stop_pose_ = stop_pose_opt.has_value() ? PoseWithDetailOpt(PoseWithDetail(stop_pose_opt.value())) - : PoseWithDetailOpt(); + stop_pose_ = module_type_->getStopPose(); const auto & lane_change_debug = module_type_->getDebugData(); for (const auto & [uuid, data] : lane_change_debug.collision_check_objects_after_approval) { @@ -171,9 +169,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() } path_reference_ = std::make_shared(out.reference_path); - const auto stop_pose_opt = module_type_->getStopPose(); - stop_pose_ = stop_pose_opt.has_value() ? PoseWithDetailOpt(PoseWithDetail(stop_pose_opt.value())) - : PoseWithDetailOpt(); + stop_pose_ = module_type_->getStopPose(); if (!module_type_->isValidPath()) { path_candidate_ = std::make_shared(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index a1c2d8467d1fc..7f0e6d7f575f6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -15,6 +15,7 @@ #include "autoware/behavior_path_lane_change_module/scene.hpp" #include "autoware/behavior_path_lane_change_module/utils/calculation.hpp" +#include "autoware/behavior_path_lane_change_module/utils/path.hpp" #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" @@ -31,9 +32,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -379,48 +380,6 @@ LaneChangePath NormalLaneChange::getLaneChangePath() const return status_.lane_change_path; } -BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const -{ - auto output = prev_module_output_; - - if (isAbortState() && abort_path_) { - output.path = abort_path_->path; - extendOutputDrivableArea(output); - const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( - output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, - output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, - planner_data_->parameters.ego_nearest_yaw_threshold); - return output; - } - - const auto & current_lanes = get_current_lanes(); - if (current_lanes.empty()) { - RCLCPP_DEBUG(logger_, "Current lanes not found. Returning previous module's path as output."); - return prev_module_output_; - } - - const auto terminal_path = - calcTerminalLaneChangePath(current_lanes, get_lane_change_lanes(current_lanes)); - if (!terminal_path) { - RCLCPP_DEBUG(logger_, "Terminal path not found. Returning previous module's path as output."); - return prev_module_output_; - } - - output.path = terminal_path->path; - output.turn_signal_info = updateOutputTurnSignal(); - - extendOutputDrivableArea(output); - - const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( - output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, - output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, - planner_data_->parameters.ego_nearest_yaw_threshold); - - return output; -} - BehaviorModuleOutput NormalLaneChange::generateOutput() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -449,7 +408,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() output.path.points, output.path.points.front().point.pose.position, getEgoPosition()); const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); - set_stop_pose(stop_dist + current_dist, output.path); + set_stop_pose(stop_dist + current_dist, output.path, "incoming rear object"); } else { insert_stop_point(get_target_lanes(), output.path); } @@ -512,7 +471,7 @@ void NormalLaneChange::insert_stop_point( if (!is_current_lane) { const auto arc_length_to_stop_pose = motion_utils::calcArcLength(path.points) - common_data_ptr_->transient_data.next_dist_buffer.min; - set_stop_pose(arc_length_to_stop_pose, path); + set_stop_pose(arc_length_to_stop_pose, path, "next lc terminal"); return; } @@ -555,8 +514,9 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) const auto dist_to_terminal_stop = std::min(dist_to_terminal_start, distance_to_last_fit_width); + const auto terminal_stop_reason = status_.is_valid_path ? "no safe path" : "no valid path"; if (filtered_objects_.current_lane.empty()) { - set_stop_pose(dist_to_terminal_stop, path); + set_stop_pose(dist_to_terminal_stop, path, terminal_stop_reason); return; } @@ -582,12 +542,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 +563,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 +882,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 @@ -1107,41 +1030,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; @@ -1199,7 +1087,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 +1126,8 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) PathWithLaneId prepare_segment; try { - if (!get_prepare_segment(prepare_segment, prep_metric.length)) { + if (!utils::lane_change::get_prepare_segment( + common_data_ptr_, prev_module_output_.path, prep_metric, prepare_segment)) { debug_print("Reject: failed to get valid prepare segment!"); continue; } @@ -1258,7 +1146,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 +1165,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 +1192,6 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) return false; } -LaneChangePath NormalLaneChange::get_candidate_path( - const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, - const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, - const Pose & lc_start_pose, const double shift_length) const -{ - const auto & route_handler = *getRouteHandler(); - const auto & target_lanes = common_data_ptr_->lanes_ptr->target; - - const auto resample_interval = - utils::lane_change::calcLaneChangeResampleInterval(lc_metrics.length, prep_metrics.velocity); - const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( - common_data_ptr_, lc_start_pose, lc_metrics.length, resample_interval); - - if (target_lane_reference_path.points.empty()) { - throw std::logic_error("target_lane_reference_path is empty!"); - } - - const auto lc_end_pose = std::invoke([&]() { - const auto dist_to_lc_start = - lanelet::utils::getArcCoordinates(target_lanes, lc_start_pose).length; - const auto dist_to_lc_end = dist_to_lc_start + lc_metrics.length; - return route_handler.get_pose_from_2d_arc_length(target_lanes, dist_to_lc_end); - }); - - const auto shift_line = utils::lane_change::get_lane_changing_shift_line( - lc_start_pose, lc_end_pose, target_lane_reference_path, shift_length); - - LaneChangeInfo lane_change_info{prep_metrics, lc_metrics, lc_start_pose, lc_end_pose, shift_line}; - - const auto candidate_path = utils::lane_change::construct_candidate_path( - common_data_ptr_, lane_change_info, prep_segment, target_lane_reference_path, sorted_lane_ids); - - if (!candidate_path) { - throw std::logic_error("failed to generate candidate path!"); - } - - if ( - candidate_path.value().info.length.sum() + - common_data_ptr_->transient_data.next_dist_buffer.min > - common_data_ptr_->transient_data.dist_to_terminal_end) { - throw std::logic_error("invalid candidate path length!"); - } - - return *candidate_path; -} - bool NormalLaneChange::check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const { @@ -1377,147 +1221,30 @@ bool NormalLaneChange::check_candidate_path_safety( throw std::logic_error("Path footprint exceeds target lane boundary. Skip lane change."); } + if ((target_objects.empty()) || candidate_path.path.points.empty()) { + RCLCPP_DEBUG(logger_, "There is nothing to check."); + return true; + } + constexpr size_t decel_sampling_num = 1; + const auto ego_predicted_paths = utils::lane_change::convert_to_predicted_paths( + common_data_ptr_, candidate_path, decel_sampling_num); + const auto safety_check_with_normal_rss = isLaneChangePathSafe( - candidate_path, target_objects, common_data_ptr_->lc_param_ptr->safety.rss_params, - decel_sampling_num, lane_change_debug_.collision_check_objects); + candidate_path, ego_predicted_paths, target_objects, + common_data_ptr_->lc_param_ptr->safety.rss_params, lane_change_debug_.collision_check_objects); if (!safety_check_with_normal_rss.is_safe && is_stuck) { const auto safety_check_with_stuck_rss = isLaneChangePathSafe( - candidate_path, target_objects, common_data_ptr_->lc_param_ptr->safety.rss_params_for_stuck, - decel_sampling_num, lane_change_debug_.collision_check_objects); + candidate_path, ego_predicted_paths, target_objects, + common_data_ptr_->lc_param_ptr->safety.rss_params_for_stuck, + lane_change_debug_.collision_check_objects); return safety_check_with_stuck_rss.is_safe; } return safety_check_with_normal_rss.is_safe; } -std::optional NormalLaneChange::calcTerminalLaneChangePath( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const -{ - const auto is_empty = [&](const auto & data, const auto & s) { - if (!data.empty()) return false; - RCLCPP_WARN(logger_, "%s is empty. Not expected.", s); - return true; - }; - - const auto target_lane_neighbors_polygon_2d = - common_data_ptr_->lanes_polygon_ptr->target_neighbor; - if ( - is_empty(current_lanes, "current_lanes") || is_empty(target_lanes, "target_lanes") || - is_empty(target_lane_neighbors_polygon_2d, "target_lane_neighbors_polygon_2d")) { - return {}; - } - - const auto & route_handler = *getRouteHandler(); - - const auto minimum_lane_changing_velocity = - lane_change_parameters_->trajectory.min_lane_changing_velocity; - - const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); - - const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; - const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; - - const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); - - // lane changing start getEgoPose() is at the end of prepare segment - const auto current_lane_terminal_point = - lanelet::utils::conversion::toGeomMsgPt(current_lanes.back().centerline3d().back()); - - double distance_to_terminal_from_goal = 0; - if (is_goal_in_route) { - distance_to_terminal_from_goal = - utils::getDistanceToEndOfLane(route_handler.getGoalPose(), current_lanes); - } - - const auto lane_changing_start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( - prev_module_output_.path.points, current_lane_terminal_point, - -(current_min_dist_buffer + next_min_dist_buffer + distance_to_terminal_from_goal)); - - if (!lane_changing_start_pose) { - RCLCPP_DEBUG(logger_, "Reject: lane changing start pose not found!!!"); - return {}; - } - - const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( - current_lanes, target_lanes.front(), lane_changing_start_pose.value()); - - // Check if the lane changing start point is not on the lanes next to target lanes, - if (target_length_from_lane_change_start_pose > 0.0) { - RCLCPP_DEBUG(logger_, "lane change start getEgoPose() is behind target lanelet!"); - return {}; - } - - const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet( - target_lanes, lane_changing_start_pose.value()); - - const auto [min_lateral_acc, max_lateral_acc] = - lane_change_parameters_->trajectory.lat_acc_map.find(minimum_lane_changing_velocity); - - const auto lane_changing_time = autoware::motion_utils::calc_shift_time_from_jerk( - shift_length, lane_change_parameters_->trajectory.lateral_jerk, max_lateral_acc); - - const auto target_lane_length = common_data_ptr_->transient_data.target_lane_length; - const auto target_segment = getTargetSegment( - target_lanes, lane_changing_start_pose.value(), target_lane_length, current_min_dist_buffer, - minimum_lane_changing_velocity, next_min_dist_buffer); - - if (target_segment.points.empty()) { - RCLCPP_DEBUG(logger_, "Reject: target segment is empty!! something wrong..."); - return {}; - } - - LaneChangeInfo lane_change_info; - lane_change_info.longitudinal_acceleration = LaneChangePhaseInfo{0.0, 0.0}; - lane_change_info.duration = LaneChangePhaseInfo{0.0, lane_changing_time}; - lane_change_info.velocity = - LaneChangePhaseInfo{minimum_lane_changing_velocity, minimum_lane_changing_velocity}; - lane_change_info.length = LaneChangePhaseInfo{0.0, current_min_dist_buffer}; - lane_change_info.lane_changing_start = lane_changing_start_pose.value(); - lane_change_info.lane_changing_end = target_segment.points.front().point.pose; - lane_change_info.lateral_acceleration = max_lateral_acc; - lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity; - - if (!is_valid_start_point(common_data_ptr_, lane_changing_start_pose.value())) { - RCLCPP_DEBUG( - logger_, - "Reject: lane changing points are not inside of the target preferred lanes or its " - "neighbors"); - return {}; - } - - const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( - current_min_dist_buffer, minimum_lane_changing_velocity); - const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( - common_data_ptr_, lane_changing_start_pose.value(), current_min_dist_buffer, resample_interval); - - if (target_lane_reference_path.points.empty()) { - RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); - return {}; - } - - lane_change_info.shift_line = utils::lane_change::get_lane_changing_shift_line( - lane_changing_start_pose.value(), target_segment.points.front().point.pose, - target_lane_reference_path, shift_length); - - auto reference_segment = prev_module_output_.path; - const double length_to_lane_changing_start = autoware::motion_utils::calcSignedArcLength( - reference_segment.points, reference_segment.points.front().point.pose.position, - lane_changing_start_pose->position); - utils::clipPathLength(reference_segment, 0, length_to_lane_changing_start, 0.0); - // remove terminal points because utils::clipPathLength() calculates extra long path - reference_segment.points.pop_back(); - reference_segment.points.back().point.longitudinal_velocity_mps = - static_cast(minimum_lane_changing_velocity); - - const auto terminal_lane_change_path = utils::lane_change::construct_candidate_path( - common_data_ptr_, lane_change_info, reference_segment, target_lane_reference_path, - sorted_lane_ids); - - return terminal_lane_change_path; -} - PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { const auto & path = status_.lane_change_path; @@ -1545,9 +1272,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 +1510,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 +1568,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 +1619,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 +1689,11 @@ bool NormalLaneChange::is_ego_stuck() const return has_object_blocking; } -bool NormalLaneChange::is_valid_start_point( - const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) const -{ - const lanelet::BasicPoint2d lc_start_point(pose.position.x, pose.position.y); - - const auto & target_neighbor_poly = common_data_ptr->lanes_polygon_ptr->target_neighbor; - const auto & target_lane_poly = common_data_ptr_->lanes_polygon_ptr->target; - - return boost::geometry::covered_by(lc_start_point, target_neighbor_poly) || - boost::geometry::covered_by(lc_start_point, target_lane_poly); -} - -void NormalLaneChange::set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path) +void NormalLaneChange::set_stop_pose( + const double arc_length_to_stop_pose, PathWithLaneId & path, const std::string & reason) { const auto stop_point = utils::insertStopPoint(arc_length_to_stop_pose, path); - lane_change_stop_pose_ = stop_point.point.pose; + lane_change_stop_pose_ = PoseWithDetailOpt(PoseWithDetail(stop_point.point.pose, reason)); } void NormalLaneChange::updateStopTime() diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 3ba11d62a2be7..550c0fa290a99 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -417,7 +417,7 @@ double calc_actual_prepare_duration( if (max_acc < eps) { return params.max_prepare_duration; } - return (min_lc_velocity - current_velocity) / max_acc; + return std::max((min_lc_velocity - current_velocity) / max_acc, params.min_prepare_duration); }); return std::max(params.max_prepare_duration - active_signal_duration, min_prepare_duration); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index c7514fea01535..30af582175d14 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 @@ -25,10 +25,11 @@ #include #include +#include + #include #include #include -#include #include #include @@ -38,34 +39,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,17 +177,10 @@ 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; - - 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; - - safety_check_info_text.text = ss.str(); + 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; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp new file mode 100644 index 0000000000000..d7303d7d1df2d --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp @@ -0,0 +1,292 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_path_lane_change_module/utils/path.hpp" + +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace +{ +using autoware::behavior_path_planner::LaneChangeInfo; +using autoware::behavior_path_planner::PathPointWithLaneId; +using autoware::behavior_path_planner::PathShifter; +using autoware::behavior_path_planner::PathWithLaneId; +using autoware::behavior_path_planner::ShiftedPath; +using autoware::behavior_path_planner::lane_change::CommonDataPtr; +using autoware::behavior_path_planner::lane_change::LCParamPtr; + +using autoware::behavior_path_planner::LaneChangePhaseMetrics; +using autoware::behavior_path_planner::ShiftLine; +using geometry_msgs::msg::Pose; + +double calc_resample_interval( + const double lane_changing_length, const double lane_changing_velocity) +{ + constexpr auto min_resampling_points{30.0}; + constexpr auto resampling_dt{0.2}; + return std::max( + lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); +} + +PathWithLaneId get_reference_path_from_target_Lane( + const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, + const double lane_changing_length, const double resample_interval) +{ + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto target_lane_length = common_data_ptr->transient_data.target_lane_length; + const auto is_goal_in_route = common_data_ptr->lanes_ptr->target_lane_in_goal_section; + const auto next_lc_buffer = common_data_ptr->transient_data.next_dist_buffer.min; + const auto forward_path_length = common_data_ptr->bpp_param_ptr->forward_path_length; + + const auto lane_change_start_arc_position = + lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); + + const double s_start = lane_change_start_arc_position.length; + const double s_end = std::invoke([&]() { + const auto dist_from_lc_start = s_start + lane_changing_length + forward_path_length; + if (is_goal_in_route) { + const double s_goal = + lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length - + next_lc_buffer; + return std::min(dist_from_lc_start, s_goal); + } + return std::min(dist_from_lc_start, target_lane_length - next_lc_buffer); + }); + + constexpr double epsilon = 1e-4; + if (s_end - s_start + epsilon < lane_changing_length) { + return PathWithLaneId(); + } + + const auto lane_changing_reference_path = + route_handler.getCenterLinePath(target_lanes, s_start, s_end); + + return autoware::behavior_path_planner::utils::resamplePathWithSpline( + lane_changing_reference_path, resample_interval, true, {0.0, lane_changing_length}); +} + +ShiftLine get_lane_changing_shift_line( + const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, + const PathWithLaneId & reference_path, const double shift_length) +{ + ShiftLine shift_line; + shift_line.end_shift_length = shift_length; + shift_line.start = lane_changing_start_pose; + shift_line.end = lane_changing_end_pose; + shift_line.start_idx = autoware::motion_utils::findNearestIndex( + reference_path.points, lane_changing_start_pose.position); + shift_line.end_idx = autoware::motion_utils::findNearestIndex( + reference_path.points, lane_changing_end_pose.position); + + return shift_line; +} + +ShiftedPath get_shifted_path( + const PathWithLaneId & target_lane_reference_path, const LaneChangeInfo & lane_change_info) +{ + const auto longitudinal_acceleration = lane_change_info.longitudinal_acceleration; + + PathShifter path_shifter; + path_shifter.setPath(target_lane_reference_path); + path_shifter.addShiftLine(lane_change_info.shift_line); + path_shifter.setLongitudinalAcceleration(longitudinal_acceleration.lane_changing); + const auto initial_lane_changing_velocity = lane_change_info.velocity.lane_changing; + path_shifter.setVelocity(initial_lane_changing_velocity); + path_shifter.setLateralAccelerationLimit(std::abs(lane_change_info.lateral_acceleration)); + + constexpr auto offset_back = false; + ShiftedPath shifted_path; + [[maybe_unused]] const auto success = path_shifter.generate(&shifted_path, offset_back); + return shifted_path; +} + +std::optional exceed_yaw_threshold( + const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, + const double yaw_th_rad) +{ + const auto & prepare = prepare_segment.points; + const auto & lane_changing = lane_changing_segment.points; + + if (prepare.size() <= 1 || lane_changing.size() <= 1) { + return std::nullopt; + } + + const auto & p1 = std::prev(prepare.end() - 1)->point.pose; + const auto & p2 = std::next(lane_changing.begin())->point.pose; + const auto yaw_diff_rad = std::abs(autoware::universe_utils::normalizeRadian( + tf2::getYaw(p1.orientation) - tf2::getYaw(p2.orientation))); + if (yaw_diff_rad > yaw_th_rad) { + return yaw_diff_rad; + } + return std::nullopt; +} +}; // namespace + +namespace autoware::behavior_path_planner::utils::lane_change +{ +using behavior_path_planner::lane_change::CommonDataPtr; + +bool get_prepare_segment( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, + const LaneChangePhaseMetrics prep_metric, PathWithLaneId & prepare_segment) +{ + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto backward_path_length = common_data_ptr->bpp_param_ptr->backward_path_length; + + if (current_lanes.empty() || target_lanes.empty()) { + throw std::logic_error("Empty lanes!"); + } + + prepare_segment = prev_module_path; + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prepare_segment.points, common_data_ptr->get_ego_pose(), 3.0, 1.0); + utils::clipPathLength(prepare_segment, current_seg_idx, prep_metric.length, backward_path_length); + + if (prepare_segment.points.empty()) return false; + + const auto & lc_start_pose = prepare_segment.points.back().point.pose; + + // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is + // larger than distance to target lane start + if (!is_valid_start_point(common_data_ptr, lc_start_pose)) return false; + + // lane changing start is at the end of prepare segment + const auto target_length_from_lane_change_start_pose = + utils::getArcLengthToTargetLanelet(current_lanes, target_lanes.front(), lc_start_pose); + + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > std::numeric_limits::epsilon()) { + throw std::logic_error("lane change start is behind target lanelet!"); + } + + return true; +} + +LaneChangePath get_candidate_path( + const CommonDataPtr & common_data_ptr, const LaneChangePhaseMetrics & prep_metric, + const LaneChangePhaseMetrics & lc_metric, const PathWithLaneId & prep_segment, + const std::vector> & sorted_lane_ids, const double shift_length) +{ + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + + const auto resample_interval = calc_resample_interval(lc_metric.length, prep_metric.velocity); + + if (prep_segment.points.empty()) { + throw std::logic_error("Empty prepare segment!"); + } + + const auto & lc_start_pose = prep_segment.points.back().point.pose; + const auto target_lane_reference_path = get_reference_path_from_target_Lane( + common_data_ptr, lc_start_pose, lc_metric.length, resample_interval); + + if (target_lane_reference_path.points.empty()) { + throw std::logic_error("Empty target reference!"); + } + + const auto lc_end_pose = std::invoke([&]() { + const auto dist_to_lc_start = + lanelet::utils::getArcCoordinates(target_lanes, lc_start_pose).length; + const auto dist_to_lc_end = dist_to_lc_start + lc_metric.length; + return route_handler.get_pose_from_2d_arc_length(target_lanes, dist_to_lc_end); + }); + + const auto shift_line = get_lane_changing_shift_line( + lc_start_pose, lc_end_pose, target_lane_reference_path, shift_length); + + LaneChangeInfo lane_change_info{prep_metric, lc_metric, lc_start_pose, lc_end_pose, shift_line}; + + if ( + lane_change_info.length.sum() + common_data_ptr->transient_data.next_dist_buffer.min > + common_data_ptr->transient_data.dist_to_terminal_end) { + throw std::logic_error("invalid candidate path length!"); + } + + return utils::lane_change::construct_candidate_path( + lane_change_info, prep_segment, target_lane_reference_path, sorted_lane_ids); +} + +LaneChangePath construct_candidate_path( + const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, + const PathWithLaneId & target_lane_reference_path, + const std::vector> & sorted_lane_ids) +{ + const auto & shift_line = lane_change_info.shift_line; + const auto terminal_lane_changing_velocity = lane_change_info.terminal_lane_changing_velocity; + + auto shifted_path = get_shifted_path(target_lane_reference_path, lane_change_info); + + if (shifted_path.path.points.empty()) { + throw std::logic_error("Failed to generate shifted path."); + } + + if (shifted_path.path.points.size() < shift_line.end_idx + 1) { + throw std::logic_error("Path points are removed by PathShifter."); + } + + const auto lc_end_idx_opt = autoware::motion_utils::findNearestIndex( + shifted_path.path.points, lane_change_info.lane_changing_end); + + if (!lc_end_idx_opt) { + throw std::logic_error("Lane change end idx not found on target path."); + } + + for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { + auto & point = shifted_path.path.points.at(i); + if (i < *lc_end_idx_opt) { + point.lane_ids = replaceWithSortedIds(point.lane_ids, sorted_lane_ids); + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, static_cast(terminal_lane_changing_velocity)); + continue; + } + const auto nearest_idx = + autoware::motion_utils::findNearestIndex(target_lane_reference_path.points, point.point.pose); + point.lane_ids = target_lane_reference_path.points.at(*nearest_idx).lane_ids; + } + + constexpr auto yaw_diff_th = autoware::universe_utils::deg2rad(5.0); + if ( + const auto yaw_diff_opt = + exceed_yaw_threshold(prepare_segment, shifted_path.path, yaw_diff_th)) { + std::stringstream err_msg; + err_msg << "Excessive yaw difference " << yaw_diff_opt.value() << " which exceeds the " + << yaw_diff_th << " radian threshold."; + throw std::logic_error(err_msg.str()); + } + + LaneChangePath candidate_path; + candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); + candidate_path.shifted_path = shifted_path; + candidate_path.info = lane_change_info; + + return candidate_path; +} +} // namespace autoware::behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 2b1c744a926e3..147ef3856ac4e 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( @@ -1294,4 +1113,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..114a38a77876c 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 @@ -258,6 +258,7 @@ TEST_F(TestNormalLaneChange, testGetPathWhenValid) constexpr auto is_approved = true; ego_pose_ = autoware::test_utils::createPose(1.0, 1.75, 0.0, 0.0, 0.0, 0.0); planner_data_->self_odometry = set_odometry(ego_pose_); + normal_lane_change_->setData(planner_data_); set_previous_approved_path(); normal_lane_change_->update_lanes(!is_approved); normal_lane_change_->update_filtered_objects(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp index 3bed1e6a88bd8..741202590779c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp @@ -25,6 +25,8 @@ #include #include +#include +#include #include #include @@ -44,8 +46,8 @@ using tier4_planning_msgs::msg::PathWithLaneId; using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; using DebugPublisher = autoware::universe_utils::DebugPublisher; -using DebugDoubleMsg = tier4_debug_msgs::msg::Float64Stamped; -using DebugStringMsg = tier4_debug_msgs::msg::StringStamped; +using DebugDoubleMsg = autoware_internal_debug_msgs::msg::Float64Stamped; +using DebugStringMsg = autoware_internal_debug_msgs::msg::StringStamped; struct SceneModuleUpdateInfo { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml index 6141480d1597a..7b7e2438a3fd6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml @@ -40,6 +40,7 @@ autoware_behavior_path_planner_common autoware_freespace_planning_algorithms autoware_frenet_planner + autoware_internal_debug_msgs autoware_interpolation autoware_lane_departure_checker autoware_lanelet2_extension diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt index 3fa8ad7218fa2..e28339fb51a6c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt @@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/freespace_pull_out.cpp src/geometric_pull_out.cpp src/shift_pull_out.cpp + src/data_structs.cpp src/util.cpp ) diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp index 9c439e9b3b921..5d8331318484d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp @@ -97,6 +97,7 @@ struct StartPlannerDebugData struct StartPlannerParameters { + static StartPlannerParameters init(rclcpp::Node & node); double th_arrived_distance{0.0}; double th_stopped_velocity{0.0}; double th_stopped_time{0.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp index d1b4c9dabdd7f..5b4f78b252d22 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp @@ -56,6 +56,8 @@ class ShiftPullOut : public PullOutPlannerBase std::shared_ptr lane_departure_checker_; + friend class TestShiftPullOut; + private: // Calculate longitudinal distance based on the acceleration limit, curvature limit, and the // minimum distance requirement. diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp new file mode 100644 index 0000000000000..f613b9345ce42 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp @@ -0,0 +1,340 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_path_start_planner_module/data_structs.hpp" + +#include "autoware/behavior_path_start_planner_module/manager.hpp" + +#include +#include + +#include + +namespace autoware::behavior_path_planner +{ + +StartPlannerParameters StartPlannerParameters::init(rclcpp::Node & node) +{ + using autoware::universe_utils::getOrDeclareParameter; + StartPlannerParameters p; + { + const std::string ns = "start_planner."; + + p.th_arrived_distance = getOrDeclareParameter(node, ns + "th_arrived_distance"); + p.th_stopped_velocity = getOrDeclareParameter(node, ns + "th_stopped_velocity"); + p.th_stopped_time = getOrDeclareParameter(node, ns + "th_stopped_time"); + p.prepare_time_before_start = + getOrDeclareParameter(node, ns + "prepare_time_before_start"); + p.th_distance_to_middle_of_the_road = + getOrDeclareParameter(node, ns + "th_distance_to_middle_of_the_road"); + p.skip_rear_vehicle_check = getOrDeclareParameter(node, ns + "skip_rear_vehicle_check"); + p.extra_width_margin_for_rear_obstacle = + getOrDeclareParameter(node, ns + "extra_width_margin_for_rear_obstacle"); + p.collision_check_margins = + getOrDeclareParameter>(node, ns + "collision_check_margins"); + p.collision_check_margin_from_front_object = + getOrDeclareParameter(node, ns + "collision_check_margin_from_front_object"); + p.th_moving_object_velocity = + getOrDeclareParameter(node, ns + "th_moving_object_velocity"); + p.center_line_path_interval = + getOrDeclareParameter(node, ns + "center_line_path_interval"); + // shift pull out + p.enable_shift_pull_out = getOrDeclareParameter(node, ns + "enable_shift_pull_out"); + p.check_shift_path_lane_departure = + getOrDeclareParameter(node, ns + "check_shift_path_lane_departure"); + p.allow_check_shift_path_lane_departure_override = + getOrDeclareParameter(node, ns + "allow_check_shift_path_lane_departure_override"); + p.shift_collision_check_distance_from_end = + getOrDeclareParameter(node, ns + "shift_collision_check_distance_from_end"); + p.minimum_shift_pull_out_distance = + getOrDeclareParameter(node, ns + "minimum_shift_pull_out_distance"); + p.lateral_acceleration_sampling_num = + getOrDeclareParameter(node, ns + "lateral_acceleration_sampling_num"); + p.lateral_jerk = getOrDeclareParameter(node, ns + "lateral_jerk"); + p.maximum_lateral_acc = getOrDeclareParameter(node, ns + "maximum_lateral_acc"); + p.minimum_lateral_acc = getOrDeclareParameter(node, ns + "minimum_lateral_acc"); + p.maximum_curvature = getOrDeclareParameter(node, ns + "maximum_curvature"); + p.end_pose_curvature_threshold = + getOrDeclareParameter(node, ns + "end_pose_curvature_threshold"); + p.maximum_longitudinal_deviation = + getOrDeclareParameter(node, ns + "maximum_longitudinal_deviation"); + // geometric pull out + p.enable_geometric_pull_out = + getOrDeclareParameter(node, ns + "enable_geometric_pull_out"); + p.geometric_collision_check_distance_from_end = + getOrDeclareParameter(node, ns + "geometric_collision_check_distance_from_end"); + p.divide_pull_out_path = getOrDeclareParameter(node, ns + "divide_pull_out_path"); + p.parallel_parking_parameters.pull_out_velocity = + getOrDeclareParameter(node, ns + "geometric_pull_out_velocity"); + p.parallel_parking_parameters.pull_out_arc_path_interval = + getOrDeclareParameter(node, ns + "arc_path_interval"); + p.parallel_parking_parameters.pull_out_lane_departure_margin = + getOrDeclareParameter(node, ns + "lane_departure_margin"); + p.lane_departure_check_expansion_margin = + getOrDeclareParameter(node, ns + "lane_departure_check_expansion_margin"); + p.parallel_parking_parameters.pull_out_max_steer_angle = + getOrDeclareParameter(node, ns + "pull_out_max_steer_angle"); // 15deg + p.parallel_parking_parameters.center_line_path_interval = + p.center_line_path_interval; // for geometric parallel parking + // search start pose backward + p.search_priority = getOrDeclareParameter( + node, + ns + "search_priority"); // "efficient_path" or "short_back_distance" + p.enable_back = getOrDeclareParameter(node, ns + "enable_back"); + p.backward_velocity = getOrDeclareParameter(node, ns + "backward_velocity"); + p.max_back_distance = getOrDeclareParameter(node, ns + "max_back_distance"); + p.backward_search_resolution = + getOrDeclareParameter(node, ns + "backward_search_resolution"); + p.backward_path_update_duration = + getOrDeclareParameter(node, ns + "backward_path_update_duration"); + p.ignore_distance_from_lane_end = + getOrDeclareParameter(node, ns + "ignore_distance_from_lane_end"); + // stop condition + p.maximum_deceleration_for_stop = + getOrDeclareParameter(node, ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + getOrDeclareParameter(node, ns + "stop_condition.maximum_jerk_for_stop"); + } + { + const std::string ns = "start_planner.object_types_to_check_for_path_generation."; + p.object_types_to_check_for_path_generation.check_car = + getOrDeclareParameter(node, ns + "check_car"); + p.object_types_to_check_for_path_generation.check_truck = + getOrDeclareParameter(node, ns + "check_truck"); + p.object_types_to_check_for_path_generation.check_bus = + getOrDeclareParameter(node, ns + "check_bus"); + p.object_types_to_check_for_path_generation.check_trailer = + getOrDeclareParameter(node, ns + "check_trailer"); + p.object_types_to_check_for_path_generation.check_unknown = + getOrDeclareParameter(node, ns + "check_unknown"); + p.object_types_to_check_for_path_generation.check_bicycle = + getOrDeclareParameter(node, ns + "check_bicycle"); + p.object_types_to_check_for_path_generation.check_motorcycle = + getOrDeclareParameter(node, ns + "check_motorcycle"); + p.object_types_to_check_for_path_generation.check_pedestrian = + getOrDeclareParameter(node, ns + "check_pedestrian"); + } + // freespace planner general params + { + const std::string ns = "start_planner.freespace_planner."; + p.enable_freespace_planner = getOrDeclareParameter(node, ns + "enable_freespace_planner"); + p.freespace_planner_algorithm = + getOrDeclareParameter(node, ns + "freespace_planner_algorithm"); + p.end_pose_search_start_distance = + getOrDeclareParameter(node, ns + "end_pose_search_start_distance"); + p.end_pose_search_end_distance = + getOrDeclareParameter(node, ns + "end_pose_search_end_distance"); + p.end_pose_search_interval = + getOrDeclareParameter(node, ns + "end_pose_search_interval"); + p.freespace_planner_velocity = getOrDeclareParameter(node, ns + "velocity"); + p.vehicle_shape_margin = getOrDeclareParameter(node, ns + "vehicle_shape_margin"); + p.freespace_planner_common_parameters.time_limit = + getOrDeclareParameter(node, ns + "time_limit"); + p.freespace_planner_common_parameters.max_turning_ratio = + getOrDeclareParameter(node, ns + "max_turning_ratio"); + p.freespace_planner_common_parameters.turning_steps = + getOrDeclareParameter(node, ns + "turning_steps"); + } + // freespace planner search config + { + const std::string ns = "start_planner.freespace_planner.search_configs."; + p.freespace_planner_common_parameters.theta_size = + getOrDeclareParameter(node, ns + "theta_size"); + p.freespace_planner_common_parameters.angle_goal_range = + getOrDeclareParameter(node, ns + "angle_goal_range"); + p.freespace_planner_common_parameters.curve_weight = + getOrDeclareParameter(node, ns + "curve_weight"); + p.freespace_planner_common_parameters.reverse_weight = + getOrDeclareParameter(node, ns + "reverse_weight"); + p.freespace_planner_common_parameters.lateral_goal_range = + getOrDeclareParameter(node, ns + "lateral_goal_range"); + p.freespace_planner_common_parameters.longitudinal_goal_range = + getOrDeclareParameter(node, ns + "longitudinal_goal_range"); + } + // freespace planner costmap configs + { + const std::string ns = "start_planner.freespace_planner.costmap_configs."; + p.freespace_planner_common_parameters.obstacle_threshold = + getOrDeclareParameter(node, ns + "obstacle_threshold"); + } + // freespace planner astar + { + const std::string ns = "start_planner.freespace_planner.astar."; + p.astar_parameters.search_method = + getOrDeclareParameter(node, ns + "search_method"); + p.astar_parameters.only_behind_solutions = + getOrDeclareParameter(node, ns + "only_behind_solutions"); + p.astar_parameters.use_back = getOrDeclareParameter(node, ns + "use_back"); + p.astar_parameters.distance_heuristic_weight = + getOrDeclareParameter(node, ns + "distance_heuristic_weight"); + } + // freespace planner rrtstar + { + const std::string ns = "start_planner.freespace_planner.rrtstar."; + p.rrt_star_parameters.enable_update = getOrDeclareParameter(node, ns + "enable_update"); + p.rrt_star_parameters.use_informed_sampling = + getOrDeclareParameter(node, ns + "use_informed_sampling"); + p.rrt_star_parameters.max_planning_time = + getOrDeclareParameter(node, ns + "max_planning_time"); + p.rrt_star_parameters.neighbor_radius = + getOrDeclareParameter(node, ns + "neighbor_radius"); + p.rrt_star_parameters.margin = getOrDeclareParameter(node, ns + "margin"); + } + + const std::string base_ns = "start_planner.path_safety_check."; + // EgoPredictedPath + { + const std::string ego_path_ns = base_ns + "ego_predicted_path."; + p.ego_predicted_path_params.min_velocity = + getOrDeclareParameter(node, ego_path_ns + "min_velocity"); + p.ego_predicted_path_params.acceleration = + getOrDeclareParameter(node, ego_path_ns + "min_acceleration"); + p.ego_predicted_path_params.time_horizon_for_front_object = + getOrDeclareParameter(node, ego_path_ns + "time_horizon_for_front_object"); + p.ego_predicted_path_params.time_horizon_for_rear_object = + getOrDeclareParameter(node, ego_path_ns + "time_horizon_for_rear_object"); + p.ego_predicted_path_params.time_resolution = + getOrDeclareParameter(node, ego_path_ns + "time_resolution"); + p.ego_predicted_path_params.delay_until_departure = + getOrDeclareParameter(node, ego_path_ns + "delay_until_departure"); + } + // ObjectFilteringParams + const std::string obj_filter_ns = base_ns + "target_filtering."; + { + p.objects_filtering_params.safety_check_time_horizon = + getOrDeclareParameter(node, obj_filter_ns + "safety_check_time_horizon"); + p.objects_filtering_params.safety_check_time_resolution = + getOrDeclareParameter(node, obj_filter_ns + "safety_check_time_resolution"); + p.objects_filtering_params.object_check_forward_distance = + getOrDeclareParameter(node, obj_filter_ns + "object_check_forward_distance"); + p.objects_filtering_params.object_check_backward_distance = + getOrDeclareParameter(node, obj_filter_ns + "object_check_backward_distance"); + p.objects_filtering_params.ignore_object_velocity_threshold = + getOrDeclareParameter(node, obj_filter_ns + "ignore_object_velocity_threshold"); + p.objects_filtering_params.include_opposite_lane = + getOrDeclareParameter(node, obj_filter_ns + "include_opposite_lane"); + p.objects_filtering_params.invert_opposite_lane = + getOrDeclareParameter(node, obj_filter_ns + "invert_opposite_lane"); + p.objects_filtering_params.check_all_predicted_path = + getOrDeclareParameter(node, obj_filter_ns + "check_all_predicted_path"); + p.objects_filtering_params.use_all_predicted_path = + getOrDeclareParameter(node, obj_filter_ns + "use_all_predicted_path"); + p.objects_filtering_params.use_predicted_path_outside_lanelet = + getOrDeclareParameter(node, obj_filter_ns + "use_predicted_path_outside_lanelet"); + } + // ObjectTypesToCheck + { + const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + p.objects_filtering_params.object_types_to_check.check_car = + getOrDeclareParameter(node, obj_types_ns + "check_car"); + p.objects_filtering_params.object_types_to_check.check_truck = + getOrDeclareParameter(node, obj_types_ns + "check_truck"); + p.objects_filtering_params.object_types_to_check.check_bus = + getOrDeclareParameter(node, obj_types_ns + "check_bus"); + p.objects_filtering_params.object_types_to_check.check_trailer = + getOrDeclareParameter(node, obj_types_ns + "check_trailer"); + p.objects_filtering_params.object_types_to_check.check_unknown = + getOrDeclareParameter(node, obj_types_ns + "check_unknown"); + p.objects_filtering_params.object_types_to_check.check_bicycle = + getOrDeclareParameter(node, obj_types_ns + "check_bicycle"); + p.objects_filtering_params.object_types_to_check.check_motorcycle = + getOrDeclareParameter(node, obj_types_ns + "check_motorcycle"); + p.objects_filtering_params.object_types_to_check.check_pedestrian = + getOrDeclareParameter(node, obj_types_ns + "check_pedestrian"); + } + // ObjectLaneConfiguration + { + const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + p.objects_filtering_params.object_lane_configuration.check_current_lane = + getOrDeclareParameter(node, obj_lane_ns + "check_current_lane"); + p.objects_filtering_params.object_lane_configuration.check_right_lane = + getOrDeclareParameter(node, obj_lane_ns + "check_right_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_left_lane = + getOrDeclareParameter(node, obj_lane_ns + "check_left_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = + getOrDeclareParameter(node, obj_lane_ns + "check_shoulder_lane"); + p.objects_filtering_params.object_lane_configuration.check_other_lane = + getOrDeclareParameter(node, obj_lane_ns + "check_other_lane"); + } + // SafetyCheckParams + const std::string safety_check_ns = base_ns + "safety_check_params."; + { + p.safety_check_params.enable_safety_check = + getOrDeclareParameter(node, safety_check_ns + "enable_safety_check"); + p.safety_check_params.hysteresis_factor_expand_rate = + getOrDeclareParameter(node, safety_check_ns + "hysteresis_factor_expand_rate"); + p.safety_check_params.backward_path_length = + getOrDeclareParameter(node, safety_check_ns + "backward_path_length"); + p.safety_check_params.forward_path_length = + getOrDeclareParameter(node, safety_check_ns + "forward_path_length"); + p.safety_check_params.publish_debug_marker = + getOrDeclareParameter(node, safety_check_ns + "publish_debug_marker"); + p.safety_check_params.collision_check_yaw_diff_threshold = + getOrDeclareParameter(node, safety_check_ns + "collision_check_yaw_diff_threshold"); + } + // RSSparams + { + const std::string rss_ns = safety_check_ns + "rss_params."; + p.safety_check_params.rss_params.rear_vehicle_reaction_time = + getOrDeclareParameter(node, rss_ns + "rear_vehicle_reaction_time"); + p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = + getOrDeclareParameter(node, rss_ns + "rear_vehicle_safety_time_margin"); + p.safety_check_params.rss_params.lateral_distance_max_threshold = + getOrDeclareParameter(node, rss_ns + "lateral_distance_max_threshold"); + p.safety_check_params.rss_params.longitudinal_distance_min_threshold = + getOrDeclareParameter(node, rss_ns + "longitudinal_distance_min_threshold"); + p.safety_check_params.rss_params.longitudinal_velocity_delta_time = + getOrDeclareParameter(node, rss_ns + "longitudinal_velocity_delta_time"); + p.safety_check_params.rss_params.extended_polygon_policy = + getOrDeclareParameter(node, rss_ns + "extended_polygon_policy"); + } + // surround moving obstacle check + { + const std::string surround_moving_obstacle_check_ns = + "start_planner.surround_moving_obstacle_check."; + p.search_radius = + getOrDeclareParameter(node, surround_moving_obstacle_check_ns + "search_radius"); + p.th_moving_obstacle_velocity = getOrDeclareParameter( + node, surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity"); + // ObjectTypesToCheck + { + const std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; + p.surround_moving_obstacles_type_to_check.check_car = + getOrDeclareParameter(node, obj_types_ns + "check_car"); + p.surround_moving_obstacles_type_to_check.check_truck = + getOrDeclareParameter(node, obj_types_ns + "check_truck"); + p.surround_moving_obstacles_type_to_check.check_bus = + getOrDeclareParameter(node, obj_types_ns + "check_bus"); + p.surround_moving_obstacles_type_to_check.check_trailer = + getOrDeclareParameter(node, obj_types_ns + "check_trailer"); + p.surround_moving_obstacles_type_to_check.check_unknown = + getOrDeclareParameter(node, obj_types_ns + "check_unknown"); + p.surround_moving_obstacles_type_to_check.check_bicycle = + getOrDeclareParameter(node, obj_types_ns + "check_bicycle"); + p.surround_moving_obstacles_type_to_check.check_motorcycle = + getOrDeclareParameter(node, obj_types_ns + "check_motorcycle"); + p.surround_moving_obstacles_type_to_check.check_pedestrian = + getOrDeclareParameter(node, obj_types_ns + "check_pedestrian"); + } + } + + // debug + { + const std::string debug_ns = "start_planner.debug."; + p.print_debug_info = getOrDeclareParameter(node, debug_ns + "print_debug_info"); + } + + return p; +} +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp index f5739ce322248..9ea51d56ee1cc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -29,318 +29,18 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) // init manager interface initInterface(node, {""}); - StartPlannerParameters p; - - { - const std::string ns = "start_planner."; - - p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); - p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); - p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); - p.prepare_time_before_start = node->declare_parameter(ns + "prepare_time_before_start"); - p.th_distance_to_middle_of_the_road = - node->declare_parameter(ns + "th_distance_to_middle_of_the_road"); - p.skip_rear_vehicle_check = node->declare_parameter(ns + "skip_rear_vehicle_check"); - p.extra_width_margin_for_rear_obstacle = - node->declare_parameter(ns + "extra_width_margin_for_rear_obstacle"); - p.collision_check_margins = - node->declare_parameter>(ns + "collision_check_margins"); - p.collision_check_margin_from_front_object = - node->declare_parameter(ns + "collision_check_margin_from_front_object"); - p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); - p.center_line_path_interval = node->declare_parameter(ns + "center_line_path_interval"); - // shift pull out - p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); - p.check_shift_path_lane_departure = - node->declare_parameter(ns + "check_shift_path_lane_departure"); - p.allow_check_shift_path_lane_departure_override = - node->declare_parameter(ns + "allow_check_shift_path_lane_departure_override"); - p.shift_collision_check_distance_from_end = - node->declare_parameter(ns + "shift_collision_check_distance_from_end"); - p.minimum_shift_pull_out_distance = - node->declare_parameter(ns + "minimum_shift_pull_out_distance"); - p.lateral_acceleration_sampling_num = - node->declare_parameter(ns + "lateral_acceleration_sampling_num"); - p.lateral_jerk = node->declare_parameter(ns + "lateral_jerk"); - p.maximum_lateral_acc = node->declare_parameter(ns + "maximum_lateral_acc"); - p.minimum_lateral_acc = node->declare_parameter(ns + "minimum_lateral_acc"); - p.maximum_curvature = node->declare_parameter(ns + "maximum_curvature"); - p.end_pose_curvature_threshold = - node->declare_parameter(ns + "end_pose_curvature_threshold"); - p.maximum_longitudinal_deviation = - node->declare_parameter(ns + "maximum_longitudinal_deviation"); - // geometric pull out - p.enable_geometric_pull_out = node->declare_parameter(ns + "enable_geometric_pull_out"); - p.geometric_collision_check_distance_from_end = - node->declare_parameter(ns + "geometric_collision_check_distance_from_end"); - p.divide_pull_out_path = node->declare_parameter(ns + "divide_pull_out_path"); - p.parallel_parking_parameters.pull_out_velocity = - node->declare_parameter(ns + "geometric_pull_out_velocity"); - p.parallel_parking_parameters.pull_out_arc_path_interval = - node->declare_parameter(ns + "arc_path_interval"); - p.parallel_parking_parameters.pull_out_lane_departure_margin = - node->declare_parameter(ns + "lane_departure_margin"); - p.lane_departure_check_expansion_margin = - node->declare_parameter(ns + "lane_departure_check_expansion_margin"); - p.parallel_parking_parameters.pull_out_max_steer_angle = - node->declare_parameter(ns + "pull_out_max_steer_angle"); // 15deg - p.parallel_parking_parameters.center_line_path_interval = - p.center_line_path_interval; // for geometric parallel parking - // search start pose backward - p.search_priority = node->declare_parameter( - ns + "search_priority"); // "efficient_path" or "short_back_distance" - p.enable_back = node->declare_parameter(ns + "enable_back"); - p.backward_velocity = node->declare_parameter(ns + "backward_velocity"); - p.max_back_distance = node->declare_parameter(ns + "max_back_distance"); - p.backward_search_resolution = - node->declare_parameter(ns + "backward_search_resolution"); - p.backward_path_update_duration = - node->declare_parameter(ns + "backward_path_update_duration"); - p.ignore_distance_from_lane_end = - node->declare_parameter(ns + "ignore_distance_from_lane_end"); - // stop condition - p.maximum_deceleration_for_stop = - node->declare_parameter(ns + "stop_condition.maximum_deceleration_for_stop"); - p.maximum_jerk_for_stop = - node->declare_parameter(ns + "stop_condition.maximum_jerk_for_stop"); - } - { - const std::string ns = "start_planner.object_types_to_check_for_path_generation."; - p.object_types_to_check_for_path_generation.check_car = - node->declare_parameter(ns + "check_car"); - p.object_types_to_check_for_path_generation.check_truck = - node->declare_parameter(ns + "check_truck"); - p.object_types_to_check_for_path_generation.check_bus = - node->declare_parameter(ns + "check_bus"); - p.object_types_to_check_for_path_generation.check_trailer = - node->declare_parameter(ns + "check_trailer"); - p.object_types_to_check_for_path_generation.check_unknown = - node->declare_parameter(ns + "check_unknown"); - p.object_types_to_check_for_path_generation.check_bicycle = - node->declare_parameter(ns + "check_bicycle"); - p.object_types_to_check_for_path_generation.check_motorcycle = - node->declare_parameter(ns + "check_motorcycle"); - p.object_types_to_check_for_path_generation.check_pedestrian = - node->declare_parameter(ns + "check_pedestrian"); - } - // freespace planner general params - { - const std::string ns = "start_planner.freespace_planner."; - p.enable_freespace_planner = node->declare_parameter(ns + "enable_freespace_planner"); - p.freespace_planner_algorithm = - node->declare_parameter(ns + "freespace_planner_algorithm"); - p.end_pose_search_start_distance = - node->declare_parameter(ns + "end_pose_search_start_distance"); - p.end_pose_search_end_distance = - node->declare_parameter(ns + "end_pose_search_end_distance"); - p.end_pose_search_interval = node->declare_parameter(ns + "end_pose_search_interval"); - p.freespace_planner_velocity = node->declare_parameter(ns + "velocity"); - p.vehicle_shape_margin = node->declare_parameter(ns + "vehicle_shape_margin"); - p.freespace_planner_common_parameters.time_limit = - node->declare_parameter(ns + "time_limit"); - p.freespace_planner_common_parameters.max_turning_ratio = - node->declare_parameter(ns + "max_turning_ratio"); - p.freespace_planner_common_parameters.turning_steps = - node->declare_parameter(ns + "turning_steps"); - } - // freespace planner search config - { - const std::string ns = "start_planner.freespace_planner.search_configs."; - p.freespace_planner_common_parameters.theta_size = - node->declare_parameter(ns + "theta_size"); - p.freespace_planner_common_parameters.angle_goal_range = - node->declare_parameter(ns + "angle_goal_range"); - p.freespace_planner_common_parameters.curve_weight = - node->declare_parameter(ns + "curve_weight"); - p.freespace_planner_common_parameters.reverse_weight = - node->declare_parameter(ns + "reverse_weight"); - p.freespace_planner_common_parameters.lateral_goal_range = - node->declare_parameter(ns + "lateral_goal_range"); - p.freespace_planner_common_parameters.longitudinal_goal_range = - node->declare_parameter(ns + "longitudinal_goal_range"); - } - // freespace planner costmap configs - { - const std::string ns = "start_planner.freespace_planner.costmap_configs."; - p.freespace_planner_common_parameters.obstacle_threshold = - node->declare_parameter(ns + "obstacle_threshold"); - } - // freespace planner astar - { - const std::string ns = "start_planner.freespace_planner.astar."; - p.astar_parameters.search_method = node->declare_parameter(ns + "search_method"); - p.astar_parameters.only_behind_solutions = - node->declare_parameter(ns + "only_behind_solutions"); - p.astar_parameters.use_back = node->declare_parameter(ns + "use_back"); - p.astar_parameters.distance_heuristic_weight = - node->declare_parameter(ns + "distance_heuristic_weight"); - } - // freespace planner rrtstar - { - const std::string ns = "start_planner.freespace_planner.rrtstar."; - p.rrt_star_parameters.enable_update = node->declare_parameter(ns + "enable_update"); - p.rrt_star_parameters.use_informed_sampling = - node->declare_parameter(ns + "use_informed_sampling"); - p.rrt_star_parameters.max_planning_time = - node->declare_parameter(ns + "max_planning_time"); - p.rrt_star_parameters.neighbor_radius = node->declare_parameter(ns + "neighbor_radius"); - p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); - } - - const std::string base_ns = "start_planner.path_safety_check."; - // EgoPredictedPath - { - const std::string ego_path_ns = base_ns + "ego_predicted_path."; - p.ego_predicted_path_params.min_velocity = - node->declare_parameter(ego_path_ns + "min_velocity"); - p.ego_predicted_path_params.acceleration = - node->declare_parameter(ego_path_ns + "min_acceleration"); - p.ego_predicted_path_params.time_horizon_for_front_object = - node->declare_parameter(ego_path_ns + "time_horizon_for_front_object"); - p.ego_predicted_path_params.time_horizon_for_rear_object = - node->declare_parameter(ego_path_ns + "time_horizon_for_rear_object"); - p.ego_predicted_path_params.time_resolution = - node->declare_parameter(ego_path_ns + "time_resolution"); - p.ego_predicted_path_params.delay_until_departure = - node->declare_parameter(ego_path_ns + "delay_until_departure"); - } - // ObjectFilteringParams - const std::string obj_filter_ns = base_ns + "target_filtering."; - { - p.objects_filtering_params.safety_check_time_horizon = - node->declare_parameter(obj_filter_ns + "safety_check_time_horizon"); - p.objects_filtering_params.safety_check_time_resolution = - node->declare_parameter(obj_filter_ns + "safety_check_time_resolution"); - p.objects_filtering_params.object_check_forward_distance = - node->declare_parameter(obj_filter_ns + "object_check_forward_distance"); - p.objects_filtering_params.object_check_backward_distance = - node->declare_parameter(obj_filter_ns + "object_check_backward_distance"); - p.objects_filtering_params.ignore_object_velocity_threshold = - node->declare_parameter(obj_filter_ns + "ignore_object_velocity_threshold"); - p.objects_filtering_params.include_opposite_lane = - node->declare_parameter(obj_filter_ns + "include_opposite_lane"); - p.objects_filtering_params.invert_opposite_lane = - node->declare_parameter(obj_filter_ns + "invert_opposite_lane"); - p.objects_filtering_params.check_all_predicted_path = - node->declare_parameter(obj_filter_ns + "check_all_predicted_path"); - p.objects_filtering_params.use_all_predicted_path = - node->declare_parameter(obj_filter_ns + "use_all_predicted_path"); - p.objects_filtering_params.use_predicted_path_outside_lanelet = - node->declare_parameter(obj_filter_ns + "use_predicted_path_outside_lanelet"); - } - // ObjectTypesToCheck - { - const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; - p.objects_filtering_params.object_types_to_check.check_car = - node->declare_parameter(obj_types_ns + "check_car"); - p.objects_filtering_params.object_types_to_check.check_truck = - node->declare_parameter(obj_types_ns + "check_truck"); - p.objects_filtering_params.object_types_to_check.check_bus = - node->declare_parameter(obj_types_ns + "check_bus"); - p.objects_filtering_params.object_types_to_check.check_trailer = - node->declare_parameter(obj_types_ns + "check_trailer"); - p.objects_filtering_params.object_types_to_check.check_unknown = - node->declare_parameter(obj_types_ns + "check_unknown"); - p.objects_filtering_params.object_types_to_check.check_bicycle = - node->declare_parameter(obj_types_ns + "check_bicycle"); - p.objects_filtering_params.object_types_to_check.check_motorcycle = - node->declare_parameter(obj_types_ns + "check_motorcycle"); - p.objects_filtering_params.object_types_to_check.check_pedestrian = - node->declare_parameter(obj_types_ns + "check_pedestrian"); - } - // ObjectLaneConfiguration - { - const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; - p.objects_filtering_params.object_lane_configuration.check_current_lane = - node->declare_parameter(obj_lane_ns + "check_current_lane"); - p.objects_filtering_params.object_lane_configuration.check_right_lane = - node->declare_parameter(obj_lane_ns + "check_right_side_lane"); - p.objects_filtering_params.object_lane_configuration.check_left_lane = - node->declare_parameter(obj_lane_ns + "check_left_side_lane"); - p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = - node->declare_parameter(obj_lane_ns + "check_shoulder_lane"); - p.objects_filtering_params.object_lane_configuration.check_other_lane = - node->declare_parameter(obj_lane_ns + "check_other_lane"); - } - // SafetyCheckParams - const std::string safety_check_ns = base_ns + "safety_check_params."; - { - p.safety_check_params.enable_safety_check = - node->declare_parameter(safety_check_ns + "enable_safety_check"); - p.safety_check_params.hysteresis_factor_expand_rate = - node->declare_parameter(safety_check_ns + "hysteresis_factor_expand_rate"); - p.safety_check_params.backward_path_length = - node->declare_parameter(safety_check_ns + "backward_path_length"); - p.safety_check_params.forward_path_length = - node->declare_parameter(safety_check_ns + "forward_path_length"); - p.safety_check_params.publish_debug_marker = - node->declare_parameter(safety_check_ns + "publish_debug_marker"); - p.safety_check_params.collision_check_yaw_diff_threshold = - node->declare_parameter(safety_check_ns + "collision_check_yaw_diff_threshold"); - } - // RSSparams - { - const std::string rss_ns = safety_check_ns + "rss_params."; - p.safety_check_params.rss_params.rear_vehicle_reaction_time = - node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); - p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = - node->declare_parameter(rss_ns + "rear_vehicle_safety_time_margin"); - p.safety_check_params.rss_params.lateral_distance_max_threshold = - node->declare_parameter(rss_ns + "lateral_distance_max_threshold"); - p.safety_check_params.rss_params.longitudinal_distance_min_threshold = - node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); - p.safety_check_params.rss_params.longitudinal_velocity_delta_time = - node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); - p.safety_check_params.rss_params.extended_polygon_policy = - node->declare_parameter(rss_ns + "extended_polygon_policy"); - } - // surround moving obstacle check - { - const std::string surround_moving_obstacle_check_ns = - "start_planner.surround_moving_obstacle_check."; - p.search_radius = - node->declare_parameter(surround_moving_obstacle_check_ns + "search_radius"); - p.th_moving_obstacle_velocity = node->declare_parameter( - surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity"); - // ObjectTypesToCheck - { - const std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; - p.surround_moving_obstacles_type_to_check.check_car = - node->declare_parameter(obj_types_ns + "check_car"); - p.surround_moving_obstacles_type_to_check.check_truck = - node->declare_parameter(obj_types_ns + "check_truck"); - p.surround_moving_obstacles_type_to_check.check_bus = - node->declare_parameter(obj_types_ns + "check_bus"); - p.surround_moving_obstacles_type_to_check.check_trailer = - node->declare_parameter(obj_types_ns + "check_trailer"); - p.surround_moving_obstacles_type_to_check.check_unknown = - node->declare_parameter(obj_types_ns + "check_unknown"); - p.surround_moving_obstacles_type_to_check.check_bicycle = - node->declare_parameter(obj_types_ns + "check_bicycle"); - p.surround_moving_obstacles_type_to_check.check_motorcycle = - node->declare_parameter(obj_types_ns + "check_motorcycle"); - p.surround_moving_obstacles_type_to_check.check_pedestrian = - node->declare_parameter(obj_types_ns + "check_pedestrian"); - } - } - - // debug - { - const std::string debug_ns = "start_planner.debug."; - p.print_debug_info = node->declare_parameter(debug_ns + "print_debug_info"); - } - + StartPlannerParameters parameters = StartPlannerParameters::init(*node); // validation of parameters - if (p.lateral_acceleration_sampling_num < 1) { + if (parameters.lateral_acceleration_sampling_num < 1) { RCLCPP_FATAL_STREAM( node->get_logger().get_child(name()), "lateral_acceleration_sampling_num must be positive integer. Given parameter: " - << p.lateral_acceleration_sampling_num << std::endl + << parameters.lateral_acceleration_sampling_num << std::endl << "Terminating the program..."); exit(EXIT_FAILURE); } - parameters_ = std::make_shared(p); + parameters_ = std::make_shared(parameters); } void StartPlannerModuleManager::updateModuleParams( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp new file mode 100644 index 0000000000000..156cf62f9ac4a --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp @@ -0,0 +1,179 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +using autoware::behavior_path_planner::ShiftPullOut; +using autoware::behavior_path_planner::StartPlannerParameters; +using autoware::lane_departure_checker::LaneDepartureChecker; +using autoware::test_utils::get_absolute_path_to_config; +using autoware_planning_msgs::msg::LaneletRoute; +using RouteSections = std::vector; +using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; + +namespace autoware::behavior_path_planner +{ + +class TestShiftPullOut : public ::testing::Test +{ +public: + std::optional call_plan( + const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) + { + return shift_pull_out_->plan(start_pose, goal_pose, planner_debug_data); + } + +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + node_ = rclcpp::Node::make_shared("shift_pull_out", make_node_options()); + + initialize_lane_departure_checker(); + initialize_shift_pull_out_planner(); + } + + void TearDown() override { rclcpp::shutdown(); } + + PlannerData make_planner_data( + const Pose & start_pose, const int route_start_lane_id, const int route_goal_lane_id) + { + PlannerData planner_data; + planner_data.init_parameters(*node_); + + // Load a sample lanelet map and create a route handler + const auto shoulder_map_path = autoware::test_utils::get_absolute_path_to_lanelet_map( + "autoware_test_utils", "road_shoulder/lanelet2_map.osm"); + const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(shoulder_map_path, 0.5); + auto route_handler = std::make_shared(map_bin_msg); + + // Set up current odometry at start pose + auto odometry = std::make_shared(); + odometry->pose.pose = start_pose; + odometry->header.frame_id = "map"; + planner_data.self_odometry = odometry; + + // Setup route + const auto route = makeBehaviorRouteFromLaneId( + route_start_lane_id, route_goal_lane_id, "autoware_test_utils", + "road_shoulder/lanelet2_map.osm"); + route_handler->setRoute(route); + + // Update planner data with the route handler + planner_data.route_handler = route_handler; + + return planner_data; + } + + // Member variables + std::shared_ptr node_; + std::shared_ptr shift_pull_out_; + std::shared_ptr lane_departure_checker_; + +private: + rclcpp::NodeOptions make_node_options() const + { + // Load common configuration files + auto node_options = rclcpp::NodeOptions{}; + + const auto common_param_path = + get_absolute_path_to_config("autoware_test_utils", "test_common.param.yaml"); + const auto nearest_search_param_path = + get_absolute_path_to_config("autoware_test_utils", "test_nearest_search.param.yaml"); + const auto vehicle_info_param_path = + get_absolute_path_to_config("autoware_test_utils", "test_vehicle_info.param.yaml"); + const auto behavior_path_planner_param_path = get_absolute_path_to_config( + "autoware_behavior_path_planner", "behavior_path_planner.param.yaml"); + const auto drivable_area_expansion_param_path = get_absolute_path_to_config( + "autoware_behavior_path_planner", "drivable_area_expansion.param.yaml"); + const auto scene_module_manager_param_path = get_absolute_path_to_config( + "autoware_behavior_path_planner", "scene_module_manager.param.yaml"); + const auto start_planner_param_path = get_absolute_path_to_config( + "autoware_behavior_path_start_planner_module", "start_planner.param.yaml"); + + autoware::test_utils::updateNodeOptions( + node_options, {common_param_path, nearest_search_param_path, vehicle_info_param_path, + behavior_path_planner_param_path, drivable_area_expansion_param_path, + scene_module_manager_param_path, start_planner_param_path}); + + return node_options; + } + + void initialize_lane_departure_checker() + { + const auto vehicle_info = + autoware::vehicle_info_utils::VehicleInfoUtils(*node_).getVehicleInfo(); + lane_departure_checker_ = std::make_shared(); + lane_departure_checker_->setVehicleInfo(vehicle_info); + + autoware::lane_departure_checker::Param lane_departure_checker_params{}; + lane_departure_checker_->setParam(lane_departure_checker_params); + } + + void initialize_shift_pull_out_planner() + { + auto parameters = StartPlannerParameters::init(*node_); + + auto time_keeper = std::make_shared(); + shift_pull_out_ = + std::make_shared(*node_, parameters, lane_departure_checker_, time_keeper); + } +}; + +TEST_F(TestShiftPullOut, GenerateValidShiftPullOutPath) +{ + const auto start_pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(362.181).y(362.164).z(100.000)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(0.709650).w( + 0.704554)); + + const auto goal_pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(365.658).y(507.253).z(100.000)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(0.705897).w( + 0.708314)); + const auto planner_data = make_planner_data(start_pose, 4619, 4635); + + shift_pull_out_->setPlannerData(std::make_shared(planner_data)); + + // Plan the pull out path + PlannerDebugData debug_data; + auto result = call_plan(start_pose, goal_pose, debug_data); + + // Assert that a valid shift pull out path is generated + ASSERT_TRUE(result.has_value()) << "shift pull out path generation failed."; + EXPECT_EQ(result->partial_paths.size(), 1UL) + << "Generated shift pull out path does not have the expected number of partial paths."; + EXPECT_EQ(debug_data.conditions_evaluation.back(), "success") + << "shift pull out path planning did not succeed."; +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index be4ce4a125c68..48729c9c4fa0c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -233,11 +233,7 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( std::for_each( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { - if (!not_use_adjacent_lane) { - data.drivable_lanes.push_back( - utils::static_obstacle_avoidance::generateExpandedDrivableLanes( - lanelet, planner_data_, parameters_)); - } else if (red_signal_lane_itr->id() != lanelet.id()) { + if (!not_use_adjacent_lane || red_signal_lane_itr->id() != lanelet.id()) { data.drivable_lanes.push_back( utils::static_obstacle_avoidance::generateExpandedDrivableLanes( lanelet, planner_data_, parameters_)); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index 744af35641a59..52d5ee49c9a68 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -264,9 +264,9 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( AvoidOutlines outlines; for (auto & o : data.target_objects) { if (!o.avoid_margin.has_value()) { - if (!data.red_signal_lane.has_value()) { - o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE; - } else if (data.red_signal_lane.value().id() == o.overhang_lanelet.id()) { + if ( + data.red_signal_lane.has_value() && + data.red_signal_lane.value().id() == o.overhang_lanelet.id()) { o.info = ObjectInfo::LIMIT_DRIVABLE_SPACE_TEMPORARY; } else { o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml index 699f80ec8356e..08bd8f91c7d71 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 @@ -23,6 +23,7 @@ autoware_behavior_velocity_planner_common autoware_grid_map_utils + autoware_internal_debug_msgs autoware_interpolation autoware_lanelet2_extension autoware_motion_utils @@ -41,7 +42,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/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 1bea0626b0b2f..07b2f1deff0c7 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; @@ -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..b3fbc2f6cfaba 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 @@ -23,9 +23,9 @@ #include #include +#include #include #include -#include #include #include @@ -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_intersection_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml index b056f71614da3..7a64d1d6638ff 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml @@ -20,6 +20,7 @@ autoware_cmake autoware_behavior_velocity_planner_common + 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/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 6f7841ebb0bbb..7c492a9dd42bf 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 @@ -88,10 +88,11 @@ IntersectionModule::IntersectionModule( static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); } - ego_ttc_pub_ = node.create_publisher( + ego_ttc_pub_ = node.create_publisher( "~/debug/intersection/ego_ttc", 1); - object_ttc_pub_ = node.create_publisher( - "~/debug/intersection/object_ttc", 1); + object_ttc_pub_ = + node.create_publisher( + "~/debug/intersection/object_ttc", 1); } bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path) @@ -231,7 +232,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat // calculate the expected vehicle speed and obtain the spatiotemporal profile of ego to the // exit of intersection // ========================================================================================== - tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; const auto time_distance_array = calcIntersectionPassingTime(*path, is_prioritized, intersection_stoplines, &ego_ttc_time_array); @@ -240,7 +241,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat // passed each pass judge line for the first time, save current collision status for late // diagnosis // ========================================================================================== - tier4_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; updateObjectInfoManagerCollision( path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line, safely_passed_2nd_judge_line, &object_ttc_time_array); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index b718dd84d33af..164df7588bd68 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 @@ -27,7 +27,7 @@ #include #include -#include +#include #include #include @@ -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_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..4e898d9d28715 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 @@ -27,8 +27,8 @@ #include #include +#include #include -#include #include #include #include @@ -57,8 +57,8 @@ using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInt using autoware::rtc_interface::RTCInterface; using autoware::universe_utils::DebugPublisher; using autoware::universe_utils::getOrDeclareParameter; +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; 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..a0b54cb879cab 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 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml index 90e1d32198854..b3ced8b2e9b9f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml @@ -21,6 +21,7 @@ autoware_behavior_velocity_crosswalk_module autoware_behavior_velocity_planner_common + autoware_internal_debug_msgs autoware_motion_utils autoware_object_recognition_utils autoware_perception_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp index 2823648e184d3..3f4b9f330e2d0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp @@ -316,7 +316,7 @@ void RunOutDebug::setAccelReason(const AccelReason & accel_reason) void RunOutDebug::publishDebugValue() { // publish debug values - tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; + autoware_internal_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; debug_msg.stamp = node_.now(); for (const auto & v : debug_values_.getValues()) { debug_msg.data.push_back(v); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp index 3e913f57f30c0..1ffa826c4d1d1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp @@ -18,17 +18,17 @@ #include -#include -#include +#include +#include #include #include #include namespace autoware::behavior_velocity_planner { +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Int32Stamped; using sensor_msgs::msg::PointCloud2; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; -using tier4_debug_msgs::msg::Int32Stamped; class DebugValues { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/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/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml index eef7b6af1f9af..1063abfb61bbb 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml @@ -24,7 +24,6 @@ pluginlib rclcpp tf2 - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp index 12cb59ac46195..40aa985ee42e2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp @@ -51,8 +51,9 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo node.create_publisher("~/" + ns_ + "/virtual_walls", 1); processing_diag_publisher_ = std::make_shared( &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); - processing_time_publisher_ = node.create_publisher( - "~/debug/" + ns_ + "/processing_time_ms", 1); + processing_time_publisher_ = + node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); using autoware::universe_utils::getOrDeclareParameter; auto & p = params_; @@ -190,7 +191,7 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( processing_times["collisions"] = collisions_duration_us / 1000; processing_times["Total"] = total_time_us / 1000; processing_diag_publisher_->publish(processing_times); - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = clock_->now(); processing_time_msg.data = processing_times["Total"]; processing_time_publisher_->publish(processing_time_msg); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp index 79b817c51169c..1d980e220ccef 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp @@ -16,6 +16,7 @@ #include "../src/types.hpp" #include +#include #include #include @@ -55,45 +56,53 @@ polygon_t random_polygon() int main() { - Obstacles obstacles; - std::vector polygons; - polygons.reserve(100); - for (auto i = 0; i < 100; ++i) { - polygons.push_back(random_polygon()); - } - for (auto nb_lines = 1lu; nb_lines < 1000lu; nb_lines += 10) { - obstacles.lines.push_back(random_line()); - obstacles.points.clear(); - for (auto nb_points = 1lu; nb_points < 1000lu; nb_points += 10) { - obstacles.points.push_back(random_point()); - const auto rtt_constr_start = std::chrono::system_clock::now(); - CollisionChecker rtree_collision_checker(obstacles, 0, 0); - const auto rtt_constr_end = std::chrono::system_clock::now(); - const auto naive_constr_start = std::chrono::system_clock::now(); - CollisionChecker naive_collision_checker(obstacles, nb_points + 1, nb_lines * 100); - const auto naive_constr_end = std::chrono::system_clock::now(); - const auto rtt_check_start = std::chrono::system_clock::now(); - for (const auto & polygon : polygons) - // cppcheck-suppress unreadVariable - const auto rtree_result = rtree_collision_checker.intersections(polygon); - const auto rtt_check_end = std::chrono::system_clock::now(); - const auto naive_check_start = std::chrono::system_clock::now(); - for (const auto & polygon : polygons) - // cppcheck-suppress unreadVariable - const auto naive_result = naive_collision_checker.intersections(polygon); - const auto naive_check_end = std::chrono::system_clock::now(); - const auto rtt_constr_time = - std::chrono::duration_cast(rtt_constr_end - rtt_constr_start); - const auto naive_constr_time = - std::chrono::duration_cast(naive_constr_end - naive_constr_start); - const auto rtt_check_time = - std::chrono::duration_cast(rtt_check_end - rtt_check_start); - const auto naive_check_time = - std::chrono::duration_cast(naive_check_end - naive_check_start); - std::printf( - "%lu, %lu, %ld, %ld, %ld, %ld\n", nb_lines, nb_points, rtt_constr_time.count(), - rtt_check_time.count(), naive_constr_time.count(), naive_check_time.count()); + try { + Obstacles obstacles; + std::vector polygons; + polygons.reserve(100); + for (auto i = 0; i < 100; ++i) { + polygons.push_back(random_polygon()); + } + for (auto nb_lines = 1lu; nb_lines < 1000lu; nb_lines += 10) { + obstacles.lines.push_back(random_line()); + obstacles.points.clear(); + for (auto nb_points = 1lu; nb_points < 1000lu; nb_points += 10) { + obstacles.points.push_back(random_point()); + const auto rtt_constr_start = std::chrono::system_clock::now(); + CollisionChecker rtree_collision_checker(obstacles, 0, 0); + const auto rtt_constr_end = std::chrono::system_clock::now(); + const auto naive_constr_start = std::chrono::system_clock::now(); + CollisionChecker naive_collision_checker(obstacles, nb_points + 1, nb_lines * 100); + const auto naive_constr_end = std::chrono::system_clock::now(); + const auto rtt_check_start = std::chrono::system_clock::now(); + for (const auto & polygon : polygons) + // cppcheck-suppress unreadVariable + const auto rtree_result = rtree_collision_checker.intersections(polygon); + const auto rtt_check_end = std::chrono::system_clock::now(); + const auto naive_check_start = std::chrono::system_clock::now(); + for (const auto & polygon : polygons) + // cppcheck-suppress unreadVariable + const auto naive_result = naive_collision_checker.intersections(polygon); + const auto naive_check_end = std::chrono::system_clock::now(); + const auto rtt_constr_time = + std::chrono::duration_cast(rtt_constr_end - rtt_constr_start); + const auto naive_constr_time = std::chrono::duration_cast( + naive_constr_end - naive_constr_start); + const auto rtt_check_time = + std::chrono::duration_cast(rtt_check_end - rtt_check_start); + const auto naive_check_time = + std::chrono::duration_cast(naive_check_end - naive_check_start); + std::printf( + "%lu, %lu, %ld, %ld, %ld, %ld\n", nb_lines, nb_points, rtt_constr_time.count(), + rtt_check_time.count(), naive_constr_time.count(), naive_check_time.count()); + } } + } catch (const std::exception & e) { + std::cerr << "Exception in main(): " << e.what() << std::endl; + return {}; + } catch (...) { + std::cerr << "Unknown exception in main()" << std::endl; + return {}; } return 0; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml index 68e2ead5a5ec7..e608e158b3ffe 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml @@ -33,7 +33,6 @@ sensor_msgs tf2_eigen tf2_geometry_msgs - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index 3fa3ec7cbf782..532cc834c19ac 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -56,8 +56,9 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string node.create_publisher("~/" + ns_ + "/virtual_walls", 1); processing_diag_publisher_ = std::make_shared( &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); - processing_time_publisher_ = node.create_publisher( - "~/debug/" + ns_ + "/processing_time_ms", 1); + processing_time_publisher_ = + node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); const auto vehicle_info = vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); vehicle_lateral_offset_ = static_cast(vehicle_info.max_lateral_offset_m); @@ -236,7 +237,7 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( processing_times["slowdowns"] = slowdowns_us / 1000; processing_times["Total"] = total_us / 1000; processing_diag_publisher_->publish(processing_times); - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = clock_->now(); processing_time_msg.data = processing_times["Total"]; processing_time_publisher_->publish(processing_time_msg); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml index 857716819d8cc..f0f663355821c 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml @@ -29,7 +29,6 @@ pluginlib rclcpp tf2 - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index 63ca1b5784fe8..bd85546cb6c56 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -66,8 +66,9 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) node.create_publisher("~/" + ns_ + "/virtual_walls", 1); processing_diag_publisher_ = std::make_shared( &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); - processing_time_publisher_ = node.create_publisher( - "~/debug/" + ns_ + "/processing_time_ms", 1); + processing_time_publisher_ = + node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); } void OutOfLaneModule::init_parameters(rclcpp::Node & node) { @@ -349,7 +350,7 @@ VelocityPlanningResult OutOfLaneModule::plan( processing_times["publish_markers"] = pub_markers_us / 1000; processing_times["Total"] = total_time_us / 1000; processing_diag_publisher_->publish(processing_times); - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = clock_->now(); processing_time_msg.data = processing_times["Total"]; processing_time_publisher_->publish(processing_time_msg); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp index c39c6e17101c5..72f20f1c63d96 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp @@ -24,8 +24,6 @@ #include #include -#include -#include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp index a5f9badd9f4f7..9ef3bf5c7f756 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp @@ -31,7 +31,6 @@ #include #include #include -#include #include #include @@ -72,7 +71,6 @@ struct PlannerData // last observed infomation for UNKNOWN std::map traffic_light_id_map_raw_; std::map traffic_light_id_map_last_observed_; - std::optional external_velocity_limit; // 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..cefc84836beda 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -86,7 +86,8 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & this->create_publisher( "~/output/velocity_factors", 1); processing_time_publisher_ = - this->create_publisher("~/debug/processing_time_ms", 1); + this->create_publisher( + "~/debug/processing_time_ms", 1); debug_viz_pub_ = this->create_publisher("~/debug/markers", 1); metrics_pub_ = this->create_publisher("~/metrics", 1); @@ -299,7 +300,7 @@ void MotionVelocityPlannerNode::on_trajectory( trajectory_pub_, output_trajectory_msg.header.stamp); processing_times["Total"] = stop_watch.toc("Total"); processing_diag_publisher_.publish(processing_times); - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = processing_times["Total"]; processing_time_publisher_->publish(processing_time_msg); @@ -356,7 +357,6 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s const geometry_msgs::msg::Pose current_pose = planner_data.current_odometry.pose.pose; const double v0 = planner_data.current_odometry.twist.twist.linear.x; const double a0 = planner_data.current_acceleration.accel.accel.linear.x; - const auto & external_v_limit = planner_data.external_velocity_limit; const auto & smoother = planner_data.velocity_smoother_; const auto traj_lateral_acc_filtered = @@ -381,10 +381,6 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) { RCLCPP_ERROR(get_logger(), "failed to smooth"); } - if (external_v_limit) { - autoware::velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( - 0LU, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); - } return traj_smoothed; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index 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_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/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_;