diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index 5e1035de20c64..4119e69ed72e1 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -1,4 +1,6 @@ *:*/test/* +*:*/examples/* +*:*/benchmarks/* checkersReport missingInclude diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index cda3c8ceaca89..4fea7c85e1926 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -170,6 +170,7 @@ planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_m planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp yukinari.hisaki.2@tier4.jp zhe.shen@tier4.jp @@ -203,7 +204,7 @@ simulator/autoware_carla_interface/** maxime.clement@tier4.jp mradityagio@gmail. simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp simulator/fault_injection/** keisuke.shima@tier4.jp simulator/learning_based_vehicle_model/** maxime.clement@tier4.jp nagy.tomas@tier4.jp -simulator/simple_planning_simulator/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp temkei.kem@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +simulator/simple_planning_simulator/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp temkei.kem@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp kotaro.yoshimoto@tier4.jp simulator/tier4_dummy_object_rviz_plugin/** yukihiro.saito@tier4.jp simulator/vehicle_door_simulator/** isamu.takagi@tier4.jp system/autoware_component_monitor/** baris@leodrive.ai memin@leodrive.ai yavuz@leodrive.ai @@ -234,6 +235,7 @@ vehicle/autoware_raw_vehicle_cmd_converter/** kosuke.takeuchi@tier4.jp kyoichi.s vehicle/autoware_steer_offset_estimator/** taiki.tanaka@tier4.jp visualization/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai +visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/** satoshi.ota@tier4.jp takayuki.murooka@tier4.jp visualization/autoware_perception_rviz_plugin/** opensource@apex.ai shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp visualization/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp visualization/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp diff --git a/.github/actions/evaluate-job-result/action.yaml b/.github/actions/evaluate-job-result/action.yaml deleted file mode 100644 index c5c013080fd27..0000000000000 --- a/.github/actions/evaluate-job-result/action.yaml +++ /dev/null @@ -1,45 +0,0 @@ -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/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 0925cb0f53930..c25f6b24c5e47 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -14,6 +14,10 @@ on: default: humble required: false type: string + run-condition: + default: true + required: false + type: boolean container-suffix: required: false default: "" @@ -28,6 +32,7 @@ env: jobs: build-and-test-differential: + if: ${{ inputs.run-condition }} runs-on: ${{ inputs.runner }} container: ${{ inputs.container }}${{ inputs.container-suffix }} steps: diff --git a/.github/workflows/build-test-tidy-pr.yaml b/.github/workflows/build-test-tidy-pr.yaml index d1ea9b2d0f79d..c0442694b5c77 100644 --- a/.github/workflows/build-test-tidy-pr.yaml +++ b/.github/workflows/build-test-tidy-pr.yaml @@ -40,98 +40,44 @@ jobs: shell: bash build-and-test-differential: + if: ${{ always() }} needs: - require-label uses: ./.github/workflows/build-and-test-differential.yaml with: container: ghcr.io/autowarefoundation/autoware:universe-devel + run-condition: ${{ needs.require-label.outputs.result == 'true' }} secrets: codecov-token: ${{ secrets.CODECOV_TOKEN }} build-and-test-differential-cuda: + if: ${{ always() }} needs: check-if-cuda-job-is-needed - 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 + run-condition: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'true' }} 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: + if: ${{ always() }} # always run to provide report for status check 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 + run-condition: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'false' && needs.build-and-test-differential.result == 'success' }} clang-tidy-differential-cuda: + if: ${{ always() }} # always run to provide report for status check needs: + - check-if-cuda-job-is-needed - build-and-test-differential-cuda uses: ./.github/workflows/clang-tidy-differential.yaml with: container: ghcr.io/autowarefoundation/autoware:universe-devel container-suffix: -cuda - - 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 + run-condition: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'true' && needs.build-and-test-differential-cuda.result == 'success' }} diff --git a/.github/workflows/clang-tidy-differential.yaml b/.github/workflows/clang-tidy-differential.yaml index 51e0a8408468c..41d722c8e07b0 100644 --- a/.github/workflows/clang-tidy-differential.yaml +++ b/.github/workflows/clang-tidy-differential.yaml @@ -14,9 +14,14 @@ on: default: ubuntu-24.04 required: false type: string + run-condition: + default: true + required: false + type: boolean jobs: clang-tidy-differential: + if: ${{ inputs.run-condition }} runs-on: ${{ inputs.runner }} container: ${{ inputs.container }}${{ inputs.container-suffix }} steps: diff --git a/.pre-commit-config-optional.yaml b/.pre-commit-config-optional.yaml index ff325af5e8774..f0b5bdba2d24d 100644 --- a/.pre-commit-config-optional.yaml +++ b/.pre-commit-config-optional.yaml @@ -2,6 +2,13 @@ # https://github.com/autowarefoundation/sync-file-templates # To make changes, update the source repository and follow the guidelines in its README. +# https://pre-commit.ci/#configuration +ci: + autofix_commit_msg: "style(pre-commit-optional): autofix" + # we already have our own daily update mechanism, we set this to quarterly + autoupdate_schedule: quarterly + autoupdate_commit_msg: "ci(pre-commit-optional): quarterly autoupdate" + repos: - repo: https://github.com/tcort/markdown-link-check rev: v3.12.2 diff --git a/build_depends.repos b/build_depends.repos index 7e547be7409cf..2313a9be487a6 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -32,7 +32,7 @@ repositories: core/autoware_internal_msgs: type: git url: https://github.com/autowarefoundation/autoware_internal_msgs.git - version: 1.2.0 + version: 1.3.0 # universe universe/external/tier4_autoware_msgs: type: git diff --git a/common/autoware_kalman_filter/CHANGELOG.rst b/common/autoware_kalman_filter/CHANGELOG.rst deleted file mode 100644 index a7e68f8843e74..0000000000000 --- a/common/autoware_kalman_filter/CHANGELOG.rst +++ /dev/null @@ -1,55 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_kalman_filter -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(kalman_filter): prefix package and namespace with autoware (`#7787 `_) - * refactor(kalman_filter): prefix package and namespace with autoware - * move headers to include/autoware/ - * style(pre-commit): autofix - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/common/autoware_kalman_filter/CMakeLists.txt b/common/autoware_kalman_filter/CMakeLists.txt deleted file mode 100644 index 076d2d3cad4e8..0000000000000 --- a/common/autoware_kalman_filter/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_kalman_filter) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3 REQUIRED) - -include_directories( - SYSTEM - ${EIGEN3_INCLUDE_DIR} -) - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/kalman_filter.cpp - src/time_delay_kalman_filter.cpp - include/autoware/kalman_filter/kalman_filter.hpp - include/autoware/kalman_filter/time_delay_kalman_filter.hpp -) - -if(BUILD_TESTING) - file(GLOB_RECURSE test_files test/*.cpp) - ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files}) - - target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) -endif() - -ament_auto_package() diff --git a/common/autoware_kalman_filter/README.md b/common/autoware_kalman_filter/README.md deleted file mode 100644 index 7c0feb9c2a61a..0000000000000 --- a/common/autoware_kalman_filter/README.md +++ /dev/null @@ -1,9 +0,0 @@ -# kalman_filter - -## Purpose - -This common package contains the kalman filter with time delay and the calculation of the kalman filter. - -## Assumptions / Known limits - -TBD. diff --git a/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp b/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp deleted file mode 100644 index 74db04f6e838b..0000000000000 --- a/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp +++ /dev/null @@ -1,214 +0,0 @@ -// Copyright 2018-2019 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. - -#ifndef AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ -#define AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ - -#include -#include - -namespace autoware::kalman_filter -{ - -/** - * @file kalman_filter.h - * @brief kalman filter class - * @author Takamasa Horibe - * @date 2019.05.01 - */ - -class KalmanFilter -{ -public: - /** - * @brief No initialization constructor. - */ - KalmanFilter(); - - /** - * @brief constructor with initialization - * @param x initial state - * @param A coefficient matrix of x for process model - * @param B coefficient matrix of u for process model - * @param C coefficient matrix of x for measurement model - * @param Q covariance matrix for process model - * @param R covariance matrix for measurement model - * @param P initial covariance of estimated state - */ - KalmanFilter( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, - const Eigen::MatrixXd & P); - - /** - * @brief destructor - */ - ~KalmanFilter(); - - /** - * @brief initialization of kalman filter - * @param x initial state - * @param A coefficient matrix of x for process model - * @param B coefficient matrix of u for process model - * @param C coefficient matrix of x for measurement model - * @param Q covariance matrix for process model - * @param R covariance matrix for measurement model - * @param P initial covariance of estimated state - */ - bool init( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, - const Eigen::MatrixXd & P); - - /** - * @brief initialization of kalman filter - * @param x initial state - * @param P initial covariance of estimated state - */ - bool init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0); - - /** - * @brief set A of process model - * @param A coefficient matrix of x for process model - */ - void setA(const Eigen::MatrixXd & A); - - /** - * @brief set B of process model - * @param B coefficient matrix of u for process model - */ - void setB(const Eigen::MatrixXd & B); - - /** - * @brief set C of measurement model - * @param C coefficient matrix of x for measurement model - */ - void setC(const Eigen::MatrixXd & C); - - /** - * @brief set covariance matrix Q for process model - * @param Q covariance matrix for process model - */ - void setQ(const Eigen::MatrixXd & Q); - - /** - * @brief set covariance matrix R for measurement model - * @param R covariance matrix for measurement model - */ - void setR(const Eigen::MatrixXd & R); - - /** - * @brief get current kalman filter state - * @param x kalman filter state - */ - void getX(Eigen::MatrixXd & x) const; - - /** - * @brief get current kalman filter covariance - * @param P kalman filter covariance - */ - void getP(Eigen::MatrixXd & P) const; - - /** - * @brief get component of current kalman filter state - * @param i index of kalman filter state - * @return value of i's component of the kalman filter state x[i] - */ - double getXelement(unsigned int i) const; - - /** - * @brief calculate kalman filter state and covariance by prediction model with A, B, Q matrix. - * This is mainly for EKF with variable matrix. - * @param u input for model - * @param A coefficient matrix of x for process model - * @param B coefficient matrix of u for process model - * @param Q covariance matrix for process model - * @return bool to check matrix operations are being performed properly - */ - bool predict( - const Eigen::MatrixXd & u, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & Q); - - /** - * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is - * mainly for EKF with variable matrix. - * @param x_next predicted state - * @param A coefficient matrix of x for process model - * @param Q covariance matrix for process model - * @return bool to check matrix operations are being performed properly - */ - bool predict( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q); - - /** - * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is - * mainly for EKF with variable matrix. - * @param x_next predicted state - * @param A coefficient matrix of x for process model - * @return bool to check matrix operations are being performed properly - */ - bool predict(const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A); - - /** - * @brief calculate kalman filter state by prediction model with A, B and Q being class member - * variables. - * @param u input for the model - * @return bool to check matrix operations are being performed properly - */ - bool predict(const Eigen::MatrixXd & u); - - /** - * @brief calculate kalman filter state by measurement model with y_pred, C and R matrix. This is - * mainly for EKF with variable matrix. - * @param y measured values - * @param y output values expected from measurement model - * @param C coefficient matrix of x for measurement model - * @param R covariance matrix for measurement model - * @return bool to check matrix operations are being performed properly - */ - bool update( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & y_pred, const Eigen::MatrixXd & C, - const Eigen::MatrixXd & R); - - /** - * @brief calculate kalman filter state by measurement model with C and R matrix. This is mainly - * for EKF with variable matrix. - * @param y measured values - * @param C coefficient matrix of x for measurement model - * @param R covariance matrix for measurement model - * @return bool to check matrix operations are being performed properly - */ - bool update(const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R); - - /** - * @brief calculate kalman filter state by measurement model with C and R being class member - * variables. - * @param y measured values - * @return bool to check matrix operations are being performed properly - */ - bool update(const Eigen::MatrixXd & y); - -protected: - Eigen::MatrixXd x_; //!< @brief current estimated state - Eigen::MatrixXd - A_; //!< @brief coefficient matrix of x for process model x[k+1] = A*x[k] + B*u[k] - Eigen::MatrixXd - B_; //!< @brief coefficient matrix of u for process model x[k+1] = A*x[k] + B*u[k] - Eigen::MatrixXd C_; //!< @brief coefficient matrix of x for measurement model y[k] = C * x[k] - Eigen::MatrixXd Q_; //!< @brief covariance matrix for process model x[k+1] = A*x[k] + B*u[k] - Eigen::MatrixXd R_; //!< @brief covariance matrix for measurement model y[k] = C * x[k] - Eigen::MatrixXd P_; //!< @brief covariance of estimated state -}; -} // namespace autoware::kalman_filter -#endif // AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ diff --git a/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp b/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp deleted file mode 100644 index 80375b7579e62..0000000000000 --- a/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp +++ /dev/null @@ -1,89 +0,0 @@ -// Copyright 2018-2019 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. - -#ifndef AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ -#define AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ - -#include "autoware/kalman_filter/kalman_filter.hpp" - -#include -#include - -#include - -namespace autoware::kalman_filter -{ -/** - * @file time_delay_kalman_filter.h - * @brief kalman filter with delayed measurement class - * @author Takamasa Horibe - * @date 2019.05.01 - */ - -class TimeDelayKalmanFilter : public KalmanFilter -{ -public: - /** - * @brief No initialization constructor. - */ - TimeDelayKalmanFilter(); - - /** - * @brief initialization of kalman filter - * @param x initial state - * @param P0 initial covariance of estimated state - * @param max_delay_step Maximum number of delay steps, which determines the dimension of the - * extended kalman filter - */ - void init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P, const int max_delay_step); - - /** - * @brief get latest time estimated state - */ - Eigen::MatrixXd getLatestX() const; - - /** - * @brief get latest time estimation covariance - */ - Eigen::MatrixXd getLatestP() const; - - /** - * @brief calculate kalman filter covariance by precision model with time delay. This is mainly - * for EKF of nonlinear process model. - * @param x_next predicted state by prediction model - * @param A coefficient matrix of x for process model - * @param Q covariance matrix for process model - */ - bool predictWithDelay( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q); - - /** - * @brief calculate kalman filter covariance by measurement model with time delay. This is mainly - * for EKF of nonlinear process model. - * @param y measured values - * @param C coefficient matrix of x for measurement model - * @param R covariance matrix for measurement model - * @param delay_step measurement delay - */ - bool updateWithDelay( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R, - const int delay_step); - -private: - int max_delay_step_; //!< @brief maximum number of delay steps - int dim_x_; //!< @brief dimension of latest state - int dim_x_ex_; //!< @brief dimension of extended state with dime delay -}; -} // namespace autoware::kalman_filter -#endif // AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ diff --git a/common/autoware_kalman_filter/package.xml b/common/autoware_kalman_filter/package.xml deleted file mode 100644 index e51bb06e9de43..0000000000000 --- a/common/autoware_kalman_filter/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - autoware_kalman_filter - 0.40.0 - The kalman filter package - Yukihiro Saito - Takeshi Ishita - Koji Minoda - Apache License 2.0 - - Takamasa Horibe - - ament_cmake_auto - autoware_cmake - - eigen - eigen3_cmake_module - - ament_cmake_cppcheck - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/autoware_kalman_filter/src/kalman_filter.cpp b/common/autoware_kalman_filter/src/kalman_filter.cpp deleted file mode 100644 index bbd963675f9e2..0000000000000 --- a/common/autoware_kalman_filter/src/kalman_filter.cpp +++ /dev/null @@ -1,161 +0,0 @@ -// Copyright 2018-2019 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/kalman_filter/kalman_filter.hpp" - -namespace autoware::kalman_filter -{ -KalmanFilter::KalmanFilter() -{ -} -KalmanFilter::KalmanFilter( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, - const Eigen::MatrixXd & P) -{ - init(x, A, B, C, Q, R, P); -} -KalmanFilter::~KalmanFilter() -{ -} -bool KalmanFilter::init( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, - const Eigen::MatrixXd & P) -{ - if ( - x.cols() == 0 || x.rows() == 0 || A.cols() == 0 || A.rows() == 0 || B.cols() == 0 || - B.rows() == 0 || C.cols() == 0 || C.rows() == 0 || Q.cols() == 0 || Q.rows() == 0 || - R.cols() == 0 || R.rows() == 0 || P.cols() == 0 || P.rows() == 0) { - return false; - } - x_ = x; - A_ = A; - B_ = B; - C_ = C; - Q_ = Q; - R_ = R; - P_ = P; - return true; -} -bool KalmanFilter::init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0) -{ - if (x.cols() == 0 || x.rows() == 0 || P0.cols() == 0 || P0.rows() == 0) { - return false; - } - x_ = x; - P_ = P0; - return true; -} - -void KalmanFilter::setA(const Eigen::MatrixXd & A) -{ - A_ = A; -} -void KalmanFilter::setB(const Eigen::MatrixXd & B) -{ - B_ = B; -} -void KalmanFilter::setC(const Eigen::MatrixXd & C) -{ - C_ = C; -} -void KalmanFilter::setQ(const Eigen::MatrixXd & Q) -{ - Q_ = Q; -} -void KalmanFilter::setR(const Eigen::MatrixXd & R) -{ - R_ = R; -} -void KalmanFilter::getX(Eigen::MatrixXd & x) const -{ - x = x_; -} -void KalmanFilter::getP(Eigen::MatrixXd & P) const -{ - P = P_; -} -double KalmanFilter::getXelement(unsigned int i) const -{ - return x_(i); -} - -bool KalmanFilter::predict( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q) -{ - if ( - x_.rows() != x_next.rows() || A.cols() != P_.rows() || Q.cols() != Q.rows() || - A.rows() != Q.cols()) { - return false; - } - x_ = x_next; - P_ = A * P_ * A.transpose() + Q; - return true; -} -bool KalmanFilter::predict(const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A) -{ - return predict(x_next, A, Q_); -} - -bool KalmanFilter::predict( - const Eigen::MatrixXd & u, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & Q) -{ - if (A.cols() != x_.rows() || B.cols() != u.rows()) { - return false; - } - const Eigen::MatrixXd x_next = A * x_ + B * u; - return predict(x_next, A, Q); -} -bool KalmanFilter::predict(const Eigen::MatrixXd & u) -{ - return predict(u, A_, B_, Q_); -} - -bool KalmanFilter::update( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & y_pred, const Eigen::MatrixXd & C, - const Eigen::MatrixXd & R) -{ - if ( - P_.cols() != C.cols() || R.rows() != R.cols() || R.rows() != C.rows() || - y.rows() != y_pred.rows() || y.rows() != C.rows()) { - return false; - } - const Eigen::MatrixXd PCT = P_ * C.transpose(); - const Eigen::MatrixXd K = PCT * ((R + C * PCT).inverse()); - - if (isnan(K.array()).any() || isinf(K.array()).any()) { - return false; - } - - x_ = x_ + K * (y - y_pred); - P_ = P_ - K * (C * P_); - return true; -} - -bool KalmanFilter::update( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R) -{ - if (C.cols() != x_.rows()) { - return false; - } - const Eigen::MatrixXd y_pred = C * x_; - return update(y, y_pred, C, R); -} -bool KalmanFilter::update(const Eigen::MatrixXd & y) -{ - return update(y, C_, R_); -} -} // namespace autoware::kalman_filter diff --git a/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp b/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp deleted file mode 100644 index 4d1dd8f33b7a6..0000000000000 --- a/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2018-2019 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/kalman_filter/time_delay_kalman_filter.hpp" - -#include - -namespace autoware::kalman_filter -{ -TimeDelayKalmanFilter::TimeDelayKalmanFilter() -{ -} - -void TimeDelayKalmanFilter::init( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0, const int max_delay_step) -{ - max_delay_step_ = max_delay_step; - dim_x_ = x.rows(); - dim_x_ex_ = dim_x_ * max_delay_step; - - x_ = Eigen::MatrixXd::Zero(dim_x_ex_, 1); - P_ = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_); - - for (int i = 0; i < max_delay_step_; ++i) { - x_.block(i * dim_x_, 0, dim_x_, 1) = x; - P_.block(i * dim_x_, i * dim_x_, dim_x_, dim_x_) = P0; - } -} - -Eigen::MatrixXd TimeDelayKalmanFilter::getLatestX() const -{ - return x_.block(0, 0, dim_x_, 1); -} - -Eigen::MatrixXd TimeDelayKalmanFilter::getLatestP() const -{ - return P_.block(0, 0, dim_x_, dim_x_); -} - -bool TimeDelayKalmanFilter::predictWithDelay( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q) -{ - /* - * time delay model: - * - * [A 0 0] [P11 P12 P13] [Q 0 0] - * A = [I 0 0], P = [P21 P22 P23], Q = [0 0 0] - * [0 I 0] [P31 P32 P33] [0 0 0] - * - * covariance calculation in prediction : P = A * P * A' + Q - * - * [A*P11*A'*+Q A*P11 A*P12] - * P = [ P11*A' P11 P12] - * [ P21*A' P21 P22] - */ - - const int d_dim_x = dim_x_ex_ - dim_x_; - - /* slide states in the time direction */ - Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, 1); - x_tmp.block(0, 0, dim_x_, 1) = x_next; - x_tmp.block(dim_x_, 0, d_dim_x, 1) = x_.block(0, 0, d_dim_x, 1); - x_ = x_tmp; - - /* update P with delayed measurement A matrix structure */ - Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_); - P_tmp.block(0, 0, dim_x_, dim_x_) = A * P_.block(0, 0, dim_x_, dim_x_) * A.transpose() + Q; - P_tmp.block(0, dim_x_, dim_x_, d_dim_x) = A * P_.block(0, 0, dim_x_, d_dim_x); - P_tmp.block(dim_x_, 0, d_dim_x, dim_x_) = P_.block(0, 0, d_dim_x, dim_x_) * A.transpose(); - P_tmp.block(dim_x_, dim_x_, d_dim_x, d_dim_x) = P_.block(0, 0, d_dim_x, d_dim_x); - P_ = P_tmp; - - return true; -} - -bool TimeDelayKalmanFilter::updateWithDelay( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R, - const int delay_step) -{ - if (delay_step >= max_delay_step_) { - std::cerr << "delay step is larger than max_delay_step. ignore update." << std::endl; - return false; - } - - const int dim_y = y.rows(); - - /* set measurement matrix */ - Eigen::MatrixXd C_ex = Eigen::MatrixXd::Zero(dim_y, dim_x_ex_); - C_ex.block(0, dim_x_ * delay_step, dim_y, dim_x_) = C; - - /* update */ - if (!update(y, C_ex, R)) { - return false; - } - - return true; -} -} // namespace autoware::kalman_filter diff --git a/common/autoware_kalman_filter/test/test_kalman_filter.cpp b/common/autoware_kalman_filter/test/test_kalman_filter.cpp deleted file mode 100644 index 34e23ef9d06e2..0000000000000 --- a/common/autoware_kalman_filter/test/test_kalman_filter.cpp +++ /dev/null @@ -1,96 +0,0 @@ -// Copyright 2023 The 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/kalman_filter/kalman_filter.hpp" - -#include - -using autoware::kalman_filter::KalmanFilter; - -TEST(kalman_filter, kf) -{ - KalmanFilter kf_; - - Eigen::MatrixXd x_t(2, 1); - x_t << 1, 2; - - Eigen::MatrixXd P_t(2, 2); - P_t << 1, 0, 0, 1; - - Eigen::MatrixXd Q_t(2, 2); - Q_t << 0.01, 0, 0, 0.01; - - Eigen::MatrixXd R_t(2, 2); - R_t << 0.09, 0, 0, 0.09; - - Eigen::MatrixXd C_t(2, 2); - C_t << 1, 0, 0, 1; - - Eigen::MatrixXd A_t(2, 2); - A_t << 1, 0, 0, 1; - - Eigen::MatrixXd B_t(2, 2); - B_t << 1, 0, 0, 1; - - // Initialize the filter and check if initialization was successful - EXPECT_TRUE(kf_.init(x_t, A_t, B_t, C_t, Q_t, R_t, P_t)); - - // Perform prediction - Eigen::MatrixXd u_t(2, 1); - u_t << 0.1, 0.1; - EXPECT_TRUE(kf_.predict(u_t)); - - // Check the updated state and covariance matrix - Eigen::MatrixXd x_predict_expected = A_t * x_t + B_t * u_t; - Eigen::MatrixXd P_predict_expected = A_t * P_t * A_t.transpose() + Q_t; - - Eigen::MatrixXd x_predict; - kf_.getX(x_predict); - Eigen::MatrixXd P_predict; - kf_.getP(P_predict); - - EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5); - EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5); - - // Perform update - Eigen::MatrixXd y_t(2, 1); - y_t << 1.05, 2.05; - EXPECT_TRUE(kf_.update(y_t)); - - // Check the updated state and covariance matrix - const Eigen::MatrixXd PCT_t = P_predict_expected * C_t.transpose(); - const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_t * PCT_t).inverse()); - const Eigen::MatrixXd y_pred = C_t * x_predict_expected; - Eigen::MatrixXd x_update_expected = x_predict_expected + K_t * (y_t - y_pred); - Eigen::MatrixXd P_update_expected = P_predict_expected - K_t * (C_t * P_predict_expected); - - Eigen::MatrixXd x_update; - kf_.getX(x_update); - Eigen::MatrixXd P_update; - kf_.getP(P_update); - - EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5); - EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5); - EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5); - EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5); -} - -int main(int argc, char * argv[]) -{ - testing::InitGoogleTest(&argc, argv); - bool result = RUN_ALL_TESTS(); - return result; -} diff --git a/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp b/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp deleted file mode 100644 index 50c22fae123bc..0000000000000 --- a/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp +++ /dev/null @@ -1,123 +0,0 @@ -// Copyright 2023 The 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/kalman_filter/time_delay_kalman_filter.hpp" - -#include - -using autoware::kalman_filter::TimeDelayKalmanFilter; - -TEST(time_delay_kalman_filter, td_kf) -{ - TimeDelayKalmanFilter td_kf_; - - Eigen::MatrixXd x_t(3, 1); - x_t << 1.0, 2.0, 3.0; - Eigen::MatrixXd P_t(3, 3); - P_t << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.3; - const int max_delay_step = 5; - const int dim_x = x_t.rows(); - const int dim_x_ex = dim_x * max_delay_step; - // Initialize the filter - td_kf_.init(x_t, P_t, max_delay_step); - - // Check if initialization was successful - Eigen::MatrixXd x_init = td_kf_.getLatestX(); - Eigen::MatrixXd P_init = td_kf_.getLatestP(); - Eigen::MatrixXd x_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, 1); - Eigen::MatrixXd P_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex); - for (int i = 0; i < max_delay_step; ++i) { - x_ex_t.block(i * dim_x, 0, dim_x, 1) = x_t; - P_ex_t.block(i * dim_x, i * dim_x, dim_x, dim_x) = P_t; - } - - EXPECT_EQ(x_init.rows(), 3); - EXPECT_EQ(x_init.cols(), 1); - EXPECT_EQ(P_init.rows(), 3); - EXPECT_EQ(P_init.cols(), 3); - EXPECT_NEAR(x_init(0, 0), x_t(0, 0), 1e-5); - EXPECT_NEAR(x_init(1, 0), x_t(1, 0), 1e-5); - EXPECT_NEAR(x_init(2, 0), x_t(2, 0), 1e-5); - EXPECT_NEAR(P_init(0, 0), P_t(0, 0), 1e-5); - EXPECT_NEAR(P_init(1, 1), P_t(1, 1), 1e-5); - EXPECT_NEAR(P_init(2, 2), P_t(2, 2), 1e-5); - - // Define prediction parameters - Eigen::MatrixXd A_t(3, 3); - A_t << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0; - Eigen::MatrixXd Q_t(3, 3); - Q_t << 0.01, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.03; - Eigen::MatrixXd x_next(3, 1); - x_next << 2.0, 4.0, 6.0; - - // Perform prediction - EXPECT_TRUE(td_kf_.predictWithDelay(x_next, A_t, Q_t)); - - // Check the prediction state and covariance matrix - Eigen::MatrixXd x_predict = td_kf_.getLatestX(); - Eigen::MatrixXd P_predict = td_kf_.getLatestP(); - Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex, 1); - x_tmp.block(0, 0, dim_x, 1) = A_t * x_t; - x_tmp.block(dim_x, 0, dim_x_ex - dim_x, 1) = x_ex_t.block(0, 0, dim_x_ex - dim_x, 1); - x_ex_t = x_tmp; - Eigen::MatrixXd x_predict_expected = x_ex_t.block(0, 0, dim_x, 1); - Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex); - P_tmp.block(0, 0, dim_x, dim_x) = A_t * P_ex_t.block(0, 0, dim_x, dim_x) * A_t.transpose() + Q_t; - P_tmp.block(0, dim_x, dim_x, dim_x_ex - dim_x) = - A_t * P_ex_t.block(0, 0, dim_x, dim_x_ex - dim_x); - P_tmp.block(dim_x, 0, dim_x_ex - dim_x, dim_x) = - P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x) * A_t.transpose(); - P_tmp.block(dim_x, dim_x, dim_x_ex - dim_x, dim_x_ex - dim_x) = - P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x_ex - dim_x); - P_ex_t = P_tmp; - Eigen::MatrixXd P_predict_expected = P_ex_t.block(0, 0, dim_x, dim_x); - EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5); - EXPECT_NEAR(x_predict(2, 0), x_predict_expected(2, 0), 1e-5); - EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5); - EXPECT_NEAR(P_predict(2, 2), P_predict_expected(2, 2), 1e-5); - - // Define update parameters - Eigen::MatrixXd C_t(3, 3); - C_t << 0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5; - Eigen::MatrixXd R_t(3, 3); - R_t << 0.001, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.003; - Eigen::MatrixXd y_t(3, 1); - y_t << 1.05, 2.05, 3.05; - const int delay_step = 2; // Choose an appropriate delay step - const int dim_y = y_t.rows(); - - // Perform update - EXPECT_TRUE(td_kf_.updateWithDelay(y_t, C_t, R_t, delay_step)); - - // Check the updated state and covariance matrix - Eigen::MatrixXd x_update = td_kf_.getLatestX(); - Eigen::MatrixXd P_update = td_kf_.getLatestP(); - - Eigen::MatrixXd C_ex_t = Eigen::MatrixXd::Zero(dim_y, dim_x_ex); - const Eigen::MatrixXd PCT_t = P_ex_t * C_ex_t.transpose(); - const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_ex_t * PCT_t).inverse()); - const Eigen::MatrixXd y_pred = C_ex_t * x_ex_t; - x_ex_t = x_ex_t + K_t * (y_t - y_pred); - P_ex_t = P_ex_t - K_t * (C_ex_t * P_ex_t); - Eigen::MatrixXd x_update_expected = x_ex_t.block(0, 0, dim_x, 1); - Eigen::MatrixXd P_update_expected = P_ex_t.block(0, 0, dim_x, dim_x); - EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5); - EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5); - EXPECT_NEAR(x_update(2, 0), x_update_expected(2, 0), 1e-5); - EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5); - EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5); - EXPECT_NEAR(P_update(2, 2), P_update_expected(2, 2), 1e-5); -} diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp index ee642c93a378f..2b89fc19d1878 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp @@ -40,6 +40,11 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", const bool is_driving_forward = true); +visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward); + visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( const rclcpp::Time & now, const int32_t id); diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp index 07fcbd9840c88..fd86c54d65be9 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp @@ -29,12 +29,13 @@ namespace autoware::motion_utils { /// @brief type of virtual wall associated with different marker styles and namespace -enum VirtualWallType { stop, slowdown, deadline }; +enum VirtualWallType { stop, slowdown, deadline, pass }; /// @brief virtual wall to be visualized in rviz struct VirtualWall { geometry_msgs::msg::Pose pose{}; std::string text{}; + std::string detail{}; std::string ns{}; VirtualWallType style = stop; double longitudinal_offset{}; diff --git a/common/autoware_motion_utils/src/marker/marker_helper.cpp b/common/autoware_motion_utils/src/marker/marker_helper.cpp index 91ad1c41eecc3..44b621a53c5f1 100644 --- a/common/autoware_motion_utils/src/marker/marker_helper.cpp +++ b/common/autoware_motion_utils/src/marker/marker_helper.cpp @@ -54,7 +54,7 @@ inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray( { auto marker = createDefaultMarker( "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + createMarkerScale(0.0, 0.0, 1.0 /*font size*/), createMarkerColor(1.0, 1.0, 1.0, 1.0)); marker.pose = vehicle_front_pose; marker.pose.position.z += 2.0; @@ -85,6 +85,41 @@ inline visualization_msgs::msg::MarkerArray createDeletedVirtualWallMarkerArray( return marker_array; } + +inline visualization_msgs::msg::MarkerArray createIntendedPassArrowMarkerArray( + const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name, + const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id, + const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::MarkerArray marker_array; + + // Arrow + { + auto marker = createDefaultMarker( + "map", now, ns_prefix + "direction", id, visualization_msgs::msg::Marker::ARROW, + createMarkerScale(2.5 /*length*/, 0.15 /*width*/, 1.0 /*height*/), color); + + marker.pose = vehicle_front_pose; + + marker_array.markers.push_back(marker); + } + + // Factor Text + { + auto marker = createDefaultMarker( + "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 0.4 /*font size*/), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + + marker.pose = vehicle_front_pose; + marker.pose.position.z += 2.0; + marker.text = module_name; + + marker_array.markers.push_back(marker); + } + + return marker_array; +} + } // namespace namespace autoware::motion_utils @@ -125,6 +160,18 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( createMarkerColor(0.0, 1.0, 0.0, 0.5)); } +visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) +{ + const auto pose_with_offset = autoware::universe_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); + return createIntendedPassArrowMarkerArray( + pose_with_offset, module_name, ns_prefix + "intended_pass_", now, id, + createMarkerColor(0.77, 0.77, 0.77, 0.5)); +} + visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( const rclcpp::Time & now, const int32_t id) { diff --git a/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp index 4fecaea1bb838..fb8708b5b7baa 100644 --- a/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp +++ b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp @@ -63,10 +63,16 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( case deadline: create_fn = autoware::motion_utils::createDeadLineVirtualWallMarker; break; + case pass: + create_fn = autoware::motion_utils::createIntendedPassVirtualMarker; + break; } + const auto wall_text = virtual_wall.detail.empty() + ? virtual_wall.text + : virtual_wall.text + "(" + virtual_wall.detail + ")"; auto markers = create_fn( - virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset, - virtual_wall.ns, virtual_wall.is_driving_forward); + virtual_wall.pose, wall_text, now, 0, virtual_wall.longitudinal_offset, virtual_wall.ns, + virtual_wall.is_driving_forward); for (auto & marker : markers.markers) { marker.id = static_cast(marker_count_per_namespace_[marker.ns].current++); marker_array.markers.push_back(marker); diff --git a/common/autoware_osqp_interface/CHANGELOG.rst b/common/autoware_osqp_interface/CHANGELOG.rst deleted file mode 100644 index d400b77f4bf61..0000000000000 --- a/common/autoware_osqp_interface/CHANGELOG.rst +++ /dev/null @@ -1,51 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_osqp_interface -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* fix(autoware_osqp_interface): fix clang-tidy errors (`#9440 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Ryuta Kambe, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(osqp_interface): added autoware prefix to osqp_interface (`#8958 `_) -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/common/autoware_osqp_interface/CMakeLists.txt b/common/autoware_osqp_interface/CMakeLists.txt deleted file mode 100644 index b770af659e52a..0000000000000 --- a/common/autoware_osqp_interface/CMakeLists.txt +++ /dev/null @@ -1,58 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_osqp_interface) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Eigen3 REQUIRED) - -# after find_package(osqp_vendor) in ament_auto_find_build_dependencies -find_package(osqp REQUIRED) -get_target_property(OSQP_INCLUDE_SUB_DIR osqp::osqp INTERFACE_INCLUDE_DIRECTORIES) -get_filename_component(OSQP_INCLUDE_DIR ${OSQP_INCLUDE_SUB_DIR} PATH) - -set(OSQP_INTERFACE_LIB_SRC - src/csc_matrix_conv.cpp - src/osqp_interface.cpp -) - -set(OSQP_INTERFACE_LIB_HEADERS - include/autoware/osqp_interface/csc_matrix_conv.hpp - include/autoware/osqp_interface/osqp_interface.hpp - include/autoware/osqp_interface/visibility_control.hpp -) - -ament_auto_add_library(${PROJECT_NAME} SHARED - ${OSQP_INTERFACE_LIB_SRC} - ${OSQP_INTERFACE_LIB_HEADERS} -) -target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast) - -target_include_directories(${PROJECT_NAME} - SYSTEM PUBLIC - "${OSQP_INCLUDE_DIR}" - "${EIGEN3_INCLUDE_DIR}" -) - -ament_target_dependencies(${PROJECT_NAME} - Eigen3 - osqp_vendor -) - -# crucial so downstream package builds because autoware_osqp_interface exposes osqp.hpp -ament_export_include_directories("${OSQP_INCLUDE_DIR}") -# crucial so the linking order is correct in a downstream package: libosqp_interface.a should come before libosqp.a -ament_export_libraries(osqp::osqp) - -if(BUILD_TESTING) - set(TEST_SOURCES - test/test_osqp_interface.cpp - test/test_csc_matrix_conv.cpp - ) - set(TEST_OSQP_INTERFACE_EXE test_osqp_interface) - ament_add_ros_isolated_gtest(${TEST_OSQP_INTERFACE_EXE} ${TEST_SOURCES}) - target_link_libraries(${TEST_OSQP_INTERFACE_EXE} ${PROJECT_NAME}) -endif() - -ament_auto_package(INSTALL_TO_SHARE -) diff --git a/common/autoware_osqp_interface/design/osqp_interface-design.md b/common/autoware_osqp_interface/design/osqp_interface-design.md deleted file mode 100644 index 887a4b4d9af3f..0000000000000 --- a/common/autoware_osqp_interface/design/osqp_interface-design.md +++ /dev/null @@ -1,70 +0,0 @@ -# Interface for the OSQP library - -This is the design document for the `autoware_osqp_interface` package. - -## Purpose / Use cases - - - - -This packages provides a C++ interface for the [OSQP library](https://osqp.org/docs/solver/index.html). - -## Design - - - - -The class `OSQPInterface` takes a problem formulation as Eigen matrices and vectors, converts these objects into -C-style Compressed-Column-Sparse matrices and dynamic arrays, loads the data into the OSQP workspace dataholder, and runs the optimizer. - -## Inputs / Outputs / API - - - - -The interface can be used in several ways: - -1. Initialize the interface WITHOUT data. Load the problem formulation at the optimization call. - - ```cpp - osqp_interface = OSQPInterface(); - osqp_interface.optimize(P, A, q, l, u); - ``` - -2. Initialize the interface WITH data. - - ```cpp - osqp_interface = OSQPInterface(P, A, q, l, u); - osqp_interface.optimize(); - ``` - -3. WARM START OPTIMIZATION by modifying the problem formulation between optimization runs. - - ```cpp - osqp_interface = OSQPInterface(P, A, q, l, u); - osqp_interface.optimize(); - osqp.initializeProblem(P_new, A_new, q_new, l_new, u_new); - osqp_interface.optimize(); - ``` - - The optimization results are returned as a vector by the optimization function. - - ```cpp - std::tuple, std::vector> result = osqp_interface.optimize(); - std::vector param = std::get<0>(result); - double x_0 = param[0]; - double x_1 = param[1]; - ``` - -## References / External links - - - -- OSQP library: - -## Related issues - - diff --git a/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp deleted file mode 100644 index 8c21553152d70..0000000000000 --- a/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2021 The 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. - -#ifndef AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ -#define AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ - -#include "autoware/osqp_interface/visibility_control.hpp" -#include "osqp/glob_opts.h" // for 'c_int' type ('long' or 'long long') - -#include - -#include - -namespace autoware::osqp_interface -{ -/// \brief Compressed-Column-Sparse Matrix -struct OSQP_INTERFACE_PUBLIC CSC_Matrix -{ - /// Vector of non-zero values. Ex: [4,1,1,2] - std::vector m_vals; - /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner') - std::vector m_row_idxs; - /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') - std::vector m_col_idxs; -}; - -/// \brief Calculate CSC matrix from Eigen matrix -OSQP_INTERFACE_PUBLIC CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat); -/// \brief Calculate upper trapezoidal CSC matrix from square Eigen matrix -OSQP_INTERFACE_PUBLIC CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat); -/// \brief Print the given CSC matrix to the standard output -OSQP_INTERFACE_PUBLIC void printCSCMatrix(const CSC_Matrix & csc_mat); - -} // namespace autoware::osqp_interface - -#endif // AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ diff --git a/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp deleted file mode 100644 index ff3013bd61514..0000000000000 --- a/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp +++ /dev/null @@ -1,194 +0,0 @@ -// Copyright 2021 The 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. - -#ifndef AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ -#define AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ - -#include "autoware/osqp_interface/csc_matrix_conv.hpp" -#include "autoware/osqp_interface/visibility_control.hpp" -#include "osqp/osqp.h" - -#include -#include - -#include -#include -#include -#include -#include - -namespace autoware::osqp_interface -{ -constexpr c_float INF = 1e30; - -/** - * Implementation of a native C++ interface for the OSQP solver. - * - */ -class OSQP_INTERFACE_PUBLIC OSQPInterface -{ -private: - std::unique_ptr> m_work; - std::unique_ptr m_settings; - std::unique_ptr m_data; - // store last work info since work is cleaned up at every execution to prevent memory leak. - OSQPInfo m_latest_work_info; - // Number of parameters to optimize - int64_t m_param_n; - // Flag to check if the current work exists - bool m_work_initialized = false; - // Exitflag - int64_t m_exitflag; - - // Runs the solver on the stored problem. - std::tuple, std::vector, int64_t, int64_t, int64_t> solve(); - - static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept; - -public: - /// \brief Constructor without problem formulation - explicit OSQPInterface( - const c_float eps_abs = std::numeric_limits::epsilon(), const bool polish = true); - /// \brief Constructor with problem setup - /// \param P: (n,n) matrix defining relations between parameters. - /// \param A: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - /// \param q: (n) vector defining the linear cost of the problem. - /// \param l: (m) vector defining the lower bound problem constraint. - /// \param u: (m) vector defining the upper bound problem constraint. - /// \param eps_abs: Absolute convergence tolerance. - OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs); - OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs); - ~OSQPInterface(); - - /**************** - * OPTIMIZATION - ****************/ - /// \brief Solves the stored convex quadratic program (QP) problem using the OSQP solver. - // - /// \return The function returns a tuple containing the solution as two float vectors. - /// \return The first element of the tuple contains the 'primal' solution. - /// \return The second element contains the 'lagrange multiplier' solution. - /// \return The third element contains an integer with solver polish status information. - - /// \details How to use: - /// \details 1. Generate the Eigen matrices P, A and vectors q, l, u according to the problem. - /// \details 2. Initialize the interface and set up the problem. - /// \details osqp_interface = OSQPInterface(P, A, q, l, u, 1e-6); - /// \details 3. Call the optimization function. - /// \details std::tuple, std::vector> result; - /// \details result = osqp_interface.optimize(); - /// \details 4. Access the optimized parameters. - /// \details std::vector param = std::get<0>(result); - /// \details double x_0 = param[0]; - /// \details double x_1 = param[1]; - std::tuple, std::vector, int64_t, int64_t, int64_t> optimize(); - - /// \brief Solves convex quadratic programs (QPs) using the OSQP solver. - /// \return The function returns a tuple containing the solution as two float vectors. - /// \return The first element of the tuple contains the 'primal' solution. - /// \return The second element contains the 'lagrange multiplier' solution. - /// \return The third element contains an integer with solver polish status information. - /// \details How to use: - /// \details 1. Generate the Eigen matrices P, A and vectors q, l, u according to the problem. - /// \details 2. Initialize the interface. - /// \details osqp_interface = OSQPInterface(1e-6); - /// \details 3. Call the optimization function with the problem formulation. - /// \details std::tuple, std::vector> result; - /// \details result = osqp_interface.optimize(P, A, q, l, u, 1e-6); - /// \details 4. Access the optimized parameters. - /// \details std::vector param = std::get<0>(result); - /// \details double x_0 = param[0]; - /// \details double x_1 = param[1]; - std::tuple, std::vector, int64_t, int64_t, int64_t> optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); - - /// \brief Converts the input data and sets up the workspace object. - /// \param P (n,n) matrix defining relations between parameters. - /// \param A (m,n) matrix defining parameter constraints relative to the lower and upper bound. - /// \param q (n) vector defining the linear cost of the problem. - /// \param l (m) vector defining the lower bound problem constraint. - /// \param u (m) vector defining the upper bound problem constraint. - int64_t initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); - int64_t initializeProblem( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u); - - // Setter functions for warm start - bool setWarmStart( - const std::vector & primal_variables, const std::vector & dual_variables); - bool setPrimalVariables(const std::vector & primal_variables); - bool setDualVariables(const std::vector & dual_variables); - - // Updates problem parameters while keeping solution in memory. - // - // Args: - // P_new: (n,n) matrix defining relations between parameters. - // A_new: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - // q_new: (n) vector defining the linear cost of the problem. - // l_new: (m) vector defining the lower bound problem constraint. - // u_new: (m) vector defining the upper bound problem constraint. - void updateP(const Eigen::MatrixXd & P_new); - void updateCscP(const CSC_Matrix & P_csc); - void updateA(const Eigen::MatrixXd & A_new); - void updateCscA(const CSC_Matrix & A_csc); - void updateQ(const std::vector & q_new); - void updateL(const std::vector & l_new); - void updateU(const std::vector & u_new); - void updateBounds(const std::vector & l_new, const std::vector & u_new); - void updateEpsAbs(const double eps_abs); - void updateEpsRel(const double eps_rel); - void updateMaxIter(const int iter); - void updateVerbose(const bool verbose); - void updateRhoInterval(const int rho_interval); - void updateRho(const double rho); - void updateAlpha(const double alpha); - void updateScaling(const int scaling); - void updatePolish(const bool polish); - void updatePolishRefinementIteration(const int polish_refine_iter); - void updateCheckTermination(const int check_termination); - - /// \brief Get the number of iteration taken to solve the problem - inline int64_t getTakenIter() const { return static_cast(m_latest_work_info.iter); } - /// \brief Get the status message for the latest problem solved - inline std::string getStatusMessage() const - { - return static_cast(m_latest_work_info.status); - } - /// \brief Get the status value for the latest problem solved - inline int64_t getStatus() const { return static_cast(m_latest_work_info.status_val); } - /// \brief Get the status polish for the latest problem solved - inline int64_t getStatusPolish() const - { - return static_cast(m_latest_work_info.status_polish); - } - /// \brief Get the runtime of the latest problem solved - inline double getRunTime() const { return m_latest_work_info.run_time; } - /// \brief Get the objective value the latest problem solved - inline double getObjVal() const { return m_latest_work_info.obj_val; } - /// \brief Returns flag asserting interface condition (Healthy condition: 0). - inline int64_t getExitFlag() const { return m_exitflag; } - - void logUnsolvedStatus(const std::string & prefix_message = "") const; -}; - -} // namespace autoware::osqp_interface - -#endif // AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp deleted file mode 100644 index a6cdaa8a0a74e..0000000000000 --- a/common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2021 The 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. - -#ifndef AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ -#define AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ - -//////////////////////////////////////////////////////////////////////////////// -#if defined(__WIN32) -#if defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS) -#define OSQP_INTERFACE_PUBLIC __declspec(dllexport) -#define OSQP_INTERFACE_LOCAL -#else // defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS) -#define OSQP_INTERFACE_PUBLIC __declspec(dllimport) -#define OSQP_INTERFACE_LOCAL -#endif // defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS) -#elif defined(__linux__) -#define OSQP_INTERFACE_PUBLIC __attribute__((visibility("default"))) -#define OSQP_INTERFACE_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define OSQP_INTERFACE_PUBLIC __attribute__((visibility("default"))) -#define OSQP_INTERFACE_LOCAL __attribute__((visibility("hidden"))) -#else -#error "Unsupported Build Configuration" -#endif - -#endif // AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_osqp_interface/src/csc_matrix_conv.cpp b/common/autoware_osqp_interface/src/csc_matrix_conv.cpp deleted file mode 100644 index c6e1a0a42d938..0000000000000 --- a/common/autoware_osqp_interface/src/csc_matrix_conv.cpp +++ /dev/null @@ -1,135 +0,0 @@ -// Copyright 2021 The 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/osqp_interface/csc_matrix_conv.hpp" - -#include -#include - -#include -#include -#include - -namespace autoware::osqp_interface -{ -CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) -{ - const size_t elem = static_cast(mat.nonZeros()); - const Eigen::Index rows = mat.rows(); - const Eigen::Index cols = mat.cols(); - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - c_int elem_count = 0; - - col_idxs.push_back(0); - - for (Eigen::Index j = 0; j < cols; j++) { // col iteration - for (Eigen::Index i = 0; i < rows; i++) { // row iteration - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat) -{ - const size_t elem = static_cast(mat.nonZeros()); - const Eigen::Index rows = mat.rows(); - const Eigen::Index cols = mat.cols(); - - if (rows != cols) { - throw std::invalid_argument("Matrix must be square (n, n)"); - } - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - Eigen::Index trap_last_idx = 0; - c_int elem_count = 0; - - col_idxs.push_back(0); - - for (Eigen::Index j = 0; j < cols; j++) { // col iteration - for (Eigen::Index i = 0; i <= trap_last_idx; i++) { // row iteration - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - trap_last_idx += 1; - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -void printCSCMatrix(const CSC_Matrix & csc_mat) -{ - std::cout << "["; - for (const c_float & val : csc_mat.m_vals) { - std::cout << val << ", "; - } - std::cout << "]\n"; - - std::cout << "["; - for (const c_int & row : csc_mat.m_row_idxs) { - std::cout << row << ", "; - } - std::cout << "]\n"; - - std::cout << "["; - for (const c_int & col : csc_mat.m_col_idxs) { - std::cout << col << ", "; - } - std::cout << "]\n"; -} - -} // namespace autoware::osqp_interface diff --git a/common/autoware_osqp_interface/src/osqp_interface.cpp b/common/autoware_osqp_interface/src/osqp_interface.cpp deleted file mode 100644 index ceeae626222ca..0000000000000 --- a/common/autoware_osqp_interface/src/osqp_interface.cpp +++ /dev/null @@ -1,435 +0,0 @@ -// Copyright 2021 The 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/osqp_interface/osqp_interface.hpp" - -#include "autoware/osqp_interface/csc_matrix_conv.hpp" -#include "osqp/osqp.h" - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware::osqp_interface -{ -OSQPInterface::OSQPInterface(const c_float eps_abs, const bool polish) -: m_work{nullptr, OSQPWorkspaceDeleter} -{ - m_settings = std::make_unique(); - m_data = std::make_unique(); - - if (m_settings) { - osqp_set_default_settings(m_settings.get()); - m_settings->alpha = 1.6; // Change alpha parameter - m_settings->eps_rel = 1.0E-4; - m_settings->eps_abs = eps_abs; - m_settings->eps_prim_inf = 1.0E-4; - m_settings->eps_dual_inf = 1.0E-4; - m_settings->warm_start = true; - m_settings->max_iter = 4000; - m_settings->verbose = false; - m_settings->polish = polish; - } - m_exitflag = 0; -} - -OSQPInterface::OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs) -: OSQPInterface(eps_abs) -{ - initializeProblem(P, A, q, l, u); -} - -OSQPInterface::OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs) -: OSQPInterface(eps_abs) -{ - initializeProblem(P, A, q, l, u); -} - -OSQPInterface::~OSQPInterface() -{ - if (m_data->P) free(m_data->P); - if (m_data->A) free(m_data->A); -} - -void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept -{ - if (ptr != nullptr) { - osqp_cleanup(ptr); - } -} - -void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) -{ - /* - // Transform 'P' into an 'upper trapezoidal matrix' - Eigen::MatrixXd P_trap = P_new.triangularView(); - // Transform 'P' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix P_sparse = P_trap.sparseView(); - double *P_val_ptr = P_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int P_elem_N = P_sparse.nonZeros(); - */ - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new); - osqp_update_P( - m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); -} - -void OSQPInterface::updateCscP(const CSC_Matrix & P_csc) -{ - osqp_update_P( - m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); -} - -void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) -{ - /* - // Transform 'A' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix A_sparse = A_new.sparseView(); - double *A_val_ptr = A_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int A_elem_N = A_sparse.nonZeros(); - */ - CSC_Matrix A_csc = calCSCMatrix(A_new); - osqp_update_A( - m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); - return; -} - -void OSQPInterface::updateCscA(const CSC_Matrix & A_csc) -{ - osqp_update_A( - m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); -} - -void OSQPInterface::updateQ(const std::vector & q_new) -{ - std::vector q_tmp(q_new.begin(), q_new.end()); - double * q_dyn = q_tmp.data(); - osqp_update_lin_cost(m_work.get(), q_dyn); -} - -void OSQPInterface::updateL(const std::vector & l_new) -{ - std::vector l_tmp(l_new.begin(), l_new.end()); - double * l_dyn = l_tmp.data(); - osqp_update_lower_bound(m_work.get(), l_dyn); -} - -void OSQPInterface::updateU(const std::vector & u_new) -{ - std::vector u_tmp(u_new.begin(), u_new.end()); - double * u_dyn = u_tmp.data(); - osqp_update_upper_bound(m_work.get(), u_dyn); -} - -void OSQPInterface::updateBounds( - const std::vector & l_new, const std::vector & u_new) -{ - std::vector l_tmp(l_new.begin(), l_new.end()); - std::vector u_tmp(u_new.begin(), u_new.end()); - double * l_dyn = l_tmp.data(); - double * u_dyn = u_tmp.data(); - osqp_update_bounds(m_work.get(), l_dyn, u_dyn); -} - -void OSQPInterface::updateEpsAbs(const double eps_abs) -{ - m_settings->eps_abs = eps_abs; // for default setting - if (m_work_initialized) { - osqp_update_eps_abs(m_work.get(), eps_abs); // for current work - } -} - -void OSQPInterface::updateEpsRel(const double eps_rel) -{ - m_settings->eps_rel = eps_rel; // for default setting - if (m_work_initialized) { - osqp_update_eps_rel(m_work.get(), eps_rel); // for current work - } -} - -void OSQPInterface::updateMaxIter(const int max_iter) -{ - m_settings->max_iter = max_iter; // for default setting - if (m_work_initialized) { - osqp_update_max_iter(m_work.get(), max_iter); // for current work - } -} - -void OSQPInterface::updateVerbose(const bool is_verbose) -{ - m_settings->verbose = is_verbose; // for default setting - if (m_work_initialized) { - osqp_update_verbose(m_work.get(), is_verbose); // for current work - } -} - -void OSQPInterface::updateRhoInterval(const int rho_interval) -{ - m_settings->adaptive_rho_interval = rho_interval; // for default setting -} - -void OSQPInterface::updateRho(const double rho) -{ - m_settings->rho = rho; - if (m_work_initialized) { - osqp_update_rho(m_work.get(), rho); - } -} - -void OSQPInterface::updateAlpha(const double alpha) -{ - m_settings->alpha = alpha; - if (m_work_initialized) { - osqp_update_alpha(m_work.get(), alpha); - } -} - -void OSQPInterface::updateScaling(const int scaling) -{ - m_settings->scaling = scaling; -} - -void OSQPInterface::updatePolish(const bool polish) -{ - m_settings->polish = polish; - if (m_work_initialized) { - osqp_update_polish(m_work.get(), polish); - } -} - -void OSQPInterface::updatePolishRefinementIteration(const int polish_refine_iter) -{ - if (polish_refine_iter < 0) { - std::cerr << "Polish refinement iterations must be positive" << std::endl; - return; - } - - m_settings->polish_refine_iter = polish_refine_iter; - if (m_work_initialized) { - osqp_update_polish_refine_iter(m_work.get(), polish_refine_iter); - } -} - -void OSQPInterface::updateCheckTermination(const int check_termination) -{ - if (check_termination < 0) { - std::cerr << "Check termination must be positive" << std::endl; - return; - } - - m_settings->check_termination = check_termination; - if (m_work_initialized) { - osqp_update_check_termination(m_work.get(), check_termination); - } -} - -bool OSQPInterface::setWarmStart( - const std::vector & primal_variables, const std::vector & dual_variables) -{ - return setPrimalVariables(primal_variables) && setDualVariables(dual_variables); -} - -bool OSQPInterface::setPrimalVariables(const std::vector & primal_variables) -{ - if (!m_work_initialized) { - return false; - } - - const auto result = osqp_warm_start_x(m_work.get(), primal_variables.data()); - if (result != 0) { - std::cerr << "Failed to set primal variables for warm start" << std::endl; - return false; - } - - return true; -} - -bool OSQPInterface::setDualVariables(const std::vector & dual_variables) -{ - if (!m_work_initialized) { - return false; - } - - const auto result = osqp_warm_start_y(m_work.get(), dual_variables.data()); - if (result != 0) { - std::cerr << "Failed to set dual variables for warm start" << std::endl; - return false; - } - - return true; -} - -int64_t OSQPInterface::initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - // check if arguments are valid - std::stringstream ss; - if (P.rows() != P.cols()) { - ss << "P.rows() and P.cols() are not the same. P.rows() = " << P.rows() - << ", P.cols() = " << P.cols(); - throw std::invalid_argument(ss.str()); - } - if (P.rows() != static_cast(q.size())) { - ss << "P.rows() and q.size() are not the same. P.rows() = " << P.rows() - << ", q.size() = " << q.size(); - throw std::invalid_argument(ss.str()); - } - if (P.rows() != A.cols()) { - ss << "P.rows() and A.cols() are not the same. P.rows() = " << P.rows() - << ", A.cols() = " << A.cols(); - throw std::invalid_argument(ss.str()); - } - if (A.rows() != static_cast(l.size())) { - ss << "A.rows() and l.size() are not the same. A.rows() = " << A.rows() - << ", l.size() = " << l.size(); - throw std::invalid_argument(ss.str()); - } - if (A.rows() != static_cast(u.size())) { - ss << "A.rows() and u.size() are not the same. A.rows() = " << A.rows() - << ", u.size() = " << u.size(); - throw std::invalid_argument(ss.str()); - } - - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - return initializeProblem(P_csc, A_csc, q, l, u); -} - -int64_t OSQPInterface::initializeProblem( - CSC_Matrix P_csc, CSC_Matrix A_csc, const std::vector & q, const std::vector & l, - const std::vector & u) -{ - // Dynamic float arrays - std::vector q_tmp(q.begin(), q.end()); - std::vector l_tmp(l.begin(), l.end()); - std::vector u_tmp(u.begin(), u.end()); - double * q_dyn = q_tmp.data(); - double * l_dyn = l_tmp.data(); - double * u_dyn = u_tmp.data(); - - /********************** - * OBJECTIVE FUNCTION - **********************/ - m_param_n = static_cast(q.size()); - m_data->m = static_cast(l.size()); - - /***************** - * POPULATE DATA - *****************/ - m_data->n = m_param_n; - if (m_data->P) free(m_data->P); - m_data->P = csc_matrix( - m_data->n, m_data->n, static_cast(P_csc.m_vals.size()), P_csc.m_vals.data(), - P_csc.m_row_idxs.data(), P_csc.m_col_idxs.data()); - m_data->q = q_dyn; - if (m_data->A) free(m_data->A); - m_data->A = csc_matrix( - m_data->m, m_data->n, static_cast(A_csc.m_vals.size()), A_csc.m_vals.data(), - A_csc.m_row_idxs.data(), A_csc.m_col_idxs.data()); - m_data->l = l_dyn; - m_data->u = u_dyn; - - // Setup workspace - OSQPWorkspace * workspace; - m_exitflag = osqp_setup(&workspace, m_data.get(), m_settings.get()); - m_work.reset(workspace); - m_work_initialized = true; - - return m_exitflag; -} - -std::tuple, std::vector, int64_t, int64_t, int64_t> -OSQPInterface::solve() -{ - // Solve Problem - osqp_solve(m_work.get()); - - /******************** - * EXTRACT SOLUTION - ********************/ - double * sol_x = m_work->solution->x; - double * sol_y = m_work->solution->y; - std::vector sol_primal(sol_x, sol_x + m_param_n); - std::vector sol_lagrange_multiplier(sol_y, sol_y + m_data->m); - - int64_t status_polish = m_work->info->status_polish; - int64_t status_solution = m_work->info->status_val; - int64_t status_iteration = m_work->info->iter; - - // Result tuple - std::tuple, std::vector, int64_t, int64_t, int64_t> result = - std::make_tuple( - sol_primal, sol_lagrange_multiplier, status_polish, status_solution, status_iteration); - - m_latest_work_info = *(m_work->info); - - return result; -} - -std::tuple, std::vector, int64_t, int64_t, int64_t> -OSQPInterface::optimize() -{ - // Run the solver on the stored problem representation. - std::tuple, std::vector, int64_t, int64_t, int64_t> result = solve(); - return result; -} - -std::tuple, std::vector, int64_t, int64_t, int64_t> -OSQPInterface::optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - // Allocate memory for problem - initializeProblem(P, A, q, l, u); - - // Run the solver on the stored problem representation. - std::tuple, std::vector, int64_t, int64_t, int64_t> result = solve(); - - m_work.reset(); - m_work_initialized = false; - - return result; -} - -void OSQPInterface::logUnsolvedStatus(const std::string & prefix_message) const -{ - const int status = getStatus(); - if (status == 1) { - // No need to log since optimization was solved. - return; - } - - // create message - std::string output_message = ""; - if (prefix_message != "") { - output_message = prefix_message + " "; - } - - const auto status_message = getStatusMessage(); - output_message += "Optimization failed due to " + status_message; - - // log with warning - RCLCPP_WARN(rclcpp::get_logger("osqp_interface"), "%s", output_message.c_str()); -} -} // namespace autoware::osqp_interface diff --git a/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp b/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp deleted file mode 100644 index a63f979a966bf..0000000000000 --- a/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp +++ /dev/null @@ -1,181 +0,0 @@ -// Copyright 2021 The 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/osqp_interface/csc_matrix_conv.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include - -TEST(TestCscMatrixConv, Nominal) -{ - using autoware::osqp_interface::calCSCMatrix; - using autoware::osqp_interface::CSC_Matrix; - - Eigen::MatrixXd rect1(1, 2); - rect1 << 0.0, 1.0; - - const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - ASSERT_EQ(rect_m1.m_vals.size(), size_t(1)); - EXPECT_EQ(rect_m1.m_vals[0], 1.0); - ASSERT_EQ(rect_m1.m_row_idxs.size(), size_t(1)); - EXPECT_EQ(rect_m1.m_row_idxs[0], c_int(0)); - ASSERT_EQ(rect_m1.m_col_idxs.size(), size_t(3)); // nb of columns + 1 - EXPECT_EQ(rect_m1.m_col_idxs[0], c_int(0)); - EXPECT_EQ(rect_m1.m_col_idxs[1], c_int(0)); - EXPECT_EQ(rect_m1.m_col_idxs[2], c_int(1)); - - Eigen::MatrixXd rect2(2, 4); - rect2 << 1.0, 0.0, 3.0, 0.0, 0.0, 6.0, 7.0, 0.0; - - const CSC_Matrix rect_m2 = calCSCMatrix(rect2); - ASSERT_EQ(rect_m2.m_vals.size(), size_t(4)); - EXPECT_EQ(rect_m2.m_vals[0], 1.0); - EXPECT_EQ(rect_m2.m_vals[1], 6.0); - EXPECT_EQ(rect_m2.m_vals[2], 3.0); - EXPECT_EQ(rect_m2.m_vals[3], 7.0); - ASSERT_EQ(rect_m2.m_row_idxs.size(), size_t(4)); - EXPECT_EQ(rect_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(rect_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(rect_m2.m_row_idxs[2], c_int(0)); - EXPECT_EQ(rect_m2.m_row_idxs[3], c_int(1)); - ASSERT_EQ(rect_m2.m_col_idxs.size(), size_t(5)); // nb of columns + 1 - EXPECT_EQ(rect_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(rect_m2.m_col_idxs[1], c_int(1)); - EXPECT_EQ(rect_m2.m_col_idxs[2], c_int(2)); - EXPECT_EQ(rect_m2.m_col_idxs[3], c_int(4)); - EXPECT_EQ(rect_m2.m_col_idxs[4], c_int(4)); - - // Example from http://netlib.org/linalg/html_templates/node92.html - Eigen::MatrixXd square2(6, 6); - square2 << 10.0, 0.0, 0.0, 0.0, -2.0, 0.0, 3.0, 9.0, 0.0, 0.0, 0.0, 3.0, 0.0, 7.0, 8.0, 7.0, 0.0, - 0.0, 3.0, 0.0, 8.0, 7.0, 5.0, 0.0, 0.0, 8.0, 0.0, 9.0, 9.0, 13.0, 0.0, 4.0, 0.0, 0.0, 2.0, -1.0; - - const CSC_Matrix square_m2 = calCSCMatrix(square2); - ASSERT_EQ(square_m2.m_vals.size(), size_t(19)); - EXPECT_EQ(square_m2.m_vals[0], 10.0); - EXPECT_EQ(square_m2.m_vals[1], 3.0); - EXPECT_EQ(square_m2.m_vals[2], 3.0); - EXPECT_EQ(square_m2.m_vals[3], 9.0); - EXPECT_EQ(square_m2.m_vals[4], 7.0); - EXPECT_EQ(square_m2.m_vals[5], 8.0); - EXPECT_EQ(square_m2.m_vals[6], 4.0); - EXPECT_EQ(square_m2.m_vals[7], 8.0); - EXPECT_EQ(square_m2.m_vals[8], 8.0); - EXPECT_EQ(square_m2.m_vals[9], 7.0); - EXPECT_EQ(square_m2.m_vals[10], 7.0); - EXPECT_EQ(square_m2.m_vals[11], 9.0); - EXPECT_EQ(square_m2.m_vals[12], -2.0); - EXPECT_EQ(square_m2.m_vals[13], 5.0); - EXPECT_EQ(square_m2.m_vals[14], 9.0); - EXPECT_EQ(square_m2.m_vals[15], 2.0); - EXPECT_EQ(square_m2.m_vals[16], 3.0); - EXPECT_EQ(square_m2.m_vals[17], 13.0); - EXPECT_EQ(square_m2.m_vals[18], -1.0); - ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(19)); - EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[2], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[3], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[4], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[5], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[6], c_int(5)); - EXPECT_EQ(square_m2.m_row_idxs[7], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[8], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[9], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[10], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[11], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[12], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[13], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[14], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[15], c_int(5)); - EXPECT_EQ(square_m2.m_row_idxs[16], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[17], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[18], c_int(5)); - ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(7)); // nb of columns + 1 - EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[1], c_int(3)); - EXPECT_EQ(square_m2.m_col_idxs[2], c_int(7)); - EXPECT_EQ(square_m2.m_col_idxs[3], c_int(9)); - EXPECT_EQ(square_m2.m_col_idxs[4], c_int(12)); - EXPECT_EQ(square_m2.m_col_idxs[5], c_int(16)); - EXPECT_EQ(square_m2.m_col_idxs[6], c_int(19)); -} -TEST(TestCscMatrixConv, Trapezoidal) -{ - using autoware::osqp_interface::calCSCMatrixTrapezoidal; - using autoware::osqp_interface::CSC_Matrix; - - Eigen::MatrixXd square1(2, 2); - Eigen::MatrixXd square2(3, 3); - Eigen::MatrixXd rect1(1, 2); - square1 << 1.0, 2.0, 2.0, 4.0; - square2 << 0.0, 2.0, 0.0, 4.0, 5.0, 6.0, 0.0, 0.0, 0.0; - rect1 << 0.0, 1.0; - - const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); - // Trapezoidal: skip the lower left triangle (2.0 in this example) - ASSERT_EQ(square_m1.m_vals.size(), size_t(3)); - EXPECT_EQ(square_m1.m_vals[0], 1.0); - EXPECT_EQ(square_m1.m_vals[1], 2.0); - EXPECT_EQ(square_m1.m_vals[2], 4.0); - ASSERT_EQ(square_m1.m_row_idxs.size(), size_t(3)); - EXPECT_EQ(square_m1.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m1.m_row_idxs[1], c_int(0)); - EXPECT_EQ(square_m1.m_row_idxs[2], c_int(1)); - ASSERT_EQ(square_m1.m_col_idxs.size(), size_t(3)); - EXPECT_EQ(square_m1.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m1.m_col_idxs[1], c_int(1)); - EXPECT_EQ(square_m1.m_col_idxs[2], c_int(3)); - - const CSC_Matrix square_m2 = calCSCMatrixTrapezoidal(square2); - ASSERT_EQ(square_m2.m_vals.size(), size_t(3)); - EXPECT_EQ(square_m2.m_vals[0], 2.0); - EXPECT_EQ(square_m2.m_vals[1], 5.0); - EXPECT_EQ(square_m2.m_vals[2], 6.0); - ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(3)); - EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[2], c_int(1)); - ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(4)); - EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[1], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[2], c_int(2)); - EXPECT_EQ(square_m2.m_col_idxs[3], c_int(3)); - - try { - const CSC_Matrix rect_m1 = calCSCMatrixTrapezoidal(rect1); - FAIL() << "calCSCMatrixTrapezoidal should fail with non-square inputs"; - } catch (const std::invalid_argument & e) { - EXPECT_EQ(e.what(), std::string("Matrix must be square (n, n)")); - } -} -TEST(TestCscMatrixConv, Print) -{ - using autoware::osqp_interface::calCSCMatrix; - using autoware::osqp_interface::calCSCMatrixTrapezoidal; - using autoware::osqp_interface::CSC_Matrix; - using autoware::osqp_interface::printCSCMatrix; - Eigen::MatrixXd square1(2, 2); - Eigen::MatrixXd rect1(1, 2); - square1 << 1.0, 2.0, 2.0, 4.0; - rect1 << 0.0, 1.0; - const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); - const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - printCSCMatrix(square_m1); - printCSCMatrix(rect_m1); -} diff --git a/common/autoware_osqp_interface/test/test_osqp_interface.cpp b/common/autoware_osqp_interface/test/test_osqp_interface.cpp deleted file mode 100644 index f65b07e6d792d..0000000000000 --- a/common/autoware_osqp_interface/test/test_osqp_interface.cpp +++ /dev/null @@ -1,164 +0,0 @@ -// Copyright 2021 The 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/osqp_interface/osqp_interface.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include - -namespace -{ -// Problem taken from https://github.com/osqp/osqp/blob/master/tests/basic_qp/generate_problem.py -// -// min 1/2 * x' * P * x + q' * x -// s.t. lb <= A * x <= ub -// -// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] -// [1, 2] [1] [1, 0] [ 0] [0.7] -// [0, 1] [ 0] [0.7] -// [0, 1] [-inf] [inf] -// -// The optimal solution is -// x = [0.3, 0.7]' -// y = [-2.9, 0.0, 0.2, 0.0]` -// obj = 1.88 - -TEST(TestOsqpInterface, BasicQp) -{ - using autoware::osqp_interface::calCSCMatrix; - using autoware::osqp_interface::calCSCMatrixTrapezoidal; - using autoware::osqp_interface::CSC_Matrix; - - auto check_result = - [](const std::tuple, std::vector, int, int, int> & result) { - EXPECT_EQ(std::get<2>(result), 1); // polish succeeded - EXPECT_EQ(std::get<3>(result), 1); // solution succeeded - - static const auto ep = 1.0e-8; - - const auto prime_val = std::get<0>(result); - ASSERT_EQ(prime_val.size(), size_t(2)); - EXPECT_NEAR(prime_val[0], 0.3, ep); - EXPECT_NEAR(prime_val[1], 0.7, ep); - - const auto dual_val = std::get<1>(result); - ASSERT_EQ(dual_val.size(), size_t(4)); - EXPECT_NEAR(dual_val[0], -2.9, ep); - EXPECT_NEAR(dual_val[1], 0.0, ep); - EXPECT_NEAR(dual_val[2], 0.2, ep); - EXPECT_NEAR(dual_val[3], 0.0, ep); - }; - - const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); - const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); - const std::vector q = {1.0, 1.0}; - const std::vector l = {1.0, 0.0, 0.0, -autoware::osqp_interface::INF}; - const std::vector u = {1.0, 0.7, 0.7, autoware::osqp_interface::INF}; - - { - // Define problem during optimization - autoware::osqp_interface::OSQPInterface osqp; - std::tuple, std::vector, int, int, int> result = - osqp.optimize(P, A, q, l, u); - check_result(result); - } - - { - // Define problem during initialization - autoware::osqp_interface::OSQPInterface osqp(P, A, q, l, u, 1e-6); - std::tuple, std::vector, int, int, int> result = osqp.optimize(); - check_result(result); - } - - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem - Eigen::MatrixXd P_ini = Eigen::MatrixXd::Zero(2, 2); - Eigen::MatrixXd A_ini = Eigen::MatrixXd::Zero(4, 2); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::osqp_interface::OSQPInterface osqp(P_ini, A_ini, q_ini, l_ini, u_ini, 1e-6); - osqp.optimize(); - - // Redefine problem before optimization - osqp.initializeProblem(P, A, q, l, u); - result = osqp.optimize(); - check_result(result); - } - - { - // Define problem during initialization with csc matrix - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - autoware::osqp_interface::OSQPInterface osqp(P_csc, A_csc, q, l, u, 1e-6); - std::tuple, std::vector, int, int, int> result = osqp.optimize(); - check_result(result); - } - - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem with csc matrix - CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); - CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::osqp_interface::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); - osqp.optimize(); - - // Redefine problem before optimization - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - osqp.initializeProblem(P_csc, A_csc, q, l, u); - result = osqp.optimize(); - check_result(result); - } - - // add warm startup - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem with csc matrix - CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); - CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::osqp_interface::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); - osqp.optimize(); - - // Redefine problem before optimization - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - osqp.initializeProblem(P_csc, A_csc, q, l, u); - result = osqp.optimize(); - check_result(result); - - osqp.updateCheckTermination(1); - const auto primal_val = std::get<0>(result); - const auto dual_val = std::get<1>(result); - for (size_t i = 0; i < primal_val.size(); ++i) { - std::cerr << primal_val.at(i) << std::endl; - } - osqp.setWarmStart(primal_val, dual_val); - result = osqp.optimize(); - check_result(result); - EXPECT_EQ(osqp.getTakenIter(), 1); - } -} -} // namespace diff --git a/common/autoware_point_types/CHANGELOG.rst b/common/autoware_point_types/CHANGELOG.rst deleted file mode 100644 index 5e26002677a36..0000000000000 --- a/common/autoware_point_types/CHANGELOG.rst +++ /dev/null @@ -1,150 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_point_types -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(autoware_point_types): prefix namespace with autoware::point_types (`#9169 `_) -* feat: migrating pointcloud types (`#6996 `_) - * feat: changed most of sensing to the new type - * chore: started applying changes to the perception stack - * feat: confirmed operation until centerpoint - * feat: reverted to the original implementation of pointpainting - * chore: forgot to push a header - * feat: also implemented the changes for the subsample filters that were out of scope before - * fix: some point type changes were missing from the latest merge from main - * chore: removed unused code, added comments, and brought back a removed publish - * chore: replaced pointcloud_raw for pointcloud_raw_ex to avoid extra processing time in the drivers - * feat: added memory layout checks - * chore: updated documentation regarding the point types - * chore: added hyperlinks to the point definitions. will be valid only once the PR is merged - * fix: fixed compilation due to moving the utilities files to the base library - * chore: separated the utilities functions due to a dependency issue - * chore: forgot that perception also uses the filter class - * feature: adapted the undistortion tests to the new point type - --------- - Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> - Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> -* chore: updated maintainers for the autoware_point_types package (`#7797 `_) -* docs(common): adding .pages file (`#7148 `_) - * docs(common): adding .pages file - * fix naming - * fix naming - * fix naming - * include main page plus explanation to autoware tools - * style(pre-commit): autofix - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* Contributors: Esteve Fernandez, Kenzo Lobos Tsunekawa, Yutaka Kondo, Zulfaqar Azmi - -0.26.0 (2024-04-03) -------------------- -* build: mark autoware_cmake as (`#3616 `_) - * build: mark autoware_cmake as - with , autoware_cmake is automatically exported with ament_target_dependencies() (unecessary) - * style(pre-commit): autofix - * chore: fix pre-commit errors - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> - Co-authored-by: Kenji Miyake -* feat: isolate gtests in all packages (`#693 `_) -* chore: upgrade cmake_minimum_required to 3.14 (`#856 `_) -* refactor: use autoware cmake (`#849 `_) - * remove autoware_auto_cmake - * add build_depend of autoware_cmake - * use autoware_cmake in CMakeLists.txt - * fix bugs - * fix cmake lint errors -* feat: add blockage diagnostics (`#461 `_) - * feat!: add blockage diagnostic - * fix: typo - * docs: add documentation - * ci(pre-commit): autofix - * fix: typo - * ci(pre-commit): autofix - * fix: typo - * chore: add adjustable param - * ci(pre-commit): autofix - * feat!: add blockage diagnostic - * fix: typo - * docs: add documentation - * ci(pre-commit): autofix - * fix: typo - * ci(pre-commit): autofix - * fix: typo - * chore: add adjustable param - * ci(pre-commit): autofix - * chore: rearrange header file - * chore: fix typo - * chore: rearrange header - * fix: revert accident change - * chore: fix typo - * docs: add limits - * chore: check overflow - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* ci: check include guard (`#438 `_) - * ci: check include guard - * apply pre-commit - * Update .pre-commit-config.yaml - Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> - * fix: pre-commit - Co-authored-by: Kenji Miyake - Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> -* feat: add point_types for wrapper (`#784 `_) (`#215 `_) - * add point_types - * Revert "add point_types" - This reverts commit 5810000cd1cbd876bc22372e2bb74ccaca06187b. - * create autoware_point_types pkg - * add include - * add cmath - * fix author - * fix bug - * define epsilon as argument - * add test - * remove unnamed namespace - * update test - * fix test name - * use std::max - * change comparison method - * remove unnencessary line - * fix test - * fix comparison method name - Co-authored-by: Taichi Higashide -* Contributors: Daisuke Nishimatsu, Kenji Miyake, Maxime CLEMENT, Takagi, Isamu, Vincent Richard, badai nguyen diff --git a/common/autoware_point_types/CMakeLists.txt b/common/autoware_point_types/CMakeLists.txt deleted file mode 100644 index c149f79dab71f..0000000000000 --- a/common/autoware_point_types/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_point_types) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -include_directories( - include - SYSTEM - ${PCL_INCLUDE_DIRS} -) - -if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_autoware_point_types - test/test_point_types.cpp - ) - target_include_directories(test_autoware_point_types - PRIVATE include - ) - ament_target_dependencies(test_autoware_point_types - point_cloud_msg_wrapper - ) -endif() - -ament_auto_package() diff --git a/common/autoware_point_types/README.md b/common/autoware_point_types/README.md deleted file mode 100644 index 92f19d2bc353a..0000000000000 --- a/common/autoware_point_types/README.md +++ /dev/null @@ -1 +0,0 @@ -# Autoware Point Types diff --git a/common/autoware_point_types/include/autoware/point_types/types.hpp b/common/autoware_point_types/include/autoware/point_types/types.hpp deleted file mode 100644 index 0fde62222e276..0000000000000 --- a/common/autoware_point_types/include/autoware/point_types/types.hpp +++ /dev/null @@ -1,188 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__POINT_TYPES__TYPES_HPP_ -#define AUTOWARE__POINT_TYPES__TYPES_HPP_ - -#include - -#include - -#include -#include - -namespace autoware::point_types -{ -template -bool float_eq(const T a, const T b, const T eps = 10e-6) -{ - return std::fabs(a - b) < eps; -} - -struct PointXYZI -{ - float x{0.0F}; - float y{0.0F}; - float z{0.0F}; - float intensity{0.0F}; - friend bool operator==(const PointXYZI & p1, const PointXYZI & p2) noexcept - { - return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && - float_eq(p1.z, p2.z) && float_eq(p1.intensity, p2.intensity); - } -}; - -enum ReturnType : uint8_t { - INVALID = 0, - SINGLE_STRONGEST, - SINGLE_LAST, - DUAL_STRONGEST_FIRST, - DUAL_STRONGEST_LAST, - DUAL_WEAK_FIRST, - DUAL_WEAK_LAST, - DUAL_ONLY, -}; - -struct PointXYZIRC -{ - float x{0.0F}; - float y{0.0F}; - float z{0.0F}; - std::uint8_t intensity{0U}; - std::uint8_t return_type{0U}; - std::uint16_t channel{0U}; - - friend bool operator==(const PointXYZIRC & p1, const PointXYZIRC & p2) noexcept - { - return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && - float_eq(p1.z, p2.z) && p1.intensity == p2.intensity && - p1.return_type == p2.return_type && p1.channel == p2.channel; - } -}; - -struct PointXYZIRADRT -{ - float x{0.0F}; - float y{0.0F}; - float z{0.0F}; - float intensity{0.0F}; - uint16_t ring{0U}; - float azimuth{0.0F}; - float distance{0.0F}; - uint8_t return_type{0U}; - double time_stamp{0.0}; - friend bool operator==(const PointXYZIRADRT & p1, const PointXYZIRADRT & p2) noexcept - { - return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && - float_eq(p1.z, p2.z) && float_eq(p1.intensity, p2.intensity) && - p1.ring == p2.ring && float_eq(p1.azimuth, p2.azimuth) && - float_eq(p1.distance, p2.distance) && p1.return_type == p2.return_type && - float_eq(p1.time_stamp, p2.time_stamp); - } -}; - -struct PointXYZIRCAEDT -{ - float x{0.0F}; - float y{0.0F}; - float z{0.0F}; - std::uint8_t intensity{0U}; - std::uint8_t return_type{0U}; - std::uint16_t channel{0U}; - float azimuth{0.0F}; - float elevation{0.0F}; - float distance{0.0F}; - std::uint32_t time_stamp{0U}; - - friend bool operator==(const PointXYZIRCAEDT & p1, const PointXYZIRCAEDT & p2) noexcept - { - return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && - float_eq(p1.z, p2.z) && p1.intensity == p2.intensity && - p1.return_type == p2.return_type && p1.channel == p2.channel && - float_eq(p1.azimuth, p2.azimuth) && float_eq(p1.distance, p2.distance) && - p1.time_stamp == p2.time_stamp; - } -}; - -enum class PointXYZIIndex { X, Y, Z, Intensity }; -enum class PointXYZIRCIndex { X, Y, Z, Intensity, ReturnType, Channel }; -enum class PointXYZIRADRTIndex { - X, - Y, - Z, - Intensity, - Ring, - Azimuth, - Distance, - ReturnType, - TimeStamp -}; -enum class PointXYZIRCAEDTIndex { - X, - Y, - Z, - Intensity, - ReturnType, - Channel, - Azimuth, - Elevation, - Distance, - TimeStamp -}; - -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(azimuth); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(elevation); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(distance); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(return_type); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(time_stamp); - -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(channel); - -using PointXYZIRCGenerator = std::tuple< - point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, - point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, - field_return_type_generator, field_channel_generator>; - -using PointXYZIRADRTGenerator = std::tuple< - point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, - point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, - point_cloud_msg_wrapper::field_ring_generator, field_azimuth_generator, field_distance_generator, - field_return_type_generator, field_time_stamp_generator>; - -using PointXYZIRCAEDTGenerator = std::tuple< - point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, - point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, - field_return_type_generator, field_channel_generator, field_azimuth_generator, - field_elevation_generator, field_distance_generator, field_time_stamp_generator>; - -} // namespace autoware::point_types - -POINT_CLOUD_REGISTER_POINT_STRUCT( - autoware::point_types::PointXYZIRC, - (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)( - std::uint8_t, return_type, return_type)(std::uint16_t, channel, channel)) - -POINT_CLOUD_REGISTER_POINT_STRUCT( - autoware::point_types::PointXYZIRADRT, - (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)( - float, azimuth, azimuth)(float, distance, distance)(std::uint8_t, return_type, return_type)( - double, time_stamp, time_stamp)) - -POINT_CLOUD_REGISTER_POINT_STRUCT( - autoware::point_types::PointXYZIRCAEDT, - (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)( - std::uint8_t, return_type, - return_type)(std::uint16_t, channel, channel)(float, azimuth, azimuth)( - float, elevation, elevation)(float, distance, distance)(std::uint32_t, time_stamp, time_stamp)) -#endif // AUTOWARE__POINT_TYPES__TYPES_HPP_ diff --git a/common/autoware_point_types/package.xml b/common/autoware_point_types/package.xml deleted file mode 100644 index e35e6a63de648..0000000000000 --- a/common/autoware_point_types/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - autoware_point_types - 0.40.0 - The point types definition to use point_cloud_msg_wrapper - David Wong - Max Schmeller - Apache License 2.0 - - ament_cmake_core - ament_cmake_export_dependencies - ament_cmake_test - autoware_cmake - - ament_cmake_core - ament_cmake_test - - ament_cmake_copyright - ament_cmake_cppcheck - ament_cmake_lint_cmake - ament_cmake_xmllint - pcl_ros - point_cloud_msg_wrapper - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - point_cloud_msg_wrapper - - - ament_cmake - - diff --git a/common/autoware_point_types/test/test_point_types.cpp b/common/autoware_point_types/test/test_point_types.cpp deleted file mode 100644 index 08696a9948f60..0000000000000 --- a/common/autoware_point_types/test/test_point_types.cpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/point_types/types.hpp" - -#include - -#include - -TEST(PointEquality, PointXYZI) -{ - using autoware::point_types::PointXYZI; - - PointXYZI pt0{0, 1, 2, 3}; - PointXYZI pt1{0, 1, 2, 3}; - EXPECT_EQ(pt0, pt1); -} - -TEST(PointEquality, PointXYZIRADRT) -{ - using autoware::point_types::PointXYZIRADRT; - - PointXYZIRADRT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8}; - PointXYZIRADRT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8}; - EXPECT_EQ(pt0, pt1); -} - -TEST(PointEquality, PointXYZIRCAEDT) -{ - using autoware::point_types::PointXYZIRCAEDT; - - PointXYZIRCAEDT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; - PointXYZIRCAEDT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; - EXPECT_EQ(pt0, pt1); -} - -TEST(PointEquality, FloatEq) -{ - // test template - EXPECT_TRUE(autoware::point_types::float_eq(1, 1)); - EXPECT_TRUE(autoware::point_types::float_eq(1, 1)); - - // test floating point error - EXPECT_TRUE(autoware::point_types::float_eq(1, 1 + std::numeric_limits::epsilon())); - - // test difference of sign - EXPECT_FALSE(autoware::point_types::float_eq(2, -2)); - EXPECT_FALSE(autoware::point_types::float_eq(-2, 2)); - - // small value difference - EXPECT_FALSE(autoware::point_types::float_eq(2, 2 + 10e-6)); - - // expect same value if epsilon is larger than difference - EXPECT_TRUE(autoware::point_types::float_eq(2, 2 + 10e-6, 10e-5)); -} diff --git a/common/autoware_qp_interface/CHANGELOG.rst b/common/autoware_qp_interface/CHANGELOG.rst deleted file mode 100644 index f76dac861e1e6..0000000000000 --- a/common/autoware_qp_interface/CHANGELOG.rst +++ /dev/null @@ -1,81 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_qp_interface -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* fix: fix package names in changelog files (`#9500 `_) -* fix(autoware_osqp_interface): fix clang-tidy errors (`#9440 `_) -* 0.39.0 -* update changelog -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* refactor(qp_interface): prefix package and namespace with autoware (`#9236 `_) -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Ryuta Kambe, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* refactor(qp_interface): prefix package and namespace with autoware (`#9236 `_) -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* fix(qp_interface): fix unreadVariable (`#8349 `_) - * fix:unreadVariable - * fix:unreadVariable - * fix:unreadVariable - --------- -* perf(velocity_smoother): use ProxQP for faster optimization (`#8028 `_) - * perf(velocity_smoother): use ProxQP for faster optimization - * consider max_iteration - * disable warm start - * fix test - --------- -* refactor(qp_interface): clean up the code (`#8029 `_) - * refactor qp_interface - * variable names: m_XXX -> XXX\_ - * update - * update - --------- -* fix(fake_test_node, osqp_interface, qp_interface): remove unnecessary cppcheck inline suppressions (`#7855 `_) - * fix(fake_test_node, osqp_interface, qp_interface): remove unnecessary cppcheck inline suppressions - * style(pre-commit): autofix - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* Contributors: Ryuta Kambe, Takayuki Murooka, Yutaka Kondo, kobayu858 - -0.26.0 (2024-04-03) -------------------- -* feat(qp_interface): support proxqp (`#3699 `_) - * feat(qp_interface): add proxqp interface - * update - --------- -* feat(qp_interface): add new package which will contain various qp solvers (`#3697 `_) -* Contributors: Takayuki Murooka diff --git a/common/autoware_qp_interface/CMakeLists.txt b/common/autoware_qp_interface/CMakeLists.txt deleted file mode 100644 index 1df75d2d719a0..0000000000000 --- a/common/autoware_qp_interface/CMakeLists.txt +++ /dev/null @@ -1,64 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_qp_interface) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Eigen3 REQUIRED) -find_package(proxsuite REQUIRED) - -# after find_package(osqp_vendor) in ament_auto_find_build_dependencies -find_package(osqp REQUIRED) -get_target_property(OSQP_INCLUDE_SUB_DIR osqp::osqp INTERFACE_INCLUDE_DIRECTORIES) -get_filename_component(OSQP_INCLUDE_DIR ${OSQP_INCLUDE_SUB_DIR} PATH) - -set(QP_INTERFACE_LIB_SRC - src/qp_interface.cpp - src/osqp_interface.cpp - src/osqp_csc_matrix_conv.cpp - src/proxqp_interface.cpp -) - -set(QP_INTERFACE_LIB_HEADERS - include/autoware/qp_interface/qp_interface.hpp - include/autoware/qp_interface/osqp_interface.hpp - include/autoware/qp_interface/osqp_csc_matrix_conv.hpp - include/autoware/qp_interface/proxqp_interface.hpp -) - -ament_auto_add_library(${PROJECT_NAME} SHARED - ${QP_INTERFACE_LIB_SRC} - ${QP_INTERFACE_LIB_HEADERS} -) -target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast) - -target_include_directories(${PROJECT_NAME} - SYSTEM PUBLIC - "${OSQP_INCLUDE_DIR}" - "${EIGEN3_INCLUDE_DIR}" -) - -ament_target_dependencies(${PROJECT_NAME} - Eigen3 - osqp_vendor - proxsuite -) - -# crucial so downstream package builds because osqp_interface exposes osqp.hpp -ament_export_include_directories("${OSQP_INCLUDE_DIR}") -# crucial so the linking order is correct in a downstream package: libosqp_interface.a should come before libosqp.a -ament_export_libraries(osqp::osqp) - -if(BUILD_TESTING) - set(TEST_SOURCES - test/test_osqp_interface.cpp - test/test_csc_matrix_conv.cpp - test/test_proxqp_interface.cpp - ) - set(TEST_OSQP_INTERFACE_EXE test_osqp_interface) - ament_add_ros_isolated_gtest(${TEST_OSQP_INTERFACE_EXE} ${TEST_SOURCES}) - target_link_libraries(${TEST_OSQP_INTERFACE_EXE} ${PROJECT_NAME}) -endif() - -ament_auto_package(INSTALL_TO_SHARE -) diff --git a/common/autoware_qp_interface/design/qp_interface-design.md b/common/autoware_qp_interface/design/qp_interface-design.md deleted file mode 100644 index edc7fa9845206..0000000000000 --- a/common/autoware_qp_interface/design/qp_interface-design.md +++ /dev/null @@ -1,48 +0,0 @@ -# Interface for QP solvers - -This is the design document for the `autoware_qp_interface` package. - -## Purpose / Use cases - -This packages provides a C++ interface for QP solvers. -Currently, supported QP solvers are - -- [OSQP library](https://osqp.org/docs/solver/index.html) - -## Design - -The class `QPInterface` takes a problem formulation as Eigen matrices and vectors, converts these objects into -C-style Compressed-Column-Sparse matrices and dynamic arrays, loads the data into the QP workspace dataholder, and runs the optimizer. - -## Inputs / Outputs / API - -The interface can be used in several ways: - -1. Initialize the interface, and load the problem formulation at the optimization call. - - ```cpp - QPInterface qp_interface; - qp_interface.optimize(P, A, q, l, u); - ``` - -2. WARM START OPTIMIZATION by modifying the problem formulation between optimization runs. - - ```cpp - QPInterface qp_interface(true); - qp_interface.optimize(P, A, q, l, u); - qp_interface.optimize(P_new, A_new, q_new, l_new, u_new); - ``` - - The optimization results are returned as a vector by the optimization function. - - ```cpp - const auto solution = qp_interface.optimize(); - double x_0 = solution[0]; - double x_1 = solution[1]; - ``` - -## References / External links - -- OSQP library: - -## Related issues diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp deleted file mode 100644 index 9575d9d18c69f..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ -#define AUTOWARE__QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ - -#include "osqp/glob_opts.h" - -#include - -#include - -namespace autoware::qp_interface -{ -/// \brief Compressed-Column-Sparse Matrix -struct CSC_Matrix -{ - /// Vector of non-zero values. Ex: [4,1,1,2] - std::vector vals_; - /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner') - std::vector row_idxs_; - /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') - std::vector col_idxs_; -}; - -/// \brief Calculate CSC matrix from Eigen matrix -CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat); -/// \brief Calculate upper trapezoidal CSC matrix from square Eigen matrix -CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat); -/// \brief Print the given CSC matrix to the standard output -void printCSCMatrix(const CSC_Matrix & csc_mat); - -} // namespace autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp deleted file mode 100644 index a5777dd545e9f..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp +++ /dev/null @@ -1,147 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__QP_INTERFACE__OSQP_INTERFACE_HPP_ -#define AUTOWARE__QP_INTERFACE__OSQP_INTERFACE_HPP_ - -#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" -#include "autoware/qp_interface/qp_interface.hpp" -#include "osqp/osqp.h" - -#include -#include -#include -#include - -namespace autoware::qp_interface -{ -constexpr c_float OSQP_INF = 1e30; - -class OSQPInterface : public QPInterface -{ -public: - /// \brief Constructor without problem formulation - OSQPInterface( - const bool enable_warm_start = false, const int max_iteration = 20000, - const c_float eps_abs = std::numeric_limits::epsilon(), - const c_float eps_rel = std::numeric_limits::epsilon(), const bool polish = true, - const bool verbose = false); - /// \brief Constructor with problem setup - /// \param P: (n,n) matrix defining relations between parameters. - /// \param A: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - /// \param q: (n) vector defining the linear cost of the problem. - /// \param l: (m) vector defining the lower bound problem constraint. - /// \param u: (m) vector defining the upper bound problem constraint. - /// \param eps_abs: Absolute convergence tolerance. - OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, - const bool enable_warm_start = false, - const c_float eps_abs = std::numeric_limits::epsilon()); - OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, - const bool enable_warm_start = false, - const c_float eps_abs = std::numeric_limits::epsilon()); - ~OSQPInterface(); - - static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept; - - std::vector optimize( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u); - - int getIterationNumber() const override; - bool isSolved() const override; - std::string getStatus() const override; - - int getPolishStatus() const; - std::vector getDualSolution() const; - - void updateEpsAbs(const double eps_abs) override; - void updateEpsRel(const double eps_rel) override; - void updateVerbose(const bool verbose) override; - - // Updates problem parameters while keeping solution in memory. - // - // Args: - // P_new: (n,n) matrix defining relations between parameters. - // A_new: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - // q_new: (n) vector defining the linear cost of the problem. - // l_new: (m) vector defining the lower bound problem constraint. - // u_new: (m) vector defining the upper bound problem constraint. - void updateP(const Eigen::MatrixXd & P_new); - void updateCscP(const CSC_Matrix & P_csc); - void updateA(const Eigen::MatrixXd & A_new); - void updateCscA(const CSC_Matrix & A_csc); - void updateQ(const std::vector & q_new); - void updateL(const std::vector & l_new); - void updateU(const std::vector & u_new); - void updateBounds(const std::vector & l_new, const std::vector & u_new); - - void updateMaxIter(const int iter); - void updateRhoInterval(const int rho_interval); - void updateRho(const double rho); - void updateAlpha(const double alpha); - void updateScaling(const int scaling); - void updatePolish(const bool polish); - void updatePolishRefinementIteration(const int polish_refine_iter); - void updateCheckTermination(const int check_termination); - - /// \brief Get the number of iteration taken to solve the problem - inline int64_t getTakenIter() const { return static_cast(latest_work_info_.iter); } - /// \brief Get the status message for the latest problem solved - inline std::string getStatusMessage() const - { - return static_cast(latest_work_info_.status); - } - /// \brief Get the runtime of the latest problem solved - inline double getRunTime() const { return latest_work_info_.run_time; } - /// \brief Get the objective value the latest problem solved - inline double getObjVal() const { return latest_work_info_.obj_val; } - /// \brief Returns flag asserting interface condition (Healthy condition: 0). - inline int64_t getExitFlag() const { return exitflag_; } - - // Setter functions for warm start - bool setWarmStart( - const std::vector & primal_variables, const std::vector & dual_variables); - bool setPrimalVariables(const std::vector & primal_variables); - bool setDualVariables(const std::vector & dual_variables); - -private: - std::unique_ptr> work_; - std::unique_ptr settings_; - std::unique_ptr data_; - // store last work info since work is cleaned up at every execution to prevent memory leak. - OSQPInfo latest_work_info_; - // Number of parameters to optimize - int64_t param_n_; - // Flag to check if the current work exists - bool work__initialized = false; - // Exitflag - int64_t exitflag_; - - void initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) override; - - void initializeCSCProblemImpl( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u); - - std::vector optimizeImpl() override; -}; -} // namespace autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp deleted file mode 100644 index 324da4b18c6fd..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp +++ /dev/null @@ -1,57 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__QP_INTERFACE__PROXQP_INTERFACE_HPP_ -#define AUTOWARE__QP_INTERFACE__PROXQP_INTERFACE_HPP_ - -#include "autoware/qp_interface/qp_interface.hpp" - -#include -#include - -#include -#include -#include -#include - -namespace autoware::qp_interface -{ -class ProxQPInterface : public QPInterface -{ -public: - explicit ProxQPInterface( - const bool enable_warm_start, const int max_iteration, const double eps_abs, - const double eps_rel, const bool verbose = false); - - int getIterationNumber() const override; - bool isSolved() const override; - std::string getStatus() const override; - - void updateEpsAbs(const double eps_abs) override; - void updateEpsRel(const double eps_rel) override; - void updateVerbose(const bool verbose) override; - -private: - proxsuite::proxqp::Settings settings_{}; - std::shared_ptr> qp_ptr_{nullptr}; - - void initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) override; - - std::vector optimizeImpl() override; -}; -} // namespace autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__PROXQP_INTERFACE_HPP_ diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp deleted file mode 100644 index be3c598512bf6..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__QP_INTERFACE__QP_INTERFACE_HPP_ -#define AUTOWARE__QP_INTERFACE__QP_INTERFACE_HPP_ - -#include - -#include -#include -#include - -namespace autoware::qp_interface -{ -class QPInterface -{ -public: - explicit QPInterface(const bool enable_warm_start) : enable_warm_start_(enable_warm_start) {} - - std::vector optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); - - virtual bool isSolved() const = 0; - virtual int getIterationNumber() const = 0; - virtual std::string getStatus() const = 0; - - virtual void updateEpsAbs([[maybe_unused]] const double eps_abs) = 0; - virtual void updateEpsRel([[maybe_unused]] const double eps_rel) = 0; - virtual void updateVerbose([[maybe_unused]] const bool verbose) {} - -protected: - bool enable_warm_start_{false}; - - void initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); - - virtual void initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) = 0; - - virtual std::vector optimizeImpl() = 0; - - std::optional variables_num_{std::nullopt}; - std::optional constraints_num_{std::nullopt}; -}; -} // namespace autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__QP_INTERFACE_HPP_ diff --git a/common/autoware_qp_interface/package.xml b/common/autoware_qp_interface/package.xml deleted file mode 100644 index b61d03a36db72..0000000000000 --- a/common/autoware_qp_interface/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - autoware_qp_interface - 0.40.0 - Interface for the QP solvers - Takayuki Murooka - Fumiya Watanabe - Maxime CLEMENT - Satoshi Ota - Apache 2.0 - - ament_cmake_auto - autoware_cmake - - eigen - osqp_vendor - proxsuite - rclcpp - rclcpp_components - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - eigen - - - ament_cmake - - diff --git a/common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp b/common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp deleted file mode 100644 index 15314a9e4a5fa..0000000000000 --- a/common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp +++ /dev/null @@ -1,134 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" - -#include -#include - -#include -#include -#include - -namespace autoware::qp_interface -{ -CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) -{ - const size_t elem = static_cast(mat.nonZeros()); - const Eigen::Index rows = mat.rows(); - const Eigen::Index cols = mat.cols(); - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - c_int elem_count = 0; - - col_idxs.push_back(0); - - for (Eigen::Index j = 0; j < cols; j++) { // col iteration - for (Eigen::Index i = 0; i < rows; i++) { // row iteration - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat) -{ - const size_t elem = static_cast(mat.nonZeros()); - const Eigen::Index rows = mat.rows(); - const Eigen::Index cols = mat.cols(); - - if (rows != cols) { - throw std::invalid_argument("Matrix must be square (n, n)"); - } - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - Eigen::Index trap_last_idx = 0; - c_int elem_count = 0; - - col_idxs.push_back(0); - - for (Eigen::Index j = 0; j < cols; j++) { // col iteration - for (Eigen::Index i = 0; i <= trap_last_idx; i++) { // row iteration - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - trap_last_idx += 1; - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -void printCSCMatrix(const CSC_Matrix & csc_mat) -{ - std::cout << "["; - for (const c_float & val : csc_mat.vals_) { - std::cout << val << ", "; - } - std::cout << "]\n"; - - std::cout << "["; - for (const c_int & row : csc_mat.row_idxs_) { - std::cout << row << ", "; - } - std::cout << "]\n"; - - std::cout << "["; - for (const c_int & col : csc_mat.col_idxs_) { - std::cout << col << ", "; - } - std::cout << "]\n"; -} -} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/src/osqp_interface.cpp b/common/autoware_qp_interface/src/osqp_interface.cpp deleted file mode 100644 index fbb8e71f4c251..0000000000000 --- a/common/autoware_qp_interface/src/osqp_interface.cpp +++ /dev/null @@ -1,394 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/osqp_interface.hpp" - -#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" - -#include - -#include -#include -#include -#include - -namespace autoware::qp_interface -{ -OSQPInterface::OSQPInterface( - const bool enable_warm_start, const int max_iteration, const c_float eps_abs, - const c_float eps_rel, const bool polish, const bool verbose) -: QPInterface(enable_warm_start), work_{nullptr, OSQPWorkspaceDeleter} -{ - settings_ = std::make_unique(); - data_ = std::make_unique(); - - if (settings_) { - osqp_set_default_settings(settings_.get()); - settings_->alpha = 1.6; // Change alpha parameter - settings_->eps_rel = eps_rel; - settings_->eps_abs = eps_abs; - settings_->eps_prim_inf = 1.0E-4; - settings_->eps_dual_inf = 1.0E-4; - settings_->warm_start = enable_warm_start; - settings_->max_iter = max_iteration; - settings_->verbose = verbose; - settings_->polish = polish; - } - exitflag_ = 0; -} - -OSQPInterface::OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, const bool enable_warm_start, - const c_float eps_abs) -: OSQPInterface(enable_warm_start, eps_abs) -{ - initializeProblem(P, A, q, l, u); -} - -OSQPInterface::OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, const bool enable_warm_start, - const c_float eps_abs) -: OSQPInterface(enable_warm_start, eps_abs) -{ - initializeCSCProblemImpl(P, A, q, l, u); -} - -OSQPInterface::~OSQPInterface() -{ - if (data_->P) free(data_->P); - if (data_->A) free(data_->A); -} - -void OSQPInterface::initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - initializeCSCProblemImpl(P_csc, A_csc, q, l, u); -} - -void OSQPInterface::initializeCSCProblemImpl( - CSC_Matrix P_csc, CSC_Matrix A_csc, const std::vector & q, const std::vector & l, - const std::vector & u) -{ - // Dynamic float arrays - std::vector q_tmp(q.begin(), q.end()); - std::vector l_tmp(l.begin(), l.end()); - std::vector u_tmp(u.begin(), u.end()); - double * q_dyn = q_tmp.data(); - double * l_dyn = l_tmp.data(); - double * u_dyn = u_tmp.data(); - - /********************** - * OBJECTIVE FUNCTION - **********************/ - param_n_ = static_cast(q.size()); - data_->m = static_cast(l.size()); - - /***************** - * POPULATE DATA - *****************/ - data_->n = param_n_; - if (data_->P) free(data_->P); - data_->P = csc_matrix( - data_->n, data_->n, static_cast(P_csc.vals_.size()), P_csc.vals_.data(), - P_csc.row_idxs_.data(), P_csc.col_idxs_.data()); - data_->q = q_dyn; - if (data_->A) free(data_->A); - data_->A = csc_matrix( - data_->m, data_->n, static_cast(A_csc.vals_.size()), A_csc.vals_.data(), - A_csc.row_idxs_.data(), A_csc.col_idxs_.data()); - data_->l = l_dyn; - data_->u = u_dyn; - - // Setup workspace - OSQPWorkspace * workspace; - exitflag_ = osqp_setup(&workspace, data_.get(), settings_.get()); - work_.reset(workspace); - work__initialized = true; -} - -void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept -{ - if (ptr != nullptr) { - osqp_cleanup(ptr); - } -} - -void OSQPInterface::updateEpsAbs(const double eps_abs) -{ - settings_->eps_abs = eps_abs; // for default setting - if (work__initialized) { - osqp_update_eps_abs(work_.get(), eps_abs); // for current work - } -} - -void OSQPInterface::updateEpsRel(const double eps_rel) -{ - settings_->eps_rel = eps_rel; // for default setting - if (work__initialized) { - osqp_update_eps_rel(work_.get(), eps_rel); // for current work - } -} - -void OSQPInterface::updateMaxIter(const int max_iter) -{ - settings_->max_iter = max_iter; // for default setting - if (work__initialized) { - osqp_update_max_iter(work_.get(), max_iter); // for current work - } -} - -void OSQPInterface::updateVerbose(const bool is_verbose) -{ - settings_->verbose = is_verbose; // for default setting - if (work__initialized) { - osqp_update_verbose(work_.get(), is_verbose); // for current work - } -} - -void OSQPInterface::updateRhoInterval(const int rho_interval) -{ - settings_->adaptive_rho_interval = rho_interval; // for default setting -} - -void OSQPInterface::updateRho(const double rho) -{ - settings_->rho = rho; - if (work__initialized) { - osqp_update_rho(work_.get(), rho); - } -} - -void OSQPInterface::updateAlpha(const double alpha) -{ - settings_->alpha = alpha; - if (work__initialized) { - osqp_update_alpha(work_.get(), alpha); - } -} - -void OSQPInterface::updateScaling(const int scaling) -{ - settings_->scaling = scaling; -} - -void OSQPInterface::updatePolish(const bool polish) -{ - settings_->polish = polish; - if (work__initialized) { - osqp_update_polish(work_.get(), polish); - } -} - -void OSQPInterface::updatePolishRefinementIteration(const int polish_refine_iter) -{ - if (polish_refine_iter < 0) { - std::cerr << "Polish refinement iterations must be positive" << std::endl; - return; - } - - settings_->polish_refine_iter = polish_refine_iter; - if (work__initialized) { - osqp_update_polish_refine_iter(work_.get(), polish_refine_iter); - } -} - -void OSQPInterface::updateCheckTermination(const int check_termination) -{ - if (check_termination < 0) { - std::cerr << "Check termination must be positive" << std::endl; - return; - } - - settings_->check_termination = check_termination; - if (work__initialized) { - osqp_update_check_termination(work_.get(), check_termination); - } -} - -bool OSQPInterface::setWarmStart( - const std::vector & primal_variables, const std::vector & dual_variables) -{ - return setPrimalVariables(primal_variables) && setDualVariables(dual_variables); -} - -bool OSQPInterface::setPrimalVariables(const std::vector & primal_variables) -{ - if (!work__initialized) { - return false; - } - - const auto result = osqp_warm_start_x(work_.get(), primal_variables.data()); - if (result != 0) { - std::cerr << "Failed to set primal variables for warm start" << std::endl; - return false; - } - - return true; -} - -bool OSQPInterface::setDualVariables(const std::vector & dual_variables) -{ - if (!work__initialized) { - return false; - } - - const auto result = osqp_warm_start_y(work_.get(), dual_variables.data()); - if (result != 0) { - std::cerr << "Failed to set dual variables for warm start" << std::endl; - return false; - } - - return true; -} - -void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) -{ - /* - // Transform 'P' into an 'upper trapezoidal matrix' - Eigen::MatrixXd P_trap = P_new.triangularView(); - // Transform 'P' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix P_sparse = P_trap.sparseView(); - double *P_val_ptr = P_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int P_elem_N = P_sparse.nonZeros(); - */ - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new); - osqp_update_P(work_.get(), P_csc.vals_.data(), OSQP_NULL, static_cast(P_csc.vals_.size())); -} - -void OSQPInterface::updateCscP(const CSC_Matrix & P_csc) -{ - osqp_update_P(work_.get(), P_csc.vals_.data(), OSQP_NULL, static_cast(P_csc.vals_.size())); -} - -void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) -{ - /* - // Transform 'A' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix A_sparse = A_new.sparseView(); - double *A_val_ptr = A_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int A_elem_N = A_sparse.nonZeros(); - */ - CSC_Matrix A_csc = calCSCMatrix(A_new); - osqp_update_A(work_.get(), A_csc.vals_.data(), OSQP_NULL, static_cast(A_csc.vals_.size())); - return; -} - -void OSQPInterface::updateCscA(const CSC_Matrix & A_csc) -{ - osqp_update_A(work_.get(), A_csc.vals_.data(), OSQP_NULL, static_cast(A_csc.vals_.size())); -} - -void OSQPInterface::updateQ(const std::vector & q_new) -{ - std::vector q_tmp(q_new.begin(), q_new.end()); - double * q_dyn = q_tmp.data(); - osqp_update_lin_cost(work_.get(), q_dyn); -} - -void OSQPInterface::updateL(const std::vector & l_new) -{ - std::vector l_tmp(l_new.begin(), l_new.end()); - double * l_dyn = l_tmp.data(); - osqp_update_lower_bound(work_.get(), l_dyn); -} - -void OSQPInterface::updateU(const std::vector & u_new) -{ - std::vector u_tmp(u_new.begin(), u_new.end()); - double * u_dyn = u_tmp.data(); - osqp_update_upper_bound(work_.get(), u_dyn); -} - -void OSQPInterface::updateBounds( - const std::vector & l_new, const std::vector & u_new) -{ - std::vector l_tmp(l_new.begin(), l_new.end()); - std::vector u_tmp(u_new.begin(), u_new.end()); - double * l_dyn = l_tmp.data(); - double * u_dyn = u_tmp.data(); - osqp_update_bounds(work_.get(), l_dyn, u_dyn); -} - -int OSQPInterface::getIterationNumber() const -{ - return work_->info->iter; -} - -std::string OSQPInterface::getStatus() const -{ - return "OSQP_SOLVED"; -} - -bool OSQPInterface::isSolved() const -{ - return latest_work_info_.status_val == 1; -} - -int OSQPInterface::getPolishStatus() const -{ - return static_cast(latest_work_info_.status_polish); -} - -std::vector OSQPInterface::getDualSolution() const -{ - double * sol_y = work_->solution->y; - std::vector dual_solution(sol_y, sol_y + data_->m); - return dual_solution; -} - -std::vector OSQPInterface::optimizeImpl() -{ - osqp_solve(work_.get()); - - double * sol_x = work_->solution->x; - std::vector sol_primal(sol_x, sol_x + param_n_); - - latest_work_info_ = *(work_->info); - - if (!enable_warm_start_) { - work_.reset(); - work__initialized = false; - } - - return sol_primal; -} - -std::vector OSQPInterface::optimize( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u) -{ - initializeCSCProblemImpl(P, A, q, l, u); - const auto result = optimizeImpl(); - - // show polish status if not successful - const int status_polish = static_cast(latest_work_info_.status_polish); - if (status_polish != 1) { - const auto msg = status_polish == 0 ? "unperformed" - : status_polish == -1 ? "unsuccessful" - : "unknown"; - std::cerr << "osqp polish process failed : " << msg << ". The result may be inaccurate" - << std::endl; - } - - return result; -} - -} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/src/proxqp_interface.cpp b/common/autoware_qp_interface/src/proxqp_interface.cpp deleted file mode 100644 index a0ebbf0db0510..0000000000000 --- a/common/autoware_qp_interface/src/proxqp_interface.cpp +++ /dev/null @@ -1,157 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/proxqp_interface.hpp" - -#include -#include -#include - -namespace autoware::qp_interface -{ -using proxsuite::proxqp::QPSolverOutput; - -ProxQPInterface::ProxQPInterface( - const bool enable_warm_start, const int max_iteration, const double eps_abs, const double eps_rel, - const bool verbose) -: QPInterface(enable_warm_start) -{ - settings_.max_iter = max_iteration; - settings_.eps_abs = eps_abs; - settings_.eps_rel = eps_rel; - settings_.verbose = verbose; -} - -void ProxQPInterface::initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - const size_t variables_num = q.size(); - const size_t constraints_num = l.size(); - - const bool enable_warm_start = [&]() { - if ( - !enable_warm_start_ // Warm start is designated - || !qp_ptr_ // QP pointer is initialized - // The number of variables is the same as the previous one. - || !variables_num_ || - *variables_num_ != variables_num - // The number of constraints is the same as the previous one - || !constraints_num_ || *constraints_num_ != constraints_num) { - return false; - } - return true; - }(); - - if (!enable_warm_start) { - qp_ptr_ = std::make_shared>( - variables_num, 0, constraints_num); - } - - settings_.initial_guess = - enable_warm_start ? proxsuite::proxqp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT - : proxsuite::proxqp::InitialGuessStatus::NO_INITIAL_GUESS; - - qp_ptr_->settings = settings_; - - Eigen::SparseMatrix P_sparse(variables_num, constraints_num); - P_sparse = P.sparseView(); - - // NOTE: const std vector cannot be converted to eigen vector - std::vector non_const_q = q; - Eigen::VectorXd eigen_q = - Eigen::Map(non_const_q.data(), non_const_q.size()); - std::vector l_std_vec = l; - Eigen::VectorXd eigen_l = - Eigen::Map(l_std_vec.data(), l_std_vec.size()); - std::vector u_std_vec = u; - Eigen::VectorXd eigen_u = - Eigen::Map(u_std_vec.data(), u_std_vec.size()); - - if (enable_warm_start) { - qp_ptr_->update( - P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); - } else { - qp_ptr_->init( - P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); - } -} - -void ProxQPInterface::updateEpsAbs(const double eps_abs) -{ - settings_.eps_abs = eps_abs; -} - -void ProxQPInterface::updateEpsRel(const double eps_rel) -{ - settings_.eps_rel = eps_rel; -} - -void ProxQPInterface::updateVerbose(const bool is_verbose) -{ - settings_.verbose = is_verbose; -} - -bool ProxQPInterface::isSolved() const -{ - if (qp_ptr_) { - return qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED; - } - return false; -} - -int ProxQPInterface::getIterationNumber() const -{ - if (qp_ptr_) { - return qp_ptr_->results.info.iter; - } - return 0; -} - -std::string ProxQPInterface::getStatus() const -{ - if (qp_ptr_) { - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED) { - return "PROXQP_SOLVED"; - } - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_MAX_ITER_REACHED) { - return "PROXQP_MAX_ITER_REACHED"; - } - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE) { - return "PROXQP_PRIMAL_INFEASIBLE"; - } - // if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE) { - // return "PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE"; - // } - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_DUAL_INFEASIBLE) { - return "PROXQP_DUAL_INFEASIBLE"; - } - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_NOT_RUN) { - return "PROXQP_NOT_RUN"; - } - } - return "None"; -} - -std::vector ProxQPInterface::optimizeImpl() -{ - qp_ptr_->solve(); - - std::vector result; - for (Eigen::Index i = 0; i < qp_ptr_->results.x.size(); ++i) { - result.push_back(qp_ptr_->results.x[i]); - } - return result; -} -} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/src/qp_interface.cpp b/common/autoware_qp_interface/src/qp_interface.cpp deleted file mode 100644 index f01e57772dc4f..0000000000000 --- a/common/autoware_qp_interface/src/qp_interface.cpp +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/qp_interface.hpp" - -#include -#include -#include - -namespace autoware::qp_interface -{ -void QPInterface::initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - // check if arguments are valid - std::stringstream ss; - if (P.rows() != P.cols()) { - ss << "P.rows() and P.cols() are not the same. P.rows() = " << P.rows() - << ", P.cols() = " << P.cols(); - throw std::invalid_argument(ss.str()); - } - if (P.rows() != static_cast(q.size())) { - ss << "P.rows() and q.size() are not the same. P.rows() = " << P.rows() - << ", q.size() = " << q.size(); - throw std::invalid_argument(ss.str()); - } - if (P.rows() != A.cols()) { - ss << "P.rows() and A.cols() are not the same. P.rows() = " << P.rows() - << ", A.cols() = " << A.cols(); - throw std::invalid_argument(ss.str()); - } - if (A.rows() != static_cast(l.size())) { - ss << "A.rows() and l.size() are not the same. A.rows() = " << A.rows() - << ", l.size() = " << l.size(); - throw std::invalid_argument(ss.str()); - } - if (A.rows() != static_cast(u.size())) { - ss << "A.rows() and u.size() are not the same. A.rows() = " << A.rows() - << ", u.size() = " << u.size(); - throw std::invalid_argument(ss.str()); - } - - initializeProblemImpl(P, A, q, l, u); - - variables_num_ = q.size(); - constraints_num_ = l.size(); -} - -std::vector QPInterface::optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - initializeProblem(P, A, q, l, u); - const auto result = optimizeImpl(); - - return result; -} -} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/test/test_csc_matrix_conv.cpp b/common/autoware_qp_interface/test/test_csc_matrix_conv.cpp deleted file mode 100644 index 1f377a1a24535..0000000000000 --- a/common/autoware_qp_interface/test/test_csc_matrix_conv.cpp +++ /dev/null @@ -1,181 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include - -TEST(TestCscMatrixConv, Nominal) -{ - using autoware::qp_interface::calCSCMatrix; - using autoware::qp_interface::CSC_Matrix; - - Eigen::MatrixXd rect1(1, 2); - rect1 << 0.0, 1.0; - - const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - ASSERT_EQ(rect_m1.vals_.size(), size_t(1)); - EXPECT_EQ(rect_m1.vals_[0], 1.0); - ASSERT_EQ(rect_m1.row_idxs_.size(), size_t(1)); - EXPECT_EQ(rect_m1.row_idxs_[0], c_int(0)); - ASSERT_EQ(rect_m1.col_idxs_.size(), size_t(3)); // nb of columns + 1 - EXPECT_EQ(rect_m1.col_idxs_[0], c_int(0)); - EXPECT_EQ(rect_m1.col_idxs_[1], c_int(0)); - EXPECT_EQ(rect_m1.col_idxs_[2], c_int(1)); - - Eigen::MatrixXd rect2(2, 4); - rect2 << 1.0, 0.0, 3.0, 0.0, 0.0, 6.0, 7.0, 0.0; - - const CSC_Matrix rect_m2 = calCSCMatrix(rect2); - ASSERT_EQ(rect_m2.vals_.size(), size_t(4)); - EXPECT_EQ(rect_m2.vals_[0], 1.0); - EXPECT_EQ(rect_m2.vals_[1], 6.0); - EXPECT_EQ(rect_m2.vals_[2], 3.0); - EXPECT_EQ(rect_m2.vals_[3], 7.0); - ASSERT_EQ(rect_m2.row_idxs_.size(), size_t(4)); - EXPECT_EQ(rect_m2.row_idxs_[0], c_int(0)); - EXPECT_EQ(rect_m2.row_idxs_[1], c_int(1)); - EXPECT_EQ(rect_m2.row_idxs_[2], c_int(0)); - EXPECT_EQ(rect_m2.row_idxs_[3], c_int(1)); - ASSERT_EQ(rect_m2.col_idxs_.size(), size_t(5)); // nb of columns + 1 - EXPECT_EQ(rect_m2.col_idxs_[0], c_int(0)); - EXPECT_EQ(rect_m2.col_idxs_[1], c_int(1)); - EXPECT_EQ(rect_m2.col_idxs_[2], c_int(2)); - EXPECT_EQ(rect_m2.col_idxs_[3], c_int(4)); - EXPECT_EQ(rect_m2.col_idxs_[4], c_int(4)); - - // Example from http://netlib.org/linalg/html_templates/node92.html - Eigen::MatrixXd square2(6, 6); - square2 << 10.0, 0.0, 0.0, 0.0, -2.0, 0.0, 3.0, 9.0, 0.0, 0.0, 0.0, 3.0, 0.0, 7.0, 8.0, 7.0, 0.0, - 0.0, 3.0, 0.0, 8.0, 7.0, 5.0, 0.0, 0.0, 8.0, 0.0, 9.0, 9.0, 13.0, 0.0, 4.0, 0.0, 0.0, 2.0, -1.0; - - const CSC_Matrix square_m2 = calCSCMatrix(square2); - ASSERT_EQ(square_m2.vals_.size(), size_t(19)); - EXPECT_EQ(square_m2.vals_[0], 10.0); - EXPECT_EQ(square_m2.vals_[1], 3.0); - EXPECT_EQ(square_m2.vals_[2], 3.0); - EXPECT_EQ(square_m2.vals_[3], 9.0); - EXPECT_EQ(square_m2.vals_[4], 7.0); - EXPECT_EQ(square_m2.vals_[5], 8.0); - EXPECT_EQ(square_m2.vals_[6], 4.0); - EXPECT_EQ(square_m2.vals_[7], 8.0); - EXPECT_EQ(square_m2.vals_[8], 8.0); - EXPECT_EQ(square_m2.vals_[9], 7.0); - EXPECT_EQ(square_m2.vals_[10], 7.0); - EXPECT_EQ(square_m2.vals_[11], 9.0); - EXPECT_EQ(square_m2.vals_[12], -2.0); - EXPECT_EQ(square_m2.vals_[13], 5.0); - EXPECT_EQ(square_m2.vals_[14], 9.0); - EXPECT_EQ(square_m2.vals_[15], 2.0); - EXPECT_EQ(square_m2.vals_[16], 3.0); - EXPECT_EQ(square_m2.vals_[17], 13.0); - EXPECT_EQ(square_m2.vals_[18], -1.0); - ASSERT_EQ(square_m2.row_idxs_.size(), size_t(19)); - EXPECT_EQ(square_m2.row_idxs_[0], c_int(0)); - EXPECT_EQ(square_m2.row_idxs_[1], c_int(1)); - EXPECT_EQ(square_m2.row_idxs_[2], c_int(3)); - EXPECT_EQ(square_m2.row_idxs_[3], c_int(1)); - EXPECT_EQ(square_m2.row_idxs_[4], c_int(2)); - EXPECT_EQ(square_m2.row_idxs_[5], c_int(4)); - EXPECT_EQ(square_m2.row_idxs_[6], c_int(5)); - EXPECT_EQ(square_m2.row_idxs_[7], c_int(2)); - EXPECT_EQ(square_m2.row_idxs_[8], c_int(3)); - EXPECT_EQ(square_m2.row_idxs_[9], c_int(2)); - EXPECT_EQ(square_m2.row_idxs_[10], c_int(3)); - EXPECT_EQ(square_m2.row_idxs_[11], c_int(4)); - EXPECT_EQ(square_m2.row_idxs_[12], c_int(0)); - EXPECT_EQ(square_m2.row_idxs_[13], c_int(3)); - EXPECT_EQ(square_m2.row_idxs_[14], c_int(4)); - EXPECT_EQ(square_m2.row_idxs_[15], c_int(5)); - EXPECT_EQ(square_m2.row_idxs_[16], c_int(1)); - EXPECT_EQ(square_m2.row_idxs_[17], c_int(4)); - EXPECT_EQ(square_m2.row_idxs_[18], c_int(5)); - ASSERT_EQ(square_m2.col_idxs_.size(), size_t(7)); // nb of columns + 1 - EXPECT_EQ(square_m2.col_idxs_[0], c_int(0)); - EXPECT_EQ(square_m2.col_idxs_[1], c_int(3)); - EXPECT_EQ(square_m2.col_idxs_[2], c_int(7)); - EXPECT_EQ(square_m2.col_idxs_[3], c_int(9)); - EXPECT_EQ(square_m2.col_idxs_[4], c_int(12)); - EXPECT_EQ(square_m2.col_idxs_[5], c_int(16)); - EXPECT_EQ(square_m2.col_idxs_[6], c_int(19)); -} -TEST(TestCscMatrixConv, Trapezoidal) -{ - using autoware::qp_interface::calCSCMatrixTrapezoidal; - using autoware::qp_interface::CSC_Matrix; - - Eigen::MatrixXd square1(2, 2); - Eigen::MatrixXd square2(3, 3); - Eigen::MatrixXd rect1(1, 2); - square1 << 1.0, 2.0, 2.0, 4.0; - square2 << 0.0, 2.0, 0.0, 4.0, 5.0, 6.0, 0.0, 0.0, 0.0; - rect1 << 0.0, 1.0; - - const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); - // Trapezoidal: skip the lower left triangle (2.0 in this example) - ASSERT_EQ(square_m1.vals_.size(), size_t(3)); - EXPECT_EQ(square_m1.vals_[0], 1.0); - EXPECT_EQ(square_m1.vals_[1], 2.0); - EXPECT_EQ(square_m1.vals_[2], 4.0); - ASSERT_EQ(square_m1.row_idxs_.size(), size_t(3)); - EXPECT_EQ(square_m1.row_idxs_[0], c_int(0)); - EXPECT_EQ(square_m1.row_idxs_[1], c_int(0)); - EXPECT_EQ(square_m1.row_idxs_[2], c_int(1)); - ASSERT_EQ(square_m1.col_idxs_.size(), size_t(3)); - EXPECT_EQ(square_m1.col_idxs_[0], c_int(0)); - EXPECT_EQ(square_m1.col_idxs_[1], c_int(1)); - EXPECT_EQ(square_m1.col_idxs_[2], c_int(3)); - - const CSC_Matrix square_m2 = calCSCMatrixTrapezoidal(square2); - ASSERT_EQ(square_m2.vals_.size(), size_t(3)); - EXPECT_EQ(square_m2.vals_[0], 2.0); - EXPECT_EQ(square_m2.vals_[1], 5.0); - EXPECT_EQ(square_m2.vals_[2], 6.0); - ASSERT_EQ(square_m2.row_idxs_.size(), size_t(3)); - EXPECT_EQ(square_m2.row_idxs_[0], c_int(0)); - EXPECT_EQ(square_m2.row_idxs_[1], c_int(1)); - EXPECT_EQ(square_m2.row_idxs_[2], c_int(1)); - ASSERT_EQ(square_m2.col_idxs_.size(), size_t(4)); - EXPECT_EQ(square_m2.col_idxs_[0], c_int(0)); - EXPECT_EQ(square_m2.col_idxs_[1], c_int(0)); - EXPECT_EQ(square_m2.col_idxs_[2], c_int(2)); - EXPECT_EQ(square_m2.col_idxs_[3], c_int(3)); - - try { - const CSC_Matrix rect_m1 = calCSCMatrixTrapezoidal(rect1); - FAIL() << "calCSCMatrixTrapezoidal should fail with non-square inputs"; - } catch (const std::invalid_argument & e) { - EXPECT_EQ(e.what(), std::string("Matrix must be square (n, n)")); - } -} -TEST(TestCscMatrixConv, Print) -{ - using autoware::qp_interface::calCSCMatrix; - using autoware::qp_interface::calCSCMatrixTrapezoidal; - using autoware::qp_interface::CSC_Matrix; - using autoware::qp_interface::printCSCMatrix; - Eigen::MatrixXd square1(2, 2); - Eigen::MatrixXd rect1(1, 2); - square1 << 1.0, 2.0, 2.0, 4.0; - rect1 << 0.0, 1.0; - const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); - const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - printCSCMatrix(square_m1); - printCSCMatrix(rect_m1); -} diff --git a/common/autoware_qp_interface/test/test_osqp_interface.cpp b/common/autoware_qp_interface/test/test_osqp_interface.cpp deleted file mode 100644 index f97cc2888afa0..0000000000000 --- a/common/autoware_qp_interface/test/test_osqp_interface.cpp +++ /dev/null @@ -1,170 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/osqp_interface.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include -#include - -namespace -{ -// Problem taken from https://github.com/osqp/osqp/blob/master/tests/basic_qp/generate_problem.py -// -// min 1/2 * x' * P * x + q' * x -// s.t. lb <= A * x <= ub -// -// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] -// [1, 2] [1] [1, 0] [ 0] [0.7] -// [0, 1] [ 0] [0.7] -// [0, 1] [-inf] [inf] -// -// The optimal solution is -// x = [0.3, 0.7]' -// y = [-2.9, 0.0, 0.2, 0.0]` -// obj = 1.88 - -TEST(TestOsqpInterface, BasicQp) -{ - using autoware::qp_interface::calCSCMatrix; - using autoware::qp_interface::calCSCMatrixTrapezoidal; - using autoware::qp_interface::CSC_Matrix; - - auto check_result = - [](const auto & solution, const std::string & status, const int polish_status) { - EXPECT_EQ(status, "OSQP_SOLVED"); - EXPECT_EQ(polish_status, 1); - - static const auto ep = 1.0e-8; - - ASSERT_EQ(solution.size(), size_t(2)); - EXPECT_NEAR(solution[0], 0.3, ep); - EXPECT_NEAR(solution[1], 0.7, ep); - }; - - const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); - const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); - const std::vector q = {1.0, 1.0}; - const std::vector l = {1.0, 0.0, 0.0, -autoware::qp_interface::OSQP_INF}; - const std::vector u = {1.0, 0.7, 0.7, autoware::qp_interface::OSQP_INF}; - - { - // Define problem during optimization - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - { - // Define problem during initialization - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem - Eigen::MatrixXd P_ini = Eigen::MatrixXd::Zero(2, 2); - Eigen::MatrixXd A_ini = Eigen::MatrixXd::Zero(4, 2); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - osqp.QPInterface::optimize(P_ini, A_ini, q_ini, l_ini, u_ini); - } - - { - // Define problem during initialization with csc matrix - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem with csc matrix - CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); - CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); - - // Redefine problem before optimization - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - // add warm startup - { - // Dummy initial problem with csc matrix - CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); - CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::qp_interface::OSQPInterface osqp(true, 4000, 1e-6); // enable warm start - osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); - - // Redefine problem before optimization - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - { - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - - osqp.updateCheckTermination(1); - const auto primal_val = solution; - const auto dual_val = osqp.getDualSolution(); - for (size_t i = 0; i < primal_val.size(); ++i) { - std::cerr << primal_val.at(i) << std::endl; - } - osqp.setWarmStart(primal_val, dual_val); - } - - { - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - // NOTE: This should be true, but currently optimize function reset the workspace, which - // disables warm start. - // EXPECT_EQ(osqp.getTakenIter(), 1); - } -} -} // namespace diff --git a/common/autoware_qp_interface/test/test_proxqp_interface.cpp b/common/autoware_qp_interface/test/test_proxqp_interface.cpp deleted file mode 100644 index e47af10c7aabd..0000000000000 --- a/common/autoware_qp_interface/test/test_proxqp_interface.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/proxqp_interface.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include -#include - -namespace -{ -// Problem taken from -// https://github.com/proxqp/proxqp/blob/master/tests/basic_qp/generate_problem.py -// -// min 1/2 * x' * P * x + q' * x -// s.t. lb <= A * x <= ub -// -// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] -// [1, 2] [1] [1, 0] [ 0] [0.7] -// [0, 1] [ 0] [0.7] -// [0, 1] [-inf] [inf] -// -// The optimal solution is -// x = [0.3, 0.7]' -// y = [-2.9, 0.0, 0.2, 0.0]` -// obj = 1.88 - -TEST(TestProxqpInterface, BasicQp) -{ - auto check_result = [](const auto & solution, const std::string & status) { - EXPECT_EQ(status, "PROXQP_SOLVED"); - - static const auto ep = 1.0e-8; - ASSERT_EQ(solution.size(), size_t(2)); - EXPECT_NEAR(solution[0], 0.3, ep); - EXPECT_NEAR(solution[1], 0.7, ep); - }; - - const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); - const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); - const std::vector q = {1.0, 1.0}; - const std::vector l = {1.0, 0.0, 0.0, -std::numeric_limits::max()}; - const std::vector u = {1.0, 0.7, 0.7, std::numeric_limits::max()}; - - { - // Define problem during optimization - autoware::qp_interface::ProxQPInterface proxqp(false, 4000, 1e-9, 1e-9, false); - const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - const auto status = proxqp.getStatus(); - check_result(solution, status); - } - - { - // Define problem during optimization with warm start - autoware::qp_interface::ProxQPInterface proxqp(true, 4000, 1e-9, 1e-9, false); - { - const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - const auto status = proxqp.getStatus(); - check_result(solution, status); - EXPECT_NE(proxqp.getIterationNumber(), 1); - } - { - const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - const auto status = proxqp.getStatus(); - check_result(solution, status); - EXPECT_EQ(proxqp.getIterationNumber(), 0); - } - } -} -} // namespace diff --git a/common/autoware_test_utils/CMakeLists.txt b/common/autoware_test_utils/CMakeLists.txt index 8bfa44f7bf0ba..20b8f964f879f 100644 --- a/common/autoware_test_utils/CMakeLists.txt +++ b/common/autoware_test_utils/CMakeLists.txt @@ -25,5 +25,4 @@ ament_auto_package(INSTALL_TO_SHARE test_map test_data launch - rviz ) diff --git a/common/autoware_test_utils/config/sample_topic_snapshot.yaml b/common/autoware_test_utils/config/sample_topic_snapshot.yaml index d9eff5d579540..2ff746b84c3ac 100644 --- a/common/autoware_test_utils/config/sample_topic_snapshot.yaml +++ b/common/autoware_test_utils/config/sample_topic_snapshot.yaml @@ -24,6 +24,9 @@ fields: - name: dynamic_object type: PredictedObjects # autoware_perception_msgs::msg::PredictedObjects topic: /perception/object_recognition/objects -# - name: tracked_object -# type: TrackedObjects # autoware_perception_msgs::msg::TrackedObjects -# topic: /perception/object_recognition/tracking/objects + # - name: tracked_object + # type: TrackedObjects # autoware_perception_msgs::msg::TrackedObjects + # topic: /perception/object_recognition/tracking/objects + # - name: path_with_lane_id + # type: PathWithLaneId # tier4_planning_msgs::msg::PathWithLaneId + # topic: /planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id diff --git a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp index 5e07237e2c495..8375d3e731683 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp @@ -59,6 +59,7 @@ using autoware_perception_msgs::msg::TrafficLightGroupArray; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; +using autoware_planning_msgs::msg::PathPoint; using builtin_interfaces::msg::Duration; using builtin_interfaces::msg::Time; using geometry_msgs::msg::Accel; @@ -97,6 +98,9 @@ Duration parse(const YAML::Node & node); template <> Time parse(const YAML::Node & node); +template <> +Point parse(const YAML::Node & node); + template <> std::vector parse(const YAML::Node & node); @@ -145,6 +149,15 @@ std::vector parse(const YAML::Node & node); template <> UUID parse(const YAML::Node & node); +template <> +PathPoint parse(const YAML::Node & node); + +template <> +PathPointWithLaneId parse(const YAML::Node & node); + +template <> +PathWithLaneId parse(const YAML::Node & node); + template <> PredictedPath parse(const YAML::Node & node); diff --git a/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp b/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp index c2185e65422e8..dbd3dd6638c95 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp @@ -19,6 +19,9 @@ #include #include +#include + +#include #include #include @@ -150,7 +153,8 @@ inline void plot_lanelet2_object( const auto center = (left.front().basicPoint2d() + left.back().basicPoint2d() + right.front().basicPoint2d() + right.back().basicPoint2d()) / 4.0; - axes.text(Args(center.x(), center.y(), std::to_string(lanelet.id()))); + axes.text( + Args(center.x(), center.y(), std::to_string(lanelet.id())), Kwargs("clip_on"_a = true)); } if (config_opt && config_opt.value().label) { @@ -214,16 +218,111 @@ inline void plot_lanelet2_object( axes.add_patch(Args(poly.unwrap())); } +struct DrivableAreaConfig +{ + static DrivableAreaConfig defaults() { return {"turquoise", 2.0}; } + std::optional color{}; + std::optional linewidth{}; +}; + +struct PathWithLaneIdConfig +{ + static PathWithLaneIdConfig defaults() + { + return {std::nullopt, "k", 1.0, std::nullopt, false, 1.0}; + } + std::optional label{}; + std::optional color{}; + std::optional linewidth{}; + std::optional da{}; + bool lane_id{}; // & config_opt = std::nullopt); -*/ +inline void plot_autoware_object( + const tier4_planning_msgs::msg::PathWithLaneId & path, autoware::pyplot::Axes & axes, + const std::optional & config_opt = std::nullopt) +{ + py::dict kwargs{}; + if (config_opt) { + const auto & config = config_opt.value(); + if (config.label) { + kwargs["label"] = config.label.value(); + } + if (config.color) { + kwargs["color"] = config.color.value(); + } + if (config.linewidth) { + kwargs["linewidth"] = config.linewidth.value(); + } + } + std::vector xs; + std::vector ys; + std::vector yaw_cos; + std::vector yaw_sin; + std::vector> ids; + const bool plot_lane_id = config_opt ? config_opt.value().lane_id : false; + for (const auto & point : path.points) { + xs.push_back(point.point.pose.position.x); + ys.push_back(point.point.pose.position.y); + const auto th = autoware::universe_utils::getRPY(point.point.pose.orientation).z; + yaw_cos.push_back(std::cos(th)); + yaw_sin.push_back(std::sin(th)); + if (plot_lane_id) { + ids.emplace_back(); + for (const auto & id : point.lane_ids) { + ids.back().push_back(id); + } + } + } + // plot centerline + axes.plot(Args(xs, ys), kwargs); + const auto quiver_scale = + config_opt ? config_opt.value().quiver_size : PathWithLaneIdConfig::defaults().quiver_size; + const auto quiver_color = + config_opt ? (config_opt.value().color ? config_opt.value().color.value() : "k") : "k"; + axes.quiver( + Args(xs, ys, yaw_cos, yaw_sin), Kwargs( + "angles"_a = "xy", "scale_units"_a = "xy", + "scale"_a = quiver_scale, "color"_a = quiver_color)); + if (plot_lane_id) { + for (size_t i = 0; i < xs.size(); ++i) { + std::stringstream ss; + const char * delimiter = ""; + for (const auto id : ids[i]) { + ss << std::exchange(delimiter, ",") << id; + } + axes.text(Args(xs[i], ys[i], ss.str()), Kwargs("clip_on"_a = true)); + } + } + // plot drivable area + if (config_opt && config_opt.value().da) { + auto plot_boundary = [&](const decltype(path.left_bound) & points) { + std::vector xs; + std::vector ys; + for (const auto & point : points) { + xs.push_back(point.x); + ys.push_back(point.y); + } + const auto & cfg = config_opt.value().da.value(); + py::dict kwargs{}; + if (cfg.color) { + kwargs["color"] = cfg.color.value(); + } + if (cfg.linewidth) { + kwargs["linewidth"] = cfg.linewidth.value(); + } + axes.plot(Args(xs, ys), kwargs); + }; + plot_boundary(path.left_bound); + plot_boundary(path.right_bound); + } +} + } // namespace autoware::test_utils #endif // AUTOWARE_TEST_UTILS__VISUALIZATION_HPP_ diff --git a/common/autoware_test_utils/launch/psim_intersection.launch.xml b/common/autoware_test_utils/launch/psim_intersection.launch.xml index 723dd7352219d..dc3e75cca1d96 100644 --- a/common/autoware_test_utils/launch/psim_intersection.launch.xml +++ b/common/autoware_test_utils/launch/psim_intersection.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml b/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml index 7dd888a036f9d..58b71524e72b8 100644 --- a/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml +++ b/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/common/autoware_test_utils/package.xml b/common/autoware_test_utils/package.xml index c328f37ba357d..740e7f840141d 100644 --- a/common/autoware_test_utils/package.xml +++ b/common/autoware_test_utils/package.xml @@ -8,6 +8,7 @@ Takamasa Horibe Zulfaqar Azmi Mamoru Sobue + Yukinari Hisaki Apache License 2.0 Kyoichi Sugahara diff --git a/common/autoware_test_utils/rviz/psim_test_map.rviz b/common/autoware_test_utils/rviz/psim_test_map.rviz deleted file mode 100644 index 8475c94b86bb4..0000000000000 --- a/common/autoware_test_utils/rviz/psim_test_map.rviz +++ /dev/null @@ -1,4569 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 - Splitter Ratio: 0.557669460773468 - Tree Height: 225 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: ~ - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: AutowareDateTimePanel - Name: AutowareDateTimePanel - - Class: rviz_plugins::AutowareStatePanel - Name: AutowareStatePanel - - Class: rviz_plugins::SimulatedClockPanel - Name: SimulatedClockPanel - - Class: rviz_plugins::TrafficLightPublishPanel - Name: TrafficLightPublishPanel -Visualization Manager: - Class: "" - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: false - - Class: rviz_common/Group - Displays: - - Alpha: 0.30000001192092896 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - ars408_front_center: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera0/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera0/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera1/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera1/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera2/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera2/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera3/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera3/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera4/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera4/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera5/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera5/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera6/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera6/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera7/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera7/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - gnss_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - livox_front_left: - Alpha: 1 - Show Axes: false - Show Trail: false - livox_front_left_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - livox_front_right: - Alpha: 1 - Show Axes: false - Show Trail: false - livox_front_right_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_kit_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - tamagawa/imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_left: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_left_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_rear: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_rear_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_right: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_right_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_top: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_top_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Mass Properties: - Inertia: false - Mass: false - Name: VehicleModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz_plugins/PolarGridDisplay - Color: 255; 255; 255 - Delta Range: 10 - Enabled: true - Max Alpha: 0.5 - Max Range: 100 - Max Wave Alpha: 0.5 - Min Alpha: 0.009999999776482582 - Min Wave Alpha: 0.009999999776482582 - Name: PolarGridDisplay - Reference Frame: base_link - Value: true - Wave Color: 255; 255; 255 - Wave Velocity: 40 - - Background Alpha: 0.5 - Background Color: 23; 28; 31 - Class: autoware_overlay_rviz_plugin/SignalDisplay - Dark Traffic Color: 255; 51; 51 - Enabled: true - Gear Topic Test: /vehicle/status/gear_status - Handle Angle Scale: 17 - Hazard Lights Topic: /vehicle/status/hazard_lights_status - Height: 100 - Left: 0 - Light Traffic Color: 255; 153; 153 - Name: SignalDisplay - Primary Color: 174; 174; 174 - Signal Color: 0; 230; 120 - Speed Limit Topic: /planning/scenario_planning/current_max_velocity - Speed Topic: /vehicle/status/velocity_status - Steering Topic: /vehicle/status/steering_status - Top: 10 - Traffic Topic: /planning/scenario_planning/lane_driving/behavior_planning/debug/traffic_signal - Turn Signals Topic: /vehicle/status/turn_indicators_status - Value: true - Width: 550 - Enabled: true - Name: Vehicle - - Class: rviz_plugins/MrmSummaryOverlayDisplay - Enabled: false - Font Size: 10 - Left: 512 - Max Letter Num: 100 - Name: MRM Summary - Text Color: 25; 255; 240 - Top: 64 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /system/emergency/hazard_status - Value: false - Value height offset: 0 - Enabled: true - Name: System - - Class: rviz_common/Group - Displays: - - Alpha: 0.20000000298023224 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 28.71826171875 - Min Value: -7.4224700927734375 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 237 - Min Color: 211; 215; 207 - Min Intensity: 0 - Name: PointCloudMap - Position Transformer: XYZ - Selectable: false - Size (Pixels): 1 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/pointcloud_map - Use Fixed Frame: true - Use rainbow: false - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Lanelet2VectorMap - Namespaces: - bus_stop_area: true - center_lane_line: false - center_line_arrows: false - curbstone: true - lane_start_bound: false - lanelet direction: true - lanelet_id: false - left_lane_bound: true - no_parking_area: true - parking_lots: true - parking_space: true - partitions: true - right_lane_bound: true - road_lanelets: false - shoulder_center_lane_line: false - shoulder_center_line_arrows: true - shoulder_lane_start_bound: true - shoulder_left_lane_bound: true - shoulder_right_lane_bound: true - shoulder_road_lanelets: false - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/vector_map_marker - Value: true - Enabled: true - Name: Map - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.4000000059604645 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 5 - Min Value: -1 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: ConcatenatePointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 1 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/concatenated/pointcloud - Use Fixed Frame: false - Use rainbow: true - Value: true - - Alpha: 0.9990000128746033 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: MeasurementRange - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/lidar/crop_box_filter/crop_box_polygon - Value: false - Enabled: true - Name: LiDAR - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 233; 185; 110 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: false - Position: - Alpha: 0.20000000298023224 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Head Length: 0.699999988079071 - Head Radius: 1.2000000476837158 - Name: PoseWithCovariance - Shaft Length: 1 - Shaft Radius: 0.5 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/gnss/pose_with_covariance - Value: true - Enabled: false - Name: GNSS - Enabled: true - Name: Sensing - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 0; 170; 255 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Head Length: 0.4000000059604645 - Head Radius: 0.6000000238418579 - Name: PoseWithCovInitial - Shaft Length: 0.6000000238418579 - Shaft Radius: 0.30000001192092896 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/initial_pose_with_covariance - Value: false - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 0; 255; 0 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Head Length: 0.4000000059604645 - Head Radius: 0.6000000238418579 - Name: PoseWithCovAligned - Shaft Length: 0.6000000238418579 - Shaft Radius: 0.30000001192092896 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose_with_covariance - Value: false - - Buffer Size: 200 - Class: rviz_plugins::PoseHistory - Enabled: false - Line: - Alpha: 0.9990000128746033 - Color: 170; 255; 127 - Value: true - Width: 0.10000000149011612 - Name: PoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose - Value: false - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 0; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.5 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /localization/util/downsample/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 85; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 85; 255; 127 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Aligned - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.5 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/points_aligned - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MonteCarloInitialPose - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/monte_carlo_initial_pose_marker - Value: true - Enabled: true - Name: NDT - - Class: rviz_common/Group - Displays: - - Buffer Size: 1000 - Class: rviz_plugins::PoseHistory - Enabled: true - Line: - Alpha: 0.9990000128746033 - Color: 0; 255; 255 - Value: true - Width: 0.10000000149011612 - Name: PoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_twist_fusion_filter/pose - Value: true - Enabled: true - Name: EKF - Enabled: true - Name: Localization - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 15 - Min Value: -2 - Value: false - Axis: Z - Channel Name: z - Class: rviz_default_plugins/PointCloud2 - Color: 200; 200; 200 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 15 - Min Color: 0; 0; 0 - Min Intensity: -5 - Name: NoGroundPointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/obstacle_segmentation/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Segmentation - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: DetectedObjects - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - Enabled: false - Name: Detection - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_perception_rviz_plugin/TrackedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Dynamic Status: All - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: TrackedObjects - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/tracking/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - Enabled: false - Name: Tracking - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_perception_rviz_plugin/PredictedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: PredictedObjects - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Maneuver - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/prediction/maneuver - Value: false - Enabled: true - Name: Prediction - Enabled: true - Name: ObjectRecognition - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RecognitionResultOnImage - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/traffic_light/debug/rois - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MapBasedDetectionResult - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers - Value: true - Enabled: true - Name: TrafficLight - - Class: rviz_common/Group - Displays: - - Alpha: 0.5 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/occupancy_grid_map/map - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/occupancy_grid_map/map_updates - Use Timestamp: false - Value: true - Enabled: false - Name: OccupancyGrid - Enabled: true - Name: Perception - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: RouteArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/route_marker - Value: true - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.30000001192092896 - Class: rviz_default_plugins/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.5 - Name: GoalPose - Shaft Length: 3 - Shaft Radius: 0.20000000298023224 - Shape: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/echo_back_goal_pose - Value: true - - Background Alpha: 0.5 - Background Color: 23; 28; 31 - Class: autoware_mission_details_overlay_rviz_plugin/MissionDetailsDisplay - Enabled: true - Height: 100 - Name: MissionDetailsDisplay - Remaining Distance and Time Topic: /planning/mission_remaining_distance_time - Right: 10 - Text Color: 194; 194; 194 - Top: 10 - Value: true - Width: 170 - Enabled: true - Name: MissionPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: ScenarioTrajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/trajectory - Value: true - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.9990000128746033 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: Path - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/path - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.4000000059604645 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.4000000059604645 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_AvoidanceByLC - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/avoidance_by_lane_change - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_Avoidance - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/static_obstacle_avoidance - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_LaneChangeRight - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/lane_change_right - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_LaneChangeLeft - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/lane_change_left - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_GoalPlanner - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/goal_planner - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_StartPlanner - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/start_planner - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_AvoidanceByLC - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/avoidance_by_lane_change - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: (old)PathChangeCandidate_LaneChange - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/lane_change - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_LaneChangeRight - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/lane_change_right - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_LaneChangeLeft - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/lane_change_left - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_ExternalRequestLaneChangeRight - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/external_request_lane_change_right - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_ExternalRequestLaneChangeLeft - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/external_request_lane_change_left - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_Avoidance - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/static_obstacle_avoidance - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_StartPlanner - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/start_planner - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_GoalPlanner - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/goal_planner - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Bound - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/bound - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (StaticObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/static_obstacle_avoidance - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (AvoidanceByLC) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/avoidance_by_lane_change - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (LaneChangeRight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/lane_change_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (LaneChangeLeft) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/lane_change_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ExtLaneChangeRight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/external_request_lane_change_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ExtLaneChangeLeft) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/external_request_lane_change_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (GoalPlanner) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/goal_planner - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (StartPlanner) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/start_planner - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (BlindSpot) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/blind_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Crosswalk) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/crosswalk - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Walkway) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/walkway - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (DetectionArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/detection_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Intersection) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/intersection - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (MergeFromPrivateArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/merge_from_private - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (NoStoppingArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_stopping_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (OcclusionSpot) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/occlusion_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (StopLine) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/stop_line - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (TrafficLight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (VirtualTrafficLight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/virtual_traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (RunOut) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/run_out - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (SpeedBump) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/speed_bump - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (NoDrivableLane) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_drivable_lane - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Arrow - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Crosswalk - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Blind Spot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: TrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: VirtualTrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StopLine - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: DetectionArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: OcclusionSpot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: NoStoppingArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: RunOut - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StaticObstacleAvoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/static_obstacle_avoidance - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: LeftLaneChange - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change_left - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: RightLaneChange - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change_right - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: LaneFollowing - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_following - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: GoalPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/goal_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StartPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/start_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: SideShift - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/side_shift - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: SpeedBump - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/speed_bump - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: DynamicObstacleAvoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/dynamic_obstacle_avoidance - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: NoDrivableLane - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_drivable_lane - Value: false - Enabled: false - Name: DebugMarker - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (StaticObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/static_obstacle_avoidance - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (AvoidanceByLC) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/avoidance_by_lane_change - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (LaneChangeLeft) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/lane_change_left - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (LaneChangeRight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/lane_change_right - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (ExtLaneChangeLeft) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/external_request_lane_change_left - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (ExtLaneChangeRight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/external_request_lane_change_right - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (GoalPlanner) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/goal_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (StartPlanner) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/start_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (DynamicObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/dynamic_obstacle_avoidance - Value: false - Enabled: false - Name: InfoMarker - Enabled: true - Name: BehaviorPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: false - Name: Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/trajectory - Value: false - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.9990000128746033 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleStop) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (SurroundObstacle) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleCruise) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (MotionVelocitySmoother) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/velocity_smoother/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (OutOfLane) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/out_of_lane/virtual_walls - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleVelocityLimiter) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_velocity_limiter/virtual_walls - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (DynamicObstacleStop) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/dynamic_obstacle_stop/virtual_walls - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: SurroundObstacleCheck - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker - Value: true - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: Footprint - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 239; 41; 41 - Enabled: false - Name: FootprintOffset - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_offset - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 10; 21; 255 - Enabled: false - Name: FootprintRecoverOffset - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_recover_offset - Value: false - Enabled: false - Name: SurroundObstacleChecker - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: ObstacleStop - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: CruiseVirtualWall - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: SlowDownVirtualWall - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/slow_down/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: DebugMarker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/marker - Value: true - Enabled: false - Name: ObstacleCruise - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: ObstacleAvoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: OutOfLane - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/out_of_lane/debug_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ObstacleVelocityLimiter - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_velocity_limiter/debug_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: DynamicObstacleStop - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/dynamic_obstacle_stop/debug_markers - Value: true - Enabled: false - Name: MotionVelocityPlanner - Enabled: false - Name: DebugMarker - Enabled: true - Name: MotionPlanning - Enabled: true - Name: LaneDriving - - Class: rviz_common/Group - Displays: - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: false - Name: Costmap - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates - Use Timestamp: false - Value: false - - Alpha: 0.9990000128746033 - Arrow Length: 0.30000001192092896 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 255; 25; 0 - Enabled: true - Head Length: 0.10000000149011612 - Head Radius: 0.20000000298023224 - Name: PartialPoseArray - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.05000000074505806 - Shape: Arrow (3D) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array - Value: true - - Alpha: 0.9990000128746033 - Arrow Length: 0.5 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 0; 0; 255 - Enabled: true - Head Length: 0.10000000149011612 - Head Radius: 0.20000000298023224 - Name: PoseArray - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.05000000074505806 - Shape: Arrow (Flat) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array - Value: true - Enabled: true - Name: Parking - - Class: rviz_plugins/PoseWithUuidStamped - Enabled: true - Length: 1.5 - Name: ModifiedGoal - Radius: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/modified_goal - UUID: - Scale: 0.30000001192092896 - Value: false - Value: true - Enabled: true - Name: ScenarioPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: PlanningErrorMarker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /planning/planning_diagnostics/planning_error_monitor/debug/marker - Value: true - Enabled: false - Name: Diagnostic - Enabled: true - Name: Planning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: Predicted Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/controller_node_exe/debug/predicted_trajectory_in_frenet_coordinate - Value: true - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 1 - Constant Width: true - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 0.05000000074505806 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: false - Name: Resampled Reference Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/controller_node_exe/debug/resampled_reference_trajectory - Value: false - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 1 - Constant Width: true - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: false - Width: 0.20000000298023224 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: true - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (AEB) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/autonomous_emergency_braking/virtual_wall - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Stop Reason - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/longitudinal/stop_reason - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/MPC - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/mpc_follower/debug/markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/PurePursuit - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/controller_node_exe/debug/markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/AEB - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/autonomous_emergency_braking/debug/markers - Value: false - Enabled: true - Name: Control - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: ~ - Enabled: true - Name: Map - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/Camera - Enabled: true - Far Plane Distance: 100 - Image Rendering: background and overlay - Name: PointcloudOnCamera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera6/image_raw - Value: true - Visibility: - Control: - Debug/AEB: true - Debug/MPC: true - Debug/PurePursuit: true - Predicted Trajectory: true - Resampled Reference Trajectory: true - Stop Reason: true - Value: false - VirtualWall: - Value: true - VirtualWall (AEB): true - Debug: - Control: true - Localization: - "": true - Value: true - Map: true - Perception: - "": true - Value: true - Planning: - "": - "": true - Value: true - Value: true - Sensing: - ConcatenatePointCloud: true - RadarRawObjects(white): true - Value: true - Value: true - Localization: - EKF: - PoseHistory: true - Value: true - NDT: - Aligned: true - Initial: true - MonteCarloInitialPose: true - PoseHistory: true - PoseWithCovAligned: true - PoseWithCovInitial: true - Value: true - Value: false - Map: - Lanelet2VectorMap: true - PointCloudMap: true - Value: false - Perception: - ObjectRecognition: - Detection: - DetectedObjects: true - Value: true - Prediction: - Maneuver: true - PredictedObjects: true - Value: true - Tracking: - TrackedObjects: true - Value: true - Value: true - OccupancyGrid: - Map: true - Value: true - Segmentation: - NoGroundPointCloud: true - Value: true - TrafficLight: - MapBasedDetectionResult: true - RecognitionResultOnImage: true - Value: true - Value: false - Planning: - Diagnostic: - PlanningErrorMarker: true - Value: true - MissionPlanning: - GoalPose: true - MissionDetailsDisplay: true - RouteArea: true - Value: true - ScenarioPlanning: - LaneDriving: - BehaviorPlanning: - (old)PathChangeCandidate_LaneChange: true - Bound: true - DebugMarker: - Arrow: true - Blind Spot: true - Crosswalk: true - DetectionArea: true - DynamicObstacleAvoidance: true - GoalPlanner: true - Intersection: true - LaneFollowing: true - LeftLaneChange: true - NoDrivableLane: true - NoStoppingArea: true - OcclusionSpot: true - RightLaneChange: true - RunOut: true - SideShift: true - SpeedBump: true - StartPlanner: true - StaticObstacleAvoidance: true - StopLine: true - TrafficLight: true - Value: true - VirtualTrafficLight: true - InfoMarker: - Info (AvoidanceByLC): true - Info (DynamicObstacleAvoidance): true - Info (ExtLaneChangeLeft): true - Info (ExtLaneChangeRight): true - Info (GoalPlanner): true - Info (LaneChangeLeft): true - Info (LaneChangeRight): true - Info (StartPlanner): true - Info (StaticObstacleAvoidance): true - Value: true - Path: true - PathChangeCandidate_Avoidance: true - PathChangeCandidate_AvoidanceByLC: true - PathChangeCandidate_ExternalRequestLaneChangeLeft: true - PathChangeCandidate_ExternalRequestLaneChangeRight: true - PathChangeCandidate_GoalPlanner: true - PathChangeCandidate_LaneChangeLeft: true - PathChangeCandidate_LaneChangeRight: true - PathChangeCandidate_StartPlanner: true - PathReference_Avoidance: true - PathReference_AvoidanceByLC: true - PathReference_GoalPlanner: true - PathReference_LaneChangeLeft: true - PathReference_LaneChangeRight: true - PathReference_StartPlanner: true - Value: true - VirtualWall: - Value: true - VirtualWall (AvoidanceByLC): true - VirtualWall (BlindSpot): true - VirtualWall (Crosswalk): true - VirtualWall (DetectionArea): true - VirtualWall (ExtLaneChangeLeft): true - VirtualWall (ExtLaneChangeRight): true - VirtualWall (GoalPlanner): true - VirtualWall (Intersection): true - VirtualWall (LaneChangeLeft): true - VirtualWall (LaneChangeRight): true - VirtualWall (MergeFromPrivateArea): true - VirtualWall (NoDrivableLane): true - VirtualWall (NoStoppingArea): true - VirtualWall (OcclusionSpot): true - VirtualWall (RunOut): true - VirtualWall (SpeedBump): true - VirtualWall (StartPlanner): true - VirtualWall (StaticObstacleAvoidance): true - VirtualWall (StopLine): true - VirtualWall (TrafficLight): true - VirtualWall (VirtualTrafficLight): true - VirtualWall (Walkway): true - MotionPlanning: - DebugMarker: - MotionVelocityPlanner: - DynamicObstacleStop: true - ObstacleVelocityLimiter: true - OutOfLane: true - Value: true - ObstacleAvoidance: true - ObstacleCruise: - CruiseVirtualWall: true - DebugMarker: true - SlowDownVirtualWall: true - Value: true - ObstacleStop: true - SurroundObstacleChecker: - Footprint: true - FootprintOffset: true - FootprintRecoverOffset: true - SurroundObstacleCheck: true - Value: true - Value: true - Trajectory: true - Value: true - VirtualWall: - Value: true - VirtualWall (DynamicObstacleStop): true - VirtualWall (MotionVelocitySmoother): true - VirtualWall (ObstacleAvoidance): true - VirtualWall (ObstacleCruise): true - VirtualWall (ObstacleStop): true - VirtualWall (ObstacleVelocityLimiter): true - VirtualWall (OutOfLane): true - VirtualWall (SurroundObstacle): true - Value: true - ModifiedGoal: true - Parking: - Costmap: true - PartialPoseArray: true - PoseArray: true - Value: true - ScenarioTrajectory: true - Value: true - Value: false - Sensing: - GNSS: - PoseWithCovariance: true - Value: true - LiDAR: - ConcatenatePointCloud: true - MeasurementRange: true - Value: true - Value: true - System: - Grid: true - MRM Summary: true - TF: true - Value: false - Vehicle: - PolarGridDisplay: true - SignalDisplay: true - Value: true - VehicleModel: true - Value: true - Zoom Factor: 1 - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 5 - Min Value: -1 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: ConcatenatePointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/concatenated/pointcloud - Use Fixed Frame: false - Use rainbow: true - Value: true - - BUS: - Alpha: 0.5 - Color: 255; 255; 255 - CAR: - Alpha: 0.5 - Color: 255; 255; 255 - CYCLIST: - Alpha: 0.5 - Color: 255; 255; 255 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.5 - Color: 255; 255; 255 - Name: RadarRawObjects(white) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.5 - Color: 255; 255; 255 - Polygon Type: 3d - TRAILER: - Alpha: 0.5 - Color: 255; 255; 255 - TRUCK: - Alpha: 0.5 - Color: 255; 255; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/radar/detected_objects - UNKNOWN: - Alpha: 0.5 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - Enabled: false - Name: Sensing - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 85; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 85; 255; 127 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: NDT pointclouds - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.5 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/points_aligned - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: NDTLoadedPCDMap - Position Transformer: "" - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/debug/loaded_pointcloud_map - Use Fixed Frame: true - Use rainbow: true - Value: true - - Buffer Size: 200 - Class: rviz_plugins::PoseHistory - Enabled: true - Line: - Alpha: 0.9990000128746033 - Color: 170; 255; 127 - Value: true - Width: 0.10000000149011612 - Name: NDTPoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose - Value: true - - Buffer Size: 1000 - Class: rviz_plugins::PoseHistory - Enabled: true - Line: - Alpha: 0.9990000128746033 - Color: 0; 255; 255 - Value: true - Width: 0.10000000149011612 - Name: EKFPoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_twist_fusion_filter/pose - Value: true - Enabled: true - Name: Localization - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Name: Centerpoint(red1) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/centerpoint/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Name: CenterpointROIFusion(red2) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/centerpoint/roi_fusion/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - CAR: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Name: CenterpointValidator(red3) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - TRUCK: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/centerpoint/validation/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - CAR: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Name: PointPainting(light_green1) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - TRUCK: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/pointpainting/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - CAR: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Name: PointPaintingROIFusion(light_green2) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - TRUCK: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/pointpainting/roi_fusion/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - CAR: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Name: PointPaintingValidator(light_green3) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - TRUCK: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/pointpainting/validation/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Name: DetectionByTracker(orange) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/detection_by_tracker/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - CAR: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Name: CameraLidarFusion(purple) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - TRUCK: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/clustering/camera_lidar_fusion/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Name: RadarFarObjects(white) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/radar/far_objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Name: Detection(yellow) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - CAR: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Class: autoware_perception_rviz_plugin/TrackedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Dynamic Status: All - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Name: Tracking(green) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - TRUCK: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/tracking/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Class: autoware_perception_rviz_plugin/PredictedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Name: Prediction(light_blue) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Value: true - Visualization Type: Normal - Enabled: true - Name: Perception - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: LaneChangeLeft - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/lane_change_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: LaneChangeRight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/lane_change_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: AvoidanceLeft - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/static_obstacle_avoidance_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: AvoidanceRight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/static_obstacle_avoidance_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: AvoidanceByLCLeft - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/avoidance_by_lc_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: AvoidanceByLCRight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/avoidance_by_lc_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: StartPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/start_planner - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: GoalPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/goal_planner - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Crosswalk - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/crosswalk - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Intersection - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/intersection - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: BlindSpot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/blind_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: TrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: DetectionArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/detection_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: NoStoppingArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/no_stopping_area - Value: true - Enabled: true - Name: Objects Of Interest - Enabled: true - Name: Planning - - Class: rviz_common/Group - Displays: ~ - Enabled: true - Name: Control - Enabled: false - Name: Debug - Enabled: true - Global Options: - Background Color: 15; 20; 23 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/goal - - Class: tier4_adapi_rviz_plugins::RouteTool - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rviz/routing/rough_goal - - Acceleration: 0 - Class: rviz_plugins/PedestrianInitialPoseTool - Interactive: false - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 1 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/CarInitialPoseTool - H vehicle height: 2 - Interactive: false - L vehicle length: 4 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 3 - W vehicle width: 1.7999999523162842 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/BusInitialPoseTool - H vehicle height: 3.5 - Interactive: false - L vehicle length: 10.5 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - W vehicle width: 2.5 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Class: rviz_plugins/MissionCheckpointTool - Pose Topic: /planning/mission_planning/checkpoint - Theta std deviation: 0.2617993950843811 - X std deviation: 0.5 - Y std deviation: 0.5 - Z position: 0 - - Class: rviz_plugins/DeleteAllObjectsTool - Pose Topic: /simulation/dummy_perception_publisher/object_info - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz_default_plugins) - X: 0 - Y: 0 - Saved: - - Class: rviz_default_plugins/ThirdPersonFollower - Distance: 18 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: ThirdPersonFollower - Near Clip Distance: 0.009999999776482582 - Pitch: 0.20000000298023224 - Target Frame: base_link - Value: ThirdPersonFollower (rviz) - Yaw: 3.141592025756836 - - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: TopDownOrtho - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 -Window Geometry: - AutowareDateTimePanel: - collapsed: false - AutowareStatePanel: - collapsed: false - Displays: - collapsed: false - Height: 939 - Hide Left Dock: false - Hide Right Dock: false - PointcloudOnCamera: - collapsed: false - QMainWindow State: 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 - RecognitionResultOnImage: - collapsed: false - Selection: - collapsed: false - SimulatedClockPanel: - collapsed: false - Tool Properties: - collapsed: false - TrafficLightPublishPanel: - collapsed: false - Views: - collapsed: false - Width: 1920 - X: 0 - Y: 34 diff --git a/common/autoware_test_utils/src/mock_data_parser.cpp b/common/autoware_test_utils/src/mock_data_parser.cpp index 6e7002501bf30..29aed75357441 100644 --- a/common/autoware_test_utils/src/mock_data_parser.cpp +++ b/common/autoware_test_utils/src/mock_data_parser.cpp @@ -65,6 +65,16 @@ std::array parse(const YAML::Node & node) return msg; } +template <> +Point parse(const YAML::Node & node) +{ + Point geom_point; + geom_point.x = node["x"].as(); + geom_point.y = node["y"].as(); + geom_point.z = node["z"].as(); + return geom_point; +} + template <> std::vector parse(const YAML::Node & node) { @@ -285,6 +295,46 @@ Shape parse(const YAML::Node & node) return msg; } +template <> +PathPoint parse(const YAML::Node & node) +{ + PathPoint point; + point.pose = parse(node["pose"]); + point.longitudinal_velocity_mps = node["longitudinal_velocity_mps"].as(); + point.lateral_velocity_mps = node["lateral_velocity_mps"].as(); + point.heading_rate_rps = node["heading_rate_rps"].as(); + point.is_final = node["is_final"].as(); + return point; +} + +template <> +PathPointWithLaneId parse(const YAML::Node & node) +{ + PathPointWithLaneId point; + point.point = parse(node["point"]); + for (const auto & lane_id_node : node["lane_ids"]) { + point.lane_ids.push_back(lane_id_node.as()); + } + return point; +} + +template <> +PathWithLaneId parse(const YAML::Node & node) +{ + PathWithLaneId path; + path.header = parse
(node["header"]); + for (const auto & point_node : node["points"]) { + path.points.push_back(parse(point_node)); + } + for (const auto & left_bound_node : node["left_bound"]) { + path.left_bound.push_back(parse(left_bound_node)); + } + for (const auto & right_bound_node : node["right_bound"]) { + path.right_bound.push_back(parse(right_bound_node)); + } + return path; +} + template <> PredictedPath parse(const YAML::Node & node) { diff --git a/common/autoware_test_utils/src/topic_snapshot_saver.cpp b/common/autoware_test_utils/src/topic_snapshot_saver.cpp index b2cb2a17c9621..bd9af2d5672b8 100644 --- a/common/autoware_test_utils/src/topic_snapshot_saver.cpp +++ b/common/autoware_test_utils/src/topic_snapshot_saver.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include @@ -46,7 +47,8 @@ using MessageType = std::variant< autoware_adapi_v1_msgs::msg::OperationModeState, // 3 autoware_planning_msgs::msg::LaneletRoute, // 4 autoware_perception_msgs::msg::TrafficLightGroupArray, // 5 - autoware_perception_msgs::msg::TrackedObjects // 6 + autoware_perception_msgs::msg::TrackedObjects, // 6 + tier4_planning_msgs::msg::PathWithLaneId // 7 >; std::optional get_topic_index(const std::string & name) @@ -72,6 +74,9 @@ std::optional get_topic_index(const std::string & name) if (name == "TrackedObjects") { return 6; } + if (name == "PathWithLaneId") { + return 7; + } return std::nullopt; } @@ -196,6 +201,7 @@ class TopicSnapShotSaver REGISTER_CALLBACK(4); REGISTER_CALLBACK(5); REGISTER_CALLBACK(6); + REGISTER_CALLBACK(7); } } @@ -243,6 +249,7 @@ class TopicSnapShotSaver REGISTER_WRITE_TYPE(4); REGISTER_WRITE_TYPE(5); REGISTER_WRITE_TYPE(6); + REGISTER_WRITE_TYPE(7); } const std::string desc = std::string(R"(# @@ -253,7 +260,7 @@ class TopicSnapShotSaver # map_path_uri: package:/// # fields(this is array) # - name: -# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | TBD} +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | PathWithLaneId | TBD} # topic: # )"); diff --git a/common/autoware_test_utils/test_map/intersection/lanelet2_map.osm b/common/autoware_test_utils/test_map/intersection/lanelet2_map.osm index c5c5b7b1fc657..9f28ed79a91ae 100644 --- a/common/autoware_test_utils/test_map/intersection/lanelet2_map.osm +++ b/common/autoware_test_utils/test_map/intersection/lanelet2_map.osm @@ -1,6 +1,6 @@ - + @@ -8002,391 +8002,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -10004,16 +9619,6 @@ - - - - - - - - - - @@ -15586,6 +15191,181 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -18669,142 +18449,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -19364,12 +19008,6 @@ - - - - - - @@ -21131,6 +20769,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -22269,15 +21969,6 @@ - - - - - - - - - @@ -22853,6 +22544,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp index 120aed7c7548e..db370eb46ef43 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp @@ -24,10 +24,10 @@ namespace autoware::universe_utils { -class DiagnosticInterface +class DiagnosticsInterface { public: - DiagnosticInterface(rclcpp::Node * node, const std::string & diagnostic_name); + DiagnosticsInterface(rclcpp::Node * node, const std::string & diagnostic_name); void clear(); void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg); template @@ -48,7 +48,7 @@ class DiagnosticInterface }; template -void DiagnosticInterface::add_key_value(const std::string & key, const T & value) +void DiagnosticsInterface::add_key_value(const std::string & key, const T & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; diff --git a/common/autoware_universe_utils/src/ros/diagnostics_interface.cpp b/common/autoware_universe_utils/src/ros/diagnostics_interface.cpp index 978af27f202d4..e4d1ec8494113 100644 --- a/common/autoware_universe_utils/src/ros/diagnostics_interface.cpp +++ b/common/autoware_universe_utils/src/ros/diagnostics_interface.cpp @@ -23,7 +23,7 @@ namespace autoware::universe_utils { -DiagnosticInterface::DiagnosticInterface(rclcpp::Node * node, const std::string & diagnostic_name) +DiagnosticsInterface::DiagnosticsInterface(rclcpp::Node * node, const std::string & diagnostic_name) : clock_(node->get_clock()) { diagnostics_pub_ = @@ -34,7 +34,7 @@ DiagnosticInterface::DiagnosticInterface(rclcpp::Node * node, const std::string diagnostics_status_msg_.hardware_id = node->get_name(); } -void DiagnosticInterface::clear() +void DiagnosticsInterface::clear() { diagnostics_status_msg_.values.clear(); diagnostics_status_msg_.values.shrink_to_fit(); @@ -43,7 +43,7 @@ void DiagnosticInterface::clear() diagnostics_status_msg_.message = ""; } -void DiagnosticInterface::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) +void DiagnosticsInterface::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) { auto it = std::find_if( std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values), @@ -56,7 +56,7 @@ void DiagnosticInterface::add_key_value(const diagnostic_msgs::msg::KeyValue & k } } -void DiagnosticInterface::add_key_value(const std::string & key, const std::string & value) +void DiagnosticsInterface::add_key_value(const std::string & key, const std::string & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; @@ -64,7 +64,7 @@ void DiagnosticInterface::add_key_value(const std::string & key, const std::stri add_key_value(key_value); } -void DiagnosticInterface::add_key_value(const std::string & key, bool value) +void DiagnosticsInterface::add_key_value(const std::string & key, bool value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; @@ -72,7 +72,7 @@ void DiagnosticInterface::add_key_value(const std::string & key, bool value) add_key_value(key_value); } -void DiagnosticInterface::update_level_and_message(const int8_t level, const std::string & message) +void DiagnosticsInterface::update_level_and_message(const int8_t level, const std::string & message) { if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { if (!diagnostics_status_msg_.message.empty()) { @@ -85,12 +85,12 @@ void DiagnosticInterface::update_level_and_message(const int8_t level, const std } } -void DiagnosticInterface::publish(const rclcpp::Time & publish_time_stamp) +void DiagnosticsInterface::publish(const rclcpp::Time & publish_time_stamp) { diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp)); } -diagnostic_msgs::msg::DiagnosticArray DiagnosticInterface::create_diagnostics_array( +diagnostic_msgs::msg::DiagnosticArray DiagnosticsInterface::create_diagnostics_array( const rclcpp::Time & publish_time_stamp) const { diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; 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 index a0683694cc2b7..6ec4fccf78b43 100644 --- a/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp +++ b/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp @@ -25,9 +25,9 @@ #include #include -using autoware::universe_utils::DiagnosticInterface; +using autoware::universe_utils::DiagnosticsInterface; -class TestDiagnosticInterface : public ::testing::Test +class TestDiagnosticsInterface : public ::testing::Test { protected: void SetUp() override @@ -44,9 +44,9 @@ class TestDiagnosticInterface : public ::testing::Test * Test clear(): * Verify that calling clear() resets DiagnosticStatus values, level, and message. */ -TEST_F(TestDiagnosticInterface, ClearTest) +TEST_F(TestDiagnosticsInterface, ClearTest) { - DiagnosticInterface module(node_.get(), "test_diagnostic"); + DiagnosticsInterface module(node_.get(), "test_diagnostic"); // Add some key-value pairs and update level/message module.add_key_value("param1", 42); @@ -87,9 +87,9 @@ TEST_F(TestDiagnosticInterface, ClearTest) * Test add_key_value(): * Verify that adding the same key updates its value instead of adding a duplicate. */ -TEST_F(TestDiagnosticInterface, AddKeyValueTest) +TEST_F(TestDiagnosticsInterface, AddKeyValueTest) { - DiagnosticInterface module(node_.get(), "test_diagnostic"); + DiagnosticsInterface module(node_.get(), "test_diagnostic"); // Call the template version of add_key_value() with different types module.add_key_value("string_key", std::string("initial_value")); @@ -139,9 +139,9 @@ TEST_F(TestDiagnosticInterface, AddKeyValueTest) * Verify that the level is updated to the highest severity and * that messages are concatenated if level > OK. */ -TEST_F(TestDiagnosticInterface, UpdateLevelAndMessageTest) +TEST_F(TestDiagnosticsInterface, UpdateLevelAndMessageTest) { - DiagnosticInterface module(node_.get(), "test_diagnostic"); + DiagnosticsInterface module(node_.get(), "test_diagnostic"); // Initial status is level=OK(0), message="" module.update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::OK, "Initial OK"); diff --git a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp index 080e8f0e6abc3..b828c8d07ac49 100644 --- a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp +++ b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp @@ -25,10 +25,10 @@ #include #include +#include #include #include #include -#include #include #include @@ -139,7 +139,8 @@ class ControlValidator : public rclcpp::Node universe_utils::InterProcessPollingSubscriber::SharedPtr sub_reference_traj_; rclcpp::Publisher::SharedPtr pub_status_; rclcpp::Publisher::SharedPtr pub_markers_; - rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr + pub_processing_time_; // system parameters int64_t diag_error_count_threshold_ = 0; diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp index 0c16fae065939..e0b9c7135e5f0 100644 --- a/control/autoware_control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -48,8 +48,8 @@ ControlValidator::ControlValidator(const rclcpp::NodeOptions & options) pub_markers_ = create_publisher("~/output/markers", 1); - pub_processing_time_ = - this->create_publisher("~/debug/processing_time_ms", 1); + pub_processing_time_ = this->create_publisher( + "~/debug/processing_time_ms", 1); debug_pose_publisher_ = std::make_shared(this); @@ -181,7 +181,7 @@ void ControlValidator::publish_debug_info() debug_pose_publisher_->publish(); // Publish ProcessingTime - 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 = stop_watch.toc(); pub_processing_time_->publish(processing_time_msg); diff --git a/control/autoware_lane_departure_checker/CMakeLists.txt b/control/autoware_lane_departure_checker/CMakeLists.txt index 199195fc88b08..19046a09922f0 100644 --- a/control/autoware_lane_departure_checker/CMakeLists.txt +++ b/control/autoware_lane_departure_checker/CMakeLists.txt @@ -14,6 +14,7 @@ ament_auto_add_library(autoware_lane_departure_checker SHARED src/lane_departure_checker_node/lane_departure_checker.cpp src/lane_departure_checker_node/lane_departure_checker_node.cpp src/lane_departure_checker_node/utils.cpp + src/lane_departure_checker_node/parameters.cpp ) rclcpp_components_register_node(${PROJECT_NAME} diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp index 1ac984394a25e..101338551cbf3 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp @@ -15,15 +15,12 @@ #ifndef AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ #define AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ -#include -#include +#include "autoware/lane_departure_checker/parameters.hpp" + #include #include #include -#include -#include -#include #include #include #include @@ -48,81 +45,33 @@ namespace autoware::lane_departure_checker { -using autoware::universe_utils::LinearRing2d; -using autoware::universe_utils::PoseDeviation; using autoware::universe_utils::Segment2d; -using autoware_planning_msgs::msg::LaneletRoute; -using autoware_planning_msgs::msg::Trajectory; -using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_planning_msgs::msg::PathWithLaneId; -using TrajectoryPoints = std::vector; typedef boost::geometry::index::rtree> SegmentRtree; -struct Param -{ - double footprint_margin_scale{0.0}; - double footprint_extra_margin{0.0}; - double resample_interval{0.0}; - double max_deceleration{0.0}; - double delay_time{0.0}; - double max_lateral_deviation{0.0}; - double max_longitudinal_deviation{0.0}; - double max_yaw_deviation_deg{0.0}; - double min_braking_distance{0.0}; - // nearest search to ego - double ego_nearest_dist_threshold{0.0}; - double ego_nearest_yaw_threshold{0.0}; -}; - -struct Input -{ - nav_msgs::msg::Odometry::ConstSharedPtr current_odom{}; - lanelet::LaneletMapPtr lanelet_map{}; - LaneletRoute::ConstSharedPtr route{}; - lanelet::ConstLanelets route_lanelets{}; - lanelet::ConstLanelets shoulder_lanelets{}; - Trajectory::ConstSharedPtr reference_trajectory{}; - Trajectory::ConstSharedPtr predicted_trajectory{}; - std::vector boundary_types_to_detect{}; -}; - -struct Output -{ - std::map processing_time_map{}; - bool will_leave_lane{}; - bool is_out_of_lane{}; - bool will_cross_boundary{}; - PoseDeviation trajectory_deviation{}; - lanelet::ConstLanelets candidate_lanelets{}; - TrajectoryPoints resampled_trajectory{}; - std::vector vehicle_footprints{}; - std::vector vehicle_passing_areas{}; -}; - class LaneDepartureChecker { public: - LaneDepartureChecker( + explicit LaneDepartureChecker( std::shared_ptr time_keeper = std::make_shared()) : time_keeper_(time_keeper) { } - Output update(const Input & input); - void setParam(const Param & param, const autoware::vehicle_info_utils::VehicleInfo vehicle_info) + LaneDepartureChecker( + const Param & param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + std::shared_ptr time_keeper = + std::make_shared()) + : param_(param), + vehicle_info_ptr_(std::make_shared(vehicle_info)), + time_keeper_(time_keeper) { - param_ = param; - vehicle_info_ptr_ = std::make_shared(vehicle_info); } + Output update(const Input & input); void setParam(const Param & param) { param_ = param; } - void setVehicleInfo(const autoware::vehicle_info_utils::VehicleInfo vehicle_info) - { - vehicle_info_ptr_ = std::make_shared(vehicle_info); - } - bool checkPathWillLeaveLane( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const; diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp index 7e2229d7b5754..7aaf4816708bd 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ #include "autoware/lane_departure_checker/lane_departure_checker.hpp" +#include "autoware/lane_departure_checker/parameters.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" #include @@ -24,6 +25,7 @@ #include #include +#include #include #include #include @@ -31,7 +33,6 @@ #include #include #include -#include #include #include @@ -47,21 +48,6 @@ namespace autoware::lane_departure_checker { using autoware_map_msgs::msg::LaneletMapBin; -struct NodeParam -{ - bool will_out_of_lane_checker; - bool out_of_lane_checker; - bool boundary_departure_checker; - - double update_rate; - bool visualize_lanelet; - bool include_right_lanes; - bool include_left_lanes; - bool include_opposite_lanes; - bool include_conflicting_lanes; - std::vector boundary_types_to_detect; -}; - class LaneDepartureCheckerNode : public rclcpp::Node { public: @@ -115,7 +101,8 @@ class LaneDepartureCheckerNode : public rclcpp::Node autoware::universe_utils::DebugPublisher debug_publisher_{this, "~/debug"}; 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_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/parameters.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/parameters.hpp new file mode 100644 index 0000000000000..425b032af425f --- /dev/null +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/parameters.hpp @@ -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. + +#ifndef AUTOWARE__LANE_DEPARTURE_CHECKER__PARAMETERS_HPP_ +#define AUTOWARE__LANE_DEPARTURE_CHECKER__PARAMETERS_HPP_ + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace autoware::lane_departure_checker +{ +using autoware::universe_utils::PoseDeviation; +using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using autoware::universe_utils::LinearRing2d; + +struct Param +{ + static Param init(rclcpp::Node & node); + double footprint_margin_scale{}; + double footprint_extra_margin{}; + double resample_interval{}; + double max_deceleration{}; + double delay_time{}; + double max_lateral_deviation{}; + double max_longitudinal_deviation{}; + double max_yaw_deviation_deg{}; + double min_braking_distance{}; + // nearest search to ego + double ego_nearest_dist_threshold{}; + double ego_nearest_yaw_threshold{}; +}; + +struct NodeParam +{ + static NodeParam init(rclcpp::Node & node); + bool will_out_of_lane_checker{}; + bool out_of_lane_checker{}; + bool boundary_departure_checker{}; + + double update_rate{}; + bool visualize_lanelet{}; + bool include_right_lanes{}; + bool include_left_lanes{}; + bool include_opposite_lanes{}; + bool include_conflicting_lanes{}; + std::vector boundary_types_to_detect{}; +}; + +struct Input +{ + nav_msgs::msg::Odometry::ConstSharedPtr current_odom{}; + lanelet::LaneletMapPtr lanelet_map{}; + LaneletRoute::ConstSharedPtr route{}; + lanelet::ConstLanelets route_lanelets{}; + lanelet::ConstLanelets shoulder_lanelets{}; + Trajectory::ConstSharedPtr reference_trajectory{}; + Trajectory::ConstSharedPtr predicted_trajectory{}; + std::vector boundary_types_to_detect{}; +}; + +struct Output +{ + std::map processing_time_map{}; + bool will_leave_lane{}; + bool is_out_of_lane{}; + bool will_cross_boundary{}; + PoseDeviation trajectory_deviation{}; + lanelet::ConstLanelets candidate_lanelets{}; + TrajectoryPoints resampled_trajectory{}; + std::vector vehicle_footprints{}; + std::vector vehicle_passing_areas{}; +}; +} // namespace autoware::lane_departure_checker + +#endif // AUTOWARE__LANE_DEPARTURE_CHECKER__PARAMETERS_HPP_ diff --git a/control/autoware_lane_departure_checker/package.xml b/control/autoware_lane_departure_checker/package.xml index 2689b4a4dbcb7..97ad5eaf82e00 100644 --- a/control/autoware_lane_departure_checker/package.xml +++ b/control/autoware_lane_departure_checker/package.xml @@ -14,6 +14,7 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_internal_debug_msgs autoware_lanelet2_extension autoware_map_msgs autoware_motion_utils @@ -32,7 +33,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 366b84a1f6131..32384b7e6c7dd 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -126,52 +126,24 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o : Node("lane_departure_checker_node", options) { using std::placeholders::_1; - - // Enable feature - node_param_.will_out_of_lane_checker = declare_parameter("will_out_of_lane_checker"); - node_param_.out_of_lane_checker = declare_parameter("out_of_lane_checker"); - node_param_.boundary_departure_checker = declare_parameter("boundary_departure_checker"); - - // Node Parameter - node_param_.update_rate = declare_parameter("update_rate"); - node_param_.visualize_lanelet = declare_parameter("visualize_lanelet"); - node_param_.include_right_lanes = declare_parameter("include_right_lanes"); - node_param_.include_left_lanes = declare_parameter("include_left_lanes"); - node_param_.include_opposite_lanes = declare_parameter("include_opposite_lanes"); - node_param_.include_conflicting_lanes = declare_parameter("include_conflicting_lanes"); - - // Boundary_departure_checker - node_param_.boundary_types_to_detect = - declare_parameter>("boundary_types_to_detect"); + node_param_ = NodeParam::init(*this); + param_ = Param::init(*this); // Vehicle Info const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); vehicle_length_m_ = vehicle_info.vehicle_length_m; - // Core Parameter - param_.footprint_margin_scale = declare_parameter("footprint_margin_scale"); - param_.footprint_extra_margin = declare_parameter("footprint_extra_margin"); - param_.resample_interval = declare_parameter("resample_interval"); - param_.max_deceleration = declare_parameter("max_deceleration"); - param_.delay_time = declare_parameter("delay_time"); - param_.max_lateral_deviation = declare_parameter("max_lateral_deviation"); - param_.max_longitudinal_deviation = declare_parameter("max_longitudinal_deviation"); - param_.max_yaw_deviation_deg = declare_parameter("max_yaw_deviation_deg"); - param_.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); - param_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); - param_.min_braking_distance = declare_parameter("min_braking_distance"); - // Parameter Callback set_param_res_ = add_on_set_parameters_callback(std::bind(&LaneDepartureCheckerNode::onParameter, this, _1)); // Core - lane_departure_checker_ = std::make_unique(); - lane_departure_checker_->setParam(param_, vehicle_info); + lane_departure_checker_ = std::make_unique(param_, vehicle_info); // Publisher processing_time_publisher_ = - this->create_publisher("~/debug/processing_time_ms", 1); + this->create_publisher( + "~/debug/processing_time_ms", 1); // Nothing // Diagnostic Updater @@ -342,10 +314,11 @@ void LaneDepartureCheckerNode::onTimer() { const auto & deviation = output_.trajectory_deviation; - debug_publisher_.publish( + debug_publisher_.publish( "deviation/lateral", deviation.lateral); - debug_publisher_.publish("deviation/yaw", deviation.yaw); - debug_publisher_.publish( + debug_publisher_.publish( + "deviation/yaw", deviation.yaw); + debug_publisher_.publish( "deviation/yaw_deg", rad2deg(deviation.yaw)); } processing_time_map["Node: publishTrajectoryDeviation"] = stop_watch.toc(true); @@ -361,7 +334,7 @@ void LaneDepartureCheckerNode::onTimer() processing_time_map["Total"] = stop_watch.toc("Total"); processing_diag_publisher_.publish(processing_time_map); - 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_time_map["Total"]; processing_time_publisher_->publish(processing_time_msg); diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/parameters.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/parameters.cpp new file mode 100644 index 0000000000000..f3aa23275e35c --- /dev/null +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/parameters.cpp @@ -0,0 +1,59 @@ +// 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/lane_departure_checker/parameters.hpp" + +#include +#include + +#include + +namespace autoware::lane_departure_checker +{ +using autoware::universe_utils::getOrDeclareParameter; + +Param Param::init(rclcpp::Node & node) +{ + Param p; + p.footprint_margin_scale = getOrDeclareParameter(node, "footprint_margin_scale"); + p.footprint_extra_margin = getOrDeclareParameter(node, "footprint_extra_margin"); + p.resample_interval = getOrDeclareParameter(node, "resample_interval"); + p.max_deceleration = getOrDeclareParameter(node, "max_deceleration"); + p.delay_time = getOrDeclareParameter(node, "delay_time"); + p.max_lateral_deviation = getOrDeclareParameter(node, "max_lateral_deviation"); + p.max_longitudinal_deviation = getOrDeclareParameter(node, "max_longitudinal_deviation"); + p.max_yaw_deviation_deg = getOrDeclareParameter(node, "max_yaw_deviation_deg"); + p.min_braking_distance = getOrDeclareParameter(node, "min_braking_distance"); + p.ego_nearest_dist_threshold = getOrDeclareParameter(node, "ego_nearest_dist_threshold"); + p.ego_nearest_yaw_threshold = getOrDeclareParameter(node, "ego_nearest_yaw_threshold"); + return p; +} + +NodeParam NodeParam::init(rclcpp::Node & node) +{ + NodeParam p; + p.will_out_of_lane_checker = getOrDeclareParameter(node, "will_out_of_lane_checker"); + p.out_of_lane_checker = getOrDeclareParameter(node, "out_of_lane_checker"); + p.boundary_departure_checker = getOrDeclareParameter(node, "boundary_departure_checker"); + p.update_rate = getOrDeclareParameter(node, "update_rate"); + p.visualize_lanelet = getOrDeclareParameter(node, "visualize_lanelet"); + p.include_right_lanes = getOrDeclareParameter(node, "include_right_lanes"); + p.include_left_lanes = getOrDeclareParameter(node, "include_left_lanes"); + p.include_opposite_lanes = getOrDeclareParameter(node, "include_opposite_lanes"); + p.include_conflicting_lanes = getOrDeclareParameter(node, "include_conflicting_lanes"); + p.boundary_types_to_detect = + getOrDeclareParameter>(node, "boundary_types_to_detect"); + return p; +} +} // namespace autoware::lane_departure_checker diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp index 4c8d5df2c22a7..166dfa1814562 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp @@ -24,11 +24,11 @@ #include "rclcpp/rclcpp.hpp" #include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" #include #include @@ -41,11 +41,11 @@ namespace autoware::motion::control::mpc_lateral_controller using autoware::motion::control::trajectory_follower::LateralHorizon; using autoware_control_msgs::msg::Lateral; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; using autoware_planning_msgs::msg::Trajectory; using autoware_vehicle_msgs::msg::SteeringReport; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; using Eigen::MatrixXd; using Eigen::VectorXd; diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp index d7442f64b028a..0f3004775e5bd 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -26,13 +26,13 @@ #include #include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" -#include "tier4_debug_msgs/msg/float32_stamped.hpp" #include #include @@ -45,11 +45,11 @@ namespace autoware::motion::control::mpc_lateral_controller namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; using autoware_control_msgs::msg::Lateral; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Float32Stamped; using autoware_planning_msgs::msg::Trajectory; using autoware_vehicle_msgs::msg::SteeringReport; using nav_msgs::msg::Odometry; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; -using tier4_debug_msgs::msg::Float32Stamped; using trajectory_follower::LateralHorizon; class MpcLateralController : public trajectory_follower::LateralControllerBase diff --git a/control/autoware_mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml index b450af05ea0e1..719d11ef7948b 100644 --- a/control/autoware_mpc_lateral_controller/package.xml +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -19,6 +19,7 @@ autoware_cmake autoware_control_msgs + autoware_internal_debug_msgs autoware_interpolation autoware_motion_utils autoware_osqp_interface @@ -36,7 +37,6 @@ std_msgs tf2 tf2_ros - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/control/autoware_mpc_lateral_controller/test/test_mpc.cpp b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp index 9c30369305f6e..0a34302f8f60c 100644 --- a/control/autoware_mpc_lateral_controller/test/test_mpc.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp @@ -24,11 +24,11 @@ #include #include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" #ifdef ROS_DISTRO_GALACTIC #include "tf2_geometry_msgs/tf2_geometry_msgs.h" @@ -45,12 +45,12 @@ namespace autoware::motion::control::mpc_lateral_controller using autoware::motion::control::trajectory_follower::LateralHorizon; using autoware_control_msgs::msg::Lateral; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using autoware_vehicle_msgs::msg::SteeringReport; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; TrajectoryPoint makePoint(const double x, const double y, const float vx) { diff --git a/control/autoware_pid_longitudinal_controller/README.md b/control/autoware_pid_longitudinal_controller/README.md index aba0f36f04d65..957379866b62a 100644 --- a/control/autoware_pid_longitudinal_controller/README.md +++ b/control/autoware_pid_longitudinal_controller/README.md @@ -195,8 +195,6 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | stopped_state_entry_vel | double | threshold of the ego velocity in transition to the STOPPED state [m/s] | 0.01 | | stopped_state_entry_acc | double | threshold of the ego acceleration in transition to the STOPPED state [m/s^2] | 0.1 | | emergency_state_overshoot_stop_dist | double | If `enable_overshoot_emergency` is true and the ego is `emergency_state_overshoot_stop_dist`-meter ahead of the stop point, the state will transit to EMERGENCY. [m] | 1.5 | -| emergency_state_traj_trans_dev | double | If the ego's position is `emergency_state_traj_tran_dev` meter away from the nearest trajectory point, the state will transit to EMERGENCY. [m] | 3.0 | -| emergency_state_traj_rot_dev | double | If the ego's orientation is `emergency_state_traj_rot_dev` rad away from the nearest trajectory point orientation, the state will transit to EMERGENCY. [rad] | 0.784 | ### DRIVE Parameter diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 1d03d1b339af6..db5fa09b5eeee 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -34,11 +34,11 @@ #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" #include "autoware_control_msgs/msg/longitudinal.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "visualization_msgs/msg/marker.hpp" #include @@ -98,8 +98,10 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro rclcpp::Clock::SharedPtr clock_; rclcpp::Logger logger_; // ros variables - rclcpp::Publisher::SharedPtr m_pub_slope; - rclcpp::Publisher::SharedPtr m_pub_debug; + rclcpp::Publisher::SharedPtr + m_pub_slope; + rclcpp::Publisher::SharedPtr + m_pub_debug; rclcpp::Publisher::SharedPtr m_pub_virtual_wall_marker; rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res; diff --git a/control/autoware_pid_longitudinal_controller/package.xml b/control/autoware_pid_longitudinal_controller/package.xml index 67e95a9d4c0ac..127ce8e76d69d 100644 --- a/control/autoware_pid_longitudinal_controller/package.xml +++ b/control/autoware_pid_longitudinal_controller/package.xml @@ -21,6 +21,7 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_internal_debug_msgs autoware_interpolation autoware_motion_utils autoware_planning_msgs @@ -37,7 +38,6 @@ std_msgs tf2 tf2_ros - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index ec95ce656fa6f..c87e936de3e40 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -214,9 +214,9 @@ PidLongitudinalController::PidLongitudinalController( : node.declare_parameter("ego_nearest_yaw_threshold"); // [rad] // subscriber, publisher - m_pub_slope = node.create_publisher( + m_pub_slope = node.create_publisher( "~/output/slope_angle", rclcpp::QoS{1}); - m_pub_debug = node.create_publisher( + m_pub_debug = node.create_publisher( "~/output/longitudinal_diagnostic", rclcpp::QoS{1}); m_pub_virtual_wall_marker = node.create_publisher("~/virtual_wall", 1); @@ -931,7 +931,7 @@ void PidLongitudinalController::publishDebugData( m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_PUBLISHED, ctrl_cmd.acc); // publish debug values - tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; + autoware_internal_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; debug_msg.stamp = clock_->now(); for (const auto & v : m_debug_values.getValues()) { debug_msg.data.push_back(static_cast(v)); @@ -939,7 +939,7 @@ void PidLongitudinalController::publishDebugData( m_pub_debug->publish(debug_msg); // slope angle - tier4_debug_msgs::msg::Float32MultiArrayStamped slope_msg{}; + autoware_internal_debug_msgs::msg::Float32MultiArrayStamped slope_msg{}; slope_msg.stamp = clock_->now(); slope_msg.data.push_back( static_cast(control_data.slope_angle)); diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash index d2687b352a17d..7a50a7e118298 100644 --- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash @@ -7,27 +7,27 @@ gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/steering_status autow gnome-terminal -- bash -c 'ros2 topic echo /control/command/control_cmd autoware_control_msgs/msg/Control --csv --qos-history keep_all --qos-reliability reliable > control_cmd.csv' gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/control_cmd autoware_control_msgs/msg/Control --csv --qos-history keep_all --qos-reliability reliable > control_cmd_orig.csv' -gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/lateral tier4_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > lateral_error.csv' -gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/yaw tier4_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > yaw_error.csv' +gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/lateral autoware_internal_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > lateral_error.csv' +gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/yaw autoware_internal_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > yaw_error.csv' gnome-terminal -- bash -c 'ros2 topic echo /system/operation_mode/state autoware_adapi_v1_msgs/msg/OperationModeState --csv --qos-history keep_all --qos-reliability reliable > system_operation_mode_state.csv' gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/control_mode autoware_vehicle_msgs/msg/ControlModeReport --csv --qos-history keep_all --qos-reliability reliable > vehicle_status_control_mode.csv' gnome-terminal -- bash -c 'ros2 topic echo /sensing/imu/imu_data sensor_msgs/msg/Imu --csv --qos-history keep_all --qos-reliability reliable > imu.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_x_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_x_des.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_y_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_y_des.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_v_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_v_des.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_yaw_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_yaw_des.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_acc_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_acc_des.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_steer_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_steer_des.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_X_des_converted tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_X_des_converted.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_x_current tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_x_current.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_error_prediction tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_error_prediction.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_max_trajectory_err tier4_debug_msgs/msg/Float32Stamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_max_trajectory_err.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_emergency_stop_mode tier4_debug_msgs/msg/BoolStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_emergency_stop_mode.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_goal_stop_mode tier4_debug_msgs/msg/BoolStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_goal_stop_mode.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_total_ctrl_time tier4_debug_msgs/msg/Float32Stamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_total_ctrl_time.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_calc_u_opt_time tier4_debug_msgs/msg/Float32Stamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_calc_u_opt_time.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_x_des autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_x_des.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_y_des autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_y_des.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_v_des autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_v_des.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_yaw_des autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_yaw_des.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_acc_des autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_acc_des.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_steer_des autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_steer_des.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_X_des_converted autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_X_des_converted.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_x_current autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_x_current.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_error_prediction autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_error_prediction.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_max_trajectory_err autoware_internal_debug_msgs/msg/Float32Stamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_max_trajectory_err.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_emergency_stop_mode autoware_internal_debug_msgs/msg/BoolStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_emergency_stop_mode.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_goal_stop_mode autoware_internal_debug_msgs/msg/BoolStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_goal_stop_mode.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_total_ctrl_time autoware_internal_debug_msgs/msg/Float32Stamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_total_ctrl_time.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_calc_u_opt_time autoware_internal_debug_msgs/msg/Float32Stamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_calc_u_opt_time.csv' # wait a moment to open new terminals converting rosbag2 to csv sleep 8 diff --git a/control/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py b/control/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py index f767dad223f0d..104261866cf98 100755 --- a/control/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py +++ b/control/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py @@ -21,6 +21,9 @@ from autoware_adapi_v1_msgs.msg import OperationModeState from autoware_control_msgs.msg import Control +from autoware_internal_debug_msgs.msg import BoolStamped +from autoware_internal_debug_msgs.msg import Float32MultiArrayStamped +from autoware_internal_debug_msgs.msg import Float32Stamped from autoware_planning_msgs.msg import Trajectory from autoware_planning_msgs.msg import TrajectoryPoint from autoware_smart_mpc_trajectory_follower.scripts import drive_controller @@ -43,9 +46,6 @@ from scipy.spatial.transform import Rotation as R from scipy.spatial.transform import Slerp from std_msgs.msg import String -from tier4_debug_msgs.msg import BoolStamped -from tier4_debug_msgs.msg import Float32MultiArrayStamped -from tier4_debug_msgs.msg import Float32Stamped from visualization_msgs.msg import Marker from visualization_msgs.msg import MarkerArray diff --git a/control/autoware_trajectory_follower_base/package.xml b/control/autoware_trajectory_follower_base/package.xml index e7b00e4b19cdc..195c077835910 100644 --- a/control/autoware_trajectory_follower_base/package.xml +++ b/control/autoware_trajectory_follower_base/package.xml @@ -21,6 +21,7 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_internal_debug_msgs autoware_interpolation autoware_motion_utils autoware_osqp_interface @@ -37,7 +38,6 @@ std_msgs tf2 tf2_ros - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/control/autoware_trajectory_follower_node/README.md b/control/autoware_trajectory_follower_node/README.md index ac05dd67f18e6..c56154f909d1a 100644 --- a/control/autoware_trajectory_follower_node/README.md +++ b/control/autoware_trajectory_follower_node/README.md @@ -151,7 +151,7 @@ Giving the longitudinal controller information about steer convergence allows it ## Debugging -Debug information are published by the lateral and longitudinal controller using `tier4_debug_msgs/Float32MultiArrayStamped` messages. +Debug information are published by the lateral and longitudinal controller using `autoware_internal_debug_msgs/Float32MultiArrayStamped` messages. A configuration file for [PlotJuggler](https://github.com/facontidavide/PlotJuggler) is provided in the `config` folder which, when loaded, allow to automatically subscribe and visualize information useful for debugging. diff --git a/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp index 1733b4bcbbb7d..c84355cc202fa 100644 --- a/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp +++ b/control/autoware_trajectory_follower_node/include/autoware/trajectory_follower_node/controller_node.hpp @@ -44,7 +44,7 @@ #include "tf2_msgs/msg/tf_message.hpp" #include "visualization_msgs/msg/marker_array.hpp" #include -#include +#include #include #include @@ -63,7 +63,7 @@ namespace trajectory_follower_node using autoware::universe_utils::StopWatch; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_control_msgs::msg::ControlHorizon; -using tier4_debug_msgs::msg::Float64Stamped; +using autoware_internal_debug_msgs::msg::Float64Stamped; namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; diff --git a/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml b/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml index b358e95f86f99..1d30a28d05cb8 100644 --- a/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml +++ b/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml @@ -3,8 +3,6 @@ # -- system -- traj_resample_dist: 0.1 # path resampling interval [m] use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) - admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value - admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value use_delayed_initial_state: true # flag to use x0_delayed as initial state for predicted trajectory # -- path smoothing -- diff --git a/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml index 5c5d65a13b2d5..f3139308db37c 100644 --- a/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml +++ b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml @@ -16,8 +16,6 @@ stopped_state_entry_vel: 0.01 stopped_state_entry_acc: 0.1 emergency_state_overshoot_stop_dist: 1.5 - emergency_state_traj_trans_dev: 3.0 - emergency_state_traj_rot_dev: 0.7854 # drive state kp: 1.0 diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index 9c2ef69e0137b..f2630a7d47f62 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -25,6 +25,7 @@ builtin_interfaces autoware_control_msgs + autoware_internal_debug_msgs autoware_motion_utils autoware_planning_msgs autoware_signal_processing @@ -41,7 +42,6 @@ tf2 tf2_eigen tf2_ros - tier4_debug_msgs autoware_global_parameter_loader builtin_interfaces rosidl_default_runtime diff --git a/control/control_performance_analysis/scripts/control_performance_plot.py b/control/control_performance_analysis/scripts/control_performance_plot.py index c5e5cabd5b059..d4a6038465e12 100644 --- a/control/control_performance_analysis/scripts/control_performance_plot.py +++ b/control/control_performance_analysis/scripts/control_performance_plot.py @@ -17,13 +17,13 @@ import argparse import math +from autoware_internal_debug_msgs.msg import BoolStamped from control_performance_analysis.msg import DrivingMonitorStamped from control_performance_analysis.msg import ErrorStamped import matplotlib.pyplot as plt from nav_msgs.msg import Odometry import rclpy from rclpy.node import Node -from tier4_debug_msgs.msg import BoolStamped parser = argparse.ArgumentParser() parser.add_argument("-i", "--interval", help="interval distance to plot") diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index c510b2ea46779..9606dea577bd0 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -25,9 +25,9 @@ #include #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include #include #include -#include #include #include @@ -85,7 +85,8 @@ class ControlEvaluatorNode : public rclcpp::Node LaneletMapBin, autoware::universe_utils::polling_policy::Newest> vector_map_subscriber_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; - rclcpp::Publisher::SharedPtr processing_time_pub_; + rclcpp::Publisher::SharedPtr + processing_time_pub_; rclcpp::Publisher::SharedPtr metrics_pub_; // update Route Handler diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index b0db41b0fdc73..79e89423af6dd 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -34,8 +34,8 @@ ControlEvaluatorNode::ControlEvaluatorNode(const rclcpp::NodeOptions & node_opti using std::placeholders::_1; // Publisher - processing_time_pub_ = - this->create_publisher("~/debug/processing_time_ms", 1); + processing_time_pub_ = this->create_publisher( + "~/debug/processing_time_ms", 1); metrics_pub_ = create_publisher("~/metrics", 1); // Parameters @@ -288,7 +288,7 @@ void ControlEvaluatorNode::onTimer() metrics_msg_ = MetricArrayMsg{}; // Publish processing time - 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 = stop_watch.toc(); processing_time_pub_->publish(processing_time_msg); diff --git a/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml b/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml index 73c1f3dfded09..7605ed2a5e859 100644 --- a/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml +++ b/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml @@ -14,7 +14,8 @@ - lateral_deviation - yaw_deviation - velocity_deviation - - trajectory_lateral_displacement + - lateral_trajectory_displacement_local + - lateral_trajectory_displacement_lookahead - stability - stability_frechet - obstacle_distance diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp index 0e08398ffa87e..2341ad2bb6ba3 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp @@ -45,7 +45,7 @@ Accumulator calcLateralDeviation(const Trajectory & ref, const Trajector * @param [in] base_pose base pose * @return calculated statistics */ -Accumulator calcLateralTrajectoryDisplacement( +Accumulator calcLocalLateralTrajectoryDisplacement( const Trajectory & prev, const Trajectory & traj, const Pose & base_pose); /** diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp index 22b365881ce28..7c207bf6c8f57 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp @@ -37,10 +37,10 @@ enum class Metric { lateral_deviation, yaw_deviation, velocity_deviation, - lateral_trajectory_displacement, + lateral_trajectory_displacement_local, + lateral_trajectory_displacement_lookahead, stability, stability_frechet, - trajectory_lateral_displacement, obstacle_distance, obstacle_ttc, modified_goal_longitudinal_deviation, @@ -64,10 +64,10 @@ static const std::unordered_map str_to_metric = { {"lateral_deviation", Metric::lateral_deviation}, {"yaw_deviation", Metric::yaw_deviation}, {"velocity_deviation", Metric::velocity_deviation}, - {"lateral_trajectory_displacement", Metric::lateral_trajectory_displacement}, + {"lateral_trajectory_displacement_local", Metric::lateral_trajectory_displacement_local}, + {"lateral_trajectory_displacement_lookahead", Metric::lateral_trajectory_displacement_lookahead}, {"stability", Metric::stability}, {"stability_frechet", Metric::stability_frechet}, - {"trajectory_lateral_displacement", Metric::trajectory_lateral_displacement}, {"obstacle_distance", Metric::obstacle_distance}, {"obstacle_ttc", Metric::obstacle_ttc}, {"modified_goal_longitudinal_deviation", Metric::modified_goal_longitudinal_deviation}, @@ -86,10 +86,10 @@ static const std::unordered_map metric_to_str = { {Metric::lateral_deviation, "lateral_deviation"}, {Metric::yaw_deviation, "yaw_deviation"}, {Metric::velocity_deviation, "velocity_deviation"}, - {Metric::lateral_trajectory_displacement, "lateral_trajectory_displacement"}, + {Metric::lateral_trajectory_displacement_local, "lateral_trajectory_displacement_local"}, + {Metric::lateral_trajectory_displacement_lookahead, "lateral_trajectory_displacement_lookahead"}, {Metric::stability, "stability"}, {Metric::stability_frechet, "stability_frechet"}, - {Metric::trajectory_lateral_displacement, "trajectory_lateral_displacement"}, {Metric::obstacle_distance, "obstacle_distance"}, {Metric::obstacle_ttc, "obstacle_ttc"}, {Metric::modified_goal_longitudinal_deviation, "modified_goal_longitudinal_deviation"}, @@ -109,10 +109,10 @@ static const std::unordered_map metric_descriptions = { {Metric::lateral_deviation, "Lateral_deviation[m]"}, {Metric::yaw_deviation, "Yaw_deviation[rad]"}, {Metric::velocity_deviation, "Velocity_deviation[m/s]"}, - {Metric::lateral_trajectory_displacement, "Nearest Pose Lateral Deviation[m]"}, + {Metric::lateral_trajectory_displacement_local, "Nearest Pose Lateral Deviation[m]"}, + {Metric::lateral_trajectory_displacement_lookahead, "Lateral_Offset_Over_Distance_Ahead[m]"}, {Metric::stability, "Stability[m]"}, {Metric::stability_frechet, "StabilityFrechet[m]"}, - {Metric::trajectory_lateral_displacement, "Trajectory_lateral_displacement[m]"}, {Metric::obstacle_distance, "Obstacle_distance[m]"}, {Metric::obstacle_ttc, "Obstacle_time_to_collision[s]"}, {Metric::modified_goal_longitudinal_deviation, "Modified_goal_longitudinal_deviation[m]"}, diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp index 69df00b26551b..1b46fbddfb297 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp @@ -56,7 +56,7 @@ Accumulator calcLateralDistance(const Trajectory & traj1, const Trajecto * @param [in] trajectory_eval_time_s time duration for trajectory evaluation in seconds * @return statistical accumulator containing the total lateral displacement */ -Accumulator calcTrajectoryLateralDisplacement( +Accumulator calcLookaheadLateralTrajectoryDisplacement( const Trajectory traj1, const Trajectory traj2, const nav_msgs::msg::Odometry & ego_odom, const double trajectory_eval_time_s); diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp index 9c268206846d9..8c0b49ce2dd26 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp @@ -31,8 +31,8 @@ #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" +#include #include -#include #include #include @@ -145,7 +145,8 @@ class PlanningEvaluatorNode : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ this, "~/input/acceleration"}; - rclcpp::Publisher::SharedPtr processing_time_pub_; + rclcpp::Publisher::SharedPtr + processing_time_pub_; rclcpp::Publisher::SharedPtr metrics_pub_; std::shared_ptr transform_listener_{nullptr}; std::unique_ptr tf_buffer_; diff --git a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp index ffb56baf29f17..9b5959948f8cb 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp @@ -45,7 +45,7 @@ Accumulator calcLateralDeviation(const Trajectory & ref, const Trajector return stat; } -Accumulator calcLateralTrajectoryDisplacement( +Accumulator calcLocalLateralTrajectoryDisplacement( const Trajectory & prev, const Trajectory & traj, const Pose & ego_pose) { Accumulator stat; @@ -58,7 +58,8 @@ Accumulator calcLateralTrajectoryDisplacement( autoware::motion_utils::calcLateralOffset(prev.points, ego_pose.position); const auto traj_lateral_deviation = autoware::motion_utils::calcLateralOffset(traj.points, ego_pose.position); - const auto lateral_trajectory_displacement = traj_lateral_deviation - prev_lateral_deviation; + const auto lateral_trajectory_displacement = + std::abs(traj_lateral_deviation - prev_lateral_deviation); stat.add(lateral_trajectory_displacement); return stat; } diff --git a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp index b99a4ebd20050..61e18a6ad0b63 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp @@ -98,7 +98,7 @@ Accumulator calcLateralDistance(const Trajectory & traj1, const Trajecto return stat; } -Accumulator calcTrajectoryLateralDisplacement( +Accumulator calcLookaheadLateralTrajectoryDisplacement( const Trajectory traj1, const Trajectory traj2, const nav_msgs::msg::Odometry & ego_odom, const double trajectory_eval_time_s) { diff --git a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp index 201fbcba0e9f7..c30420a5632fa 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp @@ -50,8 +50,11 @@ std::optional> MetricsCalculator::calculate( return metrics::calcYawDeviation(reference_trajectory_, traj); case Metric::velocity_deviation: return metrics::calcVelocityDeviation(reference_trajectory_, traj); - case Metric::lateral_trajectory_displacement: - return metrics::calcLateralTrajectoryDisplacement(previous_trajectory_, traj, ego_pose_); + case Metric::lateral_trajectory_displacement_local: + return metrics::calcLocalLateralTrajectoryDisplacement(previous_trajectory_, traj, ego_pose_); + case Metric::lateral_trajectory_displacement_lookahead: + return metrics::calcLookaheadLateralTrajectoryDisplacement( + previous_trajectory_, traj, ego_odometry_, parameters.trajectory.evaluation_time_s); case Metric::stability_frechet: return metrics::calcFrechetDistance( metrics::utils::get_lookahead_trajectory( @@ -68,9 +71,6 @@ std::optional> MetricsCalculator::calculate( metrics::utils::get_lookahead_trajectory( traj, ego_pose_, parameters.trajectory.lookahead.max_dist_m, parameters.trajectory.lookahead.max_time_s)); - case Metric::trajectory_lateral_displacement: - return metrics::calcTrajectoryLateralDisplacement( - previous_trajectory_, traj, ego_odometry_, parameters.trajectory.evaluation_time_s); case Metric::obstacle_distance: return metrics::calcDistanceToObstacle(dynamic_objects_, traj); case Metric::obstacle_ttc: diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index 5889f15e48390..53319ffa4c1fd 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -59,8 +59,8 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op output_metrics_ = declare_parameter("output_metrics"); ego_frame_str_ = declare_parameter("ego_frame"); - processing_time_pub_ = - this->create_publisher("~/debug/processing_time_ms", 1); + processing_time_pub_ = this->create_publisher( + "~/debug/processing_time_ms", 1); // List of metrics to calculate and publish metrics_pub_ = create_publisher("~/metrics", 1); @@ -282,7 +282,7 @@ void PlanningEvaluatorNode::onTimer() metrics_msg_ = MetricArrayMsg{}; // Publish ProcessingTime - 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 = stop_watch.toc(); processing_time_pub_->publish(processing_time_msg); diff --git a/localization/autoware_ekf_localizer/README.md b/localization/autoware_ekf_localizer/README.md index aad65da2516d1..a46ea7181315f 100644 --- a/localization/autoware_ekf_localizer/README.md +++ b/localization/autoware_ekf_localizer/README.md @@ -44,17 +44,17 @@ This package includes the following features: ### Published Topics -| Name | Type | Description | -| --------------------------------- | ------------------------------------------------ | ----------------------------------------------------- | -| `ekf_odom` | `nav_msgs::msg::Odometry` | Estimated odometry. | -| `ekf_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose. | -| `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance. | -| `ekf_biased_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose including the yaw bias | -| `ekf_biased_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance including the yaw bias | -| `ekf_twist` | `geometry_msgs::msg::TwistStamped` | Estimated twist. | -| `ekf_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | The estimated twist with covariance. | -| `diagnostics` | `diagnostics_msgs::msg::DiagnosticArray` | The diagnostic information. | -| `debug/processing_time_ms` | `tier4_debug_msgs::msg::Float64Stamped` | The processing time [ms]. | +| Name | Type | Description | +| --------------------------------- | --------------------------------------------------- | ----------------------------------------------------- | +| `ekf_odom` | `nav_msgs::msg::Odometry` | Estimated odometry. | +| `ekf_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose. | +| `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance. | +| `ekf_biased_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose including the yaw bias | +| `ekf_biased_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance including the yaw bias | +| `ekf_twist` | `geometry_msgs::msg::TwistStamped` | Estimated twist. | +| `ekf_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | The estimated twist with covariance. | +| `diagnostics` | `diagnostics_msgs::msg::DiagnosticArray` | The diagnostic information. | +| `debug/processing_time_ms` | `autoware_internal_debug_msgs::msg::Float64Stamped` | The processing time [ms]. | ### Published TF @@ -191,6 +191,7 @@ Note that, although the dimension gets larger since the analytical expansion can ### The conditions that result in a WARN state - The node is not in the activate state. +- The initial pose is not set. - The number of consecutive no measurement update via the Pose/Twist topic exceeds the `pose_no_update_count_threshold_warn`/`twist_no_update_count_threshold_warn`. - The timestamp of the Pose/Twist topic is beyond the delay compensation range. - The Pose/Twist topic is beyond the range of Mahalanobis distance for covariance estimation. diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp index 20a77354c0996..b194c3e956341 100644 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp +++ b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp @@ -24,6 +24,7 @@ namespace autoware::ekf_localizer { diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_activated); +diagnostic_msgs::msg::DiagnosticStatus check_set_initialpose(const bool is_set_initialpose); diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated( const std::string & measurement_type, const size_t no_update_count, diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp index 1b437f26e9c7d..84018c043cc14 100644 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp +++ b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp @@ -25,6 +25,8 @@ #include #include +#include +#include #include #include #include @@ -34,8 +36,6 @@ #include #include #include -#include -#include #include #include @@ -78,7 +78,7 @@ class EKFLocalizer : public rclcpp::Node //!< @brief ekf estimated twist with covariance publisher rclcpp::Publisher::SharedPtr pub_twist_cov_; //!< @brief ekf estimated yaw bias publisher - rclcpp::Publisher::SharedPtr pub_yaw_bias_; + rclcpp::Publisher::SharedPtr pub_yaw_bias_; //!< @brief ekf estimated yaw bias publisher rclcpp::Publisher::SharedPtr pub_biased_pose_; //!< @brief ekf estimated yaw bias publisher @@ -86,7 +86,8 @@ class EKFLocalizer : public rclcpp::Node //!< @brief diagnostics publisher rclcpp::Publisher::SharedPtr pub_diag_; //!< @brief processing_time publisher - rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr + pub_processing_time_; //!< @brief initial pose subscriber rclcpp::Subscription::SharedPtr sub_initialpose_; //!< @brief measurement pose with covariance subscriber @@ -119,6 +120,7 @@ class EKFLocalizer : public rclcpp::Node double ekf_dt_; bool is_activated_; + bool is_set_initialpose_; EKFDiagnosticInfo pose_diag_info_; EKFDiagnosticInfo twist_diag_info_; diff --git a/localization/autoware_ekf_localizer/media/ekf_diagnostics.png b/localization/autoware_ekf_localizer/media/ekf_diagnostics.png index 234b2f1e19b6d..a1561c3f52707 100644 Binary files a/localization/autoware_ekf_localizer/media/ekf_diagnostics.png and b/localization/autoware_ekf_localizer/media/ekf_diagnostics.png differ diff --git a/localization/autoware_ekf_localizer/package.xml b/localization/autoware_ekf_localizer/package.xml index 8dc3cc9844c50..e0e37f59d1acb 100644 --- a/localization/autoware_ekf_localizer/package.xml +++ b/localization/autoware_ekf_localizer/package.xml @@ -22,6 +22,7 @@ eigen + autoware_internal_debug_msgs autoware_kalman_filter autoware_localization_util autoware_universe_utils @@ -34,7 +35,6 @@ std_srvs tf2 tf2_ros - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/localization/autoware_ekf_localizer/src/diagnostics.cpp b/localization/autoware_ekf_localizer/src/diagnostics.cpp index a1af492487fe4..45468abf72d6c 100644 --- a/localization/autoware_ekf_localizer/src/diagnostics.cpp +++ b/localization/autoware_ekf_localizer/src/diagnostics.cpp @@ -41,6 +41,25 @@ diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_act return stat; } +diagnostic_msgs::msg::DiagnosticStatus check_set_initialpose(const bool is_set_initialpose) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "is_set_initialpose"; + key_value.value = is_set_initialpose ? "True" : "False"; + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + if (!is_set_initialpose) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "[WARN]initial pose is not set"; + } + + return stat; +} + diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated( const std::string & measurement_type, const size_t no_update_count, const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error) diff --git a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp index d34be2a537ef1..5638a5416e6ab 100644 --- a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp +++ b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp @@ -56,6 +56,7 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) twist_queue_(params_.twist_smoothing_steps) { is_activated_ = false; + is_set_initialpose_ = false; /* initialize ros system */ timer_control_ = rclcpp::create_timer( @@ -69,13 +70,14 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) pub_twist_ = create_publisher("ekf_twist", 1); pub_twist_cov_ = create_publisher( "ekf_twist_with_covariance", 1); - pub_yaw_bias_ = create_publisher("estimated_yaw_bias", 1); + pub_yaw_bias_ = + create_publisher("estimated_yaw_bias", 1); pub_biased_pose_ = create_publisher("ekf_biased_pose", 1); pub_biased_pose_cov_ = create_publisher( "ekf_biased_pose_with_covariance", 1); pub_diag_ = this->create_publisher("/diagnostics", 10); - pub_processing_time_ = - create_publisher("debug/processing_time_ms", 1); + pub_processing_time_ = create_publisher( + "debug/processing_time_ms", 1); sub_initialpose_ = create_subscription( "initialpose", 1, std::bind(&EKFLocalizer::callback_initial_pose, this, _1)); sub_pose_with_cov_ = create_subscription( @@ -143,6 +145,13 @@ void EKFLocalizer::timer_callback() return; } + if (!is_set_initialpose_) { + warning_->warn_throttle( + "Initial pose is not set. Provide initial pose to pose_initializer", 2000); + publish_diagnostics(geometry_msgs::msg::PoseStamped{}, current_time); + return; + } + DEBUG_INFO(get_logger(), "========================= timer called ========================="); /* update predict frequency with measured timer rate */ @@ -227,9 +236,10 @@ void EKFLocalizer::timer_callback() /* publish processing time */ const double elapsed_time = stop_watch_timer_cb_.toc(); - pub_processing_time_->publish(tier4_debug_msgs::build() - .stamp(current_time) - .data(elapsed_time)); + pub_processing_time_->publish( + autoware_internal_debug_msgs::build() + .stamp(current_time) + .data(elapsed_time)); } /* @@ -264,6 +274,8 @@ void EKFLocalizer::callback_initial_pose( params_.pose_frame_id.c_str(), msg->header.frame_id.c_str()); } ekf_module_->initialize(*msg, transform); + + is_set_initialpose_ = true; } /* @@ -272,7 +284,7 @@ void EKFLocalizer::callback_initial_pose( void EKFLocalizer::callback_pose_with_covariance( geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - if (!is_activated_) { + if (!is_activated_ && !is_set_initialpose_) { return; } @@ -333,7 +345,7 @@ void EKFLocalizer::publish_estimate_result( pub_twist_cov_->publish(twist_cov); /* publish yaw bias */ - tier4_debug_msgs::msg::Float64Stamped yawb; + autoware_internal_debug_msgs::msg::Float64Stamped yawb; yawb.stamp = current_ekf_twist.header.stamp; yawb.data = ekf_module_->get_yaw_bias(); pub_yaw_bias_->publish(yawb); @@ -359,8 +371,9 @@ void EKFLocalizer::publish_diagnostics( std::vector diag_status_array; diag_status_array.push_back(check_process_activated(is_activated_)); + diag_status_array.push_back(check_set_initialpose(is_set_initialpose_)); - if (is_activated_) { + if (is_activated_ && is_set_initialpose_) { diag_status_array.push_back(check_measurement_updated( "pose", pose_diag_info_.no_update_count, params_.pose_no_update_count_threshold_warn, params_.pose_no_update_count_threshold_error)); @@ -439,6 +452,7 @@ void EKFLocalizer::service_trigger_node( is_activated_ = true; } else { is_activated_ = false; + is_set_initialpose_ = false; } res->success = true; } diff --git a/localization/autoware_ekf_localizer/test/test_diagnostics.cpp b/localization/autoware_ekf_localizer/test/test_diagnostics.cpp index 165102adec1d7..5ce39df484e98 100644 --- a/localization/autoware_ekf_localizer/test/test_diagnostics.cpp +++ b/localization/autoware_ekf_localizer/test/test_diagnostics.cpp @@ -35,6 +35,19 @@ TEST(TestEkfDiagnostics, check_process_activated) EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); } +TEST(TestEkfDiagnostics, check_set_initialpose) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + bool is_set_initialpose = true; + stat = check_set_initialpose(is_set_initialpose); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + is_set_initialpose = false; + stat = check_set_initialpose(is_set_initialpose); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); +} + TEST(TestEkfDiagnostics, check_measurement_updated) { diagnostic_msgs::msg::DiagnosticStatus stat; diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp index d52fb5e798b58..0a85f75b74ad7 100644 --- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp @@ -73,7 +73,7 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) "twist_with_covariance", rclcpp::QoS{10}); diagnostics_ = - std::make_unique(this, "gyro_odometer_status"); + std::make_unique(this, "gyro_odometer_status"); // TODO(YamatoAndo) createTimer } diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp index 70334738e9ce3..b59e6af341ec2 100644 --- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.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 bc7dbbcfc9ca1..34fc61797231b 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp @@ -125,7 +125,7 @@ LidarMarkerLocalizer::LidarMarkerLocalizer(const rclcpp::NodeOptions & node_opti tf_listener_ = std::make_shared(*tf_buffer_, this, false); diagnostics_interface_.reset( - new autoware::universe_utils::DiagnosticInterface(this, "marker_detection_status")); + new autoware::universe_utils::DiagnosticsInterface(this, "marker_detection_status")); } void LidarMarkerLocalizer::initialize_diagnostics() 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 1b5672cff9d04..22a0c3f642563 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp @@ -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_interface_; + std::shared_ptr diagnostics_interface_; Param param_; bool is_activated_; 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 5ebcd105d57ba..1f26f6b80aa17 100644 --- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp +++ b/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp @@ -59,7 +59,7 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o logger_configure_ = std::make_unique(this); diagnostics_error_monitor_ = - std::make_unique(this, "ellipse_error_status"); + std::make_unique(this, "ellipse_error_status"); } void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg) diff --git a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp index b38958c420b19..b7d2454eb9f75 100644 --- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp +++ b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp @@ -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/CHANGELOG.rst b/localization/autoware_localization_util/CHANGELOG.rst deleted file mode 100644 index be40e3ee8560e..0000000000000 --- a/localization/autoware_localization_util/CHANGELOG.rst +++ /dev/null @@ -1,51 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_localization_util -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - localization (`#9567 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(localization_util)!: prefix package and namespace with autoware (`#8922 `_) - add autoware prefix to localization_util -* Contributors: Masaki Baba, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/localization/autoware_localization_util/CMakeLists.txt b/localization/autoware_localization_util/CMakeLists.txt deleted file mode 100644 index de62633124f3d..0000000000000 --- a/localization/autoware_localization_util/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_localization_util) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/util_func.cpp - src/smart_pose_buffer.cpp - src/tree_structured_parzen_estimator.cpp - src/covariance_ellipse.cpp -) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - ament_auto_add_gtest(test_smart_pose_buffer - test/test_smart_pose_buffer.cpp - src/smart_pose_buffer.cpp - ) - - ament_auto_add_gtest(test_tpe - test/test_tpe.cpp - src/tree_structured_parzen_estimator.cpp - ) -endif() - -ament_auto_package( - INSTALL_TO_SHARE -) diff --git a/localization/autoware_localization_util/README.md b/localization/autoware_localization_util/README.md deleted file mode 100644 index f7fddd9eebf05..0000000000000 --- a/localization/autoware_localization_util/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# autoware_localization_util - -`autoware_localization_util` is a localization utility package. - -This package does not have a node, it is just a library. diff --git a/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp b/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp deleted file mode 100644 index abd0af46856b0..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp +++ /dev/null @@ -1,44 +0,0 @@ -// 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. - -#ifndef AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ - -#include - -#include -#include - -namespace autoware::localization_util -{ - -struct Ellipse -{ - double long_radius; - double short_radius; - double yaw; - Eigen::Matrix2d P; - double size_lateral_direction; -}; - -Ellipse calculate_xy_ellipse( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale); - -visualization_msgs::msg::Marker create_ellipse_marker( - const Ellipse & ellipse, const std_msgs::msg::Header & header, - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance); - -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp b/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp deleted file mode 100644 index bda6ff19f2867..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2021 TierIV -// -// 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__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ - -#include - -namespace autoware::localization_util -{ -using Matrix6d = Eigen::Matrix; -using RowMatrixXd = Eigen::Matrix; -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp b/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp deleted file mode 100644 index 8c10506c36753..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp +++ /dev/null @@ -1,71 +0,0 @@ -// Copyright 2015-2019 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. - -#ifndef AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ - -#include "autoware/localization_util/util_func.hpp" - -#include - -#include - -#include - -namespace autoware::localization_util -{ -class SmartPoseBuffer -{ -private: - using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; - -public: - struct InterpolateResult - { - PoseWithCovarianceStamped old_pose; - PoseWithCovarianceStamped new_pose; - PoseWithCovarianceStamped interpolated_pose; - }; - - SmartPoseBuffer() = delete; - SmartPoseBuffer( - const rclcpp::Logger & logger, const double & pose_timeout_sec, - const double & pose_distance_tolerance_meters); - - std::optional interpolate(const rclcpp::Time & target_ros_time); - - void push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr); - - void pop_old(const rclcpp::Time & target_ros_time); - - void clear(); - -private: - rclcpp::Logger logger_; - std::deque pose_buffer_; - std::mutex mutex_; // This mutex is for pose_buffer_ - - const double pose_timeout_sec_; - const double pose_distance_tolerance_meters_; - - [[nodiscard]] bool validate_time_stamp_difference( - const rclcpp::Time & target_time, const rclcpp::Time & reference_time, - const double time_tolerance_sec) const; - [[nodiscard]] bool validate_position_difference( - const geometry_msgs::msg::Point & target_point, - const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const; -}; -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp b/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp deleted file mode 100644 index ddf7625c202ec..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp +++ /dev/null @@ -1,87 +0,0 @@ -// Copyright 2023 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. - -#ifndef AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ - -/* -A implementation of tree-structured parzen estimator (TPE) -See below pdf for the TPE algorithm detail. -https://papers.nips.cc/paper_files/paper/2011/file/86e8f7ab32cfd12577bc2619bc635690-Paper.pdf - -Optuna is also used as a reference for implementation. -https://github.com/optuna/optuna -*/ - -#include -#include -#include - -namespace autoware::localization_util -{ -class TreeStructuredParzenEstimator -{ -public: - using Input = std::vector; - using Score = double; - struct Trial - { - Input input; - Score score; - }; - - enum Direction { - MINIMIZE = 0, - MAXIMIZE = 1, - }; - - enum Index { - TRANS_X = 0, - TRANS_Y = 1, - TRANS_Z = 2, - ANGLE_X = 3, - ANGLE_Y = 4, - ANGLE_Z = 5, - INDEX_NUM = 6, - }; - - TreeStructuredParzenEstimator() = delete; - TreeStructuredParzenEstimator( - const Direction direction, const int64_t n_startup_trials, std::vector sample_mean, - std::vector sample_stddev); - void add_trial(const Trial & trial); - [[nodiscard]] Input get_next_input() const; - -private: - static constexpr double max_good_rate = 0.10; - static constexpr int64_t n_ei_candidates = 100; - - static std::mt19937_64 engine; - - [[nodiscard]] double compute_log_likelihood_ratio(const Input & input) const; - [[nodiscard]] static double log_gaussian_pdf( - const Input & input, const Input & mu, const Input & sigma); - - std::vector trials_; - int64_t above_num_; - const Direction direction_; - const int64_t n_startup_trials_; - const int64_t input_dimension_; - const std::vector sample_mean_; - const std::vector sample_stddev_; - Input base_stddev_; -}; -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp b/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp deleted file mode 100644 index bef9968f34b6f..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp +++ /dev/null @@ -1,88 +0,0 @@ -// Copyright 2015-2019 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. - -#ifndef AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ - -#include -#include -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#include -#else -#include - -#include -#endif - -#include -#include -#include -#include -#include -#include - -namespace autoware::localization_util -{ -// ref by http://takacity.blog.fc2.com/blog-entry-69.html -std_msgs::msg::ColorRGBA exchange_color_crc(double x); - -double calc_diff_for_radian(const double lhs_rad, const double rhs_rad); - -// x: roll, y: pitch, z: yaw -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose); -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose); -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); - -geometry_msgs::msg::Quaternion rpy_rad_to_quaternion( - const double r_rad, const double p_rad, const double y_rad); -geometry_msgs::msg::Quaternion rpy_deg_to_quaternion( - const double r_deg, const double p_deg, const double y_deg); - -geometry_msgs::msg::Twist calc_twist( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b); - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, - const rclcpp::Time & time_stamp); - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp); - -Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose); -Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose); -geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix); -Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos); - -template -T transform(const T & input, const geometry_msgs::msg::TransformStamped & transform) -{ - T output; - tf2::doTransform(input, output, transform); - return output; -} - -double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2); - -void output_pose_with_cov_to_log( - const rclcpp::Logger & logger, const std::string & prefix, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov); - -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ diff --git a/localization/autoware_localization_util/package.xml b/localization/autoware_localization_util/package.xml deleted file mode 100644 index 89ad7c24840dd..0000000000000 --- a/localization/autoware_localization_util/package.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - - autoware_localization_util - 0.40.0 - The autoware_localization_util package - Yamato Ando - Masahiro Sakamoto - Shintaro Sakoda - Kento Yabuuchi - NGUYEN Viet Anh - Taiki Yamada - Ryu Yamamoto - Apache License 2.0 - Yamato Ando - - ament_cmake_auto - autoware_cmake - - diagnostic_msgs - geometry_msgs - rclcpp - std_msgs - tf2 - tf2_eigen - tf2_geometry_msgs - visualization_msgs - - ament_cmake_cppcheck - ament_lint_auto - - - ament_cmake - - diff --git a/localization/autoware_localization_util/src/covariance_ellipse.cpp b/localization/autoware_localization_util/src/covariance_ellipse.cpp deleted file mode 100644 index 847f89e0da9b3..0000000000000 --- a/localization/autoware_localization_util/src/covariance_ellipse.cpp +++ /dev/null @@ -1,92 +0,0 @@ -// 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/localization_util/covariance_ellipse.hpp" - -#include - -#include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -namespace autoware::localization_util -{ - -Ellipse calculate_xy_ellipse( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale) -{ - // input geometry_msgs::PoseWithCovariance contain 6x6 matrix - Eigen::Matrix2d xy_covariance; - const auto cov = pose_with_covariance.covariance; - xy_covariance(0, 0) = cov[0 * 6 + 0]; - xy_covariance(0, 1) = cov[0 * 6 + 1]; - xy_covariance(1, 0) = cov[1 * 6 + 0]; - xy_covariance(1, 1) = cov[1 * 6 + 1]; - - Eigen::SelfAdjointEigenSolver eigensolver(xy_covariance); - - Ellipse ellipse; - - // eigen values and vectors are sorted in ascending order - ellipse.long_radius = scale * std::sqrt(eigensolver.eigenvalues()(1)); - ellipse.short_radius = scale * std::sqrt(eigensolver.eigenvalues()(0)); - - // principal component vector - const Eigen::Vector2d pc_vector = eigensolver.eigenvectors().col(1); - ellipse.yaw = std::atan2(pc_vector.y(), pc_vector.x()); - - // ellipse size along lateral direction (body-frame) - ellipse.P = xy_covariance; - const double yaw_vehicle = tf2::getYaw(pose_with_covariance.pose.orientation); - const Eigen::Matrix2d & p_inv = ellipse.P.inverse(); - Eigen::MatrixXd e(2, 1); - e(0, 0) = std::cos(yaw_vehicle); - e(1, 0) = std::sin(yaw_vehicle); - const double d = std::sqrt((e.transpose() * p_inv * e)(0, 0) / p_inv.determinant()); - ellipse.size_lateral_direction = scale * d; - - return ellipse; -} - -visualization_msgs::msg::Marker create_ellipse_marker( - const Ellipse & ellipse, const std_msgs::msg::Header & header, - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) -{ - tf2::Quaternion quat; - quat.setEuler(0, 0, ellipse.yaw); - - const double ellipse_long_radius = std::min(ellipse.long_radius, 30.0); - const double ellipse_short_radius = std::min(ellipse.short_radius, 30.0); - visualization_msgs::msg::Marker marker; - marker.header = header; - marker.ns = "error_ellipse"; - marker.id = 0; - marker.type = visualization_msgs::msg::Marker::SPHERE; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose = pose_with_covariance.pose; - marker.pose.orientation = tf2::toMsg(quat); - marker.scale.x = ellipse_long_radius * 2; - marker.scale.y = ellipse_short_radius * 2; - marker.scale.z = 0.01; - marker.color.a = 0.1; - marker.color.r = 0.0; - marker.color.g = 0.0; - marker.color.b = 1.0; - return marker; -} - -} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/smart_pose_buffer.cpp b/localization/autoware_localization_util/src/smart_pose_buffer.cpp deleted file mode 100644 index 3b529d68cf6c5..0000000000000 --- a/localization/autoware_localization_util/src/smart_pose_buffer.cpp +++ /dev/null @@ -1,158 +0,0 @@ -// Copyright 2023- 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/localization_util/smart_pose_buffer.hpp" - -namespace autoware::localization_util -{ -SmartPoseBuffer::SmartPoseBuffer( - const rclcpp::Logger & logger, const double & pose_timeout_sec, - const double & pose_distance_tolerance_meters) -: logger_(logger), - pose_timeout_sec_(pose_timeout_sec), - pose_distance_tolerance_meters_(pose_distance_tolerance_meters) -{ -} - -std::optional SmartPoseBuffer::interpolate( - const rclcpp::Time & target_ros_time) -{ - InterpolateResult result; - - { - std::lock_guard lock(mutex_); - - if (pose_buffer_.size() < 2) { - RCLCPP_INFO(logger_, "pose_buffer_.size() < 2"); - return std::nullopt; - } - - const rclcpp::Time time_first = pose_buffer_.front()->header.stamp; - const rclcpp::Time time_last = pose_buffer_.back()->header.stamp; - - if (target_ros_time < time_first) { - RCLCPP_INFO(logger_, "Mismatch between pose timestamp and current timestamp"); - return std::nullopt; - } - - // [time_last < target_ros_time] is acceptable here. - // It is possible that the target_ros_time (often sensor timestamp) is newer than the latest - // timestamp of buffered pose (often EKF). - // However, if the timestamp difference is too large, - // it will later be rejected by validate_time_stamp_difference. - - // get the nearest poses - result.old_pose = *pose_buffer_.front(); - for (const PoseWithCovarianceStamped::ConstSharedPtr & pose_cov_msg_ptr : pose_buffer_) { - result.new_pose = *pose_cov_msg_ptr; - const rclcpp::Time pose_time_stamp = result.new_pose.header.stamp; - if (pose_time_stamp > target_ros_time) { - break; - } - result.old_pose = *pose_cov_msg_ptr; - } - } - - // check the time stamp - const bool is_old_pose_valid = validate_time_stamp_difference( - result.old_pose.header.stamp, target_ros_time, pose_timeout_sec_); - const bool is_new_pose_valid = validate_time_stamp_difference( - result.new_pose.header.stamp, target_ros_time, pose_timeout_sec_); - - // check the position jumping (ex. immediately after the initial pose estimation) - const bool is_pose_diff_valid = validate_position_difference( - result.old_pose.pose.pose.position, result.new_pose.pose.pose.position, - pose_distance_tolerance_meters_); - - // all validations must be true - if (!(is_old_pose_valid && is_new_pose_valid && is_pose_diff_valid)) { - return std::nullopt; - } - - // interpolate the pose - const geometry_msgs::msg::PoseStamped interpolated_pose_msg = - interpolate_pose(result.old_pose, result.new_pose, target_ros_time); - result.interpolated_pose.header = interpolated_pose_msg.header; - result.interpolated_pose.pose.pose = interpolated_pose_msg.pose; - // This does not interpolate the covariance. - // interpolated_pose.pose.covariance is always set to old_pose.covariance - result.interpolated_pose.pose.covariance = result.old_pose.pose.covariance; - return result; -} - -void SmartPoseBuffer::push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr) -{ - std::lock_guard lock(mutex_); - if (!pose_buffer_.empty()) { - // Check for non-chronological timestamp order - // This situation can arise when replaying a rosbag multiple times - const rclcpp::Time end_time = pose_buffer_.back()->header.stamp; - const rclcpp::Time msg_time = pose_msg_ptr->header.stamp; - if (msg_time < end_time) { - // Clear the buffer if timestamps are reversed to maintain chronological order - pose_buffer_.clear(); - } - } - pose_buffer_.push_back(pose_msg_ptr); -} - -void SmartPoseBuffer::pop_old(const rclcpp::Time & target_ros_time) -{ - std::lock_guard lock(mutex_); - while (!pose_buffer_.empty()) { - if (rclcpp::Time(pose_buffer_.front()->header.stamp) >= target_ros_time) { - break; - } - pose_buffer_.pop_front(); - } -} - -void SmartPoseBuffer::clear() -{ - std::lock_guard lock(mutex_); - pose_buffer_.clear(); -} - -bool SmartPoseBuffer::validate_time_stamp_difference( - const rclcpp::Time & target_time, const rclcpp::Time & reference_time, - const double time_tolerance_sec) const -{ - const double dt = std::abs((target_time - reference_time).seconds()); - const bool success = dt < time_tolerance_sec; - if (!success) { - RCLCPP_WARN( - logger_, - "Validation error. The reference time is %lf[sec], but the target time is %lf[sec]. The " - "difference is %lf[sec] (the tolerance is %lf[sec]).", - reference_time.seconds(), target_time.seconds(), dt, time_tolerance_sec); - } - return success; -} - -bool SmartPoseBuffer::validate_position_difference( - const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, - const double distance_tolerance_m_) const -{ - const double distance = norm(target_point, reference_point); - const bool success = distance < distance_tolerance_m_; - if (!success) { - RCLCPP_WARN( - logger_, - "Validation error. The distance from reference position to target position is %lf[m] (the " - "tolerance is %lf[m]).", - distance, distance_tolerance_m_); - } - return success; -} -} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp b/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp deleted file mode 100644 index e9f0318d1e06d..0000000000000 --- a/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp +++ /dev/null @@ -1,185 +0,0 @@ -// Copyright 2023 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/localization_util/tree_structured_parzen_estimator.hpp" - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware::localization_util -{ -// random number generator -std::mt19937_64 TreeStructuredParzenEstimator::engine(0); - -TreeStructuredParzenEstimator::TreeStructuredParzenEstimator( - const Direction direction, const int64_t n_startup_trials, std::vector sample_mean, - std::vector sample_stddev) -: above_num_(0), - direction_(direction), - n_startup_trials_(n_startup_trials), - input_dimension_(INDEX_NUM), - sample_mean_(std::move(sample_mean)), - sample_stddev_(std::move(sample_stddev)) -{ - if (sample_mean_.size() != ANGLE_Z) { - std::cerr << "sample_mean size is invalid" << std::endl; - throw std::runtime_error("sample_mean size is invalid"); - } - if (sample_stddev_.size() != ANGLE_Z) { - std::cerr << "sample_stddev size is invalid" << std::endl; - throw std::runtime_error("sample_stddev size is invalid"); - } - // base_stddev_ is defined as the stable convergence range of ndt_scan_matcher. - base_stddev_.resize(input_dimension_); - base_stddev_[TRANS_X] = 0.25; // [m] - base_stddev_[TRANS_Y] = 0.25; // [m] - base_stddev_[TRANS_Z] = 0.25; // [m] - base_stddev_[ANGLE_X] = 1.0 / 180.0 * M_PI; // [rad] - base_stddev_[ANGLE_Y] = 1.0 / 180.0 * M_PI; // [rad] - base_stddev_[ANGLE_Z] = 2.5 / 180.0 * M_PI; // [rad] -} - -void TreeStructuredParzenEstimator::add_trial(const Trial & trial) -{ - trials_.push_back(trial); - std::sort(trials_.begin(), trials_.end(), [this](const Trial & lhs, const Trial & rhs) { - return (direction_ == Direction::MAXIMIZE ? lhs.score > rhs.score : lhs.score < rhs.score); - }); - above_num_ = std::min( - {static_cast(10), - static_cast(static_cast(trials_.size()) * max_good_rate)}); -} - -TreeStructuredParzenEstimator::Input TreeStructuredParzenEstimator::get_next_input() const -{ - std::normal_distribution dist_normal_trans_x( - sample_mean_[TRANS_X], sample_stddev_[TRANS_X]); - std::normal_distribution dist_normal_trans_y( - sample_mean_[TRANS_Y], sample_stddev_[TRANS_Y]); - std::normal_distribution dist_normal_trans_z( - sample_mean_[TRANS_Z], sample_stddev_[TRANS_Z]); - std::normal_distribution dist_normal_angle_x( - sample_mean_[ANGLE_X], sample_stddev_[ANGLE_X]); - std::normal_distribution dist_normal_angle_y( - sample_mean_[ANGLE_Y], sample_stddev_[ANGLE_Y]); - std::uniform_real_distribution dist_uniform_angle_z(-M_PI, M_PI); - - if (static_cast(trials_.size()) < n_startup_trials_ || above_num_ == 0) { - // Random sampling based on prior until the number of trials reaches `n_startup_trials_`. - Input input(input_dimension_); - input[TRANS_X] = dist_normal_trans_x(engine); - input[TRANS_Y] = dist_normal_trans_y(engine); - input[TRANS_Z] = dist_normal_trans_z(engine); - input[ANGLE_X] = dist_normal_angle_x(engine); - input[ANGLE_Y] = dist_normal_angle_y(engine); - input[ANGLE_Z] = dist_uniform_angle_z(engine); - return input; - } - - Input best_input; - double best_log_likelihood_ratio = std::numeric_limits::lowest(); - for (int64_t i = 0; i < n_ei_candidates; i++) { - Input input(input_dimension_); - input[TRANS_X] = dist_normal_trans_x(engine); - input[TRANS_Y] = dist_normal_trans_y(engine); - input[TRANS_Z] = dist_normal_trans_z(engine); - input[ANGLE_X] = dist_normal_angle_x(engine); - input[ANGLE_Y] = dist_normal_angle_y(engine); - input[ANGLE_Z] = dist_uniform_angle_z(engine); - const double log_likelihood_ratio = compute_log_likelihood_ratio(input); - if (log_likelihood_ratio > best_log_likelihood_ratio) { - best_log_likelihood_ratio = log_likelihood_ratio; - best_input = input; - } - } - return best_input; -} - -double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & input) const -{ - const auto n = static_cast(trials_.size()); - - // The above KDE and the below KDE are calculated respectively, and the ratio is the criteria to - // select best sample. - std::vector above_logs; - std::vector below_logs; - - for (int64_t i = 0; i < n; i++) { - const double log_p = log_gaussian_pdf(input, trials_[i].input, base_stddev_); - if (i < above_num_) { - const double w = 1.0 / static_cast(above_num_); - const double log_w = std::log(w); - above_logs.push_back(log_p + log_w); - } else { - const double w = 1.0 / static_cast(n - above_num_); - const double log_w = std::log(w); - below_logs.push_back(log_p + log_w); - } - } - - auto log_sum_exp = [](const std::vector & log_vec) { - const double max = *std::max_element(log_vec.begin(), log_vec.end()); - double sum = std::accumulate( - log_vec.begin(), log_vec.end(), 0.0, - [max](double total, double log_v) { return total + std::exp(log_v - max); }); - return max + std::log(sum); - }; - - const double above = log_sum_exp(above_logs); - const double below = log_sum_exp(below_logs); - - // Multiply by a constant so that the score near the "below sample" becomes lower. - // cspell:disable-line TODO(Shintaro Sakoda): It's theoretically incorrect, consider it again - // later. - const double r = above - below * 5.0; - return r; -} - -double TreeStructuredParzenEstimator::log_gaussian_pdf( - const Input & input, const Input & mu, const Input & sigma) -{ - const double log_2pi = std::log(2.0 * M_PI); - auto log_gaussian_pdf_1d = [&](const double diff, const double sigma) { - return -0.5 * log_2pi - std::log(sigma) - (diff * diff) / (2.0 * sigma * sigma); - }; - - const auto n = static_cast(input.size()); - - double result = 0.0; - for (int64_t i = 0; i < n; i++) { - double diff = input[i] - mu[i]; - if (i == ANGLE_Z) { - // Normalize the loop variable to [-pi, pi) - while (diff >= M_PI) { - diff -= 2 * M_PI; - } - while (diff < -M_PI) { - diff += 2 * M_PI; - } - } - // Experimentally, it is better to consider only trans_xy and yaw, so ignore trans_z, angle_x, - // angle_y. - if (i == TRANS_Z || i == ANGLE_X || i == ANGLE_Y) { - continue; - } - result += log_gaussian_pdf_1d(diff, sigma[i]); - } - return result; -} -} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/util_func.cpp b/localization/autoware_localization_util/src/util_func.cpp deleted file mode 100644 index 17187a8d732f0..0000000000000 --- a/localization/autoware_localization_util/src/util_func.cpp +++ /dev/null @@ -1,257 +0,0 @@ -// Copyright 2015-2019 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/localization_util/util_func.hpp" - -#include "autoware/localization_util/matrix_type.hpp" - -#include -#include - -namespace autoware::localization_util -{ -// ref by http://takacity.blog.fc2.com/blog-entry-69.html -std_msgs::msg::ColorRGBA exchange_color_crc(double x) -{ - std_msgs::msg::ColorRGBA color; - - x = std::max(x, 0.0); - x = std::min(x, 0.9999); - - if (x <= 0.25) { - color.b = 1.0; - color.g = static_cast(std::sin(x * 2.0 * M_PI)); - color.r = 0; - } else if (x <= 0.5) { - color.b = static_cast(std::sin(x * 2 * M_PI)); - color.g = 1.0; - color.r = 0; - } else if (x <= 0.75) { - color.b = 0; - color.g = 1.0; - color.r = static_cast(-std::sin(x * 2.0 * M_PI)); - } else { - color.b = 0; - color.g = static_cast(-std::sin(x * 2.0 * M_PI)); - color.r = 1.0; - } - color.a = 0.999; - return color; -} - -double calc_diff_for_radian(const double lhs_rad, const double rhs_rad) -{ - double diff_rad = lhs_rad - rhs_rad; - if (diff_rad > M_PI) { - diff_rad = diff_rad - 2 * M_PI; - } else if (diff_rad < -M_PI) { - diff_rad = diff_rad + 2 * M_PI; - } - return diff_rad; -} - -Eigen::Map make_eigen_covariance(const std::array & covariance) -{ - return {covariance.data(), 6, 6}; -} - -// x: roll, y: pitch, z: yaw -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose) -{ - geometry_msgs::msg::Vector3 rpy; - tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); - tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z); - return rpy; -} - -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose) -{ - return get_rpy(pose.pose); -} - -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) -{ - return get_rpy(pose.pose.pose); -} - -geometry_msgs::msg::Quaternion rpy_rad_to_quaternion( - const double r_rad, const double p_rad, const double y_rad) -{ - tf2::Quaternion q; - q.setRPY(r_rad, p_rad, y_rad); - geometry_msgs::msg::Quaternion quaternion_msg; - quaternion_msg.x = q.x(); - quaternion_msg.y = q.y(); - quaternion_msg.z = q.z(); - quaternion_msg.w = q.w(); - return quaternion_msg; -} - -geometry_msgs::msg::Quaternion rpy_deg_to_quaternion( - const double r_deg, const double p_deg, const double y_deg) -{ - const double r_rad = r_deg * M_PI / 180.0; - const double p_rad = p_deg * M_PI / 180.0; - const double y_rad = y_deg * M_PI / 180.0; - return rpy_rad_to_quaternion(r_rad, p_rad, y_rad); -} - -geometry_msgs::msg::Twist calc_twist( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b) -{ - const rclcpp::Duration dt = rclcpp::Time(pose_b.header.stamp) - rclcpp::Time(pose_a.header.stamp); - const double dt_s = dt.seconds(); - - if (dt_s == 0) { - return geometry_msgs::msg::Twist(); - } - - const auto pose_a_rpy = get_rpy(pose_a); - const auto pose_b_rpy = get_rpy(pose_b); - - geometry_msgs::msg::Vector3 diff_xyz; - geometry_msgs::msg::Vector3 diff_rpy; - - diff_xyz.x = pose_b.pose.position.x - pose_a.pose.position.x; - diff_xyz.y = pose_b.pose.position.y - pose_a.pose.position.y; - diff_xyz.z = pose_b.pose.position.z - pose_a.pose.position.z; - diff_rpy.x = calc_diff_for_radian(pose_b_rpy.x, pose_a_rpy.x); - diff_rpy.y = calc_diff_for_radian(pose_b_rpy.y, pose_a_rpy.y); - diff_rpy.z = calc_diff_for_radian(pose_b_rpy.z, pose_a_rpy.z); - - geometry_msgs::msg::Twist twist; - twist.linear.x = diff_xyz.x / dt_s; - twist.linear.y = diff_xyz.y / dt_s; - twist.linear.z = diff_xyz.z / dt_s; - twist.angular.x = diff_rpy.x / dt_s; - twist.angular.y = diff_rpy.y / dt_s; - twist.angular.z = diff_rpy.z / dt_s; - - return twist; -} - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, - const rclcpp::Time & time_stamp) -{ - const rclcpp::Time pose_a_time_stamp = pose_a.header.stamp; - const rclcpp::Time pose_b_time_stamp = pose_b.header.stamp; - if ( - (pose_a_time_stamp.seconds() == 0.0) || (pose_b_time_stamp.seconds() == 0.0) || - (time_stamp.seconds() == 0.0)) { - return geometry_msgs::msg::PoseStamped(); - } - - const auto twist = calc_twist(pose_a, pose_b); - const double dt = (time_stamp - pose_a_time_stamp).seconds(); - - const auto pose_a_rpy = get_rpy(pose_a); - - geometry_msgs::msg::Vector3 xyz; - geometry_msgs::msg::Vector3 rpy; - xyz.x = pose_a.pose.position.x + twist.linear.x * dt; - xyz.y = pose_a.pose.position.y + twist.linear.y * dt; - xyz.z = pose_a.pose.position.z + twist.linear.z * dt; - rpy.x = pose_a_rpy.x + twist.angular.x * dt; - rpy.y = pose_a_rpy.y + twist.angular.y * dt; - rpy.z = pose_a_rpy.z + twist.angular.z * dt; - - tf2::Quaternion tf_quaternion; - tf_quaternion.setRPY(rpy.x, rpy.y, rpy.z); - - geometry_msgs::msg::PoseStamped pose; - pose.header = pose_a.header; - pose.header.stamp = time_stamp; - pose.pose.position.x = xyz.x; - pose.pose.position.y = xyz.y; - pose.pose.position.z = xyz.z; - pose.pose.orientation = tf2::toMsg(tf_quaternion); - return pose; -} - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp) -{ - geometry_msgs::msg::PoseStamped tmp_pose_a; - tmp_pose_a.header = pose_a.header; - tmp_pose_a.pose = pose_a.pose.pose; - - geometry_msgs::msg::PoseStamped tmp_pose_b; - tmp_pose_b.header = pose_b.header; - tmp_pose_b.pose = pose_b.pose.pose; - - return interpolate_pose(tmp_pose_a, tmp_pose_b, time_stamp); -} - -Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose) -{ - Eigen::Affine3d eigen_pose; - tf2::fromMsg(ros_pose, eigen_pose); - return eigen_pose; -} - -Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose) -{ - Eigen::Affine3d eigen_pose_affine = pose_to_affine3d(ros_pose); - Eigen::Matrix4f eigen_pose_matrix = eigen_pose_affine.matrix().cast(); - return eigen_pose_matrix; -} - -Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos) -{ - Eigen::Vector3d eigen_pos; - eigen_pos.x() = ros_pos.x; - eigen_pos.y() = ros_pos.y; - eigen_pos.z() = ros_pos.z; - return eigen_pos; -} - -geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix) -{ - Eigen::Affine3d eigen_pose_affine; - eigen_pose_affine.matrix() = eigen_pose_matrix.cast(); - geometry_msgs::msg::Pose ros_pose = tf2::toMsg(eigen_pose_affine); - return ros_pose; -} - -double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) -{ - const double dx = p1.x - p2.x; - const double dy = p1.y - p2.y; - const double dz = p1.z - p2.z; - return std::sqrt(dx * dx + dy * dy + dz * dz); -} - -void output_pose_with_cov_to_log( - const rclcpp::Logger & logger, const std::string & prefix, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov) -{ - const Eigen::Map covariance = - make_eigen_covariance(pose_with_cov.pose.covariance); - const geometry_msgs::msg::Pose pose = pose_with_cov.pose.pose; - geometry_msgs::msg::Vector3 rpy = get_rpy(pose); - rpy.x = rpy.x * 180.0 / M_PI; - rpy.y = rpy.y * 180.0 / M_PI; - rpy.z = rpy.z * 180.0 / M_PI; - - RCLCPP_DEBUG_STREAM( - logger, std::fixed << prefix << "," << pose.position.x << "," << pose.position.y << "," - << pose.position.z << "," << pose.orientation.x << "," << pose.orientation.y - << "," << pose.orientation.z << "," << pose.orientation.w << "," << rpy.x - << "," << rpy.y << "," << rpy.z << "," << covariance(0, 0) << "," - << covariance(1, 1) << "," << covariance(2, 2) << "," << covariance(3, 3) - << "," << covariance(4, 4) << "," << covariance(5, 5)); -} -} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp b/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp deleted file mode 100644 index d55555682be84..0000000000000 --- a/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2023- 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/localization_util/smart_pose_buffer.hpp" -#include "autoware/localization_util/util_func.hpp" - -#include -#include - -#include -#include - -#include -#include -#include - -using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; -using SmartPoseBuffer = autoware::localization_util::SmartPoseBuffer; - -bool compare_pose( - const PoseWithCovarianceStamped & pose_a, const PoseWithCovarianceStamped & pose_b) -{ - return pose_a.header.stamp == pose_b.header.stamp && - pose_a.header.frame_id == pose_b.header.frame_id && - pose_a.pose.covariance == pose_b.pose.covariance && - pose_a.pose.pose.position.x == pose_b.pose.pose.position.x && - pose_a.pose.pose.position.y == pose_b.pose.pose.position.y && - pose_a.pose.pose.position.z == pose_b.pose.pose.position.z && - pose_a.pose.pose.orientation.x == pose_b.pose.pose.orientation.x && - pose_a.pose.pose.orientation.y == pose_b.pose.pose.orientation.y && - pose_a.pose.pose.orientation.z == pose_b.pose.pose.orientation.z && - pose_a.pose.pose.orientation.w == pose_b.pose.pose.orientation.w; -} - -TEST(TestSmartPoseBuffer, interpolate_pose) // NOLINT -{ - rclcpp::Logger logger = rclcpp::get_logger("test_logger"); - const double pose_timeout_sec = 10.0; - const double pose_distance_tolerance_meters = 100.0; - SmartPoseBuffer smart_pose_buffer(logger, pose_timeout_sec, pose_distance_tolerance_meters); - - // first data - PoseWithCovarianceStamped::SharedPtr old_pose_ptr = std::make_shared(); - old_pose_ptr->header.stamp.sec = 10; - old_pose_ptr->header.stamp.nanosec = 0; - old_pose_ptr->pose.pose.position.x = 10.0; - old_pose_ptr->pose.pose.position.y = 20.0; - old_pose_ptr->pose.pose.position.z = 0.0; - old_pose_ptr->pose.pose.orientation = - autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 0.0); - smart_pose_buffer.push_back(old_pose_ptr); - - // second data - PoseWithCovarianceStamped::SharedPtr new_pose_ptr = std::make_shared(); - new_pose_ptr->header.stamp.sec = 20; - new_pose_ptr->header.stamp.nanosec = 0; - new_pose_ptr->pose.pose.position.x = 20.0; - new_pose_ptr->pose.pose.position.y = 40.0; - new_pose_ptr->pose.pose.position.z = 0.0; - new_pose_ptr->pose.pose.orientation = - autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 90.0); - smart_pose_buffer.push_back(new_pose_ptr); - - // interpolate - builtin_interfaces::msg::Time target_ros_time_msg; - target_ros_time_msg.sec = 15; - target_ros_time_msg.nanosec = 0; - const std::optional & interpolate_result = - smart_pose_buffer.interpolate(target_ros_time_msg); - ASSERT_TRUE(interpolate_result.has_value()); - const SmartPoseBuffer::InterpolateResult result = interpolate_result.value(); - - // check old - EXPECT_TRUE(compare_pose(result.old_pose, *old_pose_ptr)); - - // check new - EXPECT_TRUE(compare_pose(result.new_pose, *new_pose_ptr)); - - // check interpolated - EXPECT_EQ(result.interpolated_pose.header.stamp.sec, 15); - EXPECT_EQ(result.interpolated_pose.header.stamp.nanosec, static_cast(0)); - EXPECT_EQ(result.interpolated_pose.pose.pose.position.x, 15.0); - EXPECT_EQ(result.interpolated_pose.pose.pose.position.y, 30.0); - EXPECT_EQ(result.interpolated_pose.pose.pose.position.z, 0.0); - const auto rpy = autoware::localization_util::get_rpy(result.interpolated_pose.pose.pose); - EXPECT_NEAR(rpy.x * 180 / M_PI, 0.0, 1e-6); - EXPECT_NEAR(rpy.y * 180 / M_PI, 0.0, 1e-6); - EXPECT_NEAR(rpy.z * 180 / M_PI, 45.0, 1e-6); -} - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} diff --git a/localization/autoware_localization_util/test/test_tpe.cpp b/localization/autoware_localization_util/test/test_tpe.cpp deleted file mode 100644 index 2d71a385246b7..0000000000000 --- a/localization/autoware_localization_util/test/test_tpe.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// 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/localization_util/tree_structured_parzen_estimator.hpp" - -#include - -#include -#include -#include -#include -#include - -using TreeStructuredParzenEstimator = autoware::localization_util::TreeStructuredParzenEstimator; - -TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphere_function) -{ - auto sphere_function = [](const TreeStructuredParzenEstimator::Input & input) { - double value = 0.0; - const auto n = static_cast(input.size()); - for (int64_t i = 0; i < n; i++) { - const double v = input[i] * 10; - value += v * v; - } - return value; - }; - - constexpr int64_t k_outer_trials_num = 20; - constexpr int64_t k_inner_trials_num = 200; - std::cout << std::fixed; - std::vector mean_scores; - std::vector sample_mean(5, 0.0); - std::vector sample_stddev{1.0, 1.0, 0.1, 0.1, 0.1}; - - for (const int64_t n_startup_trials : {k_inner_trials_num, k_inner_trials_num / 2}) { - const std::string method = ((n_startup_trials == k_inner_trials_num) ? "Random" : "TPE"); - - std::vector scores; - for (int64_t i = 0; i < k_outer_trials_num; i++) { - double best_score = std::numeric_limits::lowest(); - TreeStructuredParzenEstimator estimator( - TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials, sample_mean, - sample_stddev); - for (int64_t trial = 0; trial < k_inner_trials_num; trial++) { - const TreeStructuredParzenEstimator::Input input = estimator.get_next_input(); - const double score = -sphere_function(input); - estimator.add_trial({input, score}); - best_score = std::max(best_score, score); - } - scores.push_back(best_score); - } - - const double sum = std::accumulate(scores.begin(), scores.end(), 0.0); - const double mean = sum / static_cast(scores.size()); - mean_scores.push_back(mean); - double sq_sum = std::accumulate( - scores.begin(), scores.end(), 0.0, - [mean](double total, double score) { return total + (score - mean) * (score - mean); }); - const double stddev = std::sqrt(sq_sum / static_cast(scores.size())); - - std::cout << method << ", mean = " << mean << ", stddev = " << stddev << std::endl; - } - ASSERT_LT(mean_scores[0], mean_scores[1]); -} diff --git a/localization/autoware_ndt_scan_matcher/README.md b/localization/autoware_ndt_scan_matcher/README.md index 22e56930a0048..ee177608bdefe 100644 --- a/localization/autoware_ndt_scan_matcher/README.md +++ b/localization/autoware_ndt_scan_matcher/README.md @@ -25,26 +25,26 @@ One optional function is regularization. Please see the regularization chapter i ### Output -| Name | Type | Description | -| --------------------------------- | ----------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------- | -| `ndt_pose` | `geometry_msgs::msg::PoseStamped` | estimated pose | -| `ndt_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | estimated pose with covariance | -| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | -| `points_aligned` | `sensor_msgs::msg::PointCloud2` | [debug topic] pointcloud aligned by scan matching | -| `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] no ground pointcloud aligned by scan matching | -| `initial_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] initial pose used in scan matching | -| `multi_ndt_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] estimated poses from multiple initial poses in real-time covariance estimation | -| `multi_initial_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] initial poses for real-time covariance estimation | -| `exe_time_ms` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] execution time for scan matching [ms] | -| `transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching | -| `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on no ground LiDAR scan | -| `iteration_num` | `tier4_debug_msgs::msg::Int32Stamped` | [debug topic] number of scan matching iterations | -| `initial_to_result_relative_pose` | `geometry_msgs::msg::PoseStamped` | [debug topic] relative pose between the initial point and the convergence point | -| `initial_to_result_distance` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the initial point and the convergence point [m] | -| `initial_to_result_distance_old` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the older of the two initial points used in linear interpolation and the convergence point [m] | -| `initial_to_result_distance_new` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the newer of the two initial points used in linear interpolation and the convergence point [m] | -| `ndt_marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] markers for debugging | -| `monte_carlo_initial_pose_marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] particles used in initial position estimation | +| Name | Type | Description | +| --------------------------------- | --------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------- | +| `ndt_pose` | `geometry_msgs::msg::PoseStamped` | estimated pose | +| `ndt_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | estimated pose with covariance | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | +| `points_aligned` | `sensor_msgs::msg::PointCloud2` | [debug topic] pointcloud aligned by scan matching | +| `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] no ground pointcloud aligned by scan matching | +| `initial_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] initial pose used in scan matching | +| `multi_ndt_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] estimated poses from multiple initial poses in real-time covariance estimation | +| `multi_initial_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] initial poses for real-time covariance estimation | +| `exe_time_ms` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] execution time for scan matching [ms] | +| `transform_probability` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching | +| `no_ground_transform_probability` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on no ground LiDAR scan | +| `iteration_num` | `autoware_internal_debug_msgs::msg::Int32Stamped` | [debug topic] number of scan matching iterations | +| `initial_to_result_relative_pose` | `geometry_msgs::msg::PoseStamped` | [debug topic] relative pose between the initial point and the convergence point | +| `initial_to_result_distance` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the initial point and the convergence point [m] | +| `initial_to_result_distance_old` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the older of the two initial points used in linear interpolation and the convergence point [m] | +| `initial_to_result_distance_new` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the newer of the two initial points used in linear interpolation and the convergence point [m] | +| `ndt_marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] markers for debugging | +| `monte_carlo_initial_pose_marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] particles used in initial position estimation | ### Service diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp index bf8fce2b302c0..12990259f88cd 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp @@ -42,7 +42,7 @@ namespace autoware::ndt_scan_matcher { -using DiagnosticInterface = autoware::universe_utils::DiagnosticInterface; +using DiagnosticsInterface = autoware::universe_utils::DiagnosticsInterface; class MapUpdateModule { @@ -63,19 +63,19 @@ class MapUpdateModule void callback_timer( const bool is_activated, const std::optional & position, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); [[nodiscard]] bool should_update_map( const geometry_msgs::msg::Point & position, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); void update_map( const geometry_msgs::msg::Point & position, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); // Update the specified NDT bool update_ndt( const geometry_msgs::msg::Point & position, NdtType & ndt, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); void publish_partial_pcd_map(); rclcpp::Publisher::SharedPtr loaded_pcd_pub_; diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 25b7417ffbe3c..d6e629ee2f1c3 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -26,6 +26,8 @@ #include #include +#include +#include #include #include #include @@ -33,8 +35,6 @@ #include #include #include -#include -#include #include #include @@ -166,23 +166,24 @@ class NDTScanMatcher : public rclcpp::Node initial_pose_with_covariance_pub_; rclcpp::Publisher::SharedPtr multi_ndt_pose_pub_; rclcpp::Publisher::SharedPtr multi_initial_pose_pub_; - rclcpp::Publisher::SharedPtr exe_time_pub_; - rclcpp::Publisher::SharedPtr transform_probability_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr exe_time_pub_; + rclcpp::Publisher::SharedPtr + transform_probability_pub_; + rclcpp::Publisher::SharedPtr nearest_voxel_transformation_likelihood_pub_; rclcpp::Publisher::SharedPtr voxel_score_points_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr no_ground_transform_probability_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr no_ground_nearest_voxel_transformation_likelihood_pub_; - rclcpp::Publisher::SharedPtr iteration_num_pub_; + rclcpp::Publisher::SharedPtr iteration_num_pub_; rclcpp::Publisher::SharedPtr initial_to_result_relative_pose_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr initial_to_result_distance_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr initial_to_result_distance_old_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr initial_to_result_distance_new_pub_; rclcpp::Publisher::SharedPtr ndt_marker_pub_; rclcpp::Publisher::SharedPtr @@ -211,12 +212,12 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr regularization_pose_buffer_; std::atomic is_activated_; - std::unique_ptr diagnostics_scan_points_; - std::unique_ptr diagnostics_initial_pose_; - std::unique_ptr diagnostics_regularization_pose_; - std::unique_ptr diagnostics_map_update_; - std::unique_ptr diagnostics_ndt_align_; - std::unique_ptr diagnostics_trigger_node_; + std::unique_ptr diagnostics_scan_points_; + std::unique_ptr diagnostics_initial_pose_; + std::unique_ptr diagnostics_regularization_pose_; + std::unique_ptr diagnostics_map_update_; + std::unique_ptr diagnostics_ndt_align_; + std::unique_ptr diagnostics_trigger_node_; std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; diff --git a/localization/autoware_ndt_scan_matcher/package.xml b/localization/autoware_ndt_scan_matcher/package.xml index f2047418310aa..ebd1ebda2d573 100644 --- a/localization/autoware_ndt_scan_matcher/package.xml +++ b/localization/autoware_ndt_scan_matcher/package.xml @@ -17,6 +17,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_localization_util autoware_map_msgs autoware_universe_utils @@ -35,7 +36,6 @@ tf2_geometry_msgs tf2_ros tf2_sensor_msgs - tier4_debug_msgs tier4_localization_msgs visualization_msgs diff --git a/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp index eea0b8f3d06c4..299d596401b19 100644 --- a/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp @@ -56,7 +56,7 @@ MapUpdateModule::MapUpdateModule( void MapUpdateModule::callback_timer( const bool is_activated, const std::optional & position, - std::unique_ptr & diagnostics_ptr) + std::unique_ptr & diagnostics_ptr) { // check is_activated diagnostics_ptr->add_key_value("is_activated", is_activated); @@ -87,7 +87,7 @@ void MapUpdateModule::callback_timer( bool MapUpdateModule::should_update_map( const geometry_msgs::msg::Point & position, - std::unique_ptr & diagnostics_ptr) + std::unique_ptr & diagnostics_ptr) { last_update_position_mtx_.lock(); @@ -143,7 +143,7 @@ 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) + std::unique_ptr & diagnostics_ptr) { diagnostics_ptr->add_key_value("is_need_rebuild", need_rebuild_); @@ -231,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 cef8724423bed..3d06dfffa16ed 100644 --- a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -51,20 +51,20 @@ using autoware::localization_util::pose_to_matrix4f; using autoware::localization_util::SmartPoseBuffer; using autoware::localization_util::TreeStructuredParzenEstimator; -using autoware::universe_utils::DiagnosticInterface; +using autoware::universe_utils::DiagnosticsInterface; -tier4_debug_msgs::msg::Float32Stamped make_float32_stamped( +autoware_internal_debug_msgs::msg::Float32Stamped make_float32_stamped( const builtin_interfaces::msg::Time & stamp, const float data) { - using T = tier4_debug_msgs::msg::Float32Stamped; - return tier4_debug_msgs::build().stamp(stamp).data(data); + using T = autoware_internal_debug_msgs::msg::Float32Stamped; + return autoware_internal_debug_msgs::build().stamp(stamp).data(data); } -tier4_debug_msgs::msg::Int32Stamped make_int32_stamped( +autoware_internal_debug_msgs::msg::Int32Stamped make_int32_stamped( const builtin_interfaces::msg::Time & stamp, const int32_t data) { - using T = tier4_debug_msgs::msg::Int32Stamped; - return tier4_debug_msgs::build().stamp(stamp).data(data); + using T = autoware_internal_debug_msgs::msg::Int32Stamped; + return autoware_internal_debug_msgs::build().stamp(stamp).data(data); } std::array rotate_covariance( @@ -141,7 +141,7 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) std::make_unique(this->get_logger(), value_as_unlimited, value_as_unlimited); diagnostics_regularization_pose_ = - std::make_unique(this, "regularization_pose_subscriber_status"); + std::make_unique(this, "regularization_pose_subscriber_status"); } sensor_aligned_pose_pub_ = @@ -158,31 +158,34 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) multi_ndt_pose_pub_ = this->create_publisher("multi_ndt_pose", 10); multi_initial_pose_pub_ = this->create_publisher("multi_initial_pose", 10); - exe_time_pub_ = this->create_publisher("exe_time_ms", 10); + exe_time_pub_ = + this->create_publisher("exe_time_ms", 10); transform_probability_pub_ = - this->create_publisher("transform_probability", 10); + this->create_publisher( + "transform_probability", 10); nearest_voxel_transformation_likelihood_pub_ = - this->create_publisher( + this->create_publisher( "nearest_voxel_transformation_likelihood", 10); voxel_score_points_pub_ = this->create_publisher("voxel_score_points", 10); no_ground_transform_probability_pub_ = - this->create_publisher( + this->create_publisher( "no_ground_transform_probability", 10); no_ground_nearest_voxel_transformation_likelihood_pub_ = - this->create_publisher( + this->create_publisher( "no_ground_nearest_voxel_transformation_likelihood", 10); iteration_num_pub_ = - this->create_publisher("iteration_num", 10); + this->create_publisher("iteration_num", 10); initial_to_result_relative_pose_pub_ = this->create_publisher("initial_to_result_relative_pose", 10); initial_to_result_distance_pub_ = - this->create_publisher("initial_to_result_distance", 10); + this->create_publisher( + "initial_to_result_distance", 10); initial_to_result_distance_old_pub_ = - this->create_publisher( + this->create_publisher( "initial_to_result_distance_old", 10); initial_to_result_distance_new_pub_ = - this->create_publisher( + this->create_publisher( "initial_to_result_distance_new", 10); ndt_marker_pub_ = this->create_publisher("ndt_marker", 10); ndt_monte_carlo_initial_pose_marker_pub_ = @@ -209,13 +212,13 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, param_.dynamic_map_loading); - diagnostics_scan_points_ = std::make_unique(this, "scan_matching_status"); + diagnostics_scan_points_ = std::make_unique(this, "scan_matching_status"); diagnostics_initial_pose_ = - std::make_unique(this, "initial_pose_subscriber_status"); - diagnostics_map_update_ = std::make_unique(this, "map_update_status"); - diagnostics_ndt_align_ = std::make_unique(this, "ndt_align_service_status"); + std::make_unique(this, "initial_pose_subscriber_status"); + diagnostics_map_update_ = std::make_unique(this, "map_update_status"); + diagnostics_ndt_align_ = std::make_unique(this, "ndt_align_service_status"); diagnostics_trigger_node_ = - std::make_unique(this, "trigger_node_service_status"); + std::make_unique(this, "trigger_node_service_status"); logger_configure_ = std::make_unique(this); } diff --git a/localization/autoware_pose2twist/README.md b/localization/autoware_pose2twist/README.md index 55ca4667c423d..7f81e0dece5b0 100644 --- a/localization/autoware_pose2twist/README.md +++ b/localization/autoware_pose2twist/README.md @@ -17,11 +17,11 @@ The `twist.angular` is calculated as `relative_rotation_vector / dt` for each fi ### Output -| Name | Type | Description | -| --------- | ------------------------------------- | --------------------------------------------- | -| twist | geometry_msgs::msg::TwistStamped | twist calculated from the input pose history. | -| linear_x | tier4_debug_msgs::msg::Float32Stamped | linear-x field of the output twist. | -| angular_z | tier4_debug_msgs::msg::Float32Stamped | angular-z field of the output twist. | +| Name | Type | Description | +| --------- | ------------------------------------------------- | --------------------------------------------- | +| twist | geometry_msgs::msg::TwistStamped | twist calculated from the input pose history. | +| linear_x | autoware_internal_debug_msgs::msg::Float32Stamped | linear-x field of the output twist. | +| angular_z | autoware_internal_debug_msgs::msg::Float32Stamped | angular-z field of the output twist. | ## Parameters diff --git a/localization/autoware_pose2twist/package.xml b/localization/autoware_pose2twist/package.xml index 3fd16856ef6af..cfde18d430cfd 100644 --- a/localization/autoware_pose2twist/package.xml +++ b/localization/autoware_pose2twist/package.xml @@ -17,11 +17,11 @@ ament_cmake autoware_cmake + autoware_internal_debug_msgs geometry_msgs rclcpp rclcpp_components tf2_geometry_msgs - tier4_debug_msgs ament_lint_auto autoware_lint_common diff --git a/localization/autoware_pose2twist/src/pose2twist_core.cpp b/localization/autoware_pose2twist/src/pose2twist_core.cpp index 4dc7b5fb04209..f071d7c8c66e2 100644 --- a/localization/autoware_pose2twist/src/pose2twist_core.cpp +++ b/localization/autoware_pose2twist/src/pose2twist_core.cpp @@ -29,9 +29,10 @@ Pose2Twist::Pose2Twist(const rclcpp::NodeOptions & options) : rclcpp::Node("pose durable_qos.transient_local(); twist_pub_ = create_publisher("twist", durable_qos); - linear_x_pub_ = create_publisher("linear_x", durable_qos); + linear_x_pub_ = + create_publisher("linear_x", durable_qos); angular_z_pub_ = - create_publisher("angular_z", durable_qos); + create_publisher("angular_z", durable_qos); // Note: this callback publishes topics above pose_sub_ = create_subscription( "pose", queue_size, std::bind(&Pose2Twist::callback_pose, this, _1)); @@ -105,12 +106,12 @@ void Pose2Twist::callback_pose(geometry_msgs::msg::PoseStamped::SharedPtr pose_m twist_msg.header.frame_id = "base_link"; twist_pub_->publish(twist_msg); - tier4_debug_msgs::msg::Float32Stamped linear_x_msg; + autoware_internal_debug_msgs::msg::Float32Stamped linear_x_msg; linear_x_msg.stamp = this->now(); linear_x_msg.data = static_cast(twist_msg.twist.linear.x); linear_x_pub_->publish(linear_x_msg); - tier4_debug_msgs::msg::Float32Stamped angular_z_msg; + autoware_internal_debug_msgs::msg::Float32Stamped angular_z_msg; angular_z_msg.stamp = this->now(); angular_z_msg.data = static_cast(twist_msg.twist.angular.z); angular_z_pub_->publish(angular_z_msg); diff --git a/localization/autoware_pose2twist/src/pose2twist_core.hpp b/localization/autoware_pose2twist/src/pose2twist_core.hpp index ed3e542beb857..22540c9aee473 100644 --- a/localization/autoware_pose2twist/src/pose2twist_core.hpp +++ b/localization/autoware_pose2twist/src/pose2twist_core.hpp @@ -17,9 +17,9 @@ #include +#include #include #include -#include #ifdef ROS_DISTRO_GALACTIC #include @@ -44,8 +44,8 @@ class Pose2Twist : public rclcpp::Node rclcpp::Subscription::SharedPtr pose_sub_; rclcpp::Publisher::SharedPtr twist_pub_; - rclcpp::Publisher::SharedPtr linear_x_pub_; - rclcpp::Publisher::SharedPtr angular_z_pub_; + rclcpp::Publisher::SharedPtr linear_x_pub_; + rclcpp::Publisher::SharedPtr angular_z_pub_; }; } // namespace autoware::pose2twist diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp index 67d5c447c09b6..5bde25a564f1d 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp @@ -40,7 +40,7 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) output_pose_covariance_ = get_covariance_parameter(this, "output_pose_covariance"); gnss_particle_covariance_ = get_covariance_parameter(this, "gnss_particle_covariance"); - diagnostics_pose_reliable_ = std::make_unique( + diagnostics_pose_reliable_ = std::make_unique( this, "pose_initializer_status"); if (declare_parameter("ekf_enabled")) { diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp index 5305bcc3ad347..a0c1ed3a86576 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp @@ -60,7 +60,7 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; std::unique_ptr logger_configure_; - std::unique_ptr diagnostics_pose_reliable_; + std::unique_ptr diagnostics_pose_reliable_; double stop_check_duration_; void change_node_trigger(bool flag, bool need_spin = false); diff --git a/localization/autoware_stop_filter/README.md b/localization/autoware_stop_filter/README.md index 9904707a59996..9136f6b6fc8a0 100644 --- a/localization/autoware_stop_filter/README.md +++ b/localization/autoware_stop_filter/README.md @@ -18,10 +18,10 @@ This node aims to: ### Output -| Name | Type | Description | -| ----------------- | ------------------------------------ | -------------------------------------------------------- | -| `output/odom` | `nav_msgs::msg::Odometry` | odometry with suppressed longitudinal and yaw twist | -| `debug/stop_flag` | `tier4_debug_msgs::msg::BoolStamped` | flag to represent whether the vehicle is stopping or not | +| Name | Type | Description | +| ----------------- | ------------------------------------------------ | -------------------------------------------------------- | +| `output/odom` | `nav_msgs::msg::Odometry` | odometry with suppressed longitudinal and yaw twist | +| `debug/stop_flag` | `autoware_internal_debug_msgs::msg::BoolStamped` | flag to represent whether the vehicle is stopping or not | ## Parameters diff --git a/localization/autoware_stop_filter/package.xml b/localization/autoware_stop_filter/package.xml index 1ed5e2c2082be..50ba98484f59d 100644 --- a/localization/autoware_stop_filter/package.xml +++ b/localization/autoware_stop_filter/package.xml @@ -17,12 +17,12 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs geometry_msgs nav_msgs rclcpp rclcpp_components tf2 - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/localization/autoware_stop_filter/src/stop_filter.cpp b/localization/autoware_stop_filter/src/stop_filter.cpp index 17eaafdc3002a..b0f1bc8eba053 100644 --- a/localization/autoware_stop_filter/src/stop_filter.cpp +++ b/localization/autoware_stop_filter/src/stop_filter.cpp @@ -34,12 +34,13 @@ StopFilter::StopFilter(const rclcpp::NodeOptions & node_options) "input/odom", 1, std::bind(&StopFilter::callback_odometry, this, std::placeholders::_1)); pub_odom_ = create_publisher("output/odom", 1); - pub_stop_flag_ = create_publisher("debug/stop_flag", 1); + pub_stop_flag_ = + create_publisher("debug/stop_flag", 1); } void StopFilter::callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg) { - tier4_debug_msgs::msg::BoolStamped stop_flag_msg; + autoware_internal_debug_msgs::msg::BoolStamped stop_flag_msg; stop_flag_msg.stamp = msg->header.stamp; stop_flag_msg.data = false; diff --git a/localization/autoware_stop_filter/src/stop_filter.hpp b/localization/autoware_stop_filter/src/stop_filter.hpp index b6d56b5f77059..71864fab0a580 100644 --- a/localization/autoware_stop_filter/src/stop_filter.hpp +++ b/localization/autoware_stop_filter/src/stop_filter.hpp @@ -17,10 +17,10 @@ #include +#include #include #include #include -#include #include #include @@ -43,7 +43,7 @@ class StopFilter : public rclcpp::Node private: rclcpp::Publisher::SharedPtr pub_odom_; //!< @brief odom publisher - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr pub_stop_flag_; //!< @brief stop flag publisher rclcpp::Subscription::SharedPtr sub_odom_; //!< @brief measurement odometry subscriber diff --git a/localization/autoware_twist2accel/package.xml b/localization/autoware_twist2accel/package.xml index 75cea94546516..092b227c2ad40 100644 --- a/localization/autoware_twist2accel/package.xml +++ b/localization/autoware_twist2accel/package.xml @@ -23,7 +23,6 @@ rclcpp rclcpp_components tf2 - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/localization/autoware_twist2accel/src/twist2accel.hpp b/localization/autoware_twist2accel/src/twist2accel.hpp index d338b256fec77..48bc03a326091 100644 --- a/localization/autoware_twist2accel/src/twist2accel.hpp +++ b/localization/autoware_twist2accel/src/twist2accel.hpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include diff --git a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp index ae07d54ad53d6..ac759504de557 100644 --- a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp @@ -173,9 +173,9 @@ void DistanceBasedCompareMapFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp index 9f325b36676be..5794cfe37a45b 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp @@ -141,9 +141,9 @@ void VoxelBasedApproximateCompareMapFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp index a31d5ec5dd6d6..db5e46a0eff5a 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp @@ -100,9 +100,9 @@ void VoxelBasedCompareMapFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp index d0bcf381cd62f..775a931ac4c4c 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp @@ -173,9 +173,9 @@ void VoxelDistanceBasedCompareMapFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_crosswalk_traffic_light_estimator/README.md b/perception/autoware_crosswalk_traffic_light_estimator/README.md index b14fefbd43beb..1b24b115f5812 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/README.md +++ b/perception/autoware_crosswalk_traffic_light_estimator/README.md @@ -1,40 +1,85 @@ -# crosswalk_traffic_light_estimator +# autoware_crosswalk_traffic_light_estimator ## Purpose -`crosswalk_traffic_light_estimator` is a module that estimates pedestrian traffic signals from HDMap and detected vehicle traffic signals. +`autoware_crosswalk_traffic_light_estimator` estimates pedestrian traffic signals which can be summarized as the following two tasks: + +- Estimate pedestrian traffic signals that are not subject to be detected by perception pipeline. +- Estimate whether pedestrian traffic signals are flashing and modify the result. + +This module works without `~/input/route`, but its behavior is outputting the subscribed results as is. ## Inputs / Outputs ### Input -| Name | Type | Description | -| ------------------------------------ | ------------------------------------------------ | ------------------ | -| `~/input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map | -| `~/input/route` | `autoware_planning_msgs::msg::LaneletRoute` | route | -| `~/input/classified/traffic_signals` | `tier4_perception_msgs::msg::TrafficSignalArray` | classified signals | +| Name | Type | Description | +| ------------------------------------ | ----------------------------------------------------- | ------------------ | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/route` | autoware_planning_msgs::msg::LaneletRoute | optional: route | +| `~/input/classified/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | classified signals | ### Output -| Name | Type | Description | -| -------------------------- | ------------------------------------------------------- | --------------------------------------------------------- | -| `~/output/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | output that contains estimated pedestrian traffic signals | +| Name | Type | Description | +| -------------------------- | ----------------------------------------------------- | --------------------------------------------------------- | +| `~/output/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | output that contains estimated pedestrian traffic signals | ## Parameters -| Name | Type | Description | Default value | -| :---------------------------- | :------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| `use_last_detect_color` | `bool` | If this parameter is `true`, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN/AMBER but also when detection results change GREEN/AMBER to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is `false`, this module use only latest detection results for estimation. (Only when the detection result is GREEN/AMBER, this module estimates pedestrian's traffic signal as RED.) | `true` | -| `last_detect_color_hold_time` | `double` | The time threshold to hold for last detect color. | `2.0` | +| Name | Type | Description | Default value | +| :---------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `use_last_detect_color` | bool | If this parameter is `true`, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN/AMBER but also when detection results change GREEN/AMBER to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is `false`, this module use only latest detection results for estimation. (Only when the detection result is GREEN/AMBER, this module estimates pedestrian's traffic signal as RED.) | true | +| `last_detect_color_hold_time` | double | The time threshold to hold for last detect color. The unit is second. | 2.0 | +| `last_colors_hold_time` | double | The time threshold to hold for history detected pedestrian traffic light color. The unit is second. | 1.0 | ## Inner-workings / Algorithms +When the pedestrian traffic signals **are detected** by perception pipeline + +- If estimates the pedestrian traffic signals are flashing, overwrite the results +- Prefer the output from perception pipeline, but overwrite it if the pedestrian traffic signals are invalid(`no detection`, `backlight`, or `occlusion`) + +When the pedestrian traffic signals **are NOT detected** by perception pipeline + +- Estimate the color of pedestrian traffic signals based on detected vehicle traffic signals, HDMap, and route + +### Estimate whether pedestrian traffic signals are flashing + +```plantumul +start +if (the pedestrian traffic light classification result exists)then + : update the flashing flag according to the classification result(in_signal) and last_signals + if (the traffic light is flashing?)then(yes) + : update the traffic light state + else(no) + : the traffic light state is the same with the classification result +if (the classification result not exists) + : the traffic light state is the same with the estimation + : output the current traffic light state +end +``` + +#### Update flashing flag + +
+ +
+ +#### Update traffic light status + +
+ +
+ +### Estimate the color of pedestrian traffic signals + ```plantuml start -:subscribe detected traffic signals & HDMap; +:subscribe detected traffic signals, HDMap, and route; :extract crosswalk lanelets from HDMap; -:extract road lanelets that conflicts crosswalk; +:extract road lanelets that conflicts crosswalk from route; :initialize non_red_lanelets(lanelet::ConstLanelets); if (Latest detection result is **GREEN** or **AMBER**?) then (yes) :push back non_red_lanelets; @@ -58,7 +103,7 @@ end If traffic between pedestrians and vehicles is controlled by traffic signals, the crosswalk traffic signal maybe **RED** in order to prevent pedestrian from crossing when the following conditions are satisfied. -### Situation1 +#### Situation1 - crosswalk conflicts **STRAIGHT** lanelet - the lanelet refers **GREEN** or **AMBER** traffic signal (The following pictures show only **GREEN** case) @@ -70,7 +115,7 @@ If traffic between pedestrians and vehicles is controlled by traffic signals, th -### Situation2 +#### Situation2 - crosswalk conflicts different turn direction lanelets (STRAIGHT and LEFT, LEFT and RIGHT, RIGHT and STRAIGHT) - the lanelets refer **GREEN** or **AMBER** traffic signal (The following pictures show only **GREEN** case) diff --git a/perception/autoware_crosswalk_traffic_light_estimator/images/flashing_state.png b/perception/autoware_crosswalk_traffic_light_estimator/images/flashing_state.png new file mode 100644 index 0000000000000..7686f3842e75c Binary files /dev/null and b/perception/autoware_crosswalk_traffic_light_estimator/images/flashing_state.png differ diff --git a/perception/autoware_crosswalk_traffic_light_estimator/images/traffic_light.png b/perception/autoware_crosswalk_traffic_light_estimator/images/traffic_light.png new file mode 100644 index 0000000000000..a03e7a88ee100 Binary files /dev/null and b/perception/autoware_crosswalk_traffic_light_estimator/images/traffic_light.png differ diff --git a/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp b/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp index 8efb90cc87c89..085e423db38ba 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp +++ b/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp @@ -97,6 +97,8 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node void removeDuplicateIds(TrafficSignalArray & signal_array) const; + bool isInvalidDetectionStatus(const TrafficSignal & signal) const; + // Node param bool use_last_detect_color_; double last_detect_color_hold_time_; diff --git a/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp b/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp index b0ec4d0e5ffd0..d7cc6c725edfd 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp @@ -298,6 +298,14 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal( if (valid_id2idx_map.count(id)) { size_t idx = valid_id2idx_map[id]; auto signal = msg.traffic_light_groups[idx]; + if (isInvalidDetectionStatus(signal)) { + TrafficSignalElement output_traffic_signal_element; + output_traffic_signal_element.color = color; + output_traffic_signal_element.shape = TrafficSignalElement::CIRCLE; + output_traffic_signal_element.confidence = 1.0; + output.traffic_light_groups[idx].elements[0] = output_traffic_signal_element; + continue; + } updateFlashingState(signal); // check if it is flashing // update output msg according to flashing and current state output.traffic_light_groups[idx].elements[0].color = updateAndGetColorState(signal); @@ -314,6 +322,19 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal( } } +bool CrosswalkTrafficLightEstimatorNode::isInvalidDetectionStatus( + const TrafficSignal & signal) const +{ + // check occlusion, backlight(shape is unknown) and no detection(shape is circle) + if ( + signal.elements.front().color == TrafficSignalElement::UNKNOWN && + signal.elements.front().confidence == 0.0) { + return true; + } + + return false; +} + void CrosswalkTrafficLightEstimatorNode::updateFlashingState(const TrafficSignal & signal) { const auto id = signal.traffic_light_group_id; diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp index f91c4e2036227..f1c25e8573e85 100644 --- a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp +++ b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp @@ -180,9 +180,9 @@ void ObjectLaneletFilterNode::objectCallback( std::chrono::nanoseconds( (this->get_clock()->now() - output_object_msg.header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp index 979cdd3909f14..0c69b45291281 100644 --- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp +++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp @@ -386,7 +386,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( std::chrono::duration( std::chrono::nanoseconds((this->get_clock()->now() - output.header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency); } diff --git a/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp b/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp index 39dd3db10cfa9..8bd1deba35ccb 100644 --- a/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp +++ b/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp @@ -89,9 +89,9 @@ class Debugger void startMeasureProcessingTime() { stop_watch_ptr_->tic("processing_time"); } void publishProcessingTime() { - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp index 74b4edbe595ea..8fda0c1395caf 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp @@ -369,9 +369,9 @@ void ScanGroundFilterComponent::faster_filter( if (debug_publisher_ptr_ && stop_watch_ptr_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); } } 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 239d406650ce8..e05339771f667 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp @@ -40,7 +40,6 @@ #include #include -#include #include #include #include @@ -52,14 +51,33 @@ using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; using sensor_msgs::msg::CameraInfo; using sensor_msgs::msg::Image; -using sensor_msgs::msg::PointCloud2; -using tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using PointCloudMsgType = sensor_msgs::msg::PointCloud2; +using RoiMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using ClusterMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using ClusterObjType = tier4_perception_msgs::msg::DetectedObjectWithFeature; using tier4_perception_msgs::msg::DetectedObjectWithFeature; using PointCloud = pcl::PointCloud; using autoware::image_projection_based_fusion::CameraProjection; using autoware_perception_msgs::msg::ObjectClassification; -template +template +struct Det2dStatus +{ + // camera index + std::size_t id = 0; + // camera projection + std::unique_ptr camera_projector_ptr; + bool project_to_unrectified_image = false; + bool approximate_camera_projection = false; + // process flags + bool is_fused = false; + // timing + double input_offset_ms = 0.0; + // cache + std::map cached_det2d_msgs; +}; + +template class FusionNode : public rclcpp::Node { public: @@ -71,75 +89,71 @@ class FusionNode : public rclcpp::Node explicit FusionNode( const std::string & node_name, const rclcpp::NodeOptions & options, int queue_size); -protected: +private: + // Common process methods void cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id); - virtual void preprocess(TargetMsg3D & output_msg); - - // callback for Msg subscription - virtual void subCallback(const typename TargetMsg3D::ConstSharedPtr input_msg); - - // callback for roi subscription - - virtual void roiCallback( - const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i); - - virtual void fuseOnSingleImage( - const TargetMsg3D & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg, - TargetMsg3D & output_msg) = 0; - - // set args if you need - virtual void postprocess(TargetMsg3D & output_msg); - - virtual void publish(const TargetMsg3D & output_msg); - + // callback for timer void timer_callback(); void setPeriod(const int64_t new_period); + void exportProcess(); + + // 2d detection management methods + bool checkAllDet2dFused() + { + for (const auto & det2d : det2d_list_) { + if (!det2d.is_fused) { + return false; + } + } + return true; + } - std::size_t rois_number_{1}; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - - 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_; + // timer rclcpp::TimerBase::SharedPtr timer_; double timeout_ms_{}; double match_threshold_ms_{}; - std::vector input_rois_topics_; - std::vector input_camera_info_topics_; - std::vector input_camera_topics_; - /** \brief A vector of subscriber. */ - typename rclcpp::Subscription::SharedPtr sub_; std::vector::SharedPtr> rois_subs_; - - // offsets between cameras and the lidars - std::vector input_offset_ms_; + std::vector::SharedPtr> camera_info_subs_; // cache for fusion - std::vector is_fused_; - std::pair - cached_msg_; // first element is the timestamp in nanoseconds, second element is the message - std::vector> cached_roi_msgs_; - std::mutex mutex_cached_msgs_; + int64_t cached_det3d_msg_timestamp_; + typename Msg3D::SharedPtr cached_det3d_msg_ptr_; - // output publisher - typename rclcpp::Publisher::SharedPtr pub_ptr_; +protected: + void setDet2DStatus(std::size_t rois_number); - // debugger - std::shared_ptr debugger_; - virtual bool out_of_scope(const ObjType & obj) = 0; + // callback for main subscription + void subCallback(const typename Msg3D::ConstSharedPtr input_msg); + // callback for roi subscription + void roiCallback(const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i); + + // Custom process methods + virtual void preprocess(Msg3D & output_msg); + virtual void fuseOnSingleImage( + const Msg3D & input_msg, const Det2dStatus & det2d, const Msg2D & input_roi_msg, + Msg3D & output_msg) = 0; + virtual void postprocess(const Msg3D & processing_msg, ExportObj & output_msg); + virtual void publish(const ExportObj & output_msg); + + // Members + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + // 2d detection management + std::vector> det2d_list_; + + /** \brief A vector of subscriber. */ + typename rclcpp::Subscription::SharedPtr sub_; + + // parameters for out_of_scope filter float filter_scope_min_x_; float filter_scope_max_x_; float filter_scope_min_y_; @@ -147,6 +161,12 @@ class FusionNode : public rclcpp::Node float filter_scope_min_z_; float filter_scope_max_z_; + // output publisher + typename rclcpp::Publisher::SharedPtr pub_ptr_; + + // debugger + std::shared_ptr debugger_; + /** \brief processing time publisher. **/ std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp index cd6cd87976522..9505a926df2f8 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -41,26 +42,25 @@ inline bool isInsideBbox( proj_y >= roi.y_offset * zc && proj_y <= (roi.y_offset + roi.height) * zc; } -class PointPaintingFusionNode -: public FusionNode +class PointPaintingFusionNode : public FusionNode { public: explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options); -protected: - void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; +private: + void preprocess(PointCloudMsgType & pointcloud_msg) override; void fuseOnSingleImage( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override; + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, PointCloudMsgType & painted_pointcloud_msg) override; - void postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override; + void postprocess( + const PointCloudMsgType & painted_pointcloud_msg, DetectedObjects & output_msg) override; - rclcpp::Publisher::SharedPtr obj_pub_ptr_; + rclcpp::Publisher::SharedPtr point_pub_ptr_; + std::unique_ptr diagnostics_interface_ptr_; int omp_num_threads_{1}; - float score_threshold_{0.0}; std::vector class_names_; std::map class_index_; std::map> isClassTable_; @@ -72,8 +72,6 @@ class PointPaintingFusionNode autoware::lidar_centerpoint::DetectionClassRemapper detection_class_remapper_; std::unique_ptr detector_ptr_{nullptr}; - - bool out_of_scope(const DetectedObjects & obj) override; }; } // namespace autoware::image_projection_based_fusion #endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__NODE_HPP_ diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp index a89ee3d02cd8b..a1d12bcb56384 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -29,8 +30,8 @@ class PointPaintingTRT : public autoware::lidar_centerpoint::CenterPointTRT using autoware::lidar_centerpoint::CenterPointTRT::CenterPointTRT; explicit PointPaintingTRT( - const autoware::lidar_centerpoint::NetworkParam & encoder_param, - const autoware::lidar_centerpoint::NetworkParam & head_param, + const autoware::tensorrt_common::TrtCommonConfig & encoder_param, + const autoware::tensorrt_common::TrtCommonConfig & head_param, const autoware::lidar_centerpoint::DensificationParam & densification_param, const autoware::lidar_centerpoint::CenterPointConfig & config); 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 903b153af0681..b7bf8738b68a4 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -24,21 +24,19 @@ namespace autoware::image_projection_based_fusion { const std::map IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}}; -class RoiClusterFusionNode -: public FusionNode< - DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature> +class RoiClusterFusionNode : public FusionNode { public: explicit RoiClusterFusionNode(const rclcpp::NodeOptions & options); -protected: - void preprocess(DetectedObjectsWithFeature & output_cluster_msg) override; - void postprocess(DetectedObjectsWithFeature & output_cluster_msg) override; +private: + void preprocess(ClusterMsgType & output_cluster_msg) override; void fuseOnSingleImage( - const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - DetectedObjectsWithFeature & output_cluster_msg) override; + const ClusterMsgType & input_cluster_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) override; + + void postprocess(const ClusterMsgType & output_cluster_msg, ClusterMsgType & output_msg) override; std::string trust_object_iou_mode_{"iou"}; bool use_cluster_semantic_type_{false}; @@ -53,12 +51,11 @@ class RoiClusterFusionNode double trust_object_distance_; std::string non_trust_object_iou_mode_{"iou_x"}; - bool is_far_enough(const DetectedObjectWithFeature & obj, const double distance_threshold); - bool out_of_scope(const DetectedObjectWithFeature & obj) override; + bool is_far_enough(const ClusterObjType & obj, const double distance_threshold); + bool out_of_scope(const ClusterObjType & obj); double cal_iou_by_mode( const sensor_msgs::msg::RegionOfInterest & roi_1, const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode); - // bool CheckUnknown(const DetectedObjectsWithFeature & obj); }; } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index 43ec134168ef9..ba197126277a0 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -31,21 +31,20 @@ namespace autoware::image_projection_based_fusion using sensor_msgs::msg::RegionOfInterest; -class RoiDetectedObjectFusionNode -: public FusionNode +class RoiDetectedObjectFusionNode : public FusionNode { public: explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options); -protected: +private: void preprocess(DetectedObjects & output_msg) override; void fuseOnSingleImage( - const DetectedObjects & input_object_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, DetectedObjects & output_object_msg) override; + const DetectedObjects & input_object_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, DetectedObjects & output_object_msg) override; std::map generateDetectedObjectRoIs( - const DetectedObjects & input_object_msg, const std::size_t & image_id, + const DetectedObjects & input_object_msg, const Det2dStatus & det2d, const Eigen::Affine3d & object2camera_affine); void fuseObjectsOnImage( @@ -53,11 +52,10 @@ class RoiDetectedObjectFusionNode const std::vector & image_rois, const std::map & object_roi_map); - void publish(const DetectedObjects & output_msg) override; + void postprocess(const DetectedObjects & processing_msg, DetectedObjects & output_msg) override; - bool out_of_scope(const DetectedObject & obj) override; + bool out_of_scope(const DetectedObject & obj); -private: struct { std::vector passthrough_lower_bound_probability_thresholds{}; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp index 2f2c8222e196f..77a1745faa7e5 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -25,32 +25,29 @@ namespace autoware::image_projection_based_fusion { -class RoiPointCloudFusionNode -: public FusionNode +class RoiPointCloudFusionNode : public FusionNode { +public: + explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options); + private: - int min_cluster_size_{1}; - int max_cluster_size_{20}; - bool fuse_unknown_only_{true}; - double cluster_2d_tolerance_; + rclcpp::Publisher::SharedPtr point_pub_ptr_; + rclcpp::Publisher::SharedPtr cluster_debug_pub_; - rclcpp::Publisher::SharedPtr pub_objects_ptr_; - std::vector output_fused_objects_; - rclcpp::Publisher::SharedPtr cluster_debug_pub_; + void fuseOnSingleImage( + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, PointCloudMsgType & output_pointcloud_msg) override; - /* data */ -public: - explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options); + void postprocess(const PointCloudMsgType & pointcloud_msg, ClusterMsgType & output_msg) override; -protected: - void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + void publish(const ClusterMsgType & output_msg) override; - void postprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + int min_cluster_size_{1}; + int max_cluster_size_{20}; + bool fuse_unknown_only_{true}; + double cluster_2d_tolerance_; - void fuseOnSingleImage( - const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, PointCloud2 & output_pointcloud_msg) override; - bool out_of_scope(const DetectedObjectWithFeature & obj) override; + std::vector output_fused_objects_; }; } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index 7454cb7bcd173..5414f98e142cd 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -34,10 +34,32 @@ namespace autoware::image_projection_based_fusion { -class SegmentPointCloudFusionNode : public FusionNode +class SegmentPointCloudFusionNode : public FusionNode { +public: + explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options); + private: - rclcpp::Publisher::SharedPtr pub_pointcloud_ptr_; + void preprocess(PointCloudMsgType & pointcloud_msg) override; + + void fuseOnSingleImage( + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + const Image & input_mask, PointCloudMsgType & output_pointcloud_msg) override; + + inline void copyPointCloud( + const PointCloudMsgType & input, const int point_step, const size_t global_offset, + PointCloudMsgType & output, size_t & output_pointcloud_size) + { + std::memcpy(&output.data[output_pointcloud_size], &input.data[global_offset], point_step); + output_pointcloud_size += point_step; + } + + void postprocess( + const PointCloudMsgType & pointcloud_msg, PointCloudMsgType & output_msg) override; + + // debug + image_transport::Publisher pub_debug_mask_ptr_; + std::vector filter_semantic_label_target_; float filter_distance_threshold_; // declare list of semantic label target, depend on trained data of yolox segmentation model @@ -47,30 +69,8 @@ class SegmentPointCloudFusionNode : public FusionNode filter_global_offset_set_; - -public: - explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options); - -protected: - void preprocess(PointCloud2 & pointcloud_msg) override; - - void postprocess(PointCloud2 & pointcloud_msg) override; - - void fuseOnSingleImage( - const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, const Image & input_mask, - PointCloud2 & output_pointcloud_msg) override; - - bool out_of_scope(const PointCloud2 & filtered_cloud) override; - inline void copyPointCloud( - const PointCloud2 & input, const int point_step, const size_t global_offset, - PointCloud2 & output, size_t & output_pointcloud_size) - { - std::memcpy(&output.data[output_pointcloud_size], &input.data[global_offset], point_step); - output_pointcloud_size += point_step; - } }; } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp index 12577081713be..044a71ab57533 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp @@ -56,7 +56,7 @@ namespace autoware::image_projection_based_fusion { using PointCloud = pcl::PointCloud; -using PointCloud2 = sensor_msgs::msg::PointCloud2; + struct PointData { float distance; @@ -72,17 +72,17 @@ std::optional getTransformStamped( Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t); void closest_cluster( - const PointCloud2 & cluster, const double cluster_2d_tolerance, const int min_cluster_size, - const pcl::PointXYZ & center, PointCloud2 & out_cluster); + const PointCloudMsgType & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center, PointCloudMsgType & out_cluster); void updateOutputFusedObjects( - std::vector & output_objs, std::vector & clusters, - const std::vector & clusters_data_size, const PointCloud2 & in_cloud, + std::vector & output_objs, std::vector & clusters, + const std::vector & clusters_data_size, const PointCloudMsgType & in_cloud, const std_msgs::msg::Header & in_roi_header, const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const int max_cluster_size, const float cluster_2d_tolerance, std::vector & output_fused_objects); -geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); +geometry_msgs::msg::Point getCentroid(const PointCloudMsgType & pointcloud); } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index ea0904215cb86..700d249831000 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -45,79 +46,66 @@ namespace autoware::image_projection_based_fusion { using autoware::universe_utils::ScopedTimeTrack; -template -FusionNode::FusionNode( +template +FusionNode::FusionNode( const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { // set rois_number - rois_number_ = static_cast(declare_parameter("rois_number")); - if (rois_number_ < 1) { - RCLCPP_WARN( - this->get_logger(), "minimum rois_number is 1. current rois_number is %zu", rois_number_); - rois_number_ = 1; + const std::size_t rois_number = + static_cast(declare_parameter("rois_number")); + if (rois_number < 1) { + RCLCPP_ERROR( + this->get_logger(), "minimum rois_number is 1. current rois_number is %zu", rois_number); } - if (rois_number_ > 8) { + if (rois_number > 8) { RCLCPP_WARN( this->get_logger(), - "Current rois_number is %zu. Large rois number may cause performance issue.", rois_number_); + "Current rois_number is %zu. Large rois number may cause performance issue.", rois_number); } // Set parameters match_threshold_ms_ = declare_parameter("match_threshold_ms"); timeout_ms_ = declare_parameter("timeout_ms"); - input_rois_topics_.resize(rois_number_); - input_camera_topics_.resize(rois_number_); - input_camera_info_topics_.resize(rois_number_); + std::vector input_rois_topics; + std::vector input_camera_info_topics; + + input_rois_topics.resize(rois_number); + input_camera_info_topics.resize(rois_number); - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - input_rois_topics_.at(roi_i) = declare_parameter( + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { + input_rois_topics.at(roi_i) = declare_parameter( "input/rois" + std::to_string(roi_i), "/perception/object_recognition/detection/rois" + std::to_string(roi_i)); - input_camera_info_topics_.at(roi_i) = declare_parameter( + input_camera_info_topics.at(roi_i) = declare_parameter( "input/camera_info" + std::to_string(roi_i), "/sensing/camera/camera" + std::to_string(roi_i) + "/camera_info"); - - input_camera_topics_.at(roi_i) = declare_parameter( - "input/image" + std::to_string(roi_i), - "/sensing/camera/camera" + std::to_string(roi_i) + "/image_rect_color"); } - input_offset_ms_ = declare_parameter>("input_offset_ms"); - if (!input_offset_ms_.empty() && rois_number_ > input_offset_ms_.size()) { - throw std::runtime_error("The number of offsets does not match the number of topics."); - } - - // sub camera info - camera_info_subs_.resize(rois_number_); - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { + // subscribe camera info + camera_info_subs_.resize(rois_number); + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { std::function fnc = std::bind(&FusionNode::cameraInfoCallback, this, std::placeholders::_1, roi_i); camera_info_subs_.at(roi_i) = this->create_subscription( - input_camera_info_topics_.at(roi_i), rclcpp::QoS{1}.best_effort(), fnc); + input_camera_info_topics.at(roi_i), rclcpp::QoS{1}.best_effort(), fnc); } - // sub rois - rois_subs_.resize(rois_number_); - cached_roi_msgs_.resize(rois_number_); - is_fused_.resize(rois_number_, false); - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { + // subscribe rois + rois_subs_.resize(rois_number); + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { std::function roi_callback = std::bind(&FusionNode::roiCallback, this, std::placeholders::_1, roi_i); rois_subs_.at(roi_i) = this->create_subscription( - input_rois_topics_.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); + input_rois_topics.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); } - // subscribers - std::function sub_callback = + // subscribe 3d detection + std::function sub_callback = std::bind(&FusionNode::subCallback, this, std::placeholders::_1); - sub_ = - this->create_subscription("input", rclcpp::QoS(1).best_effort(), sub_callback); - - // publisher - pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); + sub_ = this->create_subscription("input", rclcpp::QoS(1).best_effort(), sub_callback); // Set timer const auto period_ns = std::chrono::duration_cast( @@ -125,40 +113,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"); + // initialization on each 2d detections + setDet2DStatus(rois_number); - 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; - } - } - } + // parameters for approximation grid approx_grid_cell_w_size_ = declare_parameter("approximation_grid_cell_width"); approx_grid_cell_h_size_ = declare_parameter("approximation_grid_cell_height"); + // parameters for out_of_scope filter + filter_scope_min_x_ = declare_parameter("filter_scope_min_x"); + filter_scope_max_x_ = declare_parameter("filter_scope_max_x"); + filter_scope_min_y_ = declare_parameter("filter_scope_min_y"); + filter_scope_max_y_ = declare_parameter("filter_scope_max_y"); + filter_scope_min_z_ = declare_parameter("filter_scope_min_z"); + filter_scope_max_z_ = declare_parameter("filter_scope_max_z"); + // debugger if (declare_parameter("debug_mode", false)) { + std::vector input_camera_topics; + input_camera_topics.resize(rois_number); + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { + input_camera_topics.at(roi_i) = declare_parameter( + "input/image" + std::to_string(roi_i), + "/sensing/camera/camera" + std::to_string(roi_i) + "/image_rect_color"); + } std::size_t image_buffer_size = static_cast(declare_parameter("image_buffer_size")); debugger_ = - std::make_shared(this, rois_number_, image_buffer_size, input_camera_topics_); + std::make_shared(this, rois_number, image_buffer_size, input_camera_topics); } // time keeper @@ -180,77 +162,125 @@ FusionNode::FusionNode( stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } +} - filter_scope_min_x_ = declare_parameter("filter_scope_min_x"); - filter_scope_max_x_ = declare_parameter("filter_scope_max_x"); - filter_scope_min_y_ = declare_parameter("filter_scope_min_y"); - filter_scope_max_y_ = declare_parameter("filter_scope_max_y"); - filter_scope_min_z_ = declare_parameter("filter_scope_min_z"); - filter_scope_max_z_ = declare_parameter("filter_scope_max_z"); +template +void FusionNode::setDet2DStatus(std::size_t rois_number) +{ + // camera offset settings + std::vector input_offset_ms = declare_parameter>("input_offset_ms"); + if (!input_offset_ms.empty() && rois_number > input_offset_ms.size()) { + throw std::runtime_error("The number of offsets does not match the number of topics."); + } + + // camera projection settings + std::vector point_project_to_unrectified_image = + declare_parameter>("point_project_to_unrectified_image"); + if (rois_number > point_project_to_unrectified_image.size()) { + throw std::runtime_error( + "The number of point_project_to_unrectified_image does not match the number of rois " + "topics."); + } + std::vector approx_camera_projection = + declare_parameter>("approximate_camera_projection"); + if (rois_number != approx_camera_projection.size()) { + const std::size_t current_size = approx_camera_projection.size(); + RCLCPP_WARN( + get_logger(), + "The number of elements in approximate_camera_projection should be the same as in " + "rois_number. " + "It has %zu elements.", + current_size); + if (current_size < rois_number) { + approx_camera_projection.resize(rois_number); + for (std::size_t i = current_size; i < rois_number; i++) { + approx_camera_projection.at(i) = true; + } + } + } + + // 2d detection status initialization + det2d_list_.resize(rois_number); + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { + det2d_list_.at(roi_i).id = roi_i; + det2d_list_.at(roi_i).project_to_unrectified_image = + point_project_to_unrectified_image.at(roi_i); + det2d_list_.at(roi_i).approximate_camera_projection = approx_camera_projection.at(roi_i); + det2d_list_.at(roi_i).input_offset_ms = input_offset_ms.at(roi_i); + } } -template -void FusionNode::cameraInfoCallback( +template +void FusionNode::cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id) { // 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( + auto & det2d = det2d_list_.at(camera_id); + if (!det2d.camera_projector_ptr && checkCameraInfo(*input_camera_info_msg)) { + det2d.camera_projector_ptr = std::make_unique( *input_camera_info_msg, approx_grid_cell_w_size_, approx_grid_cell_h_size_, - 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; + det2d.project_to_unrectified_image, det2d.approximate_camera_projection); + det2d.camera_projector_ptr->initialize(); } } -template -void FusionNode::preprocess(TargetMsg3D & ouput_msg - __attribute__((unused))) +template +void FusionNode::preprocess(Msg3D & ouput_msg __attribute__((unused))) { // do nothing by default } -template -void FusionNode::subCallback( - const typename TargetMsg3D::ConstSharedPtr input_msg) +template +void FusionNode::exportProcess() +{ + timer_->cancel(); + + ExportObj output_msg; + postprocess(*(cached_det3d_msg_ptr_), output_msg); + publish(output_msg); + + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - cached_det3d_msg_ptr_->header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", + processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); + processing_time_ms = 0; + } + cached_det3d_msg_ptr_ = nullptr; +} + +template +void FusionNode::subCallback( + const typename Msg3D::ConstSharedPtr det3d_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - if (cached_msg_.second != nullptr) { + if (cached_det3d_msg_ptr_ != nullptr) { + // PROCESS: if the main message is remained (and roi is not collected all) publish the main + // message may processed partially with arrived 2d rois stop_watch_ptr_->toc("processing_time", true); - timer_->cancel(); - postprocess(*(cached_msg_.second)); - publish(*(cached_msg_.second)); - std::fill(is_fused_.begin(), is_fused_.end(), false); - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - const double pipeline_latency_ms = - std::chrono::duration( - std::chrono::nanoseconds( - (this->get_clock()->now() - cached_msg_.second->header.stamp).nanoseconds())) - .count(); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", - processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); - debug_publisher_->publish( - "debug/pipeline_latency_ms", pipeline_latency_ms); - processing_time_ms = 0; - } + exportProcess(); - cached_msg_.second = nullptr; + // reset flags + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; + } } - std::lock_guard lock(mutex_cached_msgs_); + // TIMING: reset timer to the timeout time auto period = std::chrono::duration_cast( std::chrono::duration(timeout_ms_)); try { @@ -262,154 +292,141 @@ void FusionNode::subCallback( stop_watch_ptr_->toc("processing_time", true); - typename TargetMsg3D::SharedPtr output_msg = std::make_shared(*input_msg); - + // PROCESS: preprocess the main message + typename Msg3D::SharedPtr output_msg = std::make_shared(*det3d_msg); preprocess(*output_msg); + // PROCESS: fuse the main message with the cached roi messages + // (please ask maintainers before parallelize this loop because debugger is not thread safe) int64_t timestamp_nsec = (*output_msg).header.stamp.sec * static_cast(1e9) + (*output_msg).header.stamp.nanosec; + // for loop for each roi + for (auto & det2d : det2d_list_) { + const auto roi_i = det2d.id; - // if matching rois exist, fuseOnSingle - // please ask maintainers before parallelize this loop because debugger is not thread safe - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { + // check camera info + if (det2d.camera_projector_ptr == nullptr) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); continue; } - - if ((cached_roi_msgs_.at(roi_i)).size() > 0) { - int64_t min_interval = 1e9; - int64_t matched_stamp = -1; - std::list outdate_stamps; - - for (const auto & [k, v] : cached_roi_msgs_.at(roi_i)) { - int64_t new_stamp = timestamp_nsec + input_offset_ms_.at(roi_i) * static_cast(1e6); - int64_t interval = abs(static_cast(k) - new_stamp); - - if ( - interval <= min_interval && interval <= match_threshold_ms_ * static_cast(1e6)) { - min_interval = interval; - matched_stamp = k; - } else if ( - static_cast(k) < new_stamp && - interval > match_threshold_ms_ * static_cast(1e6)) { - outdate_stamps.push_back(static_cast(k)); - } + auto & det2d_msgs = det2d.cached_det2d_msgs; + + // check if the roi is collected + if (det2d_msgs.size() == 0) continue; + + // MATCH: get the closest roi message, and remove outdated messages + int64_t min_interval = 1e9; + int64_t matched_stamp = -1; + std::list outdate_stamps; + for (const auto & [roi_stamp, value] : det2d_msgs) { + int64_t new_stamp = timestamp_nsec + det2d.input_offset_ms * static_cast(1e6); + int64_t interval = abs(static_cast(roi_stamp) - new_stamp); + + if (interval <= min_interval && interval <= match_threshold_ms_ * static_cast(1e6)) { + min_interval = interval; + matched_stamp = roi_stamp; + } else if ( + static_cast(roi_stamp) < new_stamp && + interval > match_threshold_ms_ * static_cast(1e6)) { + outdate_stamps.push_back(static_cast(roi_stamp)); } + } + for (auto stamp : outdate_stamps) { + det2d_msgs.erase(stamp); + } - // remove outdated stamps - for (auto stamp : outdate_stamps) { - (cached_roi_msgs_.at(roi_i)).erase(stamp); + // PROCESS: if matched, fuse the main message with the roi message + if (matched_stamp != -1) { + if (debugger_) { + debugger_->clear(); } - // fuseOnSingle - if (matched_stamp != -1) { - if (debugger_) { - debugger_->clear(); - } + fuseOnSingleImage(*det3d_msg, det2d, *(det2d_msgs[matched_stamp]), *output_msg); + det2d_msgs.erase(matched_stamp); + det2d.is_fused = true; - fuseOnSingleImage( - *input_msg, roi_i, *((cached_roi_msgs_.at(roi_i))[matched_stamp]), *output_msg); - (cached_roi_msgs_.at(roi_i)).erase(matched_stamp); - is_fused_.at(roi_i) = true; - - // add timestamp interval for debug - if (debug_publisher_) { - double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6; - debug_publisher_->publish( - "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); - debug_publisher_->publish( - "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", - timestamp_interval_ms - input_offset_ms_.at(roi_i)); - } + // add timestamp interval for debug + if (debug_publisher_) { + double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6; + debug_publisher_->publish( + "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); + debug_publisher_->publish( + "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", + timestamp_interval_ms - det2d.input_offset_ms); } } } - // if all camera fused, postprocess; else, publish the old Msg(if exists) and cache the current - // Msg - if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { - timer_->cancel(); - postprocess(*output_msg); - publish(*output_msg); - std::fill(is_fused_.begin(), is_fused_.end(), false); - cached_msg_.second = nullptr; - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", processing_time_ms); - processing_time_ms = 0; + // PROCESS: check if the fused message is ready to publish + cached_det3d_msg_timestamp_ = timestamp_nsec; + cached_det3d_msg_ptr_ = output_msg; + if (checkAllDet2dFused()) { + // if all camera fused, postprocess and publish the main message + exportProcess(); + + // reset flags + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; } } else { - cached_msg_.first = timestamp_nsec; - cached_msg_.second = output_msg; + // if all of rois are not collected, publish the old Msg(if exists) and cache the + // current Msg processing_time_ms = stop_watch_ptr_->toc("processing_time", true); } } -template -void FusionNode::roiCallback( - const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i) +template +void FusionNode::roiCallback( + const typename Msg2D::ConstSharedPtr det2d_msg, const std::size_t roi_i) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); stop_watch_ptr_->toc("processing_time", true); - int64_t timestamp_nsec = (*input_roi_msg).header.stamp.sec * static_cast(1e9) + - (*input_roi_msg).header.stamp.nanosec; + auto & det2d = det2d_list_.at(roi_i); + int64_t timestamp_nsec = + (*det2d_msg).header.stamp.sec * static_cast(1e9) + (*det2d_msg).header.stamp.nanosec; // if cached Msg exist, try to match - if (cached_msg_.second != nullptr) { - int64_t new_stamp = cached_msg_.first + input_offset_ms_.at(roi_i) * static_cast(1e6); + if (cached_det3d_msg_ptr_ != nullptr) { + int64_t new_stamp = + cached_det3d_msg_timestamp_ + det2d.input_offset_ms * static_cast(1e6); int64_t interval = abs(timestamp_nsec - new_stamp); - if ( - interval < match_threshold_ms_ * static_cast(1e6) && is_fused_.at(roi_i) == false) { - if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { + // PROCESS: if matched, fuse the main message with the roi message + if (interval < match_threshold_ms_ * static_cast(1e6) && det2d.is_fused == false) { + // check camera info + if (det2d.camera_projector_ptr == nullptr) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); - (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; + det2d.cached_det2d_msgs[timestamp_nsec] = det2d_msg; return; } + if (debugger_) { debugger_->clear(); } - - fuseOnSingleImage(*(cached_msg_.second), roi_i, *input_roi_msg, *(cached_msg_.second)); - is_fused_.at(roi_i) = true; + // PROCESS: fuse the main message with the roi message + fuseOnSingleImage(*(cached_det3d_msg_ptr_), det2d, *det2d_msg, *(cached_det3d_msg_ptr_)); + det2d.is_fused = true; if (debug_publisher_) { - double timestamp_interval_ms = (timestamp_nsec - cached_msg_.first) / 1e6; + double timestamp_interval_ms = (timestamp_nsec - cached_det3d_msg_timestamp_) / 1e6; debug_publisher_->publish( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); debug_publisher_->publish( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", - timestamp_interval_ms - input_offset_ms_.at(roi_i)); + timestamp_interval_ms - det2d.input_offset_ms); } - if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { - timer_->cancel(); - postprocess(*(cached_msg_.second)); - publish(*(cached_msg_.second)); - std::fill(is_fused_.begin(), is_fused_.end(), false); - cached_msg_.second = nullptr; - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", - processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); - processing_time_ms = 0; + // PROCESS: if all camera fused, postprocess and publish the main message + if (checkAllDet2dFused()) { + exportProcess(); + // reset flags + for (auto & status : det2d_list_) { + status.is_fused = false; } } processing_time_ms = processing_time_ms + stop_watch_ptr_->toc("processing_time", true); @@ -417,60 +434,32 @@ void FusionNode::roiCallback( } } // store roi msg if not matched - (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; + det2d.cached_det2d_msgs[timestamp_nsec] = det2d_msg; } -template -void FusionNode::postprocess(TargetMsg3D & output_msg - __attribute__((unused))) -{ - // do nothing by default -} - -template -void FusionNode::timer_callback() +template +void FusionNode::timer_callback() { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); using std::chrono_literals::operator""ms; timer_->cancel(); - if (mutex_cached_msgs_.try_lock()) { - // timeout, postprocess cached msg - if (cached_msg_.second != nullptr) { - stop_watch_ptr_->toc("processing_time", true); - - postprocess(*(cached_msg_.second)); - publish(*(cached_msg_.second)); - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", - processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); - processing_time_ms = 0; - } - } - std::fill(is_fused_.begin(), is_fused_.end(), false); - cached_msg_.second = nullptr; + // PROCESS: if timeout, postprocess cached msg + if (cached_det3d_msg_ptr_ != nullptr) { + stop_watch_ptr_->toc("processing_time", true); + exportProcess(); + } - mutex_cached_msgs_.unlock(); - } else { - try { - std::chrono::nanoseconds period = 10ms; - setPeriod(period.count()); - } catch (rclcpp::exceptions::RCLError & ex) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); - } - timer_->reset(); + // reset flags whether the message is fused or not + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; } } -template -void FusionNode::setPeriod(const int64_t new_period) +template +void FusionNode::setPeriod(const int64_t new_period) { if (!timer_) { return; @@ -486,8 +475,16 @@ void FusionNode::setPeriod(const int64_t new_period) } } -template -void FusionNode::publish(const TargetMsg3D & output_msg) +template +void FusionNode::postprocess( + const Msg3D & processing_msg __attribute__((unused)), + ExportObj & output_msg __attribute__((unused))) +{ + // do nothing by default +} + +template +void FusionNode::publish(const ExportObj & output_msg) { if (pub_ptr_->get_subscription_count() < 1) { return; @@ -495,10 +492,21 @@ void FusionNode::publish(const TargetMsg3D & output_msg pub_ptr_->publish(output_msg); } -template class FusionNode; -template class FusionNode< - DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature>; -template class FusionNode; -template class FusionNode; -template class FusionNode; +// Explicit instantiation for the supported types + +// pointpainting fusion +template class FusionNode; + +// roi cluster fusion +template class FusionNode; + +// roi detected-object fusion +template class FusionNode; + +// roi pointcloud fusion +template class FusionNode; + +// segment pointcloud fusion +template class FusionNode; + } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 2837bac458541..ef03cf6c3a7cf 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -91,8 +91,7 @@ inline bool isUnknown(int label2d) } PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "pointpainting_fusion", options) +: FusionNode("pointpainting_fusion", options) { omp_num_threads_ = this->declare_parameter("omp_params.num_threads"); const float score_threshold = @@ -156,11 +155,16 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); - std::function sub_callback = + // subscriber + std::function sub_callback = std::bind(&PointPaintingFusionNode::subCallback, this, std::placeholders::_1); - sub_ = this->create_subscription( + sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS().keep_last(3), sub_callback); + // publisher + point_pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); + pub_ptr_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); + detection_class_remapper_.setParameters( allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); @@ -172,10 +176,10 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt iou_bev_nms_.setParameters(p); } - autoware::lidar_centerpoint::NetworkParam encoder_param( - encoder_onnx_path, encoder_engine_path, trt_precision); - autoware::lidar_centerpoint::NetworkParam head_param( - head_onnx_path, head_engine_path, trt_precision); + autoware::tensorrt_common::TrtCommonConfig encoder_param( + encoder_onnx_path, trt_precision, encoder_engine_path); + autoware::tensorrt_common::TrtCommonConfig head_param( + head_onnx_path, trt_precision, head_engine_path); autoware::lidar_centerpoint::DensificationParam densification_param( densification_world_frame_id, densification_num_past_frames); autoware::lidar_centerpoint::CenterPointConfig config( @@ -186,8 +190,8 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt // create detector detector_ptr_ = std::make_unique( encoder_param, head_param, densification_param, config); - - obj_pub_ptr_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); + diagnostics_interface_ptr_ = + std::make_unique(this, "pointpainting_trt"); if (this->declare_parameter("build_only", false)) { RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); @@ -195,7 +199,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt } } -void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) +void PointPaintingFusionNode::preprocess(PointCloudMsgType & painted_pointcloud_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -252,9 +256,9 @@ void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted } void PointPaintingFusionNode::fuseOnSingleImage( - __attribute__((unused)) const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, - const std::size_t image_id, const DetectedObjectsWithFeature & input_roi_msg, - sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) + __attribute__((unused)) const PointCloudMsgType & input_pointcloud_msg, + const Det2dStatus & det2d, const RoiMsgType & input_roi_msg, + PointCloudMsgType & painted_pointcloud_msg) { if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { RCLCPP_WARN_STREAM_THROTTLE( @@ -286,10 +290,6 @@ void PointPaintingFusionNode::fuseOnSingleImage( lidar2cam_affine = _transformToEigen(transform_stamped_optional.value().transform); } - // transform - // sensor_msgs::msg::PointCloud2 transformed_pointcloud; - // tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped); - const auto x_offset = painted_pointcloud_msg.fields .at(static_cast(autoware::point_types::PointXYZIRCIndex::X)) .offset; @@ -316,76 +316,87 @@ dc | dc dc dc dc ||zc| // iterate points // Requires 'OMP_NUM_THREADS=N' omp_set_num_threads(omp_num_threads_); -#pragma omp parallel for - for (int i = 0; i < iterations; i++) { - int stride = p_step * i; - unsigned char * data = &painted_pointcloud_msg.data[0]; - unsigned char * output = &painted_pointcloud_msg.data[0]; - // cppcheck-suppress-begin invalidPointerCast - float p_x = *reinterpret_cast(&data[stride + x_offset]); - float p_y = *reinterpret_cast(&data[stride + y_offset]); - float p_z = *reinterpret_cast(&data[stride + z_offset]); - // cppcheck-suppress-end invalidPointerCast - point_lidar << p_x, p_y, p_z; - point_camera = lidar2cam_affine * point_lidar; - p_x = point_camera.x(); - p_y = point_camera.y(); - p_z = point_camera.z(); - - if (camera_projectors_[image_id].isOutsideHorizontalView(p_x, p_z)) { - continue; - } + std::vector> local_vectors(omp_num_threads_); +#pragma omp parallel + { +#pragma omp for + for (int i = 0; i < iterations; i++) { + int stride = p_step * i; + unsigned char * data = &painted_pointcloud_msg.data[0]; + unsigned char * output = &painted_pointcloud_msg.data[0]; + // cppcheck-suppress-begin invalidPointerCast + float p_x = *reinterpret_cast(&data[stride + x_offset]); + float p_y = *reinterpret_cast(&data[stride + y_offset]); + float p_z = *reinterpret_cast(&data[stride + z_offset]); + // cppcheck-suppress-end invalidPointerCast + point_lidar << p_x, p_y, p_z; + point_camera = lidar2cam_affine * point_lidar; + p_x = point_camera.x(); + p_y = point_camera.y(); + p_z = point_camera.z(); + + if (det2d.camera_projector_ptr->isOutsideHorizontalView(p_x, p_z)) { + continue; + } - // project - 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; + // project + Eigen::Vector2d projected_point; + if (det2d.camera_projector_ptr->calcImageProjectedPoint( + cv::Point3d(p_x, p_y, p_z), projected_point)) { + // iterate 2d bbox + for (const auto & feature_object : objects) { + sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; + // paint current point if it is inside bbox + int label2d = feature_object.object.classification.front().label; + if ( + !isUnknown(label2d) && + isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) { + // cppcheck-suppress invalidPointerCast + auto p_class = reinterpret_cast(&output[stride + class_offset]); + for (const auto & cls : isClassTable_) { + // add up the class values if the point belongs to multiple classes + *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; + } + } + if (debugger_) { + int thread_id = omp_get_thread_num(); + local_vectors[thread_id].push_back(projected_point); } } } -#if 0 - // Parallelizing loop don't support push_back - if (debugger_) { - debug_image_points.push_back(projected_point); - } -#endif } - } + if (debugger_) { + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = std::make_unique("publish debug message", *time_keeper_); - if (debugger_) { - std::unique_ptr inner_st_ptr; - if (time_keeper_) - inner_st_ptr = std::make_unique("publish debug message", *time_keeper_); + for (const auto & feature_object : input_roi_msg.feature_objects) { + debug_image_rois.push_back(feature_object.feature.roi); + } - for (const auto & feature_object : input_roi_msg.feature_objects) { - debug_image_rois.push_back(feature_object.feature.roi); - } + for (const auto & local_vec : local_vectors) { + debug_image_points.insert(debug_image_points.end(), local_vec.begin(), local_vec.end()); + } - debugger_->image_rois_ = debug_image_rois; - debugger_->obstacle_points_ = debug_image_points; - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->image_rois_ = debug_image_rois; + debugger_->obstacle_points_ = debug_image_points; + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); + } } } -void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) +void PointPaintingFusionNode::postprocess( + const PointCloudMsgType & painted_pointcloud_msg, DetectedObjects & output_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + diagnostics_interface_ptr_->clear(); + + output_msg.header = painted_pointcloud_msg.header; + output_msg.objects.clear(); const auto objects_sub_count = - obj_pub_ptr_->get_subscription_count() + obj_pub_ptr_->get_intra_process_subscription_count(); + pub_ptr_->get_subscription_count() + pub_ptr_->get_intra_process_subscription_count(); if (objects_sub_count < 1) { return; } @@ -397,10 +408,21 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte } std::vector det_boxes3d; - bool is_success = detector_ptr_->detect(painted_pointcloud_msg, tf_buffer_, det_boxes3d); + bool is_num_pillars_within_range = true; + bool is_success = detector_ptr_->detect( + painted_pointcloud_msg, tf_buffer_, det_boxes3d, is_num_pillars_within_range); if (!is_success) { return; } + diagnostics_interface_ptr_->add_key_value( + "is_num_pillars_within_range", is_num_pillars_within_range); + if (!is_num_pillars_within_range) { + std::stringstream message; + message << "PointPaintingTRT::detect: The actual number of pillars exceeds its maximum value, " + << "which may limit the detection performance."; + diagnostics_interface_ptr_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } std::vector raw_objects; raw_objects.reserve(det_boxes3d.size()); @@ -410,21 +432,18 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte raw_objects.emplace_back(obj); } - autoware_perception_msgs::msg::DetectedObjects output_msg; - output_msg.header = painted_pointcloud_msg.header; + // prepare output message output_msg.objects = iou_bev_nms_.apply(raw_objects); detection_class_remapper_.mapClasses(output_msg); - if (objects_sub_count > 0) { - obj_pub_ptr_->publish(output_msg); + // publish debug message: painted pointcloud + if (point_pub_ptr_->get_subscription_count() > 0) { + point_pub_ptr_->publish(painted_pointcloud_msg); } + diagnostics_interface_ptr_->publish(painted_pointcloud_msg.header.stamp); } -bool PointPaintingFusionNode::out_of_scope(__attribute__((unused)) const DetectedObjects & obj) -{ - return false; -} } // namespace autoware::image_projection_based_fusion #include diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp index cc3eb17cc32ab..1fe860e68a967 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp @@ -27,8 +27,8 @@ namespace autoware::image_projection_based_fusion { PointPaintingTRT::PointPaintingTRT( - const autoware::lidar_centerpoint::NetworkParam & encoder_param, - const autoware::lidar_centerpoint::NetworkParam & head_param, + const autoware::tensorrt_common::TrtCommonConfig & encoder_param, + const autoware::tensorrt_common::TrtCommonConfig & head_param, const autoware::lidar_centerpoint::DensificationParam & densification_param, const autoware::lidar_centerpoint::CenterPointConfig & config) : autoware::lidar_centerpoint::CenterPointTRT( 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 32db5319bb487..0a9d9e3391882 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -41,8 +41,7 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "roi_cluster_fusion", options) +: FusionNode("roi_cluster_fusion", options) { trust_object_iou_mode_ = declare_parameter("trust_object_iou_mode"); non_trust_object_iou_mode_ = declare_parameter("non_trust_object_iou_mode"); @@ -54,9 +53,12 @@ RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) remove_unknown_ = declare_parameter("remove_unknown"); fusion_distance_ = declare_parameter("fusion_distance"); trust_object_distance_ = declare_parameter("trust_object_distance"); + + // publisher + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); } -void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) +void RoiClusterFusionNode::preprocess(ClusterMsgType & output_cluster_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -71,36 +73,14 @@ void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluste } } -void RoiClusterFusionNode::postprocess(DetectedObjectsWithFeature & output_cluster_msg) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - if (!remove_unknown_) { - return; - } - DetectedObjectsWithFeature known_objects; - known_objects.feature_objects.reserve(output_cluster_msg.feature_objects.size()); - for (auto & feature_object : output_cluster_msg.feature_objects) { - bool is_roi_label_known = feature_object.object.classification.front().label != - autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; - if ( - is_roi_label_known || - feature_object.object.existence_probability >= min_roi_existence_prob_) { - known_objects.feature_objects.push_back(feature_object); - } - } - output_cluster_msg.feature_objects = known_objects.feature_objects; -} - void RoiClusterFusionNode::fuseOnSingleImage( - const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, DetectedObjectsWithFeature & output_cluster_msg) + const ClusterMsgType & input_cluster_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - const sensor_msgs::msg::CameraInfo & camera_info = camera_projectors_[image_id].getCameraInfo(); + const sensor_msgs::msg::CameraInfo & camera_info = det2d.camera_projector_ptr->getCameraInfo(); // get transform from cluster frame id to camera optical frame id geometry_msgs::msg::TransformStamped transform_stamped; @@ -149,7 +129,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( } Eigen::Vector2d projected_point; - if (camera_projectors_[image_id].calcImageProjectedPoint( + if (det2d.camera_projector_ptr->calcImageProjectedPoint( cv::Point3d(*iter_x, *iter_y, *iter_z), projected_point)) { const int px = static_cast(projected_point.x()); const int py = static_cast(projected_point.y()); @@ -168,7 +148,6 @@ void RoiClusterFusionNode::fuseOnSingleImage( } sensor_msgs::msg::RegionOfInterest roi; - // roi.do_rectify = m_camera_info_.at(id).do_rectify; roi.x_offset = min_x; roi.y_offset = min_y; roi.width = max_x - min_x; @@ -231,7 +210,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( // note: debug objects are safely cleared in fusion_node.cpp if (debugger_) { - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); } } @@ -291,6 +270,28 @@ double RoiClusterFusionNode::cal_iou_by_mode( } } +void RoiClusterFusionNode::postprocess( + const ClusterMsgType & processing_msg, ClusterMsgType & output_msg) +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + output_msg = processing_msg; + + if (remove_unknown_) { + // filter by object classification and existence probability + output_msg.feature_objects.clear(); + for (const auto & feature_object : processing_msg.feature_objects) { + if ( + feature_object.object.classification.front().label != + autoware_perception_msgs::msg::ObjectClassification::UNKNOWN || + feature_object.object.existence_probability >= min_roi_existence_prob_) { + output_msg.feature_objects.push_back(feature_object); + } + } + } +} + } // namespace autoware::image_projection_based_fusion #include diff --git a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 7be18d59fdbf1..37769ad73e8c7 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -31,8 +31,7 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "roi_detected_object_fusion", options) +: FusionNode("roi_detected_object_fusion", options) { fusion_params_.passthrough_lower_bound_probability_thresholds = declare_parameter>("passthrough_lower_bound_probability_thresholds"); @@ -48,6 +47,9 @@ RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptio can_assign_vector.data(), label_num, label_num); fusion_params_.can_assign_matrix = can_assign_matrix_tmp.transpose(); } + + // publisher + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); } void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) @@ -82,9 +84,8 @@ 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, - DetectedObjects & output_object_msg __attribute__((unused))) + const DetectedObjects & input_object_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, DetectedObjects & output_object_msg __attribute__((unused))) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -101,7 +102,7 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( } const auto object_roi_map = - generateDetectedObjectRoIs(input_object_msg, image_id, object2camera_affine); + generateDetectedObjectRoIs(input_object_msg, det2d, object2camera_affine); fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map); if (debugger_) { @@ -109,13 +110,13 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( for (std::size_t roi_i = 0; roi_i < input_roi_msg.feature_objects.size(); ++roi_i) { debugger_->image_rois_.push_back(input_roi_msg.feature_objects.at(roi_i).feature.roi); } - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); } } std::map RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( - const DetectedObjects & input_object_msg, const std::size_t & image_id, + const DetectedObjects & input_object_msg, const Det2dStatus & det2d, const Eigen::Affine3d & object2camera_affine) { std::unique_ptr st_ptr; @@ -131,7 +132,7 @@ 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 sensor_msgs::msg::CameraInfo & camera_info = det2d.camera_projector_ptr->getCameraInfo(); const double image_width = static_cast(camera_info.width); const double image_height = static_cast(camera_info.height); @@ -162,7 +163,7 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( } Eigen::Vector2d proj_point; - if (camera_projectors_[image_id].calcImageProjectedPoint( + if (det2d.camera_projector_ptr->calcImageProjectedPoint( cv::Point3d(point.x(), point.y(), point.z()), proj_point)) { const double px = proj_point.x(); const double py = proj_point.y(); @@ -289,14 +290,15 @@ bool RoiDetectedObjectFusionNode::out_of_scope(const DetectedObject & obj) return is_out; } -void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) +void RoiDetectedObjectFusionNode::postprocess( + const DetectedObjects & processing_msg, DetectedObjects & output_msg) { - if (pub_ptr_->get_subscription_count() < 1) { - return; - } + output_msg.header = processing_msg.header; + output_msg.objects.clear(); - int64_t timestamp_nsec = - output_msg.header.stamp.sec * static_cast(1e9) + output_msg.header.stamp.nanosec; + // filter out ignored objects + int64_t timestamp_nsec = processing_msg.header.stamp.sec * static_cast(1e9) + + processing_msg.header.stamp.nanosec; if ( passthrough_object_flags_map_.size() == 0 || fused_object_flags_map_.size() == 0 || ignored_object_flags_map_.size() == 0) { @@ -308,33 +310,34 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) ignored_object_flags_map_.count(timestamp_nsec) == 0) { return; } + auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); auto & fused_object_flags = fused_object_flags_map_.at(timestamp_nsec); - auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); - - DetectedObjects output_objects_msg, debug_fused_objects_msg, debug_ignored_objects_msg; - output_objects_msg.header = output_msg.header; - debug_fused_objects_msg.header = output_msg.header; - debug_ignored_objects_msg.header = output_msg.header; - for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) { - const auto & obj = output_msg.objects.at(obj_i); - if (passthrough_object_flags.at(obj_i)) { - output_objects_msg.objects.emplace_back(obj); + for (std::size_t obj_i = 0; obj_i < processing_msg.objects.size(); ++obj_i) { + const auto & obj = processing_msg.objects.at(obj_i); + if (passthrough_object_flags.at(obj_i) || fused_object_flags.at(obj_i)) { + output_msg.objects.emplace_back(obj); } + } + + // debug messages + auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); + DetectedObjects debug_fused_objects_msg, debug_ignored_objects_msg; + debug_fused_objects_msg.header = processing_msg.header; + debug_ignored_objects_msg.header = processing_msg.header; + for (std::size_t obj_i = 0; obj_i < processing_msg.objects.size(); ++obj_i) { + const auto & obj = processing_msg.objects.at(obj_i); if (fused_object_flags.at(obj_i)) { - output_objects_msg.objects.emplace_back(obj); debug_fused_objects_msg.objects.emplace_back(obj); } if (ignored_object_flags.at(obj_i)) { debug_ignored_objects_msg.objects.emplace_back(obj); } } - - pub_ptr_->publish(output_objects_msg); - debug_publisher_->publish("debug/fused_objects", debug_fused_objects_msg); debug_publisher_->publish("debug/ignored_objects", debug_ignored_objects_msg); + // clear flags passthrough_object_flags_map_.erase(timestamp_nsec); fused_object_flags_map_.erase(timestamp_nsec); ignored_object_flags_map_.erase(timestamp_nsec); diff --git a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index 998072d87774e..b1eef00053946 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -37,58 +37,23 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "roi_pointcloud_fusion", options) +: FusionNode("roi_pointcloud_fusion", options) { fuse_unknown_only_ = declare_parameter("fuse_unknown_only"); min_cluster_size_ = declare_parameter("min_cluster_size"); max_cluster_size_ = declare_parameter("max_cluster_size"); cluster_2d_tolerance_ = declare_parameter("cluster_2d_tolerance"); - pub_objects_ptr_ = - this->create_publisher("output_clusters", rclcpp::QoS{1}); - cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); -} - -void RoiPointCloudFusionNode::preprocess( - __attribute__((unused)) sensor_msgs::msg::PointCloud2 & pointcloud_msg) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - return; + // publisher + point_pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); + pub_ptr_ = this->create_publisher("output_clusters", rclcpp::QoS{1}); + cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); } -void RoiPointCloudFusionNode::postprocess( - __attribute__((unused)) sensor_msgs::msg::PointCloud2 & pointcloud_msg) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - const auto objects_sub_count = pub_objects_ptr_->get_subscription_count() + - pub_objects_ptr_->get_intra_process_subscription_count(); - if (objects_sub_count < 1) { - return; - } - - DetectedObjectsWithFeature output_msg; - output_msg.header = pointcloud_msg.header; - output_msg.feature_objects = output_fused_objects_; - - if (objects_sub_count > 0) { - pub_objects_ptr_->publish(output_msg); - } - output_fused_objects_.clear(); - // publish debug cluster - if (cluster_debug_pub_->get_subscription_count() > 0) { - sensor_msgs::msg::PointCloud2 debug_cluster_msg; - autoware::euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); - cluster_debug_pub_->publish(debug_cluster_msg); - } -} void RoiPointCloudFusionNode::fuseOnSingleImage( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - __attribute__((unused)) sensor_msgs::msg::PointCloud2 & output_pointcloud_msg) + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, + __attribute__((unused)) PointCloudMsgType & output_pointcloud_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -137,10 +102,10 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( const int z_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "z")].offset; - sensor_msgs::msg::PointCloud2 transformed_cloud; + PointCloudMsgType transformed_cloud; tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); - std::vector clusters; + std::vector clusters; std::vector clusters_data_size; clusters.resize(output_objs.size()); for (auto & cluster : clusters) { @@ -162,7 +127,7 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( } Eigen::Vector2d projected_point; - if (camera_projectors_[image_id].calcImageProjectedPoint( + if (det2d.camera_projector_ptr->calcImageProjectedPoint( cv::Point3d(transformed_x, transformed_y, transformed_z), projected_point)) { for (std::size_t i = 0; i < output_objs.size(); ++i) { auto & feature_obj = output_objs.at(i); @@ -202,14 +167,40 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( if (debugger_) { debugger_->image_rois_ = debug_image_rois; debugger_->obstacle_points_ = debug_image_points; - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); } } -bool RoiPointCloudFusionNode::out_of_scope(__attribute__((unused)) - const DetectedObjectWithFeature & obj) +void RoiPointCloudFusionNode::postprocess( + const PointCloudMsgType & pointcloud_msg, ClusterMsgType & output_msg) { - return false; + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + output_msg.header = pointcloud_msg.header; + output_msg.feature_objects = output_fused_objects_; + + output_fused_objects_.clear(); + + // publish debug cluster + if (cluster_debug_pub_->get_subscription_count() > 0) { + PointCloudMsgType debug_cluster_msg; + autoware::euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); + cluster_debug_pub_->publish(debug_cluster_msg); + } + if (point_pub_ptr_->get_subscription_count() > 0) { + point_pub_ptr_->publish(pointcloud_msg); + } +} + +void RoiPointCloudFusionNode::publish(const ClusterMsgType & output_msg) +{ + const auto objects_sub_count = + pub_ptr_->get_subscription_count() + pub_ptr_->get_intra_process_subscription_count(); + if (objects_sub_count < 1) { + return; + } + pub_ptr_->publish(output_msg); } } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index 486ae291abc6a..7d63cac7273c6 100644 --- a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -36,7 +36,7 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("segmentation_pointcloud_fusion", options) +: FusionNode("segmentation_pointcloud_fusion", options) { filter_distance_threshold_ = declare_parameter("filter_distance_threshold"); for (auto & item : filter_semantic_label_target_list_) { @@ -48,47 +48,24 @@ SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptio } is_publish_debug_mask_ = declare_parameter("is_publish_debug_mask"); pub_debug_mask_ptr_ = image_transport::create_publisher(this, "~/debug/mask"); -} - -void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - return; + // publisher + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); } -void SegmentPointCloudFusionNode::postprocess(PointCloud2 & pointcloud_msg) +void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) + PointCloudMsgType & pointcloud_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - auto original_cloud = std::make_shared(pointcloud_msg); - - int point_step = original_cloud->point_step; - size_t output_pointcloud_size = 0; - pointcloud_msg.data.clear(); - pointcloud_msg.data.resize(original_cloud->data.size()); - - for (size_t global_offset = 0; global_offset < original_cloud->data.size(); - global_offset += point_step) { - if (filter_global_offset_set_.find(global_offset) == filter_global_offset_set_.end()) { - copyPointCloud( - *original_cloud, point_step, global_offset, pointcloud_msg, output_pointcloud_size); - } - } - - pointcloud_msg.data.resize(output_pointcloud_size); - pointcloud_msg.row_step = output_pointcloud_size / pointcloud_msg.height; - pointcloud_msg.width = output_pointcloud_size / pointcloud_msg.point_step / pointcloud_msg.height; - - filter_global_offset_set_.clear(); return; } void SegmentPointCloudFusionNode::fuseOnSingleImage( - const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, - [[maybe_unused]] const Image & input_mask, __attribute__((unused)) PointCloud2 & output_cloud) + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + [[maybe_unused]] const Image & input_mask, + __attribute__((unused)) PointCloudMsgType & output_cloud) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -100,7 +77,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( return; } - const sensor_msgs::msg::CameraInfo & camera_info = camera_projectors_[image_id].getCameraInfo(); + const sensor_msgs::msg::CameraInfo & camera_info = det2d.camera_projector_ptr->getCameraInfo(); std::vector mask_data(input_mask.data.begin(), input_mask.data.end()); cv::Mat mask = perception_utils::runLengthDecoder(mask_data, input_mask.height, input_mask.width); @@ -128,7 +105,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( transform_stamped = transform_stamped_optional.value(); } - PointCloud2 transformed_cloud; + PointCloudMsgType transformed_cloud; tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); int point_step = input_pointcloud_msg.point_step; @@ -150,7 +127,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( } Eigen::Vector2d projected_point; - if (!camera_projectors_[image_id].calcImageProjectedPoint( + if (!det2d.camera_projector_ptr->calcImageProjectedPoint( cv::Point3d(transformed_x, transformed_y, transformed_z), projected_point)) { continue; } @@ -168,11 +145,33 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( } } -bool SegmentPointCloudFusionNode::out_of_scope(__attribute__((unused)) - const PointCloud2 & filtered_cloud) +void SegmentPointCloudFusionNode::postprocess( + const PointCloudMsgType & pointcloud_msg, PointCloudMsgType & output_msg) { - return false; + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + output_msg.header = pointcloud_msg.header; + output_msg.data.clear(); + output_msg.data.resize(pointcloud_msg.data.size()); + const int point_step = pointcloud_msg.point_step; + + size_t output_pointcloud_size = 0; + for (size_t global_offset = 0; global_offset < pointcloud_msg.data.size(); + global_offset += point_step) { + if (filter_global_offset_set_.find(global_offset) == filter_global_offset_set_.end()) { + copyPointCloud(pointcloud_msg, point_step, global_offset, output_msg, output_pointcloud_size); + } + } + + output_msg.data.resize(output_pointcloud_size); + output_msg.row_step = output_pointcloud_size / output_msg.height; + output_msg.width = output_pointcloud_size / output_msg.point_step / output_msg.height; + + filter_global_offset_set_.clear(); + return; } + } // namespace autoware::image_projection_based_fusion #include diff --git a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp index 4456d25b18167..81424d4c23a34 100644 --- a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp @@ -67,8 +67,8 @@ Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t) } void closest_cluster( - const PointCloud2 & cluster, const double cluster_2d_tolerance, const int min_cluster_size, - const pcl::PointXYZ & center, PointCloud2 & out_cluster) + const PointCloudMsgType & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center, PointCloudMsgType & out_cluster) { // sort point by distance to camera origin @@ -123,8 +123,8 @@ void closest_cluster( } void updateOutputFusedObjects( - std::vector & output_objs, std::vector & clusters, - const std::vector & clusters_data_size, const PointCloud2 & in_cloud, + std::vector & output_objs, std::vector & clusters, + const std::vector & clusters_data_size, const PointCloudMsgType & in_cloud, const std_msgs::msg::Header & in_roi_header, const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const int max_cluster_size, const float cluster_2d_tolerance, std::vector & output_fused_objects) @@ -162,7 +162,7 @@ void updateOutputFusedObjects( // TODO(badai-nguyen): change to interface to select refine criteria like closest, largest // to output refine cluster and centroid - sensor_msgs::msg::PointCloud2 refine_cluster; + PointCloudMsgType refine_cluster; closest_cluster( cluster, cluster_2d_tolerance, min_cluster_size, camera_orig_point_frame, refine_cluster); if ( @@ -184,7 +184,7 @@ void updateOutputFusedObjects( } } -geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) +geometry_msgs::msg::Point getCentroid(const PointCloudMsgType & pointcloud) { geometry_msgs::msg::Point centroid; centroid.x = 0.0f; diff --git a/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt b/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt index 16ab7ecccae60..0555cedc235d9 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt +++ b/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt @@ -9,7 +9,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(tf2_eigen REQUIRED) find_package(autoware_universe_utils REQUIRED) -find_package(tier4_debug_msgs REQUIRED) +find_package(autoware_internal_debug_msgs REQUIRED) find_package(tier4_perception_msgs REQUIRED) find_package(autoware_tensorrt_common) @@ -50,7 +50,7 @@ target_link_libraries(${PROJECT_NAME} rclcpp_components::component tf2_eigen::tf2_eigen ${autoware_tensorrt_common_TARGETS} - ${tier4_debug_msgs_TARGETS} + ${autoware_internal_debug_msgs_TARGETS} ${tier4_perception_msgs_TARGETS} ) diff --git a/perception/autoware_lidar_apollo_instance_segmentation/package.xml b/perception/autoware_lidar_apollo_instance_segmentation/package.xml index 8a3d5a7ea6f76..8ff7ce77d943d 100755 --- a/perception/autoware_lidar_apollo_instance_segmentation/package.xml +++ b/perception/autoware_lidar_apollo_instance_segmentation/package.xml @@ -14,6 +14,7 @@ ament_cmake autoware_cuda_utils + autoware_internal_debug_msgs autoware_perception_msgs autoware_tensorrt_common autoware_universe_utils @@ -22,7 +23,6 @@ rclcpp rclcpp_components tf2_eigen - tier4_debug_msgs tier4_perception_msgs ament_lint_auto diff --git a/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp b/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp index 60aa09c8e16d7..2e3a7a2243399 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp @@ -18,7 +18,6 @@ #include -#include #include #include @@ -51,12 +50,9 @@ LidarApolloInstanceSegmentation::LidarApolloInstanceSegmentation(rclcpp::Node * const auto precision = node_->declare_parameter("precision", "fp32"); trt_common_ = std::make_unique( - onnx_file, precision, nullptr, autoware::tensorrt_common::BatchConfig{1, 1, 1}, 1 << 30); - trt_common_->setup(); - - if (!trt_common_->isInitialized()) { - RCLCPP_ERROR_STREAM(node_->get_logger(), "failed to create tensorrt engine file."); - return; + tensorrt_common::TrtCommonConfig(onnx_file, precision)); + if (!trt_common_->setup()) { + throw std::runtime_error("Failed to setup TensorRT"); } if (node_->declare_parameter("build_only", false)) { @@ -65,11 +61,11 @@ LidarApolloInstanceSegmentation::LidarApolloInstanceSegmentation(rclcpp::Node * } // GPU memory allocation - const auto input_dims = trt_common_->getBindingDimensions(0); + const auto input_dims = trt_common_->getTensorShape(0); const auto input_size = std::accumulate(input_dims.d + 1, input_dims.d + input_dims.nbDims, 1, std::multiplies()); input_d_ = autoware::cuda_utils::make_unique(input_size); - const auto output_dims = trt_common_->getBindingDimensions(1); + const auto output_dims = trt_common_->getTensorShape(1); output_size_ = std::accumulate( output_dims.d + 1, output_dims.d + output_dims.nbDims, 1, std::multiplies()); output_d_ = autoware::cuda_utils::make_unique(output_size_); @@ -127,10 +123,6 @@ bool LidarApolloInstanceSegmentation::detectDynamicObjects( const sensor_msgs::msg::PointCloud2 & input, tier4_perception_msgs::msg::DetectedObjectsWithFeature & output) { - if (!trt_common_->isInitialized()) { - return false; - } - // move up pointcloud z_offset in z axis sensor_msgs::msg::PointCloud2 transformed_cloud; transformCloud(input, transformed_cloud, z_offset_); @@ -169,7 +161,10 @@ bool LidarApolloInstanceSegmentation::detectDynamicObjects( std::vector buffers = {input_d_.get(), output_d_.get()}; - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); CHECK_CUDA_ERROR(cudaMemcpyAsync( output_h_.get(), output_d_.get(), sizeof(float) * output_size_, cudaMemcpyDeviceToHost, diff --git a/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp b/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp index 9a3e13b81120f..c535fc6762e1f 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp @@ -62,9 +62,9 @@ void LidarInstanceSegmentationNode::pointCloudCallback( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_lidar_centerpoint/CMakeLists.txt b/perception/autoware_lidar_centerpoint/CMakeLists.txt index 88244b3153349..322907c67c6ae 100644 --- a/perception/autoware_lidar_centerpoint/CMakeLists.txt +++ b/perception/autoware_lidar_centerpoint/CMakeLists.txt @@ -84,8 +84,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) lib/detection_class_remapper.cpp lib/utils.cpp lib/ros_utils.cpp - lib/network/network_trt.cpp - lib/network/tensorrt_wrapper.cpp lib/postprocess/non_maximum_suppression.cpp lib/preprocess/pointcloud_densification.cpp lib/preprocess/voxel_generator.cpp diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp index ce5ec3ea0abfe..4b531e34877d6 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp @@ -16,12 +16,13 @@ #define AUTOWARE__LIDAR_CENTERPOINT__CENTERPOINT_TRT_HPP_ #include "autoware/lidar_centerpoint/cuda_utils.hpp" -#include "autoware/lidar_centerpoint/network/network_trt.hpp" #include "autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp" #include "autoware/lidar_centerpoint/preprocess/voxel_generator.hpp" #include "pcl/point_cloud.h" #include "pcl/point_types.h" +#include + #include "sensor_msgs/msg/point_cloud2.hpp" #include @@ -31,41 +32,25 @@ namespace autoware::lidar_centerpoint { -class NetworkParam -{ -public: - NetworkParam(std::string onnx_path, std::string engine_path, std::string trt_precision) - : onnx_path_(std::move(onnx_path)), - engine_path_(std::move(engine_path)), - trt_precision_(std::move(trt_precision)) - { - } - - std::string onnx_path() const { return onnx_path_; } - std::string engine_path() const { return engine_path_; } - std::string trt_precision() const { return trt_precision_; } - -private: - std::string onnx_path_; - std::string engine_path_; - std::string trt_precision_; -}; +using autoware::tensorrt_common::ProfileDims; +using autoware::tensorrt_common::TrtCommonConfig; class CenterPointTRT { public: explicit CenterPointTRT( - const NetworkParam & encoder_param, const NetworkParam & head_param, + const TrtCommonConfig & encoder_param, const TrtCommonConfig & head_param, const DensificationParam & densification_param, const CenterPointConfig & config); virtual ~CenterPointTRT(); bool detect( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d); + std::vector & det_boxes3d, bool & is_num_pillars_within_range); protected: void initPtr(); + void initTrt(const TrtCommonConfig & encoder_param, const TrtCommonConfig & head_param); virtual bool preprocess( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer); @@ -75,8 +60,8 @@ class CenterPointTRT void postProcess(std::vector & det_boxes3d); std::unique_ptr vg_ptr_{nullptr}; - std::unique_ptr encoder_trt_ptr_{nullptr}; - std::unique_ptr head_trt_ptr_{nullptr}; + std::unique_ptr encoder_trt_ptr_{nullptr}; + std::unique_ptr head_trt_ptr_{nullptr}; std::unique_ptr post_proc_ptr_{nullptr}; cudaStream_t stream_{nullptr}; diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/network_trt.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/network_trt.hpp deleted file mode 100644 index 17778d77ed798..0000000000000 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/network_trt.hpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2021 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_ -#define AUTOWARE__LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_ - -#include "autoware/lidar_centerpoint/centerpoint_config.hpp" -#include "autoware/lidar_centerpoint/network/tensorrt_wrapper.hpp" - -#include - -namespace autoware::lidar_centerpoint -{ -class VoxelEncoderTRT : public TensorRTWrapper -{ -public: - using TensorRTWrapper::TensorRTWrapper; - -protected: - bool setProfile( - nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, - nvinfer1::IBuilderConfig & config) override; -}; - -class HeadTRT : public TensorRTWrapper -{ -public: - using TensorRTWrapper::TensorRTWrapper; - - HeadTRT(const std::vector & out_channel_sizes, const CenterPointConfig & config); - -protected: - bool setProfile( - nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, - nvinfer1::IBuilderConfig & config) override; - - std::vector out_channel_sizes_; -}; - -} // namespace autoware::lidar_centerpoint - -#endif // AUTOWARE__LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_ diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/tensorrt_wrapper.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/tensorrt_wrapper.hpp deleted file mode 100644 index d56ccc699add3..0000000000000 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/tensorrt_wrapper.hpp +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright 2021 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ -#define AUTOWARE__LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ - -#include "NvInfer.h" -#include "autoware/lidar_centerpoint/centerpoint_config.hpp" -#include "autoware/tensorrt_common/tensorrt_common.hpp" - -#include -#include -#include - -namespace autoware::lidar_centerpoint -{ - -class TensorRTWrapper -{ -public: - explicit TensorRTWrapper(const CenterPointConfig & config); - - virtual ~TensorRTWrapper(); - - bool init( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision); - - autoware::tensorrt_common::TrtUniquePtr context_{nullptr}; - -protected: - virtual bool setProfile( - nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, - nvinfer1::IBuilderConfig & config) = 0; - - CenterPointConfig config_; - autoware::tensorrt_common::Logger logger_; - -private: - bool parseONNX( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision, - std::size_t workspace_size = (1ULL << 30)); - - bool saveEngine(const std::string & engine_path); - - bool loadEngine(const std::string & engine_path); - - bool createContext(); - - autoware::tensorrt_common::TrtUniquePtr runtime_{nullptr}; - autoware::tensorrt_common::TrtUniquePtr plan_{nullptr}; - autoware::tensorrt_common::TrtUniquePtr engine_{nullptr}; -}; - -} // namespace autoware::lidar_centerpoint - -#endif // AUTOWARE__LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp index a49451fde9d0d..1748db0e48392 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp @@ -20,6 +20,7 @@ #include "autoware/lidar_centerpoint/postprocess/non_maximum_suppression.hpp" #include +#include #include #include #include @@ -59,6 +60,7 @@ class LidarCenterPointNode : public rclcpp::Node DetectionClassRemapper detection_class_remapper_; std::unique_ptr detector_ptr_{nullptr}; + std::unique_ptr diagnostics_interface_ptr_; // debugger std::unique_ptr> stop_watch_ptr_{ diff --git a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp index 6ee2b3733bdb0..c586eeb961716 100644 --- a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp @@ -19,6 +19,7 @@ #include "autoware/lidar_centerpoint/preprocess/preprocess_kernel.hpp" #include +#include #include #include @@ -27,39 +28,22 @@ #include #include #include +#include +#include #include namespace autoware::lidar_centerpoint { CenterPointTRT::CenterPointTRT( - const NetworkParam & encoder_param, const NetworkParam & head_param, + const TrtCommonConfig & encoder_param, const TrtCommonConfig & head_param, const DensificationParam & densification_param, const CenterPointConfig & config) : config_(config) { vg_ptr_ = std::make_unique(densification_param, config_); post_proc_ptr_ = std::make_unique(config_); - // encoder - encoder_trt_ptr_ = std::make_unique(config_); - encoder_trt_ptr_->init( - encoder_param.onnx_path(), encoder_param.engine_path(), encoder_param.trt_precision()); - encoder_trt_ptr_->context_->setBindingDimensions( - 0, - nvinfer1::Dims3( - config_.max_voxel_size_, config_.max_point_in_voxel_size_, config_.encoder_in_feature_size_)); - - // head - std::vector out_channel_sizes = { - config_.class_size_, config_.head_out_offset_size_, config_.head_out_z_size_, - config_.head_out_dim_size_, config_.head_out_rot_size_, config_.head_out_vel_size_}; - head_trt_ptr_ = std::make_unique(out_channel_sizes, config_); - head_trt_ptr_->init(head_param.onnx_path(), head_param.engine_path(), head_param.trt_precision()); - head_trt_ptr_->context_->setBindingDimensions( - 0, nvinfer1::Dims4( - config_.batch_size_, config_.encoder_out_feature_size_, config_.grid_size_y_, - config_.grid_size_x_)); - initPtr(); + initTrt(encoder_param, head_param); cudaStreamCreate(&stream_); } @@ -125,10 +109,72 @@ void CenterPointTRT::initPtr() cudaMemcpyHostToDevice, stream_)); } +void CenterPointTRT::initTrt( + const TrtCommonConfig & encoder_param, const TrtCommonConfig & head_param) +{ + // encoder input profile + auto enc_in_dims = nvinfer1::Dims{ + 3, + {static_cast(config_.max_voxel_size_), + static_cast(config_.max_point_in_voxel_size_), + static_cast(config_.encoder_in_feature_size_)}}; + std::vector encoder_profile_dims{ + tensorrt_common::ProfileDims(0, enc_in_dims, enc_in_dims, enc_in_dims)}; + auto encoder_profile_dims_ptr = + std::make_unique>(encoder_profile_dims); + + // head input profile + auto head_in_dims = nvinfer1::Dims{ + 4, + {static_cast(config_.batch_size_), + static_cast(config_.encoder_out_feature_size_), + static_cast(config_.grid_size_y_), static_cast(config_.grid_size_x_)}}; + std::vector head_profile_dims{ + tensorrt_common::ProfileDims(0, head_in_dims, head_in_dims, head_in_dims)}; + std::unordered_map out_channel_map = { + {1, static_cast(config_.class_size_)}, + {2, static_cast(config_.head_out_offset_size_)}, + {3, static_cast(config_.head_out_z_size_)}, + {4, static_cast(config_.head_out_dim_size_)}, + {5, static_cast(config_.head_out_rot_size_)}, + {6, static_cast(config_.head_out_vel_size_)}}; + for (const auto & [tensor_name, channel_size] : out_channel_map) { + auto dims = nvinfer1::Dims{ + 4, + {static_cast(config_.batch_size_), channel_size, + static_cast(config_.down_grid_size_y_), + static_cast(config_.down_grid_size_x_)}}; + head_profile_dims.emplace_back(tensor_name, dims, dims, dims); + } + auto head_profile_dims_ptr = + std::make_unique>(head_profile_dims); + + // initialize trt wrappers + encoder_trt_ptr_ = std::make_unique(encoder_param); + + head_trt_ptr_ = std::make_unique(head_param); + + // setup trt engines + if ( + !encoder_trt_ptr_->setup(std::move(encoder_profile_dims_ptr)) || + !head_trt_ptr_->setup(std::move(head_profile_dims_ptr))) { + throw std::runtime_error("Failed to setup TRT engine."); + } + + // set input shapes + if ( + !encoder_trt_ptr_->setInputShape(0, enc_in_dims) || + !head_trt_ptr_->setInputShape(0, head_in_dims)) { + throw std::runtime_error("Failed to set input shape."); + } +} + bool CenterPointTRT::detect( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d) + std::vector & det_boxes3d, bool & is_num_pillars_within_range) { + is_num_pillars_within_range = true; + CHECK_CUDA_ERROR(cudaMemsetAsync( encoder_in_features_d_.get(), 0, encoder_in_feature_size_ * sizeof(float), stream_)); CHECK_CUDA_ERROR( @@ -155,6 +201,7 @@ bool CenterPointTRT::detect( "The actual number of pillars (%u) exceeds its maximum value (%zu). " "Please considering increasing it since it may limit the detection performance.", num_pillars, config_.max_voxel_size_); + is_num_pillars_within_range = false; } return true; @@ -206,13 +253,10 @@ bool CenterPointTRT::preprocess( void CenterPointTRT::inference() { - if (!encoder_trt_ptr_->context_ || !head_trt_ptr_->context_) { - throw std::runtime_error("Failed to create tensorrt context."); - } - // pillar encoder network - std::vector encoder_buffers{encoder_in_features_d_.get(), pillar_features_d_.get()}; - encoder_trt_ptr_->context_->enqueueV2(encoder_buffers.data(), stream_, nullptr); + std::vector encoder_tensors = {encoder_in_features_d_.get(), pillar_features_d_.get()}; + encoder_trt_ptr_->setTensorsAddresses(encoder_tensors); + encoder_trt_ptr_->enqueueV3(stream_); // scatter CHECK_CUDA_ERROR(scatterFeatures_launch( @@ -221,11 +265,13 @@ void CenterPointTRT::inference() spatial_features_d_.get(), stream_)); // head network - std::vector head_buffers = {spatial_features_d_.get(), head_out_heatmap_d_.get(), + std::vector head_tensors = {spatial_features_d_.get(), head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), head_out_rot_d_.get(), head_out_vel_d_.get()}; - head_trt_ptr_->context_->enqueueV2(head_buffers.data(), stream_, nullptr); + head_trt_ptr_->setTensorsAddresses(head_tensors); + + head_trt_ptr_->enqueueV3(stream_); } void CenterPointTRT::postProcess(std::vector & det_boxes3d) diff --git a/perception/autoware_lidar_centerpoint/lib/network/network_trt.cpp b/perception/autoware_lidar_centerpoint/lib/network/network_trt.cpp deleted file mode 100644 index 7767c0b5cbb02..0000000000000 --- a/perception/autoware_lidar_centerpoint/lib/network/network_trt.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright 2021 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/lidar_centerpoint/network/network_trt.hpp" - -#include -#include - -namespace autoware::lidar_centerpoint -{ -bool VoxelEncoderTRT::setProfile( - nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, - nvinfer1::IBuilderConfig & config) -{ - auto profile = builder.createOptimizationProfile(); - auto in_name = network.getInput(0)->getName(); - auto in_dims = nvinfer1::Dims3( - config_.max_voxel_size_, config_.max_point_in_voxel_size_, config_.encoder_in_feature_size_); - profile->setDimensions(in_name, nvinfer1::OptProfileSelector::kMIN, in_dims); - profile->setDimensions(in_name, nvinfer1::OptProfileSelector::kOPT, in_dims); - profile->setDimensions(in_name, nvinfer1::OptProfileSelector::kMAX, in_dims); - - auto out_name = network.getOutput(0)->getName(); - auto out_dims = nvinfer1::Dims2(config_.max_voxel_size_, config_.encoder_out_feature_size_); - profile->setDimensions(out_name, nvinfer1::OptProfileSelector::kMIN, out_dims); - profile->setDimensions(out_name, nvinfer1::OptProfileSelector::kOPT, out_dims); - profile->setDimensions(out_name, nvinfer1::OptProfileSelector::kMAX, out_dims); - config.addOptimizationProfile(profile); - - return true; -} - -HeadTRT::HeadTRT( - const std::vector & out_channel_sizes, const CenterPointConfig & config) -: TensorRTWrapper(config), out_channel_sizes_(out_channel_sizes) -{ -} - -bool HeadTRT::setProfile( - nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, - nvinfer1::IBuilderConfig & config) -{ - auto profile = builder.createOptimizationProfile(); - auto in_name = network.getInput(0)->getName(); - auto in_dims = nvinfer1::Dims4( - config_.batch_size_, config_.encoder_out_feature_size_, config_.grid_size_y_, - config_.grid_size_x_); - profile->setDimensions(in_name, nvinfer1::OptProfileSelector::kMIN, in_dims); - profile->setDimensions(in_name, nvinfer1::OptProfileSelector::kOPT, in_dims); - profile->setDimensions(in_name, nvinfer1::OptProfileSelector::kMAX, in_dims); - - for (std::size_t ci = 0; ci < out_channel_sizes_.size(); ci++) { - auto out_name = network.getOutput(ci)->getName(); - - if ( - out_name == std::string("heatmap") && - network.getOutput(ci)->getDimensions().d[1] != static_cast(out_channel_sizes_[ci])) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Expected and actual number of classes do not match" << std::endl; - return false; - } - auto out_dims = nvinfer1::Dims4( - config_.batch_size_, out_channel_sizes_[ci], config_.down_grid_size_y_, - config_.down_grid_size_x_); - profile->setDimensions(out_name, nvinfer1::OptProfileSelector::kMIN, out_dims); - profile->setDimensions(out_name, nvinfer1::OptProfileSelector::kOPT, out_dims); - profile->setDimensions(out_name, nvinfer1::OptProfileSelector::kMAX, out_dims); - } - config.addOptimizationProfile(profile); - - return true; -} - -} // namespace autoware::lidar_centerpoint diff --git a/perception/autoware_lidar_centerpoint/lib/network/tensorrt_wrapper.cpp b/perception/autoware_lidar_centerpoint/lib/network/tensorrt_wrapper.cpp deleted file mode 100644 index 6d4c2e053b34f..0000000000000 --- a/perception/autoware_lidar_centerpoint/lib/network/tensorrt_wrapper.cpp +++ /dev/null @@ -1,169 +0,0 @@ -// Copyright 2021 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/lidar_centerpoint/network/tensorrt_wrapper.hpp" - -#include "NvOnnxParser.h" - -#include -#include -#include - -namespace autoware::lidar_centerpoint -{ -TensorRTWrapper::TensorRTWrapper(const CenterPointConfig & config) : config_(config) -{ -} - -TensorRTWrapper::~TensorRTWrapper() -{ - context_.reset(); - runtime_.reset(); - plan_.reset(); - engine_.reset(); -} - -bool TensorRTWrapper::init( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision) -{ - runtime_ = autoware::tensorrt_common::TrtUniquePtr( - nvinfer1::createInferRuntime(logger_)); - if (!runtime_) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create runtime" << std::endl; - return false; - } - - bool success; - std::ifstream engine_file(engine_path); - if (engine_file.is_open()) { - success = loadEngine(engine_path); - } else { - auto log_thread = logger_.log_throttle( - nvinfer1::ILogger::Severity::kINFO, - "Applying optimizations and building TRT CUDA engine. Please wait a minutes...", 5); - success = parseONNX(onnx_path, engine_path, precision); - logger_.stop_throttle(log_thread); - } - success &= createContext(); - - return success; -} - -bool TensorRTWrapper::createContext() -{ - if (!engine_) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Failed to create context: Engine was not created" << std::endl; - return false; - } - - context_ = autoware::tensorrt_common::TrtUniquePtr( - engine_->createExecutionContext()); - if (!context_) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create context" << std::endl; - return false; - } - - return true; -} - -bool TensorRTWrapper::parseONNX( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision, - const std::size_t workspace_size) -{ - auto builder = autoware::tensorrt_common::TrtUniquePtr( - nvinfer1::createInferBuilder(logger_)); - if (!builder) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create builder" << std::endl; - return false; - } - - auto config = autoware::tensorrt_common::TrtUniquePtr( - builder->createBuilderConfig()); - if (!config) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create config" << std::endl; - return false; - } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 - config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, workspace_size); -#else - config->setMaxWorkspaceSize(workspace_size); -#endif - if (precision == "fp16") { - if (builder->platformHasFastFp16()) { - autoware::tensorrt_common::LOG_INFO(logger_) << "Using TensorRT FP16 Inference" << std::endl; - config->setFlag(nvinfer1::BuilderFlag::kFP16); - } else { - autoware::tensorrt_common::LOG_INFO(logger_) - << "TensorRT FP16 Inference isn't supported in this environment" << std::endl; - } - } - - const auto flag = - 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); - auto network = autoware::tensorrt_common::TrtUniquePtr( - builder->createNetworkV2(flag)); - if (!network) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create network" << std::endl; - return false; - } - - auto parser = autoware::tensorrt_common::TrtUniquePtr( - nvonnxparser::createParser(*network, logger_)); - parser->parseFromFile(onnx_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); - - if (!setProfile(*builder, *network, *config)) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to set profile" << std::endl; - return false; - } - - plan_ = autoware::tensorrt_common::TrtUniquePtr( - builder->buildSerializedNetwork(*network, *config)); - if (!plan_) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Failed to create serialized network" << std::endl; - return false; - } - engine_ = autoware::tensorrt_common::TrtUniquePtr( - runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); - if (!engine_) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create engine" << std::endl; - return false; - } - - return saveEngine(engine_path); -} - -bool TensorRTWrapper::saveEngine(const std::string & engine_path) -{ - autoware::tensorrt_common::LOG_INFO(logger_) << "Writing to " << engine_path << std::endl; - std::ofstream file(engine_path, std::ios::out | std::ios::binary); - file.write(reinterpret_cast(plan_->data()), plan_->size()); - return true; -} - -bool TensorRTWrapper::loadEngine(const std::string & engine_path) -{ - std::ifstream engine_file(engine_path); - std::stringstream engine_buffer; - engine_buffer << engine_file.rdbuf(); - std::string engine_str = engine_buffer.str(); - engine_ = - autoware::tensorrt_common::TrtUniquePtr(runtime_->deserializeCudaEngine( - reinterpret_cast(engine_str.data()), engine_str.size())); - autoware::tensorrt_common::LOG_INFO(logger_) << "Loaded engine from " << engine_path << std::endl; - return true; -} - -} // namespace autoware::lidar_centerpoint diff --git a/perception/autoware_lidar_centerpoint/lib/preprocess/preprocess_kernel.cu b/perception/autoware_lidar_centerpoint/lib/preprocess/preprocess_kernel.cu index f300411a44aad..95ac7b4353e90 100644 --- a/perception/autoware_lidar_centerpoint/lib/preprocess/preprocess_kernel.cu +++ b/perception/autoware_lidar_centerpoint/lib/preprocess/preprocess_kernel.cu @@ -148,6 +148,8 @@ __global__ void generateVoxels_random_kernel( int voxel_idx = floorf((point.x - min_x_range) / pillar_x_size); int voxel_idy = floorf((point.y - min_y_range) / pillar_y_size); + voxel_idx = voxel_idx < 0 ? 0 : voxel_idx >= grid_x_size ? grid_x_size - 1 : voxel_idx; + voxel_idy = voxel_idy < 0 ? 0 : voxel_idy >= grid_y_size ? grid_y_size - 1 : voxel_idy; unsigned int voxel_index = (grid_x_size - 1 - voxel_idx) * grid_y_size + voxel_idy; unsigned int point_id = atomicAdd(&(mask[voxel_index]), 1); diff --git a/perception/autoware_lidar_centerpoint/src/node.cpp b/perception/autoware_lidar_centerpoint/src/node.cpp index 59acceeac54d6..19d1475db9051 100644 --- a/perception/autoware_lidar_centerpoint/src/node.cpp +++ b/perception/autoware_lidar_centerpoint/src/node.cpp @@ -86,8 +86,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti iou_bev_nms_.setParameters(p); } - NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision); - NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision); + TrtCommonConfig encoder_param(encoder_onnx_path, trt_precision, encoder_engine_path); + TrtCommonConfig head_param(head_onnx_path, trt_precision, head_engine_path); DensificationParam densification_param( densification_world_frame_id, densification_num_past_frames); @@ -107,6 +107,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti circle_nms_dist_threshold, yaw_norm_thresholds, has_variance_); detector_ptr_ = std::make_unique(encoder_param, head_param, densification_param, config); + diagnostics_interface_ptr_ = + std::make_unique(this, "centerpoint_trt"); pointcloud_sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1), @@ -144,12 +146,24 @@ void LidarCenterPointNode::pointCloudCallback( if (stop_watch_ptr_) { stop_watch_ptr_->toc("processing_time", true); } + diagnostics_interface_ptr_->clear(); std::vector det_boxes3d; - bool is_success = detector_ptr_->detect(*input_pointcloud_msg, tf_buffer_, det_boxes3d); + bool is_num_pillars_within_range = true; + bool is_success = detector_ptr_->detect( + *input_pointcloud_msg, tf_buffer_, det_boxes3d, is_num_pillars_within_range); if (!is_success) { return; } + diagnostics_interface_ptr_->add_key_value( + "is_num_pillars_within_range", is_num_pillars_within_range); + if (!is_num_pillars_within_range) { + std::stringstream message; + message << "CenterPointTRT::detect: The actual number of pillars exceeds its maximum value, " + << "which may limit the detection performance."; + diagnostics_interface_ptr_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } std::vector raw_objects; raw_objects.reserve(det_boxes3d.size()); @@ -169,6 +183,7 @@ void LidarCenterPointNode::pointCloudCallback( objects_pub_->publish(output_msg); published_time_publisher_->publish_if_subscribed(objects_pub_, output_msg.header.stamp); } + diagnostics_interface_ptr_->publish(input_pointcloud_msg->header.stamp); // add processing time for debug if (debug_publisher_ptr_ && stop_watch_ptr_) { diff --git a/perception/autoware_lidar_transfusion/CMakeLists.txt b/perception/autoware_lidar_transfusion/CMakeLists.txt index c3a56f883fbe5..fdb978c64946f 100644 --- a/perception/autoware_lidar_transfusion/CMakeLists.txt +++ b/perception/autoware_lidar_transfusion/CMakeLists.txt @@ -84,7 +84,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ament_auto_add_library(${PROJECT_NAME}_lib SHARED lib/detection_class_remapper.cpp - lib/network/network_trt.cpp lib/postprocess/non_maximum_suppression.cpp lib/preprocess/voxel_generator.cpp lib/preprocess/pointcloud_densification.cpp diff --git a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/network/network_trt.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/network/network_trt.hpp deleted file mode 100644 index 71cd08acdb8c8..0000000000000 --- a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/network/network_trt.hpp +++ /dev/null @@ -1,87 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ -#define AUTOWARE__LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ - -#include "autoware/lidar_transfusion/transfusion_config.hpp" -#include "autoware/lidar_transfusion/utils.hpp" - -#include - -#include - -#include -#include -#include -#include -#include -#include - -namespace autoware::lidar_transfusion -{ - -struct ProfileDimension -{ - nvinfer1::Dims min; - nvinfer1::Dims opt; - nvinfer1::Dims max; - - bool operator!=(const ProfileDimension & rhs) const - { - return min.nbDims != rhs.min.nbDims || opt.nbDims != rhs.opt.nbDims || - max.nbDims != rhs.max.nbDims || !std::equal(min.d, min.d + min.nbDims, rhs.min.d) || - !std::equal(opt.d, opt.d + opt.nbDims, rhs.opt.d) || - !std::equal(max.d, max.d + max.nbDims, rhs.max.d); - } -}; - -class NetworkTRT -{ -public: - explicit NetworkTRT(const TransfusionConfig & config); - ~NetworkTRT(); - - bool init( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision); - const char * getTensorName(NetworkIO name); - - autoware::tensorrt_common::TrtUniquePtr engine{nullptr}; - autoware::tensorrt_common::TrtUniquePtr context{nullptr}; - -private: - bool parseONNX( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision, - std::size_t workspace_size = (1ULL << 30)); - bool saveEngine(const std::string & engine_path); - bool loadEngine(const std::string & engine_path); - bool createContext(); - bool setProfile( - nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, - nvinfer1::IBuilderConfig & config); - bool validateNetworkIO(); - nvinfer1::Dims validateTensorShape(NetworkIO name, const std::vector shape); - - autoware::tensorrt_common::TrtUniquePtr runtime_{nullptr}; - autoware::tensorrt_common::TrtUniquePtr plan_{nullptr}; - autoware::tensorrt_common::Logger logger_; - TransfusionConfig config_; - std::vector tensors_names_; - - std::array in_profile_dims_; -}; - -} // namespace autoware::lidar_transfusion - -#endif // AUTOWARE__LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ diff --git a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_config.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_config.hpp index 363ee17a0d6e0..e60ee04150b5c 100644 --- a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_config.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_config.hpp @@ -35,21 +35,21 @@ class TransfusionConfig if (voxels_num.size() == 3) { max_voxels_ = voxels_num[2]; - voxels_num_[0] = voxels_num[0]; - voxels_num_[1] = voxels_num[1]; - voxels_num_[2] = voxels_num[2]; + voxels_num_[0] = static_cast(voxels_num[0]); + voxels_num_[1] = static_cast(voxels_num[1]); + voxels_num_[2] = static_cast(voxels_num[2]); - min_voxel_size_ = voxels_num[0]; - opt_voxel_size_ = voxels_num[1]; - max_voxel_size_ = voxels_num[2]; + min_voxel_size_ = static_cast(voxels_num[0]); + opt_voxel_size_ = static_cast(voxels_num[1]); + max_voxel_size_ = static_cast(voxels_num[2]); - min_points_size_ = voxels_num[0]; - opt_points_size_ = voxels_num[1]; - max_points_size_ = voxels_num[2]; + min_points_size_ = static_cast(voxels_num[0]); + opt_points_size_ = static_cast(voxels_num[1]); + max_points_size_ = static_cast(voxels_num[2]); - min_coors_size_ = voxels_num[0]; - opt_coors_size_ = voxels_num[1]; - max_coors_size_ = voxels_num[2]; + min_coors_size_ = static_cast(voxels_num[0]); + opt_coors_size_ = static_cast(voxels_num[1]); + max_coors_size_ = static_cast(voxels_num[2]); } if (point_cloud_range.size() == 6) { min_x_range_ = static_cast(point_cloud_range[0]); @@ -91,7 +91,7 @@ class TransfusionConfig std::size_t cloud_capacity_{}; ///// KERNEL PARAMETERS ///// const std::size_t threads_for_voxel_{256}; // threads number for a block - const std::size_t points_per_voxel_{20}; + const int32_t points_per_voxel_{20}; const std::size_t warp_size_{32}; // one warp(32 threads) for one pillar const std::size_t pillars_per_block_{64}; // one thread deals with one pillar // and a block has pillars_per_block threads @@ -99,9 +99,9 @@ class TransfusionConfig std::size_t max_voxels_{60000}; ///// NETWORK PARAMETERS ///// - const std::size_t batch_size_{1}; - const std::size_t num_classes_{5}; - const std::size_t num_point_feature_size_{5}; // x, y, z, intensity, lag + const int32_t batch_size_{1}; + const int32_t num_classes_{5}; + const int32_t num_point_feature_size_{5}; // x, y, z, intensity, lag // the dimension of the input cloud float min_x_range_{-76.8}; float max_x_range_{76.8}; @@ -114,9 +114,9 @@ class TransfusionConfig float voxel_y_size_{0.3}; float voxel_z_size_{8.0}; const std::size_t out_size_factor_{4}; - const std::size_t max_num_points_per_pillar_{points_per_voxel_}; - const std::size_t num_point_values_{4}; - std::size_t num_proposals_{200}; + const int32_t max_num_points_per_pillar_{points_per_voxel_}; + const int32_t num_point_values_{4}; + int32_t num_proposals_{200}; // the number of feature maps for pillar scatter const std::size_t num_feature_scatter_{pillar_feature_size_}; // the score threshold for classification @@ -126,7 +126,7 @@ class TransfusionConfig std::size_t max_num_pillars_{max_voxels_}; const std::size_t pillar_points_bev_{max_num_points_per_pillar_ * max_num_pillars_}; // the detected boxes result decode by (x, y, z, w, l, h, yaw) - const std::size_t num_box_values_{8}; + const int32_t num_box_values_{8}; // the input size of the 2D backbone network std::size_t grid_x_size_{512}; std::size_t grid_y_size_{512}; @@ -136,33 +136,33 @@ class TransfusionConfig std::size_t feature_y_size_{grid_y_size_ / out_size_factor_}; ///// RUNTIME DIMENSIONS ///// - std::vector voxels_num_{5000, 30000, 60000}; + std::vector voxels_num_{5000, 30000, 60000}; // voxels - std::size_t min_voxel_size_{voxels_num_[0]}; - std::size_t opt_voxel_size_{voxels_num_[1]}; - std::size_t max_voxel_size_{voxels_num_[2]}; + int32_t min_voxel_size_{voxels_num_[0]}; + int32_t opt_voxel_size_{voxels_num_[1]}; + int32_t max_voxel_size_{voxels_num_[2]}; - std::size_t min_point_in_voxel_size_{points_per_voxel_}; - std::size_t opt_point_in_voxel_size_{points_per_voxel_}; - std::size_t max_point_in_voxel_size_{points_per_voxel_}; + int32_t min_point_in_voxel_size_{points_per_voxel_}; + int32_t opt_point_in_voxel_size_{points_per_voxel_}; + int32_t max_point_in_voxel_size_{points_per_voxel_}; - std::size_t min_network_feature_size_{num_point_feature_size_}; - std::size_t opt_network_feature_size_{num_point_feature_size_}; - std::size_t max_network_feature_size_{num_point_feature_size_}; + int32_t min_network_feature_size_{num_point_feature_size_}; + int32_t opt_network_feature_size_{num_point_feature_size_}; + int32_t max_network_feature_size_{num_point_feature_size_}; // num_points - std::size_t min_points_size_{voxels_num_[0]}; - std::size_t opt_points_size_{voxels_num_[1]}; - std::size_t max_points_size_{voxels_num_[2]}; + int32_t min_points_size_{voxels_num_[0]}; + int32_t opt_points_size_{voxels_num_[1]}; + int32_t max_points_size_{voxels_num_[2]}; // coors - std::size_t min_coors_size_{voxels_num_[0]}; - std::size_t opt_coors_size_{voxels_num_[1]}; - std::size_t max_coors_size_{voxels_num_[2]}; + int32_t min_coors_size_{voxels_num_[0]}; + int32_t opt_coors_size_{voxels_num_[1]}; + int32_t max_coors_size_{voxels_num_[2]}; - std::size_t min_coors_dim_size_{num_point_values_}; - std::size_t opt_coors_dim_size_{num_point_values_}; - std::size_t max_coors_dim_size_{num_point_values_}; + int32_t min_coors_dim_size_{num_point_values_}; + int32_t opt_coors_dim_size_{num_point_values_}; + int32_t max_coors_dim_size_{num_point_values_}; }; } // namespace autoware::lidar_transfusion diff --git a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp index 06dd65b4b805f..00bb5e726706c 100644 --- a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp @@ -16,7 +16,6 @@ #define AUTOWARE__LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ #include "autoware/lidar_transfusion/cuda_utils.hpp" -#include "autoware/lidar_transfusion/network/network_trt.hpp" #include "autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp" #include "autoware/lidar_transfusion/preprocess/pointcloud_densification.hpp" #include "autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp" @@ -24,6 +23,7 @@ #include "autoware/lidar_transfusion/utils.hpp" #include "autoware/lidar_transfusion/visibility_control.hpp" +#include #include #include @@ -34,38 +34,17 @@ #include #include #include -#include #include namespace autoware::lidar_transfusion { -class NetworkParam -{ -public: - NetworkParam(std::string onnx_path, std::string engine_path, std::string trt_precision) - : onnx_path_(std::move(onnx_path)), - engine_path_(std::move(engine_path)), - trt_precision_(std::move(trt_precision)) - { - } - - std::string onnx_path() const { return onnx_path_; } - std::string engine_path() const { return engine_path_; } - std::string trt_precision() const { return trt_precision_; } - -private: - std::string onnx_path_; - std::string engine_path_; - std::string trt_precision_; -}; - class LIDAR_TRANSFUSION_PUBLIC TransfusionTRT { public: explicit TransfusionTRT( - const NetworkParam & network_param, const DensificationParam & densification_param, - const TransfusionConfig & config); + const tensorrt_common::TrtCommonConfig & trt_config, + const DensificationParam & densification_param, const TransfusionConfig config); virtual ~TransfusionTRT(); bool detect( @@ -73,6 +52,8 @@ class LIDAR_TRANSFUSION_PUBLIC TransfusionTRT std::vector & det_boxes3d, std::unordered_map & proc_timing); protected: + void initTrt(const tensorrt_common::TrtCommonConfig & trt_config); + void initPtr(); bool preprocess(const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer); @@ -81,7 +62,7 @@ class LIDAR_TRANSFUSION_PUBLIC TransfusionTRT bool postprocess(std::vector & det_boxes3d); - std::unique_ptr network_trt_ptr_{nullptr}; + std::unique_ptr network_trt_ptr_{nullptr}; std::unique_ptr vg_ptr_{nullptr}; std::unique_ptr> stop_watch_ptr_{ nullptr}; diff --git a/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp b/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp deleted file mode 100644 index 8e3cb8a55ec7e..0000000000000 --- a/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp +++ /dev/null @@ -1,357 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/lidar_transfusion/network/network_trt.hpp" - -#include - -#include -#include -#include -#include -#include - -namespace autoware::lidar_transfusion -{ - -inline NetworkIO nameToNetworkIO(const char * name) -{ - static const std::unordered_map name_to_enum = { - {"voxels", NetworkIO::voxels}, {"num_points", NetworkIO::num_points}, - {"coors", NetworkIO::coors}, {"cls_score0", NetworkIO::cls_score}, - {"bbox_pred0", NetworkIO::bbox_pred}, {"dir_cls_pred0", NetworkIO::dir_pred}}; - - auto it = name_to_enum.find(name); - if (it != name_to_enum.end()) { - return it->second; - } - throw std::runtime_error("Invalid input name: " + std::string(name)); -} - -std::ostream & operator<<(std::ostream & os, const ProfileDimension & profile) -{ - std::string delim = ""; - os << "min->["; - for (int i = 0; i < profile.min.nbDims; ++i) { - os << delim << profile.min.d[i]; - delim = ", "; - } - os << "], opt->["; - delim = ""; - for (int i = 0; i < profile.opt.nbDims; ++i) { - os << delim << profile.opt.d[i]; - delim = ", "; - } - os << "], max->["; - delim = ""; - for (int i = 0; i < profile.max.nbDims; ++i) { - os << delim << profile.max.d[i]; - delim = ", "; - } - os << "]"; - return os; -} - -NetworkTRT::NetworkTRT(const TransfusionConfig & config) : config_(config) -{ - ProfileDimension voxels_dims = { - nvinfer1::Dims3( - config_.min_voxel_size_, config_.min_point_in_voxel_size_, config_.min_network_feature_size_), - nvinfer1::Dims3( - config_.opt_voxel_size_, config_.opt_point_in_voxel_size_, config_.opt_network_feature_size_), - nvinfer1::Dims3( - config_.max_voxel_size_, config_.max_point_in_voxel_size_, - config_.max_network_feature_size_)}; - ProfileDimension num_points_dims = { - nvinfer1::Dims{1, {static_cast(config_.min_points_size_)}}, - nvinfer1::Dims{1, {static_cast(config_.opt_points_size_)}}, - nvinfer1::Dims{1, {static_cast(config_.max_points_size_)}}}; - ProfileDimension coors_dims = { - nvinfer1::Dims2(config_.min_coors_size_, config_.min_coors_dim_size_), - nvinfer1::Dims2(config_.opt_coors_size_, config_.opt_coors_dim_size_), - nvinfer1::Dims2(config_.max_coors_size_, config_.max_coors_dim_size_)}; - in_profile_dims_ = {voxels_dims, num_points_dims, coors_dims}; -} - -NetworkTRT::~NetworkTRT() -{ - context.reset(); - runtime_.reset(); - plan_.reset(); - engine.reset(); -} - -bool NetworkTRT::init( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision) -{ - runtime_ = autoware::tensorrt_common::TrtUniquePtr( - nvinfer1::createInferRuntime(logger_)); - if (!runtime_) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create runtime" << std::endl; - return false; - } - - bool success; - std::ifstream engine_file(engine_path); - if (engine_file.is_open()) { - success = loadEngine(engine_path); - } else { - auto log_thread = logger_.log_throttle( - nvinfer1::ILogger::Severity::kINFO, - "Applying optimizations and building TRT CUDA engine. Please wait a minutes...", 5); - success = parseONNX(onnx_path, engine_path, precision); - logger_.stop_throttle(log_thread); - } - success &= createContext(); - - return success; -} - -bool NetworkTRT::setProfile( - nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, - nvinfer1::IBuilderConfig & config) -{ - auto profile = builder.createOptimizationProfile(); - - auto voxels_name = network.getInput(NetworkIO::voxels)->getName(); - auto num_points_name = network.getInput(NetworkIO::num_points)->getName(); - auto coors_name = network.getInput(NetworkIO::coors)->getName(); - - profile->setDimensions( - voxels_name, nvinfer1::OptProfileSelector::kMIN, in_profile_dims_[NetworkIO::voxels].min); - profile->setDimensions( - voxels_name, nvinfer1::OptProfileSelector::kOPT, in_profile_dims_[NetworkIO::voxels].opt); - profile->setDimensions( - voxels_name, nvinfer1::OptProfileSelector::kMAX, in_profile_dims_[NetworkIO::voxels].max); - - profile->setDimensions( - num_points_name, nvinfer1::OptProfileSelector::kMIN, - in_profile_dims_[NetworkIO::num_points].min); - profile->setDimensions( - num_points_name, nvinfer1::OptProfileSelector::kOPT, - in_profile_dims_[NetworkIO::num_points].opt); - profile->setDimensions( - num_points_name, nvinfer1::OptProfileSelector::kMAX, - in_profile_dims_[NetworkIO::num_points].max); - - profile->setDimensions( - coors_name, nvinfer1::OptProfileSelector::kMIN, in_profile_dims_[NetworkIO::coors].min); - profile->setDimensions( - coors_name, nvinfer1::OptProfileSelector::kOPT, in_profile_dims_[NetworkIO::coors].opt); - profile->setDimensions( - coors_name, nvinfer1::OptProfileSelector::kMAX, in_profile_dims_[NetworkIO::coors].max); - - config.addOptimizationProfile(profile); - return true; -} - -bool NetworkTRT::createContext() -{ - if (!engine) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Failed to create context: Engine was not created" << std::endl; - return false; - } - - context = autoware::tensorrt_common::TrtUniquePtr( - engine->createExecutionContext()); - if (!context) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create context" << std::endl; - return false; - } - - return true; -} - -bool NetworkTRT::parseONNX( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision, - const size_t workspace_size) -{ - auto builder = autoware::tensorrt_common::TrtUniquePtr( - nvinfer1::createInferBuilder(logger_)); - if (!builder) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create builder" << std::endl; - return false; - } - - auto config = autoware::tensorrt_common::TrtUniquePtr( - builder->createBuilderConfig()); - if (!config) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create config" << std::endl; - return false; - } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 - config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, workspace_size); -#else - config->setMaxWorkspaceSize(workspace_size); -#endif - if (precision == "fp16") { - if (builder->platformHasFastFp16()) { - autoware::tensorrt_common::LOG_INFO(logger_) << "Using TensorRT FP16 Inference" << std::endl; - config->setFlag(nvinfer1::BuilderFlag::kFP16); - } else { - autoware::tensorrt_common::LOG_INFO(logger_) - << "TensorRT FP16 Inference isn't supported in this environment" << std::endl; - } - } - - const auto flag = - 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); - auto network = autoware::tensorrt_common::TrtUniquePtr( - builder->createNetworkV2(flag)); - if (!network) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create network" << std::endl; - return false; - } - - auto parser = autoware::tensorrt_common::TrtUniquePtr( - nvonnxparser::createParser(*network, logger_)); - parser->parseFromFile(onnx_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); - - if (!setProfile(*builder, *network, *config)) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to set profile" << std::endl; - return false; - } - - plan_ = autoware::tensorrt_common::TrtUniquePtr( - builder->buildSerializedNetwork(*network, *config)); - if (!plan_) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Failed to create serialized network" << std::endl; - return false; - } - engine = autoware::tensorrt_common::TrtUniquePtr( - runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); - if (!engine) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create engine" << std::endl; - return false; - } - - return saveEngine(engine_path); -} - -bool NetworkTRT::saveEngine(const std::string & engine_path) -{ - autoware::tensorrt_common::LOG_INFO(logger_) << "Writing to " << engine_path << std::endl; - std::ofstream file(engine_path, std::ios::out | std::ios::binary); - file.write(reinterpret_cast(plan_->data()), plan_->size()); - return validateNetworkIO(); -} - -bool NetworkTRT::loadEngine(const std::string & engine_path) -{ - std::ifstream engine_file(engine_path); - std::stringstream engine_buffer; - engine_buffer << engine_file.rdbuf(); - std::string engine_str = engine_buffer.str(); - engine = - autoware::tensorrt_common::TrtUniquePtr(runtime_->deserializeCudaEngine( - reinterpret_cast(engine_str.data()), engine_str.size())); - autoware::tensorrt_common::LOG_INFO(logger_) << "Loaded engine from " << engine_path << std::endl; - return validateNetworkIO(); -} - -bool NetworkTRT::validateNetworkIO() -{ - // Whether the number of IO match the expected size - if (engine->getNbIOTensors() != NetworkIO::ENUM_SIZE) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Invalid network IO. Expected size: " << NetworkIO::ENUM_SIZE - << ". Actual size: " << engine->getNbIOTensors() << "." << std::endl; - throw std::runtime_error("Failed to initialize TRT network."); - } - - // Initialize tensors_names_ with null values - tensors_names_.resize(NetworkIO::ENUM_SIZE, nullptr); - - // Loop over the tensor names and place them in the correct positions - for (int i = 0; i < NetworkIO::ENUM_SIZE; ++i) { - const char * name = engine->getIOTensorName(i); - tensors_names_[nameToNetworkIO(name)] = name; - } - - // Log the network IO - std::string tensors = std::accumulate( - tensors_names_.begin(), tensors_names_.end(), std::string(), - [](const std::string & a, const std::string & b) -> std::string { return a + b + " "; }); - autoware::tensorrt_common::LOG_INFO(logger_) << "Network IO: " << tensors << std::endl; - - // Whether the current engine input profile match the config input profile - for (int i = 0; i <= NetworkIO::coors; ++i) { - ProfileDimension engine_dims{ - engine->getProfileShape(tensors_names_[i], 0, nvinfer1::OptProfileSelector::kMIN), - engine->getProfileShape(tensors_names_[i], 0, nvinfer1::OptProfileSelector::kOPT), - engine->getProfileShape(tensors_names_[i], 0, nvinfer1::OptProfileSelector::kMAX)}; - - autoware::tensorrt_common::LOG_INFO(logger_) - << "Profile for " << tensors_names_[i] << ": " << engine_dims << std::endl; - - if (engine_dims != in_profile_dims_[i]) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Invalid network input dimension. Config: " << in_profile_dims_[i] - << ". Please change the input profile or delete the engine file and build engine again." - << std::endl; - throw std::runtime_error("Failed to initialize TRT network."); - } - } - - // Whether the IO tensor shapes match the network config, -1 for dynamic size - validateTensorShape( - NetworkIO::voxels, {-1, static_cast(config_.points_per_voxel_), - static_cast(config_.num_point_feature_size_)}); - validateTensorShape(NetworkIO::num_points, {-1}); - validateTensorShape(NetworkIO::coors, {-1, static_cast(config_.num_point_values_)}); - auto cls_score = validateTensorShape( - NetworkIO::cls_score, - {static_cast(config_.batch_size_), static_cast(config_.num_classes_), - static_cast(config_.num_proposals_)}); - autoware::tensorrt_common::LOG_INFO(logger_) - << "Network num classes: " << cls_score.d[1] << std::endl; - validateTensorShape( - NetworkIO::dir_pred, - {static_cast(config_.batch_size_), 2, static_cast(config_.num_proposals_)}); // x, y - validateTensorShape( - NetworkIO::bbox_pred, - {static_cast(config_.batch_size_), static_cast(config_.num_box_values_), - static_cast(config_.num_proposals_)}); - - return true; -} - -const char * NetworkTRT::getTensorName(NetworkIO name) -{ - return tensors_names_.at(name); -} - -nvinfer1::Dims NetworkTRT::validateTensorShape(NetworkIO name, const std::vector shape) -{ - auto tensor_shape = engine->getTensorShape(tensors_names_[name]); - if (tensor_shape.nbDims != static_cast(shape.size())) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Invalid tensor shape for " << tensors_names_[name] << ". Expected size: " << shape.size() - << ". Actual size: " << tensor_shape.nbDims << "." << std::endl; - throw std::runtime_error("Failed to initialize TRT network."); - } - for (int i = 0; i < tensor_shape.nbDims; ++i) { - if (tensor_shape.d[i] != static_cast(shape[i])) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Invalid tensor shape for " << tensors_names_[name] << ". Expected: " << shape[i] - << ". Actual: " << tensor_shape.d[i] << "." << std::endl; - throw std::runtime_error("Failed to initialize TRT network."); - } - } - return tensor_shape; -} - -} // namespace autoware::lidar_transfusion diff --git a/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu b/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu index 49131218ff698..a89268e646cfc 100644 --- a/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu +++ b/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu @@ -176,6 +176,8 @@ __global__ void generateVoxels_random_kernel( int voxel_idx = floorf((x - min_x_range) / pillar_x_size); int voxel_idy = floorf((y - min_y_range) / pillar_y_size); + voxel_idx = voxel_idx < 0 ? 0 : voxel_idx >= grid_x_size ? grid_x_size - 1 : voxel_idx; + voxel_idy = voxel_idy < 0 ? 0 : voxel_idy >= grid_y_size ? grid_y_size - 1 : voxel_idy; unsigned int voxel_index = voxel_idy * grid_x_size + voxel_idx; unsigned int point_id = atomicAdd(&(mask[voxel_index]), 1); diff --git a/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp b/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp index 4e100f2e794d5..dd075a45cb29c 100644 --- a/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp +++ b/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp @@ -17,6 +17,7 @@ #include "autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp" #include "autoware/lidar_transfusion/transfusion_config.hpp" +#include #include #include @@ -27,25 +28,23 @@ #include #include #include +#include #include namespace autoware::lidar_transfusion { TransfusionTRT::TransfusionTRT( - const NetworkParam & network_param, const DensificationParam & densification_param, - const TransfusionConfig & config) -: config_(config) + const tensorrt_common::TrtCommonConfig & trt_config, + const DensificationParam & densification_param, TransfusionConfig config) +: config_(std::move(config)) { - network_trt_ptr_ = std::make_unique(config_); - - network_trt_ptr_->init( - network_param.onnx_path(), network_param.engine_path(), network_param.trt_precision()); vg_ptr_ = std::make_unique(densification_param, config_, stream_); stop_watch_ptr_ = std::make_unique>(); stop_watch_ptr_->tic("processing/inner"); initPtr(); + initTrt(trt_config); CHECK_CUDA_ERROR(cudaStreamCreate(&stream_)); } @@ -99,6 +98,51 @@ void TransfusionTRT::initPtr() post_ptr_ = std::make_unique(config_, stream_); } +void TransfusionTRT::initTrt(const tensorrt_common::TrtCommonConfig & trt_config) +{ + std::vector network_io{ + autoware::tensorrt_common::NetworkIO( + "voxels", {3, {-1, config_.points_per_voxel_, config_.num_point_feature_size_}}), + autoware::tensorrt_common::NetworkIO("num_points", {1, {-1}}), + autoware::tensorrt_common::NetworkIO("coors", {2, {-1, config_.num_point_values_}}), + autoware::tensorrt_common::NetworkIO( + "cls_score0", {3, {config_.batch_size_, config_.num_classes_, config_.num_proposals_}}), + autoware::tensorrt_common::NetworkIO( + "bbox_pred0", {3, {config_.batch_size_, config_.num_box_values_, config_.num_proposals_}}), + autoware::tensorrt_common::NetworkIO( + "dir_cls_pred0", {3, {config_.batch_size_, 2, config_.num_proposals_}})}; + + std::vector profile_dims{ + autoware::tensorrt_common::ProfileDims( + "voxels", + {3, + {config_.min_voxel_size_, config_.min_point_in_voxel_size_, + config_.min_network_feature_size_}}, + {3, + {config_.opt_voxel_size_, config_.opt_point_in_voxel_size_, + config_.opt_network_feature_size_}}, + {3, + {config_.max_voxel_size_, config_.max_point_in_voxel_size_, + config_.max_network_feature_size_}}), + autoware::tensorrt_common::ProfileDims( + "num_points", {1, {static_cast(config_.min_points_size_)}}, + {1, {static_cast(config_.opt_points_size_)}}, + {1, {static_cast(config_.max_points_size_)}}), + autoware::tensorrt_common::ProfileDims( + "coors", {2, {config_.min_coors_size_, config_.min_coors_dim_size_}}, + {2, {config_.opt_coors_size_, config_.opt_coors_dim_size_}}, + {2, {config_.max_coors_size_, config_.max_coors_dim_size_}})}; + + auto network_io_ptr = + std::make_unique>(network_io); + auto profile_dims_ptr = + std::make_unique>(profile_dims); + + network_trt_ptr_ = std::make_unique(trt_config); + if (!network_trt_ptr_->setup(std::move(profile_dims_ptr), std::move(network_io_ptr))) + throw std::runtime_error("Failed to setup TRT engine."); +} + bool TransfusionTRT::detect( const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer, std::vector & det_boxes3d, std::unordered_map & proc_timing) @@ -166,8 +210,9 @@ bool TransfusionTRT::preprocess( CHECK_CUDA_ERROR(cudaMemcpyAsync( ¶ms_input, params_input_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost, stream_)); CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + auto params_input_i32 = static_cast(params_input); - if (params_input < config_.min_voxel_size_) { + if (params_input_i32 < config_.min_voxel_size_) { RCLCPP_WARN_STREAM( rclcpp::get_logger("lidar_transfusion"), "Too few voxels (" << params_input << ") for actual optimization profile (" @@ -185,39 +230,34 @@ bool TransfusionTRT::preprocess( params_input = config_.max_voxels_; } + RCLCPP_DEBUG_STREAM( rclcpp::get_logger("lidar_transfusion"), "Generated input voxels: " << params_input); - network_trt_ptr_->context->setTensorAddress( - network_trt_ptr_->getTensorName(NetworkIO::voxels), voxel_features_d_.get()); - network_trt_ptr_->context->setInputShape( - network_trt_ptr_->getTensorName(NetworkIO::voxels), + bool success = true; + + // Inputs + success &= network_trt_ptr_->setTensor( + "voxels", voxel_features_d_.get(), nvinfer1::Dims3{ - static_cast(params_input), static_cast(config_.max_num_points_per_pillar_), + params_input_i32, config_.max_num_points_per_pillar_, static_cast(config_.num_point_feature_size_)}); - network_trt_ptr_->context->setTensorAddress( - network_trt_ptr_->getTensorName(NetworkIO::num_points), voxel_num_d_.get()); - network_trt_ptr_->context->setInputShape( - network_trt_ptr_->getTensorName(NetworkIO::num_points), - nvinfer1::Dims{1, {static_cast(params_input)}}); - network_trt_ptr_->context->setTensorAddress( - network_trt_ptr_->getTensorName(NetworkIO::coors), voxel_idxs_d_.get()); - network_trt_ptr_->context->setInputShape( - network_trt_ptr_->getTensorName(NetworkIO::coors), - nvinfer1::Dims2{ - static_cast(params_input), static_cast(config_.num_point_values_)}); - network_trt_ptr_->context->setTensorAddress( - network_trt_ptr_->getTensorName(NetworkIO::cls_score), cls_output_d_.get()); - network_trt_ptr_->context->setTensorAddress( - network_trt_ptr_->getTensorName(NetworkIO::bbox_pred), box_output_d_.get()); - network_trt_ptr_->context->setTensorAddress( - network_trt_ptr_->getTensorName(NetworkIO::dir_pred), dir_cls_output_d_.get()); - return true; + success &= network_trt_ptr_->setTensor( + "num_points", voxel_num_d_.get(), nvinfer1::Dims{1, {params_input_i32}}); + success &= network_trt_ptr_->setTensor( + "coors", voxel_idxs_d_.get(), nvinfer1::Dims2{params_input_i32, config_.num_point_values_}); + + // Outputs + success &= network_trt_ptr_->setTensor("cls_score0", cls_output_d_.get()); + success &= network_trt_ptr_->setTensor("bbox_pred0", box_output_d_.get()); + success &= network_trt_ptr_->setTensor("dir_cls_pred0", dir_cls_output_d_.get()); + + return success; } bool TransfusionTRT::inference() { - auto status = network_trt_ptr_->context->enqueueV3(stream_); + auto status = network_trt_ptr_->enqueueV3(stream_); CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); if (!status) { diff --git a/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp b/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp index 8bb70a821621f..563abd97698e0 100644 --- a/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp +++ b/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp @@ -75,7 +75,6 @@ LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) const float score_threshold = static_cast(this->declare_parameter("score_threshold", descriptor)); - NetworkParam network_param(onnx_path, engine_path, trt_precision); DensificationParam densification_param( densification_world_frame_id, densification_num_past_frames); TransfusionConfig config( @@ -91,7 +90,9 @@ LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) detection_class_remapper_.setParameters( allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); - detector_ptr_ = std::make_unique(network_param, densification_param, config); + auto trt_config = tensorrt_common::TrtCommonConfig(onnx_path, trt_precision, engine_path); + + detector_ptr_ = std::make_unique(trt_config, densification_param, config); cloud_sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1), @@ -155,7 +156,6 @@ void LidarTransfusionNode::cloudCallback(const sensor_msgs::msg::PointCloud2::Co objects_pub_->publish(output_msg); published_time_pub_->publish_if_subscribed(objects_pub_, output_msg.header.stamp); } - // add processing time for debug if (debug_publisher_ptr_ && stop_watch_ptr_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic", true); diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 1675f8b1fbfa2..b047cd28114ba 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -71,8 +71,8 @@ struct hash } // namespace std namespace autoware::map_based_prediction { +using autoware_internal_debug_msgs::msg::StringStamped; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_debug_msgs::msg::StringStamped; using TrajectoryPoints = std::vector; class MapBasedPredictionNode : public rclcpp::Node diff --git a/perception/autoware_map_based_prediction/package.xml b/perception/autoware_map_based_prediction/package.xml index 9012590a45b14..58dfcb17c7631 100644 --- a/perception/autoware_map_based_prediction/package.xml +++ b/perception/autoware_map_based_prediction/package.xml @@ -16,6 +16,7 @@ ament_cmake autoware_cmake + autoware_internal_debug_msgs autoware_interpolation autoware_lanelet2_extension autoware_motion_utils @@ -27,7 +28,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_debug_msgs unique_identifier_msgs visualization_msgs diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index 42dbf5b83fa8c..b65020d6b4708 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -670,9 +670,9 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt if (stop_watch_ptr_) { const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true); const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_map_based_prediction/src/predictor_vru.cpp b/perception/autoware_map_based_prediction/src/predictor_vru.cpp index 0eea665cbd8a2..ffbdbd97b50a2 100644 --- a/perception/autoware_map_based_prediction/src/predictor_vru.cpp +++ b/perception/autoware_map_based_prediction/src/predictor_vru.cpp @@ -269,6 +269,7 @@ void PredictorVru::setLaneletMap(std::shared_ptr lanelet_ma const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); const auto crosswalks = lanelet::utils::query::crosswalkLanelets(all_lanelets); const auto walkways = lanelet::utils::query::walkwayLanelets(all_lanelets); + crosswalks_.clear(); crosswalks_.insert(crosswalks_.end(), crosswalks.begin(), crosswalks.end()); crosswalks_.insert(crosswalks_.end(), walkways.begin(), walkways.end()); diff --git a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp index 3eebb3c11ee3e..3761fc1e6f43a 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp @@ -127,7 +127,7 @@ void TrackerDebugger::startMeasurementTime( stamp_process_start_ = now; if (debug_settings_.publish_processing_time) { double input_latency_ms = (now - last_input_stamp_).seconds() * 1e3; - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/input_latency_ms", input_latency_ms); } // initialize debug time stamps @@ -169,15 +169,15 @@ void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Tim double measurement_to_object_ms = (object_time - last_input_stamp_).seconds() * 1e3; // starting from the measurement time - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms_); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_time_ms", processing_time_ms); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_latency_ms", processing_latency_ms); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/meas_to_tracked_object_ms", measurement_to_object_ms); } stamp_publish_output_ = now; diff --git a/perception/autoware_object_merger/src/object_association_merger_node.cpp b/perception/autoware_object_merger/src/object_association_merger_node.cpp index 0e2b88f4aa566..40bdd0a7d6938 100644 --- a/perception/autoware_object_merger/src/object_association_merger_node.cpp +++ b/perception/autoware_object_merger/src/object_association_merger_node.cpp @@ -235,9 +235,9 @@ void ObjectAssociationMergerNode::objectsCallback( merged_object_pub_->publish(output_msg); published_time_publisher_->publish_if_subscribed(merged_object_pub_, output_msg.header.stamp); // publish processing time - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } } // namespace autoware::object_merger diff --git a/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp b/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp index eb52b903b90ca..094825056e62c 100644 --- a/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp +++ b/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp @@ -420,9 +420,9 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index 880dfda03c495..7cde04ca9614b 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -332,11 +332,11 @@ void GridMapFusionNode::publish() std::chrono::duration( std::chrono::nanoseconds((this->get_clock()->now() - latest_stamp).nanoseconds())) .count(); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index 4d442ad2c33db..ff836a1ddf760 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -261,11 +261,11 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( std::chrono::nanoseconds( (this->get_clock()->now() - input_raw_msg->header.stamp).nanoseconds())) .count(); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/perception/autoware_shape_estimation/CMakeLists.txt b/perception/autoware_shape_estimation/CMakeLists.txt index d0eb2aa5535a8..d7313111e6ade 100644 --- a/perception/autoware_shape_estimation/CMakeLists.txt +++ b/perception/autoware_shape_estimation/CMakeLists.txt @@ -4,6 +4,9 @@ project(autoware_shape_estimation) find_package(autoware_cmake REQUIRED) autoware_package() +# TODO(amadeuszsz): Remove -Wno-deprecated-declarations once removing implicit quantization +add_compile_options(-Wno-deprecated-declarations) + find_package(PCL REQUIRED COMPONENTS common) find_package(pcl_conversions REQUIRED) diff --git a/perception/autoware_shape_estimation/include/autoware/shape_estimation/tensorrt_shape_estimator.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/tensorrt_shape_estimator.hpp index d39dce65d224d..4328246ff395f 100644 --- a/perception/autoware_shape_estimation/include/autoware/shape_estimation/tensorrt_shape_estimator.hpp +++ b/perception/autoware_shape_estimation/include/autoware/shape_estimation/tensorrt_shape_estimator.hpp @@ -48,11 +48,7 @@ class TrtShapeEstimator { public: TrtShapeEstimator( - const std::string & model_path, const std::string & precision, - const autoware::tensorrt_common::BatchConfig & batch_config, - const size_t max_workspace_size = (1 << 30), - const autoware::tensorrt_common::BuildConfig build_config = - autoware::tensorrt_common::BuildConfig("MinMax", -1, false, false, false, 0.0)); + const std::string & model_path, const std::string & precision, const int batch_size); ~TrtShapeEstimator() = default; diff --git a/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp b/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp index b4b1f18832da7..fa3c3cb09bad3 100644 --- a/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp +++ b/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp @@ -29,47 +29,41 @@ namespace autoware::shape_estimation { TrtShapeEstimator::TrtShapeEstimator( - const std::string & model_path, const std::string & precision, - const autoware::tensorrt_common::BatchConfig & batch_config, const size_t max_workspace_size, - const autoware::tensorrt_common::BuildConfig build_config) + const std::string & model_path, const std::string & precision, const int batch_size) { trt_common_ = std::make_unique( - model_path, precision, nullptr, batch_config, max_workspace_size, build_config); + tensorrt_common::TrtCommonConfig(model_path, precision)); + batch_size_ = batch_size; - trt_common_->setup(); - - if (!trt_common_->isInitialized()) { - std::cerr << "Failed to initialize TensorRT" << std::endl; - return; + if (!trt_common_->setup()) { + throw std::runtime_error("Failed to setup TensorRT"); } - const auto pc_input_dims = trt_common_->getBindingDimensions(0); + const auto pc_input_dims = trt_common_->getTensorShape(0); const auto pc_input_size = std::accumulate( pc_input_dims.d + 1, pc_input_dims.d + pc_input_dims.nbDims, 1, std::multiplies()); - input_pc_d_ = autoware::cuda_utils::make_unique(pc_input_size * batch_config[2]); - batch_size_ = batch_config[2]; - const auto one_hot_input_dims = trt_common_->getBindingDimensions(1); + input_pc_d_ = autoware::cuda_utils::make_unique(pc_input_size * batch_size_); + const auto one_hot_input_dims = trt_common_->getTensorShape(1); const auto one_hot_input_size = std::accumulate( one_hot_input_dims.d + 1, one_hot_input_dims.d + one_hot_input_dims.nbDims, 1, std::multiplies()); - input_one_hot_d_ = - autoware::cuda_utils::make_unique(one_hot_input_size * batch_config[2]); + input_one_hot_d_ = autoware::cuda_utils::make_unique(one_hot_input_size * batch_size_); - const auto stage1_center_out_dims = trt_common_->getBindingDimensions(2); + const auto stage1_center_out_dims = trt_common_->getTensorShape(2); out_s1center_elem_num_ = std::accumulate( stage1_center_out_dims.d + 1, stage1_center_out_dims.d + stage1_center_out_dims.nbDims, 1, std::multiplies()); - out_s1center_elem_num_ = out_s1center_elem_num_ * batch_config[2]; - out_s1center_elem_num_per_batch_ = static_cast(out_s1center_elem_num_ / batch_config[2]); + out_s1center_elem_num_ = out_s1center_elem_num_ * batch_size_; + out_s1center_elem_num_per_batch_ = static_cast(out_s1center_elem_num_ / batch_size_); out_s1center_prob_d_ = autoware::cuda_utils::make_unique(out_s1center_elem_num_); out_s1center_prob_h_ = autoware::cuda_utils::make_unique_host(out_s1center_elem_num_, cudaHostAllocPortable); - const auto pred_out_dims = trt_common_->getBindingDimensions(3); + const auto pred_out_dims = trt_common_->getTensorShape(3); out_pred_elem_num_ = std::accumulate( pred_out_dims.d + 1, pred_out_dims.d + pred_out_dims.nbDims, 1, std::multiplies()); - out_pred_elem_num_ = out_pred_elem_num_ * batch_config[2]; - out_pred_elem_num_per_batch_ = static_cast(out_pred_elem_num_ / batch_config[2]); + out_pred_elem_num_ = out_pred_elem_num_ * batch_size_; + out_pred_elem_num_per_batch_ = static_cast(out_pred_elem_num_ / batch_size_); out_pred_prob_d_ = autoware::cuda_utils::make_unique(out_pred_elem_num_); out_pred_prob_h_ = autoware::cuda_utils::make_unique_host(out_pred_elem_num_, cudaHostAllocPortable); @@ -83,10 +77,6 @@ TrtShapeEstimator::TrtShapeEstimator( bool TrtShapeEstimator::inference( const DetectedObjectsWithFeature & input, DetectedObjectsWithFeature & output) { - if (!trt_common_->isInitialized()) { - return false; - } - bool result = false; for (size_t i = 0; i < input.feature_objects.size(); i += batch_size_) { @@ -111,13 +101,13 @@ bool TrtShapeEstimator::inference( void TrtShapeEstimator::preprocess(const DetectedObjectsWithFeature & input) { - auto input_dims_pc = trt_common_->getBindingDimensions(0); + auto input_dims_pc = trt_common_->getTensorShape(0); int batch_size = static_cast(input.feature_objects.size()); const auto input_chan = static_cast(input_dims_pc.d[1]); const auto input_pc_size = static_cast(input_dims_pc.d[2]); - auto input_dims_one_hot = trt_common_->getBindingDimensions(1); + auto input_dims_one_hot = trt_common_->getTensorShape(1); const auto input_one_hot_size = static_cast(input_dims_one_hot.d[1]); int volume_pc = batch_size * input_chan * input_pc_size; @@ -229,7 +219,10 @@ bool TrtShapeEstimator::feed_forward_and_decode( int batch_size = static_cast(input.feature_objects.size()); std::vector buffers = { input_pc_d_.get(), input_one_hot_d_.get(), out_s1center_prob_d_.get(), out_pred_prob_d_.get()}; - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); CHECK_CUDA_ERROR(cudaMemcpyAsync( out_s1center_prob_h_.get(), out_s1center_prob_d_.get(), out_s1center_elem_num_ * sizeof(float), diff --git a/perception/autoware_shape_estimation/src/shape_estimation_node.cpp b/perception/autoware_shape_estimation/src/shape_estimation_node.cpp index 67402f68d63ef..28b2a6e398e91 100644 --- a/perception/autoware_shape_estimation/src/shape_estimation_node.cpp +++ b/perception/autoware_shape_estimation/src/shape_estimation_node.cpp @@ -63,9 +63,8 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option min_points_ = declare_parameter("model_params.minimum_points"); std::string precision = declare_parameter("model_params.precision"); int batch_size = declare_parameter("model_params.batch_size"); - autoware::tensorrt_common::BatchConfig batch_config{batch_size, batch_size, batch_size}; tensorrt_shape_estimator_ = - std::make_unique(model_path, precision, batch_config); + std::make_unique(model_path, precision, batch_size); if (this->declare_parameter("model_params.build_only", false)) { RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); rclcpp::shutdown(); @@ -180,9 +179,9 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared // Publish pub_->publish(output_msg); published_time_publisher_->publish_if_subscribed(pub_, output_msg.header.stamp); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } } // namespace autoware::shape_estimation diff --git a/perception/autoware_tensorrt_classifier/CMakeLists.txt b/perception/autoware_tensorrt_classifier/CMakeLists.txt index 88747a1e9240c..59ce7b18afcc3 100644 --- a/perception/autoware_tensorrt_classifier/CMakeLists.txt +++ b/perception/autoware_tensorrt_classifier/CMakeLists.txt @@ -6,6 +6,9 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -O3 -Wno-write-strings -fopen find_package(autoware_cmake REQUIRED) autoware_package() +# TODO(amadeuszsz): Remove -Wno-deprecated-declarations once removing implicit quantization +add_compile_options(-Wno-deprecated-declarations) + find_package(CUDA) find_package(CUDNN) find_package(TENSORRT) diff --git a/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp index ee16343b956d1..4f7a5865c72ae 100644 --- a/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp +++ b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -44,22 +45,18 @@ class TrtClassifier * @param[in] mode_path ONNX model_path * @param[in] precision precision for inference * @param[in] calibration_images path for calibration files (only require for quantization) - * @param[in] batch_config configuration for batched execution - * @param[in] max_workspace_size maximum workspace for building TensorRT engine * @param[in] mean mean for preprocess * @param[in] std std for preprocess - * @param[in] buildConfig configuration including precision, calibration method, dla, remaining - * fp16 for first layer, remaining fp16 for last layer and profiler for builder + * @param[in] calib_config calibration configuration * @param[in] cuda whether use cuda gpu for preprocessing */ TrtClassifier( const std::string & model_path, const std::string & precision, - const autoware::tensorrt_common::BatchConfig & batch_config = {1, 1, 1}, const std::vector & mean = {123.675, 116.28, 103.53}, const std::vector & std = {58.395, 57.12, 57.375}, - const size_t max_workspace_size = (1 << 30), const std::string & calibration_images = "", - const autoware::tensorrt_common::BuildConfig build_config = - autoware::tensorrt_common::BuildConfig("MinMax", -1, false, false, false, 0.0), + const std::string & calibration_images = "", + const tensorrt_common::CalibrationConfig & calibration_config = + tensorrt_common::CalibrationConfig("MinMax", false, false, 0.0), const bool cuda = false); /** * @brief Deconstruct TrtClassifier @@ -84,6 +81,12 @@ class TrtClassifier */ void initPreprocessBuffer(int width, int height); + /** + * @brief get batch size + * @return batch size + */ + [[nodiscard]] int getBatchSize() const; + private: /** * @brief run preprocess including resizing, letterbox, BGR2RGB, NHWC2NCHW and toFloat on CPU @@ -102,7 +105,7 @@ class TrtClassifier const std::vector & images, std::vector & results, std::vector & probabilities); - std::unique_ptr trt_common_; + std::unique_ptr trt_common_; std::vector input_h_; CudaUniquePtr input_d_; diff --git a/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp b/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp index 4aa008e42c966..7666ae4f9d068 100644 --- a/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp +++ b/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp @@ -100,11 +100,9 @@ std::vector loadImageList(const std::string & filename, const std:: namespace autoware::tensorrt_classifier { TrtClassifier::TrtClassifier( - const std::string & model_path, const std::string & precision, - const autoware::tensorrt_common::BatchConfig & batch_config, const std::vector & mean, - const std::vector & std, const size_t max_workspace_size, - const std::string & calibration_image_list_path, - autoware::tensorrt_common::BuildConfig build_config, const bool cuda) + const std::string & model_path, const std::string & precision, const std::vector & mean, + const std::vector & std, const std::string & calibration_image_list_path, + const tensorrt_common::CalibrationConfig & calib_config, const bool cuda) { src_width_ = -1; src_height_ = -1; @@ -114,22 +112,36 @@ TrtClassifier::TrtClassifier( for (size_t i = 0; i < inv_std_.size(); i++) { inv_std_[i] = 1.0 / inv_std_[i]; } - batch_size_ = batch_config[2]; + trt_common_ = std::make_unique( + tensorrt_common::TrtCommonConfig(model_path, precision)); + + const auto network_input_dims = trt_common_->getTensorShape(0); + batch_size_ = network_input_dims.d[0]; + const auto input_channel = network_input_dims.d[1]; + const auto input_height = network_input_dims.d[2]; + const auto input_width = network_input_dims.d[3]; + + std::vector profile_dims{ + autoware::tensorrt_common::ProfileDims( + 0, {4, {batch_size_, input_channel, input_height, input_width}}, + {4, {batch_size_, input_channel, input_height, input_width}}, + {4, {batch_size_, input_channel, input_height, input_width}})}; + auto profile_dims_ptr = + std::make_unique>(profile_dims); + if (precision == "int8") { - int max_batch_size = batch_config[2]; - nvinfer1::Dims input_dims = autoware::tensorrt_common::get_input_dims(model_path); std::vector calibration_images; if (calibration_image_list_path != "") { calibration_images = loadImageList(calibration_image_list_path, ""); } autoware::tensorrt_classifier::ImageStream stream( - max_batch_size, input_dims, calibration_images); + batch_size_, network_input_dims, calibration_images); fs::path calibration_table{model_path}; std::string ext = ""; - if (build_config.calib_type_str == "Entropy") { + if (calib_config.calib_type_str == "Entropy") { ext = "EntropyV2-"; } else if ( - build_config.calib_type_str == "Legacy" || build_config.calib_type_str == "Percentile") { + calib_config.calib_type_str == "Legacy" || calib_config.calib_type_str == "Percentile") { ext = "Legacy-"; } else { ext = "MinMax-"; @@ -141,11 +153,11 @@ TrtClassifier::TrtClassifier( histogram_table.replace_extension(ext); std::unique_ptr calibrator; - if (build_config.calib_type_str == "Entropy") { + if (calib_config.calib_type_str == "Entropy") { calibrator.reset(new autoware::tensorrt_classifier::Int8EntropyCalibrator( stream, calibration_table, mean_, std_)); } else if ( - build_config.calib_type_str == "Legacy" || build_config.calib_type_str == "Percentile") { + calib_config.calib_type_str == "Legacy" || calib_config.calib_type_str == "Percentile") { double quantile = 0.999999; double cutoff = 0.999999; calibrator.reset(new autoware::tensorrt_classifier::Int8LegacyCalibrator( @@ -154,29 +166,27 @@ TrtClassifier::TrtClassifier( calibrator.reset(new autoware::tensorrt_classifier::Int8MinMaxCalibrator( stream, calibration_table, mean_, std_)); } - trt_common_ = std::make_unique( - model_path, precision, std::move(calibrator), batch_config, max_workspace_size, build_config); + if (!trt_common_->setupWithCalibrator( + std::move(calibrator), calib_config, std::move((profile_dims_ptr)))) { + throw std::runtime_error("Failed to setup TensorRT engine with calibrator"); + } } else { - trt_common_ = std::make_unique( - model_path, precision, nullptr, batch_config, max_workspace_size, build_config); - } - trt_common_->setup(); - - if (!trt_common_->isInitialized()) { - return; + if (!trt_common_->setup(std::move(profile_dims_ptr))) { + throw std::runtime_error("Failed to setup TensorRT engine"); + } } // GPU memory allocation - const auto input_dims = trt_common_->getBindingDimensions(0); + const auto input_dims = trt_common_->getTensorShape(0); const auto input_size = std::accumulate(input_dims.d + 1, input_dims.d + input_dims.nbDims, 1, std::multiplies()); - const auto output_dims = trt_common_->getBindingDimensions(1); - input_d_ = autoware::cuda_utils::make_unique(batch_config[2] * input_size); + const auto output_dims = trt_common_->getTensorShape(1); + input_d_ = autoware::cuda_utils::make_unique(batch_size_ * input_size); out_elem_num_ = std::accumulate( output_dims.d + 1, output_dims.d + output_dims.nbDims, 1, std::multiplies()); - out_elem_num_ = out_elem_num_ * batch_config[2]; - out_elem_num_per_batch_ = static_cast(out_elem_num_ / batch_config[2]); + out_elem_num_ = out_elem_num_ * batch_size_; + out_elem_num_per_batch_ = static_cast(out_elem_num_ / batch_size_); out_prob_d_ = autoware::cuda_utils::make_unique(out_elem_num_); out_prob_h_ = autoware::cuda_utils::make_unique_host(out_elem_num_, cudaHostAllocPortable); @@ -224,7 +234,7 @@ void TrtClassifier::initPreprocessBuffer(int width, int height) src_width_ = width; src_height_ = height; if (m_cuda) { - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); bool const hasRuntimeDim = std::any_of( input_dims.d, input_dims.d + input_dims.nbDims, [](int32_t input_dim) { return input_dim == -1; }); @@ -232,7 +242,9 @@ void TrtClassifier::initPreprocessBuffer(int width, int height) input_dims.d[0] = batch_size_; } if (!h_img_) { - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } } if (!h_img_) { CHECK_CUDA_ERROR(cudaMallocHost( @@ -245,10 +257,15 @@ void TrtClassifier::initPreprocessBuffer(int width, int height) } } +int TrtClassifier::getBatchSize() const +{ + return batch_size_; +} + void TrtClassifier::preprocessGpu(const std::vector & images) { const auto batch_size = images.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; for (const auto & image : images) { @@ -272,7 +289,9 @@ void TrtClassifier::preprocessGpu(const std::vector & images) src_height_ = height; } if (!h_img_) { - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } } const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); @@ -305,9 +324,11 @@ void TrtClassifier::preprocessGpu(const std::vector & images) void TrtClassifier::preprocess_opt(const std::vector & images) { int batch_size = static_cast(images.size()); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } const float input_chan = static_cast(input_dims.d[1]); const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); @@ -354,9 +375,6 @@ bool TrtClassifier::doInference( const std::vector & images, std::vector & results, std::vector & probabilities) { - if (!trt_common_->isInitialized()) { - return false; - } preprocess_opt(images); return feedforwardAndDecode(images, results, probabilities); @@ -369,7 +387,10 @@ bool TrtClassifier::feedforwardAndDecode( results.clear(); probabilities.clear(); std::vector buffers = {input_d_.get(), out_prob_d_.get()}; - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); int batch_size = static_cast(images.size()); diff --git a/perception/autoware_tensorrt_common/CMakeLists.txt b/perception/autoware_tensorrt_common/CMakeLists.txt index 3105523a509e3..d7d8026866124 100644 --- a/perception/autoware_tensorrt_common/CMakeLists.txt +++ b/perception/autoware_tensorrt_common/CMakeLists.txt @@ -16,9 +16,14 @@ if(NOT (CUDAToolkit_FOUND AND CUDNN_FOUND AND TENSORRT_FOUND)) return() endif() +if(TENSORRT_VERSION VERSION_LESS 8.5) + message(WARNING "Unsupported version TensorRT ${TENSORRT_VERSION} detected. This package requires TensorRT 8.5 or later.") + return() +endif() + add_library(${PROJECT_NAME} SHARED src/tensorrt_common.cpp - src/simple_profiler.cpp + src/profiler.cpp ) target_link_libraries(${PROJECT_NAME} @@ -42,7 +47,7 @@ set_target_properties(${PROJECT_NAME} CXX_EXTENSIONS NO ) -# TODO(autoware_tensorrt_common): Remove -Wno-deprecated-declarations once upgrading to TensorRT 8.5 is complete +# TODO(amadeuszsz): Remove -Wno-deprecated-declarations once removing implicit quantization target_compile_options(${PROJECT_NAME} PRIVATE -Wall -Wextra -Wpedantic -Werror -Wno-deprecated-declarations ) diff --git a/perception/autoware_tensorrt_common/README.md b/perception/autoware_tensorrt_common/README.md index c828be58c1c1c..54bfccdfcb561 100644 --- a/perception/autoware_tensorrt_common/README.md +++ b/perception/autoware_tensorrt_common/README.md @@ -1,6 +1,70 @@ # autoware_tensorrt_common -## Purpose +This package provides a high-level API to work with TensorRT. This library simplifies the process of loading, building, and executing TensorRT inference engines using ONNX models. It also includes utilities for profiling and managing TensorRT execution contexts, making it easier to integrate TensorRT-based packages in Autoware. -This package contains a library of common functions related to TensorRT. -This package may include functions for handling TensorRT engine and calibration algorithm used for quantization +## Usage + +Here is an example usage of the library. For the full API documentation, please refer to the doxygen documentation (see header [file](include/autoware/tensorrt_common/tensorrt_common.hpp)). + +```c++ +#include + +#include +#include +#include + +using autoware::tensorrt_common::TrtCommon; +using autoware::tensorrt_common::TrtCommonConfig; +using autoware::tensorrt_common::TensorInfo; +using autoware::tensorrt_common::NetworkIO; +using autoware::tensorrt_common::ProfileDims; + +std::unique_ptr trt_common_; +``` + +### Create a tensorrt common instance and setup engine + +- With minimal configuration. + +```c++ +trt_common_ = std::make_unique(TrtCommonConfig("/path/to/onnx/model.onnx")); +trt_common_->setup(); +``` + +- With full configuration. + +```c++ +trt_common_ = std::make_unique(TrtCommonConfig("/path/to/onnx/model.onnx", "fp16", "/path/to/engine/model.engine", (1ULL << 30U), -1, false)); + +std::vector network_io{ + NetworkIO("sample_input", {3, {-1, 64, 512}}), NetworkIO("sample_output", {1, {50}})}; +std::vector profile_dims{ + ProfileDims("sample_input", {3, {1, 64, 512}}, {3, {3, 64, 512}}, {3, {9, 64, 512}})}; + +auto network_io_ptr = std::make_unique>(network_io); +auto profile_dims_ptr = std::make_unique>(profile_dims); + +trt_common_->setup(std::move(profile_dims_ptr), std::move(network_io_ptr)); +``` + +By defining network IO names and dimensions, an extra shapes validation will be performed after building / loading engine. This is useful to ensure the model is compatible with current code for preprocessing as it might consists of operations dependent on tensor shapes. + +Profile dimension is used to specify the min, opt, and max dimensions for dynamic shapes. + +Network IO or / and profile dimensions can be omitted if not needed. + +### Setting input and output tensors + +```c++ +bool success = true; +success &= trt_common_->setTensor("sample_input", sample_input_d_.get(), nvinfer1::Dims{3, {var_size, 64, 512}}); +success &= trt_common_->setTensor("sample_output", sample_output_d_.get()); +return success; +``` + +### Execute inference + +```c++ +auto success = trt_common_->enqueueV3(stream_); +return success; +``` diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/conv_profiler.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/conv_profiler.hpp new file mode 100644 index 0000000000000..0b58a797d8b9b --- /dev/null +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/conv_profiler.hpp @@ -0,0 +1,108 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TENSORRT_COMMON__CONV_PROFILER_HPP_ +#define AUTOWARE__TENSORRT_COMMON__CONV_PROFILER_HPP_ + +#include + +#include + +#include +#include +#include + +namespace autoware +{ +namespace tensorrt_common +{ +/** + * @struct ConvLayerInfo + * @brief Information of a layer. + */ +struct ConvLayerInfo +{ + //! @brief Input channel. + int in_c; + //! @brief Output channel. + int out_c; + //! @brief Width. + int w; + //! @brief Height. + int h; + //! @brief Kernel size. + int k; + //! @brief Stride. + int stride; + //! @brief Number of groups. + int groups; + //! @brief Layer type. + nvinfer1::LayerType type; +}; + +/** + * @class ConvProfiler + * @brief Collect per-layer profile information, assuming times are reported in the same order. + */ +class ConvProfiler : public tensorrt_common::Profiler +{ +public: + /** + * @brief Construct Profiler for convolutional layers. + * + * @param[in] src_profilers Source profilers to merge. + */ + explicit ConvProfiler( + const std::vector & src_profilers = std::vector()) + : Profiler(src_profilers) + { + } + + /** + * @brief Set per-layer profile information for model. + * + * @param[in] layer Layer to set profile information. + */ + void setProfDict(nvinfer1::ILayer * const layer) noexcept final + { + if (const auto type = layer->getType(); type == nvinfer1::LayerType::kCONVOLUTION) { + const auto name = layer->getName(); + auto conv = dynamic_cast(layer); + + nvinfer1::ITensor * in = layer->getInput(0); + nvinfer1::Dims in_dim = in->getDimensions(); + + nvinfer1::ITensor * out = layer->getOutput(0); + nvinfer1::Dims out_dim = out->getDimensions(); + + nvinfer1::Dims k_dims = conv->getKernelSizeNd(); + nvinfer1::Dims s_dims = conv->getStrideNd(); + + int32_t kernel = k_dims.d[0]; + int32_t stride = s_dims.d[0]; + int32_t groups = conv->getNbGroups(); + + layer_dict_.insert_or_assign( + name, ConvLayerInfo{ + in_dim.d[1], out_dim.d[1], in_dim.d[3], in_dim.d[2], kernel, stride, groups, type}); + } + } + +private: + //! @brief Per-layer information. + std::map layer_dict_; +}; +} // namespace tensorrt_common +} // namespace autoware +#endif // AUTOWARE__TENSORRT_COMMON__CONV_PROFILER_HPP_ diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/logger.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/logger.hpp index 5aa897983af72..b9b9048cd8204 100644 --- a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/logger.hpp +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/logger.hpp @@ -17,18 +17,19 @@ #ifndef AUTOWARE__TENSORRT_COMMON__LOGGER_HPP_ #define AUTOWARE__TENSORRT_COMMON__LOGGER_HPP_ -#include "NvInferRuntimeCommon.h" +#include #include #include +#include #include #include -#include #include #include #include #include #include +#include namespace autoware { @@ -46,7 +47,7 @@ class LogStreamConsumerBuffer : public std::stringbuf LogStreamConsumerBuffer(LogStreamConsumerBuffer && other) : mOutput(other.mOutput) {} - ~LogStreamConsumerBuffer() + ~LogStreamConsumerBuffer() override { // std::streambuf::pbase() gives a pointer to the beginning of the buffered part of the output // sequence std::streambuf::pptr() gives a pointer to the current position of the output @@ -60,7 +61,7 @@ class LogStreamConsumerBuffer : public std::stringbuf // synchronizes the stream buffer and returns 0 on success // synchronizing the stream buffer consists of inserting the buffer contents into the stream, // resetting the buffer and flushing the stream - virtual int sync() + int sync() override { putOutput(); return 0; @@ -252,6 +253,37 @@ class Logger : public nvinfer1::ILogger // NOLINT } } + void log(Severity severity, const char * msg, ...) const noexcept + { + if (mVerbose) { + va_list args; + va_start(args, msg); + + // Buffer size + va_list args_copy; + va_copy(args_copy, args); + int required_size = std::vsnprintf(nullptr, 0, msg, args_copy); + va_end(args_copy); + + // Formatting error + if (required_size < 0) { + log(Severity::kINTERNAL_ERROR, "Error formatting log message"); + va_end(args); + return; + } + + // Format msg + std::vector buffer(required_size + 1); + std::vsnprintf(buffer.data(), buffer.size(), msg, args); + + va_end(args); // End variadic argument processing + + // Send the formatted message to LogStreamConsumer + LogStreamConsumer(mReportableSeverity, severity) + << "[TRT] " << std::string(buffer.data()) << std::endl; + } + } + /** * @brief Logging with throttle. * diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/profiler.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/profiler.hpp new file mode 100644 index 0000000000000..d45351f97fd01 --- /dev/null +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/profiler.hpp @@ -0,0 +1,96 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TENSORRT_COMMON__PROFILER_HPP_ +#define AUTOWARE__TENSORRT_COMMON__PROFILER_HPP_ + +#include + +#include +#include +#include +#include + +namespace autoware +{ +namespace tensorrt_common +{ + +/** + * @class Profiler + * @brief Collect per-layer profile information, assuming times are reported in the same order. + */ +class Profiler : public nvinfer1::IProfiler +{ +public: + /** + * @struct Record + * @brief Record of layer profile information. + */ + struct Record + { + float time{0}; + int count{0}; + float min_time{-1.0}; + int index; + }; + + /** + * @brief Construct Profiler. + * + * @param[in] src_profilers Source profilers to merge. + */ + explicit Profiler(const std::vector & src_profilers = std::vector()); + + /** + * @brief Report layer time. + * + * @param[in] layerName Layer name. + * @param[in] ms Time in milliseconds. + */ + void reportLayerTime(const char * layerName, float ms) noexcept final; + + /** + * @brief Get printable representation of Profiler. + * + * @return String representation for current state of Profiler. + */ + [[nodiscard]] std::string toString() const; + + /** + * @brief Set per-layer profile information for model. + * + * @param[in] layer Layer to set profile information. + */ + virtual void setProfDict([[maybe_unused]] nvinfer1::ILayer * layer) noexcept {}; + + /** + * @brief Output Profiler to ostream. + * + * @param[out] out Output stream. + * @param[in] value Profiler to output. + * @return Output stream. + */ + friend std::ostream & operator<<(std::ostream & out, const Profiler & value); + +private: + //!< @brief Profile information for layers. + std::map profile_; + + //!< @brief Index for layer. + int index_; +}; +} // namespace tensorrt_common +} // namespace autoware +#endif // AUTOWARE__TENSORRT_COMMON__PROFILER_HPP_ diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/simple_profiler.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/simple_profiler.hpp deleted file mode 100644 index a8d504fb2861a..0000000000000 --- a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/simple_profiler.hpp +++ /dev/null @@ -1,73 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__TENSORRT_COMMON__SIMPLE_PROFILER_HPP_ -#define AUTOWARE__TENSORRT_COMMON__SIMPLE_PROFILER_HPP_ - -#include - -#include -#include -#include -#include - -namespace autoware -{ -namespace tensorrt_common -{ -struct LayerInfo -{ - int in_c; - int out_c; - int w; - int h; - int k; - int stride; - int groups; - nvinfer1::LayerType type; -}; - -/** - * @class Profiler - * @brief Collect per-layer profile information, assuming times are reported in the same order - */ -class SimpleProfiler : public nvinfer1::IProfiler -{ -public: - struct Record - { - float time{0}; - int count{0}; - float min_time{-1.0}; - int index; - }; - SimpleProfiler( - std::string name, - const std::vector & src_profilers = std::vector()); - - void reportLayerTime(const char * layerName, float ms) noexcept override; - - void setProfDict(nvinfer1::ILayer * layer) noexcept; - - friend std::ostream & operator<<(std::ostream & out, const SimpleProfiler & value); - -private: - std::string m_name; - std::map m_profile; - int m_index; - std::map m_layer_dict; -}; -} // namespace tensorrt_common -} // namespace autoware -#endif // AUTOWARE__TENSORRT_COMMON__SIMPLE_PROFILER_HPP_ diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_common.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_common.hpp index 2b3b3f02f315f..9355732962e23 100644 --- a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_common.hpp +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_common.hpp @@ -15,226 +15,367 @@ #ifndef AUTOWARE__TENSORRT_COMMON__TENSORRT_COMMON_HPP_ #define AUTOWARE__TENSORRT_COMMON__TENSORRT_COMMON_HPP_ -#ifndef YOLOX_STANDALONE -#include - -#include -#endif +#include "autoware/tensorrt_common/logger.hpp" +#include "autoware/tensorrt_common/profiler.hpp" +#include "autoware/tensorrt_common/utils.hpp" #include #include -#if (defined(_MSC_VER) or (defined(__GNUC__) and (7 <= __GNUC_MAJOR__))) -#include -namespace fs = ::std::filesystem; -#else -#include -namespace fs = ::std::experimental::filesystem; -#endif - -#include -#include - #include -#include #include +#include +#include #include namespace autoware { namespace tensorrt_common { -/** - * @struct BuildConfig - * @brief Configuration to provide fine control regarding TensorRT builder - */ -struct BuildConfig -{ - // type for calibration - std::string calib_type_str; - - // DLA core ID that the process uses - int dla_core_id; - - // flag for partial quantization in first layer - bool quantize_first_layer; // For partial quantization - - // flag for partial quantization in last layer - bool quantize_last_layer; // For partial quantization - - // flag for per-layer profiler using IProfiler - bool profile_per_layer; - - // clip value for implicit quantization - double clip_value; // For implicit quantization - - // Supported calibration type - const std::array valid_calib_type = {"Entropy", "Legacy", "Percentile", "MinMax"}; - - BuildConfig() - : calib_type_str("MinMax"), - dla_core_id(-1), - quantize_first_layer(false), - quantize_last_layer(false), - profile_per_layer(false), - clip_value(0.0) - { - } - - explicit BuildConfig( - const std::string & calib_type_str, const int dla_core_id = -1, - const bool quantize_first_layer = false, const bool quantize_last_layer = false, - const bool profile_per_layer = false, const double clip_value = 0.0) - : calib_type_str(calib_type_str), - dla_core_id(dla_core_id), - quantize_first_layer(quantize_first_layer), - quantize_last_layer(quantize_last_layer), - profile_per_layer(profile_per_layer), - clip_value(clip_value) - { -#ifndef YOLOX_STANDALONE - if ( - std::find(valid_calib_type.begin(), valid_calib_type.end(), calib_type_str) == - valid_calib_type.end()) { - std::stringstream message; - message << "Invalid calibration type was specified: " << calib_type_str << std::endl - << "Valid value is one of: [Entropy, (Legacy | Percentile), MinMax]" << std::endl - << "Default calibration type will be used: MinMax" << std::endl; - std::cerr << message.str(); - } -#endif - } -}; - -nvinfer1::Dims get_input_dims(const std::string & onnx_file_path); - -const std::array valid_precisions = {"fp32", "fp16", "int8"}; -bool is_valid_precision_string(const std::string & precision); - template struct InferDeleter // NOLINT { - void operator()(T * obj) const - { - if (obj) { -#if TENSORRT_VERSION_MAJOR >= 8 - delete obj; -#else - obj->destroy(); -#endif - } - } + void operator()(T * obj) const { delete obj; } }; template using TrtUniquePtr = std::unique_ptr>; -using BatchConfig = std::array; +using NetworkIOPtr = std::unique_ptr>; +using ProfileDimsPtr = std::unique_ptr>; +using TensorsVec = std::vector>; +using TensorsMap = std::unordered_map>; /** * @class TrtCommon - * @brief TensorRT common library + * @brief TensorRT common library. */ class TrtCommon // NOLINT { public: /** * @brief Construct TrtCommon. - * @param[in] mode_path ONNX model_path - * @param[in] precision precision for inference - * @param[in] calibrator pointer for any type of INT8 calibrator - * @param[in] batch_config configuration for batched execution - * @param[in] max_workspace_size maximum workspace for building TensorRT engine - * @param[in] buildConfig configuration including precision, calibration method, dla, remaining - * fp16 for first layer, remaining fp16 for last layer and profiler for builder - * @param[in] plugin_paths path for custom plugin + * + * @param[in] trt_config Base configuration with ONNX model path as minimum required. + * parameter. + * @param[in] profiler Per-layer profiler. + * @param[in] plugin_paths Paths for TensorRT plugins. */ TrtCommon( - const std::string & model_path, const std::string & precision, - std::unique_ptr calibrator = nullptr, - const BatchConfig & batch_config = {1, 1, 1}, const size_t max_workspace_size = (16 << 20), - const BuildConfig & buildConfig = BuildConfig(), + const TrtCommonConfig & trt_config, + const std::shared_ptr & profiler = std::make_shared(), const std::vector & plugin_paths = {}); - /** * @brief Deconstruct TrtCommon */ ~TrtCommon(); /** - * @brief Load TensorRT engine - * @param[in] engine_file_path path for a engine file - * @return flag for whether loading are succeeded or failed + * @brief Setup for TensorRT execution including building and loading engine. + * + * @param[in] profile_dims Optimization profile of tensors for dynamic shapes. + * @param[in] network_io Network input/output tensors information. + * @return Whether setup is successful. + */ + [[nodiscard]] virtual bool setup( + ProfileDimsPtr profile_dims = nullptr, NetworkIOPtr network_io = nullptr); + + /** + * @brief Get TensorRT engine precision. + * + * @return string representation of TensorRT engine precision. + */ + [[nodiscard]] std::string getPrecision() const; + + /** + * @brief Get tensor name by index from TensorRT engine with fallback from TensorRT network. + * + * @param[in] index Tensor index. + * @return Tensor name. + */ + [[nodiscard]] const char * getIOTensorName(const int32_t index) const; + + /** + * @brief Get number of IO tensors from TensorRT engine with fallback from TensorRT network. + * + * @return Number of IO tensors. + */ + [[nodiscard]] int32_t getNbIOTensors() const; + + /** + * @brief Get tensor shape by index from TensorRT engine with fallback from TensorRT network. + * + * @param[in] index Tensor index. + * @return Tensor shape. + */ + [[nodiscard]] nvinfer1::Dims getTensorShape(const int32_t index) const; + + /** + * @brief Get tensor shape by name from TensorRT engine. + * + * @param[in] tensor_name Tensor name. + * @return Tensor shape. + */ + [[nodiscard]] nvinfer1::Dims getTensorShape(const char * tensor_name) const; + + /** + * @brief Get input tensor shape by index from TensorRT network. + * + * @param[in] index Tensor index. + * @return Tensor shape. + */ + [[nodiscard]] nvinfer1::Dims getInputDims(const int32_t index) const; + + /** + * @brief Get output tensor shape by index from TensorRT network. + * + * @param[in] index Tensor index. + * @return Tensor shape. + */ + [[nodiscard]] nvinfer1::Dims getOutputDims(const int32_t index) const; + + /** + * @brief Set tensor address by index via TensorRT context. + * + * @param[in] index Tensor index. + * @param[in] data Tensor pointer. + * @return Whether setting tensor address is successful. + */ + bool setTensorAddress(const int32_t index, void * data); + + /** + * @brief Set tensor address by name via TensorRT context. + * + * @param[in] tensor_name Tensor name. + * @param[in] data Tensor pointer. + * @return Whether setting tensor address is successful. + */ + bool setTensorAddress(const char * tensor_name, void * data); + + /** + * @brief Set tensors addresses by indices via TensorRT context. + * + * @param[in] tensors Tensors pointers. + * @return Whether setting tensors addresses is successful. + */ + bool setTensorsAddresses(std::vector & tensors); + + /** + * @brief Set tensors addresses by names via TensorRT context. + * + * @param[in] tensors Tensors pointers. + * @return Whether setting tensors addresses is successful. + */ + bool setTensorsAddresses(std::unordered_map & tensors); + + /** + * @brief Set input shape by index via TensorRT context. + * + * @param[in] index Tensor index. + * @param[in] dimensions Tensor dimensions. + * @return Whether setting input shape is successful. + */ + bool setInputShape(const int32_t index, const nvinfer1::Dims & dimensions); + + /** + * @brief Set input shape by name via TensorRT context. + * + * @param[in] tensor_name Tensor name. + * @param[in] dimensions Tensor dimensions. + * @return Whether setting input shape is successful. + */ + bool setInputShape(const char * tensor_name, const nvinfer1::Dims & dimensions); + + /** + * @brief Set inputs shapes by indices via TensorRT context. + * + * @param[in] dimensions Vector of tensor dimensions with corresponding indices. + * @return Whether setting input shapes is successful. + */ + bool setInputsShapes(const std::vector & dimensions); + + /** + * @brief Set inputs shapes by names via TensorRT context. + * + * @param[in] dimensions Map of tensor dimensions with corresponding names as keys. + * @return Whether setting input shapes is successful. + */ + bool setInputsShapes(const std::unordered_map & dimensions); + + /** + * @brief Set tensor (address and shape) by index via TensorRT context. + * + * @param[in] index Tensor index. + * @param[in] data Tensor pointer. + * @param[in] dimensions Tensor dimensions. + * @return Whether setting tensor is successful. + */ + bool setTensor(const int32_t index, void * data, nvinfer1::Dims dimensions = {}); + + /** + * @brief Set tensor (address and shape) by name via TensorRT context. + * + * @param[in] tensor_name Tensor name. + * @param[in] data Tensor pointer. + * @param[in] dimensions Tensor dimensions. + * @return Whether setting tensor is successful. + */ + bool setTensor(const char * tensor_name, void * data, nvinfer1::Dims dimensions = {}); + + /** + * @brief Set tensors (addresses and shapes) by indices via TensorRT context. + * + * @param[in] tensors Vector of tensor pointers and dimensions with corresponding indices. + * @return Whether setting tensors is successful. */ - bool loadEngine(const std::string & engine_file_path); + bool setTensors(TensorsVec & tensors); /** - * @brief Output layer information including GFLOPs and parameters - * @param[in] onnx_file_path path for a onnx file - * @warning This function is based on darknet log. + * @brief Set tensors (addresses and shapes) by names via TensorRT context. + * + * @param[in] tensors Map of tensor pointers and dimensions with corresponding names as keys. + * @return Whether setting tensors is successful. */ - void printNetworkInfo(const std::string & onnx_file_path); + bool setTensors(TensorsMap & tensors); /** - * @brief build TensorRT engine from ONNX - * @param[in] onnx_file_path path for a onnx file - * @param[in] output_engine_file_path path for a engine file + * @brief Get per-layer profiler for model. + * + * @return Per-layer profiler. */ - bool buildEngineFromOnnx( - const std::string & onnx_file_path, const std::string & output_engine_file_path); + [[nodiscard]] std::shared_ptr getModelProfiler(); /** - * @brief setup for TensorRT execution including building and loading engine + * @brief Get per-layer profiler for host. + * + * @return Per-layer profiler. */ - void setup(); + [[nodiscard]] std::shared_ptr getHostProfiler(); -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8500 - void setupBindings(std::vector & bindings); -#endif + /** + * @brief Get TensorRT common configuration. + * + * @return TensorRT common configuration. + */ + [[nodiscard]] std::shared_ptr getTrtCommonConfig(); - bool isInitialized(); + /** + * @brief Get TensorRT builder configuration. + * + * @return TensorRT builder configuration. + */ + [[nodiscard]] std::shared_ptr getBuilderConfig(); + + /** + * @brief Get TensorRT network definition. + * + * @return TensorRT network definition. + */ + [[nodiscard]] std::shared_ptr getNetwork(); - nvinfer1::Dims getBindingDimensions(const int32_t index) const; - int32_t getNbBindings(); - bool setBindingDimensions(const int32_t index, const nvinfer1::Dims & dimensions) const; -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8500 + /** + * @brief Get TensorRT logger. + * + * @return TensorRT logger. + */ + [[nodiscard]] std::shared_ptr getLogger(); + + /** + * @brief Execute inference via TensorRT context. + * + * @param[in] stream CUDA stream. + * @return Whether inference is successful. + */ bool enqueueV3(cudaStream_t stream); -#endif -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH < 10000 - bool enqueueV2(void ** bindings, cudaStream_t stream, cudaEvent_t * input_consumed); -#endif /** - * @brief output per-layer information + * @brief Print per-layer information. + */ + void printProfiling() const; + +private: + /** + * @brief Initialize TensorRT common. + * + * @return Whether initialization is successful. */ - void printProfiling(void); + [[nodiscard]] bool initialize(); -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 /** - * @brief get per-layer information for trt-engine-profiler + * @brief Get per-layer information for trt-engine-profiler. + * + * @param[in] format Format for layer information. + * @return Layer information. */ std::string getLayerInformation(nvinfer1::LayerInformationFormat format); -#endif -private: - Logger logger_; - fs::path model_file_path_; + /** + + * @brief Build TensorRT engine from ONNX. + * + * @return Whether building engine is successful. + */ + bool buildEngineFromOnnx(); + + /** + * @brief Load TensorRT engine. + * + * @return Whether loading engine is successful. + */ + bool loadEngine(); + + /** + * @brief Validate network input/output names and dimensions. + * + * @return Whether network input/output is valid. + */ + bool validateNetworkIO(); + + /** + * @brief Validate optimization profile. + * + * @return Whether optimization profile is valid. + */ + bool validateProfileDims(); + + //! @brief TensorRT runtime. TrtUniquePtr runtime_; + + //! @brief TensorRT engine. TrtUniquePtr engine_; + + //! @brief TensorRT execution context. TrtUniquePtr context_; - std::unique_ptr calibrator_; - std::string precision_; - BatchConfig batch_config_; - size_t max_workspace_size_; - bool is_initialized_{false}; + //! @brief TensorRT builder. + TrtUniquePtr builder_; + + //! @brief TensorRT engine parser. + TrtUniquePtr parser_; + + //! @brief TensorRT builder configuration. + std::shared_ptr builder_config_; + + //! @brief TensorRT network definition. + std::shared_ptr network_; + + //! @brief TrtCommon library base configuration. + std::shared_ptr trt_config_; + + //! @brief TensorRT logger. + std::shared_ptr logger_; + + //! @brief Per-layer profiler for host. + std::shared_ptr host_profiler_; + + //! @brief Per-layer profiler for model. + std::shared_ptr model_profiler_; - // profiler for per-layer - SimpleProfiler model_profiler_; - // profiler for whole model - SimpleProfiler host_profiler_; + //! @brief Model optimization profile. + ProfileDimsPtr profile_dims_; - std::unique_ptr build_config_; + //! @brief Model network input/output tensors information. + NetworkIOPtr network_io_; }; } // namespace tensorrt_common diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_conv_calib.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_conv_calib.hpp new file mode 100644 index 0000000000000..44a944031086e --- /dev/null +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_conv_calib.hpp @@ -0,0 +1,241 @@ +// 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__TENSORRT_COMMON__TENSORRT_CONV_CALIB_HPP_ +#define AUTOWARE__TENSORRT_COMMON__TENSORRT_CONV_CALIB_HPP_ + +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace autoware +{ +namespace tensorrt_common +{ +template +bool contain(const std::string & s, const T & v) +{ + return s.find(v) != std::string::npos; +} + +using autoware::tensorrt_common::CalibrationConfig; +using autoware::tensorrt_common::NetworkIOPtr; +using autoware::tensorrt_common::ProfileDimsPtr; +using autoware::tensorrt_common::Profiler; +using autoware::tensorrt_common::TrtCommonConfig; + +/** + * @class TrtConvCalib + * @brief TensorRT common library with calibration. + */ +class TrtConvCalib : public tensorrt_common::TrtCommon +{ +public: + /** + * @brief Construct TrtCommonCalib. + * + * @param[in] trt_config base trt common configuration, ONNX model path is mandatory + * @param[in] profiler per-layer profiler + * @param[in] plugin_paths paths for TensorRT plugins + */ + explicit TrtConvCalib( + const TrtCommonConfig & trt_config, + const std::shared_ptr & profiler = std::make_shared(), + const std::vector & plugin_paths = {}) + : TrtCommon(trt_config, profiler, plugin_paths) + { + } + + /** + * @brief Setup for TensorRT execution including calibration, building and loading engine. + * + * @param[in] calibrator Pointer for any type of INT8 calibrator. + * @param[in] calib_config Calibration configuration. + * @param[in] profile_dims Optimization profile of input tensors for dynamic shapes. + * @param[in] network_io Network input/output tensors information. + * @return Whether setup is successful. + */ + [[nodiscard]] bool setupWithCalibrator( + std::unique_ptr calibrator, const CalibrationConfig & calib_config, + ProfileDimsPtr profile_dims = nullptr, NetworkIOPtr network_io = nullptr) + { + calibrator_ = std::move(calibrator); + calib_config_ = std::make_unique(calib_config); + + auto builder_config = getBuilderConfig(); + builder_config->setFlag(nvinfer1::BuilderFlag::kPREFER_PRECISION_CONSTRAINTS); + builder_config->setInt8Calibrator(calibrator_.get()); + + // Model specific quantization + auto logger = getLogger(); + auto quantization_log = quantization(); + logger->log(nvinfer1::ILogger::Severity::kINFO, quantization_log.c_str()); + + return setup(std::move(profile_dims), std::move(network_io)); + } + +private: + /** + * @brief Implicit quantization for TensorRT. + * + * @return Output log for TensorRT logger. + */ + std::string quantization() + { + auto network = getNetwork(); + auto trt_config = getTrtCommonConfig(); + auto model_profiler = getModelProfiler(); + + const int num = network->getNbLayers(); + bool first = calib_config_->quantize_first_layer; + bool last = calib_config_->quantize_last_layer; + std::stringstream ss; + + // Partial Quantization + if (getPrecision() == "int8") { + auto builder_config = getBuilderConfig(); + builder_config->setFlag(nvinfer1::BuilderFlag::kFP16); + network->getInput(0)->setDynamicRange(0, 255.0); + for (int i = 0; i < num; i++) { + nvinfer1::ILayer * layer = network->getLayer(i); + auto layer_type = layer->getType(); + std::string name = layer->getName(); + nvinfer1::ITensor * out = layer->getOutput(0); + if (calib_config_->clip_value > 0.0) { + ss << "Set max value for outputs: " << calib_config_->clip_value << " " << name + << std::endl; + out->setDynamicRange(0.0, calib_config_->clip_value); + } + + if (layer_type == nvinfer1::LayerType::kCONVOLUTION) { + if (first) { + layer->setPrecision(nvinfer1::DataType::kHALF); + ss << "Set kHALF in " << name << std::endl; + first = false; + } + if (last) { + // cspell: ignore preds + if ( + contain(name, "reg_preds") || contain(name, "cls_preds") || + contain(name, "obj_preds")) { + layer->setPrecision(nvinfer1::DataType::kHALF); + ss << "Set kHALF in " << name << std::endl; + } + for (int j = num - 1; j >= 0; j--) { + nvinfer1::ILayer * inner_layer = network->getLayer(j); + auto inner_layer_type = inner_layer->getType(); + std::string inner_name = inner_layer->getName(); + if (inner_layer_type == nvinfer1::LayerType::kCONVOLUTION) { + inner_layer->setPrecision(nvinfer1::DataType::kHALF); + ss << "Set kHALF in " << inner_name << std::endl; + break; + } + if (inner_layer_type == nvinfer1::LayerType::kMATRIX_MULTIPLY) { + inner_layer->setPrecision(nvinfer1::DataType::kHALF); + ss << "Set kHALF in " << inner_name << std::endl; + break; + } + } + } + } + } + builder_config->setFlag(nvinfer1::BuilderFlag::kINT8); + } + + // Print layer information + float total_gflops = 0.0; + int total_params = 0; + + for (int i = 0; i < num; i++) { + nvinfer1::ILayer * layer = network->getLayer(i); + auto layer_type = layer->getType(); + if (trt_config->profile_per_layer) { + model_profiler->setProfDict(layer); + } + if (layer_type == nvinfer1::LayerType::kCONSTANT) { + continue; + } + nvinfer1::ITensor * in = layer->getInput(0); + nvinfer1::Dims dim_in = in->getDimensions(); + nvinfer1::ITensor * out = layer->getOutput(0); + nvinfer1::Dims dim_out = out->getDimensions(); + + if (layer_type == nvinfer1::LayerType::kCONVOLUTION) { + auto conv = dynamic_cast(layer); + nvinfer1::Dims k_dims = conv->getKernelSizeNd(); + nvinfer1::Dims s_dims = conv->getStrideNd(); + int groups = conv->getNbGroups(); + int stride = s_dims.d[0]; + int num_weights = (dim_in.d[1] / groups) * dim_out.d[1] * k_dims.d[0] * k_dims.d[1]; + float gflops = (2.0 * num_weights) * (static_cast(dim_in.d[3]) / stride * + static_cast(dim_in.d[2]) / stride / 1e9); + total_gflops += gflops; + total_params += num_weights; + ss << "L" << i << " [conv " << k_dims.d[0] << "x" << k_dims.d[1] << " (" << groups << ") " + << ") " << "/" << s_dims.d[0] << "] " << dim_in.d[3] << "x" << dim_in.d[2] << "x" + << " -> " << dim_out.d[3] << "x" << dim_out.d[2] << "x" << dim_out.d[1]; + ss << " weights:" << num_weights; + ss << " GFLOPs:" << gflops; + ss << std::endl; + } else if (layer_type == nvinfer1::LayerType::kPOOLING) { + auto pool = dynamic_cast(layer); + auto p_type = pool->getPoolingType(); + nvinfer1::Dims dim_stride = pool->getStrideNd(); + nvinfer1::Dims dim_window = pool->getWindowSizeNd(); + + ss << "L" << i << " ["; + if (p_type == nvinfer1::PoolingType::kMAX) { + ss << "max "; + } else if (p_type == nvinfer1::PoolingType::kAVERAGE) { + ss << "avg "; + } else if (p_type == nvinfer1::PoolingType::kMAX_AVERAGE_BLEND) { + ss << "max avg blend "; + } + float gflops = static_cast(dim_in.d[1]) * + (static_cast(dim_window.d[0]) / static_cast(dim_stride.d[0])) * + (static_cast(dim_window.d[1]) / static_cast(dim_stride.d[1])) * + static_cast(dim_in.d[2]) * static_cast(dim_in.d[3]) / 1e9; + total_gflops += gflops; + ss << "pool " << dim_window.d[0] << "x" << dim_window.d[1] << "]"; + ss << " GFLOPs:" << gflops; + ss << std::endl; + } else if (layer_type == nvinfer1::LayerType::kRESIZE) { + ss << "L" << i << " [resize]" << std::endl; + } + } + ss << "Total " << total_gflops << " GFLOPs" << std::endl; + ss << "Total " << total_params / 1000.0 / 1000.0 << " M params" << std::endl; + + return ss.str(); + }; + +private: + //!< Calibration configuration + std::unique_ptr calib_config_; + + //!< Calibrator used for implicit quantization + std::unique_ptr calibrator_; +}; + +} // namespace tensorrt_common +} // namespace autoware + +#endif // AUTOWARE__TENSORRT_COMMON__TENSORRT_CONV_CALIB_HPP_ diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/utils.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/utils.hpp new file mode 100644 index 0000000000000..be640f52147d9 --- /dev/null +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/utils.hpp @@ -0,0 +1,408 @@ +// 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__TENSORRT_COMMON__UTILS_HPP_ +#define AUTOWARE__TENSORRT_COMMON__UTILS_HPP_ + +#include +#include + +#if (defined(_MSC_VER) or (defined(__GNUC__) and (7 <= __GNUC_MAJOR__))) +#include +namespace fs = ::std::filesystem; +#else +#include +namespace fs = ::std::experimental::filesystem; +#endif + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace tensorrt_common +{ +constexpr std::array valid_precisions = {"fp32", "fp16", "int8"}; + +/** + * @struct TrtCommonConfig + * @brief Configuration to provide fine control regarding TensorRT builder + */ +struct TrtCommonConfig +{ + /** @brief Construct TrtCommonConfig, ONNX model path is mandatory. + * + * @param[in] onnx_path ONNX model path + * @param[in] precision precision for inference + * @param[in] engine_path TensorRT engine path + * @param[in] max_workspace_size maximum workspace size for TensorRT + * @param[in] dla_core_id DLA core ID + * @param[in] profile_per_layer flag for per-layer profiler using IProfiler + */ + explicit TrtCommonConfig( + const std::string onnx_path, const std::string precision = "fp16", + const std::string engine_path = "", const size_t max_workspace_size = (1ULL << 30U), + const int32_t dla_core_id = -1, const bool profile_per_layer = false) + : onnx_path(onnx_path), + precision(precision), + engine_path(engine_path), + max_workspace_size(max_workspace_size), + dla_core_id(dla_core_id), + profile_per_layer(profile_per_layer) + { + validatePrecision(); + + if (engine_path.empty()) { + this->engine_path = onnx_path; + this->engine_path.replace_extension(".engine"); + } + } + + /** + * @brief Validate configured TensorRT engine precision. + */ + void validatePrecision() const + { + if ( + std::find(valid_precisions.begin(), valid_precisions.end(), precision) == + valid_precisions.end()) { + std::stringstream message; + message << "Invalid precision was specified: " << precision << std::endl + << "Valid string is one of: [" << valid_precisions[0]; + for (size_t i = 1; i < valid_precisions.size(); ++i) { + message << ", " << valid_precisions[i]; + } + message << "] (case sensitive)" << std::endl; + + throw std::runtime_error(message.str()); + } + } + + //!< @brief ONNX model path. + const fs::path onnx_path; + + //!< @brief Precision for inference. + const std::string precision; + + //!< @brief TensorRT engine path. + fs::path engine_path; + + //!< @brief TensorRT max workspace size. + const size_t max_workspace_size; + + //!< @brief DLA core ID that the process uses. + const int32_t dla_core_id; + + //!< @brief Flag for per-layer profiler using IProfiler. + const bool profile_per_layer; +}; + +/** + * @brief Represents a tensor with its name or index. + */ +struct TensorInfo +{ + /** + * @brief Construct TensorInfo with tensor name. + * + * @param[in] name Tensor name. + */ + explicit TensorInfo(std::string name) : tensor_name(std::move(name)), tensor_index(-1) {} + + /** + * @brief Construct TensorInfo with tensor index. + * + * @param[in] index Tensor index. + */ + explicit TensorInfo(int32_t index) : tensor_index(index) {} + + /** + * @brief Check if dimensions are equal. + * + * @param lhs Left-hand side tensor dimensions. + * @param rhs Right-hand side tensor dimensions. + * @return Whether dimensions are equal. + */ + static bool dimsEqual(const nvinfer1::Dims & lhs, const nvinfer1::Dims & rhs) + { + if (lhs.nbDims != rhs.nbDims) return false; + return std::equal(lhs.d, lhs.d + lhs.nbDims, rhs.d); // NOLINT + } + + /** + * @brief Get printable representation of tensor dimensions. + * + * @param[in] dims Tensor dimensions. + * @return String representation of tensor dimensions. + */ + static std::string dimsRepr(const nvinfer1::Dims & dims) + { + if (dims.nbDims == 0) return "[]"; + std::string repr = "[" + std::to_string(dims.d[0]); + for (int i = 1; i < dims.nbDims; ++i) { + repr += ", " + std::to_string(dims.d[i]); + } + repr += "]"; + return repr; + } + + //!< @brief Tensor name. + std::string tensor_name; + + //!< @brief Tensor index. + int32_t tensor_index; +}; + +/** + * @brief Represents a network input/output tensor with its dimensions. + * + * Example usage: + * @code + * nvinfer1::Dims tensor_dims = {3, {1, 2, 3}}; + * NetworkIO input("input_tensor", tensor_dims); + * NetworkIO output("output_tensor", tensor_dims); + * bool is_equal = input == output; // false + * @endcode + */ +struct NetworkIO : public TensorInfo +{ + /** + * @brief Construct NetworkIO with tensor name and dimensions. + * + * @param[in] name Tensor name. + * @param[in] tensor_dims Tensor dimensions. + */ + NetworkIO(std::string name, const nvinfer1::Dims & tensor_dims) + : TensorInfo(std::move(name)), dims(tensor_dims) + { + } + + /** + * @brief Construct NetworkIO with tensor index and dimensions. + * + * @param[in] index Tensor index. + * @param[in] tensor_dims Tensor dimensions. + */ + NetworkIO(int32_t index, const nvinfer1::Dims & tensor_dims) + : TensorInfo(index), dims(tensor_dims) + { + } + + /** + * @brief Get printable representation of NetworkIO. + * + * @return String representation of NetworkIO. + */ + [[nodiscard]] std::string toString() const + { + std::stringstream ss; + ss << tensor_name << " {" << TensorInfo::dimsRepr(dims) << "}"; + return ss.str(); + } + + /** + * @brief Check if NetworkIO is equal to another NetworkIO. + * + * @param rhs Right-hand side NetworkIO. + * @return Whether NetworkIO is equal to another NetworkIO. + */ + bool operator==(const NetworkIO & rhs) const + { + if (tensor_name != rhs.tensor_name) return false; + return dimsEqual(dims, rhs.dims); + } + + /** + * @brief Check if NetworkIO is not equal to another NetworkIO. + * + * @param rhs Right-hand side NetworkIO. + * @return Whether NetworkIO is not equal to another NetworkIO. + */ + bool operator!=(const NetworkIO & rhs) const { return !(*this == rhs); } + + /** + * @brief Output NetworkIO to ostream. + * + * @param os Output stream. + * @param io NetworkIO. + * @return Output stream. + */ + friend std::ostream & operator<<(std::ostream & os, const NetworkIO & io) + { + os << io.toString(); + return os; + } + + //!< @brief Tensor dimensions. + nvinfer1::Dims dims; +}; + +/** + * @brief Represents a tensor optimization profile with minimum, optimal, and maximum dimensions. + * + * Example usage: + * @code + * nvinfer1::Dims min = {3, {1, 2, 3}}; + * nvinfer1::Dims opt = {3, {1, 3, 4}}; + * nvinfer1::Dims max = {3, {1, 4, 5}}; + * ProfileDims entry_1("tensor_name", min, opt, max); + * ProfileDims entry_2("tensor_name", {3, {1, 2, 3}}, {3, {1, 3, 4}}, {3, {1, 4, 5}}); + * bool is_equal = entry_1 == entry_2; // true + * @endcode + */ +struct ProfileDims : public TensorInfo +{ + /** + * @brief Construct ProfileDims with tensor name and dimensions. + * + * @param[in] name Tensor name. + * @param[in] min Minimum dimensions for optimization profile. + * @param[in] opt Optimal dimensions for optimization profile. + * @param[in] max Maximum dimensions for optimization profile. + */ + ProfileDims( + std::string name, const nvinfer1::Dims & min, const nvinfer1::Dims & opt, + const nvinfer1::Dims & max) + : TensorInfo(std::move(name)), min_dims(min), opt_dims(opt), max_dims(max) + { + } + + /** + * @brief Construct ProfileDims with tensor index and dimensions. + * + * @param[in] index Tensor index. + * @param[in] min Minimum dimensions for optimization profile. + * @param[in] opt Optimal dimensions for optimization profile. + * @param[in] max Maximum dimensions for optimization profile. + */ + ProfileDims( + int32_t index, const nvinfer1::Dims & min, const nvinfer1::Dims & opt, + const nvinfer1::Dims & max) + : TensorInfo(index), min_dims(min), opt_dims(opt), max_dims(max) + { + } + + /** + * @brief Get printable representation of ProfileDims. + * + * @return String representation of ProfileDims. + */ + [[nodiscard]] std::string toString() const + { + std::ostringstream oss; + oss << tensor_name << " {min " << TensorInfo::dimsRepr(min_dims) << ", opt " + << TensorInfo::dimsRepr(opt_dims) << ", max " << TensorInfo::dimsRepr(max_dims) << "}"; + return oss.str(); + } + + /** + * @brief Check if ProfileDims is equal to another ProfileDims. + * + * @param rhs Right-hand side ProfileDims. + * @return Whether ProfileDims is equal to another ProfileDims. + */ + bool operator==(const ProfileDims & rhs) const + { + if (tensor_name != rhs.tensor_name) return false; + return dimsEqual(min_dims, rhs.min_dims) && dimsEqual(opt_dims, rhs.opt_dims) && + dimsEqual(max_dims, rhs.max_dims); + } + + /** + * @brief Check if ProfileDims is not equal to another ProfileDims. + * + * @param rhs Right-hand side ProfileDims. + * @return Whether ProfileDims is not equal to another ProfileDims. + */ + bool operator!=(const ProfileDims & rhs) const { return !(*this == rhs); } + + /** + * @brief Output ProfileDims to ostream. + * + * @param os Output stream. + * @param profile ProfileDims. + * @return Output stream. + */ + friend std::ostream & operator<<(std::ostream & os, const ProfileDims & profile) + { + os << profile.toString(); + return os; + } + + //!< @brief Minimum dimensions for optimization profile. + nvinfer1::Dims min_dims; + + //!< @brief Optimal dimensions for optimization profile. + nvinfer1::Dims opt_dims; + + //!< @brief Maximum dimensions for optimization profile. + nvinfer1::Dims max_dims; +}; + +//!< @brief Valid calibration types for TensorRT. +constexpr std::array valid_calib_type = { + "Entropy", "Legacy", "Percentile", "MinMax"}; + +/** + * @brief Configuration for implicit calibration. + */ +struct CalibrationConfig +{ + /** + * @brief Construct CalibrationConfig with its parameters. + * + * @param[in] calib_type_str Calibration type. + * @param[in] quantize_first_layer Flag for partial quantization in first layer. + * @param[in] quantize_last_layer Flag for partial quantization in last layer. + * @param[in] clip_value Clip value for implicit quantization. + */ + explicit CalibrationConfig( + const std::string & calib_type_str = "MinMax", const bool quantize_first_layer = false, + const bool quantize_last_layer = false, const double clip_value = 0.0) + : calib_type_str(calib_type_str), + quantize_first_layer(quantize_first_layer), + quantize_last_layer(quantize_last_layer), + clip_value(clip_value) + { + if ( + std::find(valid_calib_type.begin(), valid_calib_type.end(), calib_type_str) == + valid_calib_type.end()) { + throw std::runtime_error( + "Invalid calibration type was specified: " + std::string(calib_type_str) + + "\nValid value is one of: [Entropy, (Legacy | Percentile), MinMax]"); + } + } + + //!< @brief type of calibration + const std::string calib_type_str; + + //!< @brief flag for partial quantization in first layer + const bool quantize_first_layer; + + //!< @brief flag for partial quantization in last layer + const bool quantize_last_layer; + + //!< @brief clip value for implicit quantization + const double clip_value; +}; + +} // namespace tensorrt_common +} // namespace autoware + +#endif // AUTOWARE__TENSORRT_COMMON__UTILS_HPP_ diff --git a/perception/autoware_tensorrt_common/src/profiler.cpp b/perception/autoware_tensorrt_common/src/profiler.cpp new file mode 100644 index 0000000000000..433cca51f2b4f --- /dev/null +++ b/perception/autoware_tensorrt_common/src/profiler.cpp @@ -0,0 +1,113 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +namespace autoware +{ +namespace tensorrt_common +{ + +Profiler::Profiler(const std::vector & src_profilers) +{ + index_ = 0; + for (const auto & src_profiler : src_profilers) { + for (const auto & [name, record] : src_profiler.profile_) { + auto it = profile_.find(name); + if (it == profile_.end()) { + profile_.emplace(name, record); + } else { + it->second.time += record.time; + it->second.count += record.count; + } + } + } +} + +void Profiler::reportLayerTime(const char * layerName, float ms) noexcept +{ + if (profile_.find(layerName) == profile_.end()) { + return; + } + profile_[layerName].count++; + profile_[layerName].time += ms; + if (profile_[layerName].min_time == -1.0) { + profile_[layerName].min_time = ms; + profile_[layerName].index = index_; + index_++; + } else if (profile_[layerName].min_time > ms) { + profile_[layerName].min_time = ms; + } +} + +std::string Profiler::toString() const +{ + std::ostringstream out; + float total_time = 0.0; + std::string layer_name = "Operation"; + + int max_layer_name_length = static_cast(layer_name.size()); + for (const auto & elem : profile_) { + total_time += elem.second.time; + max_layer_name_length = std::max(max_layer_name_length, static_cast(elem.first.size())); + } + + auto old_settings = out.flags(); + auto old_precision = out.precision(); + // Output header + { + out << "index, " << std::setw(12); + out << std::setw(max_layer_name_length) << layer_name << " "; + out << std::setw(12) << "Runtime" << "%," << " "; + out << std::setw(12) << "Invocations" << " , "; + out << std::setw(12) << "Runtime[ms]" << " , "; + out << std::setw(12) << "Avg Runtime[ms]" << " ,"; + out << std::setw(12) << "Min Runtime[ms]" << std::endl; + } + int index = index_; + for (int i = 0; i < index; i++) { + for (const auto & elem : profile_) { + if (elem.second.index == i) { + out << i << ", "; + out << std::setw(max_layer_name_length) << elem.first << ","; + out << std::setw(12) << std::fixed << std::setprecision(1) + << (elem.second.time * 100.0F / total_time) << "%" << ","; + out << std::setw(12) << elem.second.count << ","; + out << std::setw(12) << std::fixed << std::setprecision(2) << elem.second.time << ", "; + out << std::setw(12) << std::fixed << std::setprecision(2) + << elem.second.time / elem.second.count << ", "; + out << std::setw(12) << std::fixed << std::setprecision(2) << elem.second.min_time + << std::endl; + } + } + } + out.flags(old_settings); + out.precision(old_precision); + out << "========== total runtime = " << total_time << " ms ==========" << std::endl; + + return out.str(); +} + +std::ostream & operator<<(std::ostream & out, const Profiler & value) +{ + out << value.toString(); + return out; +} +} // namespace tensorrt_common +} // namespace autoware diff --git a/perception/autoware_tensorrt_common/src/simple_profiler.cpp b/perception/autoware_tensorrt_common/src/simple_profiler.cpp deleted file mode 100644 index 2bcc1c4f9da06..0000000000000 --- a/perception/autoware_tensorrt_common/src/simple_profiler.cpp +++ /dev/null @@ -1,138 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include -#include -#include - -namespace autoware -{ -namespace tensorrt_common -{ - -SimpleProfiler::SimpleProfiler(std::string name, const std::vector & src_profilers) -: m_name(name) -{ - m_index = 0; - for (const auto & src_profiler : src_profilers) { - for (const auto & rec : src_profiler.m_profile) { - auto it = m_profile.find(rec.first); - if (it == m_profile.end()) { - m_profile.insert(rec); - } else { - it->second.time += rec.second.time; - it->second.count += rec.second.count; - } - } - } -} - -void SimpleProfiler::reportLayerTime(const char * layerName, float ms) noexcept -{ - m_profile[layerName].count++; - m_profile[layerName].time += ms; - if (m_profile[layerName].min_time == -1.0) { - m_profile[layerName].min_time = ms; - m_profile[layerName].index = m_index; - m_index++; - } else if (m_profile[layerName].min_time > ms) { - m_profile[layerName].min_time = ms; - } -} - -void SimpleProfiler::setProfDict(nvinfer1::ILayer * layer) noexcept -{ - std::string name = layer->getName(); - m_layer_dict[name]; - m_layer_dict[name].type = layer->getType(); - if (layer->getType() == nvinfer1::LayerType::kCONVOLUTION) { - nvinfer1::IConvolutionLayer * conv = (nvinfer1::IConvolutionLayer *)layer; - nvinfer1::ITensor * in = layer->getInput(0); - nvinfer1::Dims dim_in = in->getDimensions(); - nvinfer1::ITensor * out = layer->getOutput(0); - nvinfer1::Dims dim_out = out->getDimensions(); - nvinfer1::Dims k_dims = conv->getKernelSizeNd(); - nvinfer1::Dims s_dims = conv->getStrideNd(); - int groups = conv->getNbGroups(); - int stride = s_dims.d[0]; - int kernel = k_dims.d[0]; - m_layer_dict[name].in_c = dim_in.d[1]; - m_layer_dict[name].out_c = dim_out.d[1]; - m_layer_dict[name].w = dim_in.d[3]; - m_layer_dict[name].h = dim_in.d[2]; - m_layer_dict[name].k = kernel; - ; - m_layer_dict[name].stride = stride; - m_layer_dict[name].groups = groups; - } -} - -std::ostream & operator<<(std::ostream & out, const SimpleProfiler & value) -{ - out << "========== " << value.m_name << " profile ==========" << std::endl; - float totalTime = 0; - std::string layerNameStr = "Operation"; - - int maxLayerNameLength = static_cast(layerNameStr.size()); - for (const auto & elem : value.m_profile) { - totalTime += elem.second.time; - maxLayerNameLength = std::max(maxLayerNameLength, static_cast(elem.first.size())); - } - - auto old_settings = out.flags(); - auto old_precision = out.precision(); - // Output header - { - out << "index, " << std::setw(12); - out << std::setw(maxLayerNameLength) << layerNameStr << " "; - out << std::setw(12) << "Runtime" - << "%," - << " "; - out << std::setw(12) << "Invocations" - << " , "; - out << std::setw(12) << "Runtime[ms]" - << " , "; - out << std::setw(12) << "Avg Runtime[ms]" - << " ,"; - out << std::setw(12) << "Min Runtime[ms]" << std::endl; - } - int index = value.m_index; - for (int i = 0; i < index; i++) { - for (const auto & elem : value.m_profile) { - if (elem.second.index == i) { - out << i << ", "; - out << std::setw(maxLayerNameLength) << elem.first << ","; - out << std::setw(12) << std::fixed << std::setprecision(1) - << (elem.second.time * 100.0F / totalTime) << "%" - << ","; - out << std::setw(12) << elem.second.count << ","; - out << std::setw(12) << std::fixed << std::setprecision(2) << elem.second.time << ", "; - out << std::setw(12) << std::fixed << std::setprecision(2) - << elem.second.time / elem.second.count << ", "; - out << std::setw(12) << std::fixed << std::setprecision(2) << elem.second.min_time - << std::endl; - } - } - } - out.flags(old_settings); - out.precision(old_precision); - out << "========== " << value.m_name << " total runtime = " << totalTime - << " ms ==========" << std::endl; - return out; -} -} // namespace tensorrt_common -} // namespace autoware diff --git a/perception/autoware_tensorrt_common/src/tensorrt_common.cpp b/perception/autoware_tensorrt_common/src/tensorrt_common.cpp index 990433ee277a0..ba422277416ab 100644 --- a/perception/autoware_tensorrt_common/src/tensorrt_common.cpp +++ b/perception/autoware_tensorrt_common/src/tensorrt_common.cpp @@ -12,102 +12,38 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "autoware/tensorrt_common/tensorrt_common.hpp" +#include "autoware/tensorrt_common/logger.hpp" +#include "autoware/tensorrt_common/utils.hpp" + +#include #include +#include #include +#include #include -#include #include #include #include +#include #include #include -namespace -{ -template -bool contain(const std::string & s, const T & v) -{ - return s.find(v) != std::string::npos; -} -} // anonymous namespace - namespace autoware { namespace tensorrt_common { -nvinfer1::Dims get_input_dims(const std::string & onnx_file_path) -{ - Logger logger_; - auto builder = TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); - if (!builder) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder"); - } - - const auto explicitBatch = - 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); - - auto network = - TrtUniquePtr(builder->createNetworkV2(explicitBatch)); - if (!network) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create network"); - } - - auto config = TrtUniquePtr(builder->createBuilderConfig()); - if (!config) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder config"); - } - - auto parser = TrtUniquePtr(nvonnxparser::createParser(*network, logger_)); - if (!parser->parseFromFile( - onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR))) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Failed to parse onnx file"); - } - - const auto input = network->getInput(0); - return input->getDimensions(); -} - -bool is_valid_precision_string(const std::string & precision) -{ - if ( - std::find(valid_precisions.begin(), valid_precisions.end(), precision) == - valid_precisions.end()) { - std::stringstream message; - message << "Invalid precision was specified: " << precision << std::endl - << "Valid string is one of: ["; - for (const auto & s : valid_precisions) { - message << s << ", "; - } - message << "] (case sensitive)" << std::endl; - std::cerr << message.str(); - return false; - } else { - return true; - } -} TrtCommon::TrtCommon( - const std::string & model_path, const std::string & precision, - std::unique_ptr calibrator, const BatchConfig & batch_config, - const size_t max_workspace_size, const BuildConfig & build_config, + const TrtCommonConfig & trt_config, const std::shared_ptr & profiler, const std::vector & plugin_paths) -: model_file_path_(model_path), - calibrator_(std::move(calibrator)), - precision_(precision), - batch_config_(batch_config), - max_workspace_size_(max_workspace_size), - model_profiler_("Model"), - host_profiler_("Host") +: trt_config_(std::make_shared(trt_config)), + host_profiler_(profiler), + model_profiler_(profiler) { - // Check given precision is valid one - if (!is_valid_precision_string(precision)) { - return; - } - build_config_ = std::make_unique(build_config); - + logger_ = std::make_shared(); for (const auto & plugin_path : plugin_paths) { int32_t flags{RTLD_LAZY}; // cspell: ignore asan @@ -120,523 +56,595 @@ TrtCommon::TrtCommon( #endif // ENABLE_ASAN void * handle = dlopen(plugin_path.c_str(), flags); if (!handle) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Could not load plugin library"); + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Could not load plugin library"); + } else { + logger_->log( + nvinfer1::ILogger::Severity::kINFO, "Loaded plugin library: %s", plugin_path.c_str()); } } - runtime_ = TrtUniquePtr(nvinfer1::createInferRuntime(logger_)); - if (build_config_->dla_core_id != -1) { - runtime_->setDLACore(build_config_->dla_core_id); + runtime_ = TrtUniquePtr(nvinfer1::createInferRuntime(*logger_)); + if (trt_config_->dla_core_id != -1) { + runtime_->setDLACore(trt_config_->dla_core_id); } - initLibNvInferPlugins(&logger_, ""); -} + initLibNvInferPlugins(&*logger_, ""); -TrtCommon::~TrtCommon() -{ + if (!initialize()) { + throw std::runtime_error("Failed to initialize TensorRT"); + } } -void TrtCommon::setup() +TrtCommon::~TrtCommon() = default; + +bool TrtCommon::setup(ProfileDimsPtr profile_dims, NetworkIOPtr network_io) { - if (!fs::exists(model_file_path_)) { - is_initialized_ = false; - return; - } - // cppcheck-suppress unreadVariable - std::string engine_path = model_file_path_; - if (model_file_path_.extension() == ".engine") { - std::cout << "Load ... " << model_file_path_ << std::endl; - loadEngine(model_file_path_); - } else if (model_file_path_.extension() == ".onnx") { - fs::path cache_engine_path{model_file_path_}; - std::string ext; - std::string calib_name = ""; - if (precision_ == "int8") { - if (build_config_->calib_type_str == "Entropy") { - calib_name = "EntropyV2-"; - } else if ( - build_config_->calib_type_str == "Legacy" || - build_config_->calib_type_str == "Percentile") { - calib_name = "Legacy-"; - } else { - calib_name = "MinMax-"; + profile_dims_ = std::move(profile_dims); + network_io_ = std::move(network_io); + + // Set input profile + if (profile_dims_ && !profile_dims_->empty()) { + auto profile = builder_->createOptimizationProfile(); + for (auto & profile_dim : *profile_dims_) { + if (profile_dim.tensor_name.empty()) { + profile_dim.tensor_name = getIOTensorName(profile_dim.tensor_index); } + logger_->log( + nvinfer1::ILogger::Severity::kINFO, "Setting optimization profile for tensor: %s", + profile_dim.toString().c_str()); + profile->setDimensions( + profile_dim.tensor_name.c_str(), nvinfer1::OptProfileSelector::kMIN, profile_dim.min_dims); + profile->setDimensions( + profile_dim.tensor_name.c_str(), nvinfer1::OptProfileSelector::kOPT, profile_dim.opt_dims); + profile->setDimensions( + profile_dim.tensor_name.c_str(), nvinfer1::OptProfileSelector::kMAX, profile_dim.max_dims); } - if (build_config_->dla_core_id != -1) { - ext = "DLA" + std::to_string(build_config_->dla_core_id) + "-" + calib_name + precision_; - if (build_config_->quantize_first_layer) { - ext += "-firstFP16"; - } - if (build_config_->quantize_last_layer) { - ext += "-lastFP16"; - } - ext += "-batch" + std::to_string(batch_config_[0]) + ".engine"; - } else { - ext = calib_name + precision_; - if (build_config_->quantize_first_layer) { - ext += "-firstFP16"; - } - if (build_config_->quantize_last_layer) { - ext += "-lastFP16"; - } - ext += "-batch" + std::to_string(batch_config_[0]) + ".engine"; + builder_config_->addOptimizationProfile(profile); + } + + auto build_engine_with_log = [this]() -> bool { + logger_->log(nvinfer1::ILogger::Severity::kINFO, "Starting to build engine"); + auto log_thread = logger_->log_throttle( + nvinfer1::ILogger::Severity::kINFO, + "Applying optimizations and building TensorRT CUDA engine. Please wait for a few minutes...", + 5); + auto success = buildEngineFromOnnx(); + logger_->stop_throttle(log_thread); + logger_->log(nvinfer1::ILogger::Severity::kINFO, "Engine build completed"); + return success; + }; + + // Load engine file if it exists + if (fs::exists(trt_config_->engine_path)) { + logger_->log(nvinfer1::ILogger::Severity::kINFO, "Loading engine"); + if (!loadEngine()) { + return false; } - cache_engine_path.replace_extension(ext); - - // Output Network Information - printNetworkInfo(model_file_path_); - - if (fs::exists(cache_engine_path)) { - std::cout << "Loading... " << cache_engine_path << std::endl; - loadEngine(cache_engine_path); - } else { - std::cout << "Building... " << cache_engine_path << std::endl; - logger_.log(nvinfer1::ILogger::Severity::kINFO, "Start build engine"); - auto log_thread = logger_.log_throttle( - nvinfer1::ILogger::Severity::kINFO, - "Applying optimizations and building TRT CUDA engine. Please wait for a few minutes...", 5); - buildEngineFromOnnx(model_file_path_, cache_engine_path); - logger_.stop_throttle(log_thread); - logger_.log(nvinfer1::ILogger::Severity::kINFO, "End build engine"); + logger_->log(nvinfer1::ILogger::Severity::kINFO, "Network validation"); + // Validate engine tensor shapes and optimization profile + if (!validateNetworkIO() || !validateProfileDims()) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Network validation failed for loaded engine from file. Rebuilding engine"); + // Rebuild engine if the tensor shapes or optimization profile mismatch + if (!build_engine_with_log()) { + return false; + } } - // cppcheck-suppress unreadVariable - engine_path = cache_engine_path; } else { - is_initialized_ = false; - return; + // Build engine if engine has not been cached + if (!build_engine_with_log()) { + return false; + } } - context_ = TrtUniquePtr(engine_->createExecutionContext()); - if (!context_) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create context"); - is_initialized_ = false; - return; + // Validate engine nevertheless is loaded or rebuilt + if (!validateNetworkIO() || !validateProfileDims()) { + logger_->log( + nvinfer1::ILogger::Severity::kERROR, + "Final network validation failed. Possibly the input / output of the currently " + "deployed model has changed. Check your configuration file with the current model."); + return false; } - if (build_config_->profile_per_layer) { - context_->setProfiler(&model_profiler_); + logger_->log(nvinfer1::ILogger::Severity::kINFO, "Engine setup completed"); + return true; +} + +std::string TrtCommon::getPrecision() const +{ + return trt_config_->precision; +} + +const char * TrtCommon::getIOTensorName(const int32_t index) const +{ + if (!engine_) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Engine is not initialized. Retrieving data from network"); + if (!network_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Network is not initialized"); + return nullptr; + } + auto num_inputs = network_->getNbInputs(); + auto num_outputs = network_->getNbOutputs(); + if (index < 0 || index >= num_inputs + num_outputs) { + logger_->log( + nvinfer1::ILogger::Severity::kERROR, + "Invalid index for I/O tensor: %d. Total I/O tensors: %d", index, num_inputs + num_outputs); + return nullptr; + } + if (index < num_inputs) { + return network_->getInput(index)->getName(); + } + return network_->getOutput(index - num_inputs)->getName(); } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 - // Write profiles for trt-engine-explorer - // See: https://github.com/NVIDIA/TensorRT/tree/main/tools/experimental/trt-engine-explorer - std::string j_ext = ".json"; - fs::path json_path{engine_path}; - json_path.replace_extension(j_ext); - std::string ret = getLayerInformation(nvinfer1::LayerInformationFormat::kJSON); - std::ofstream os(json_path, std::ofstream::trunc); - os << ret << std::flush; -#endif - is_initialized_ = true; + return engine_->getIOTensorName(index); } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8500 -void TrtCommon::setupBindings(std::vector & bindings) +int32_t TrtCommon::getNbIOTensors() const { - for (int32_t i = 0, e = engine_->getNbIOTensors(); i < e; i++) { - auto const name = engine_->getIOTensorName(i); - context_->setTensorAddress(name, bindings.at(i)); + if (!engine_) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Engine is not initialized. Retrieving data from network"); + if (!network_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Network is not initialized"); + return 0; + } + return network_->getNbInputs() + network_->getNbOutputs(); } + return engine_->getNbIOTensors(); } -#endif -bool TrtCommon::loadEngine(const std::string & engine_file_path) +nvinfer1::Dims TrtCommon::getTensorShape(const int32_t index) const { - std::ifstream engine_file(engine_file_path); - std::stringstream engine_buffer; - engine_buffer << engine_file.rdbuf(); - std::string engine_str = engine_buffer.str(); - engine_ = TrtUniquePtr(runtime_->deserializeCudaEngine( - reinterpret_cast(engine_str.data()), engine_str.size())); - return true; + if (!engine_) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Engine is not initialized. Retrieving data from network"); + if (!network_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Network is not initialized"); + return nvinfer1::Dims{}; + } + auto num_inputs = network_->getNbInputs(); + auto num_outputs = network_->getNbOutputs(); + if (index < 0 || index >= num_inputs + num_outputs) { + logger_->log( + nvinfer1::ILogger::Severity::kERROR, + "Invalid index for I/O tensor: %d. Total I/O tensors: %d", index, num_inputs + num_outputs); + return nvinfer1::Dims{}; + } + if (index < num_inputs) { + return network_->getInput(index)->getDimensions(); + } + return network_->getOutput(index - num_inputs)->getDimensions(); + } + auto const & name = getIOTensorName(index); + return getTensorShape(name); } -void TrtCommon::printNetworkInfo(const std::string & onnx_file_path) +nvinfer1::Dims TrtCommon::getTensorShape(const char * tensor_name) const { - auto builder = TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); - if (!builder) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder"); - return; + if (!engine_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Engine is not initialized"); + return nvinfer1::Dims{}; } + return engine_->getTensorShape(tensor_name); +} - const auto explicitBatch = - 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); +nvinfer1::Dims TrtCommon::getInputDims(const int32_t index) const +{ + if (!network_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Network is not initialized"); + return {}; + } + const auto input = network_->getInput(index); + return input->getDimensions(); +} - auto network = - TrtUniquePtr(builder->createNetworkV2(explicitBatch)); - if (!network) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create network"); - return; +nvinfer1::Dims TrtCommon::getOutputDims(const int32_t index) const +{ + if (!network_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Network is not initialized"); + return {}; } + const auto output = network_->getOutput(index); + return output->getDimensions(); +} + +bool TrtCommon::setTensorAddress(const int32_t index, void * data) +{ + auto const & name = getIOTensorName(index); + return setTensorAddress(name, data); +} - auto config = TrtUniquePtr(builder->createBuilderConfig()); - if (!config) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder config"); - return; +bool TrtCommon::setTensorAddress(const char * tensor_name, void * data) +{ + if (!context_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Context is not initialized"); + return false; + } + auto success = context_->setTensorAddress(tensor_name, data); + if (!success) { + logger_->log( + nvinfer1::ILogger::Severity::kERROR, + "Failed to set tensor address for tensor: ", tensor_name); } + return success; +} - if (precision_ == "fp16" || precision_ == "int8") { - config->setFlag(nvinfer1::BuilderFlag::kFP16); +bool TrtCommon::setTensorsAddresses(std::vector & tensors) +{ + bool success = true; + for (std::size_t i = 0, e = tensors.size(); i < e; i++) { + auto const name = getIOTensorName(i); + success &= setTensorAddress(name, tensors.at(i)); } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 - config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, max_workspace_size_); -#else - config->setMaxWorkspaceSize(max_workspace_size_); -#endif + return success; +} - auto parser = TrtUniquePtr(nvonnxparser::createParser(*network, logger_)); - if (!parser->parseFromFile( - onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR))) { - return; - } - int num = network->getNbLayers(); - float total_gflops = 0.0; - int total_params = 0; - for (int i = 0; i < num; i++) { - nvinfer1::ILayer * layer = network->getLayer(i); - auto layer_type = layer->getType(); - if (build_config_->profile_per_layer) { - model_profiler_.setProfDict(layer); - } - if (layer_type == nvinfer1::LayerType::kCONSTANT) { - continue; - } - nvinfer1::ITensor * in = layer->getInput(0); - nvinfer1::Dims dim_in = in->getDimensions(); - nvinfer1::ITensor * out = layer->getOutput(0); - nvinfer1::Dims dim_out = out->getDimensions(); - - if (layer_type == nvinfer1::LayerType::kCONVOLUTION) { - nvinfer1::IConvolutionLayer * conv = (nvinfer1::IConvolutionLayer *)layer; - nvinfer1::Dims k_dims = conv->getKernelSizeNd(); - nvinfer1::Dims s_dims = conv->getStrideNd(); - int groups = conv->getNbGroups(); - int stride = s_dims.d[0]; - int num_weights = (dim_in.d[1] / groups) * dim_out.d[1] * k_dims.d[0] * k_dims.d[1]; - float gflops = (2.0 * num_weights) * (static_cast(dim_in.d[3]) / stride * - static_cast(dim_in.d[2]) / stride / 1e9); - total_gflops += gflops; - total_params += num_weights; - std::cout << "L" << i << " [conv " << k_dims.d[0] << "x" << k_dims.d[1] << " (" << groups - << ") " - << "/" << s_dims.d[0] << "] " << dim_in.d[3] << "x" << dim_in.d[2] << "x" - << dim_in.d[1] << " -> " << dim_out.d[3] << "x" << dim_out.d[2] << "x" - << dim_out.d[1]; - std::cout << " weights:" << num_weights; - std::cout << " GFLOPs:" << gflops; - std::cout << std::endl; - } else if (layer_type == nvinfer1::LayerType::kPOOLING) { - nvinfer1::IPoolingLayer * pool = (nvinfer1::IPoolingLayer *)layer; - auto p_type = pool->getPoolingType(); - nvinfer1::Dims dim_stride = pool->getStrideNd(); - nvinfer1::Dims dim_window = pool->getWindowSizeNd(); - - std::cout << "L" << i << " ["; - if (p_type == nvinfer1::PoolingType::kMAX) { - std::cout << "max "; - } else if (p_type == nvinfer1::PoolingType::kAVERAGE) { - std::cout << "avg "; - } else if (p_type == nvinfer1::PoolingType::kMAX_AVERAGE_BLEND) { - std::cout << "max avg blend "; - } - float gflops = static_cast(dim_in.d[1]) * - (static_cast(dim_window.d[0]) / static_cast(dim_stride.d[0])) * - (static_cast(dim_window.d[1]) / static_cast(dim_stride.d[1])) * - static_cast(dim_in.d[2]) * static_cast(dim_in.d[3]) / 1e9; - total_gflops += gflops; - std::cout << "pool " << dim_window.d[0] << "x" << dim_window.d[1] << "]"; - std::cout << " GFLOPs:" << gflops; - std::cout << std::endl; - } else if (layer_type == nvinfer1::LayerType::kRESIZE) { - std::cout << "L" << i << " [resize]" << std::endl; - } +bool TrtCommon::setTensorsAddresses(std::unordered_map & tensors) +{ + bool success = true; + for (auto const & tensor : tensors) { + success &= setTensorAddress(tensor.first, tensor.second); } - std::cout << "Total " << total_gflops << " GFLOPs" << std::endl; - std::cout << "Total " << total_params / 1000.0 / 1000.0 << " M params" << std::endl; - return; + return success; +} + +bool TrtCommon::setInputShape(const int32_t index, const nvinfer1::Dims & dimensions) +{ + auto const & name = getIOTensorName(index); + return setInputShape(name, dimensions); } -bool TrtCommon::buildEngineFromOnnx( - const std::string & onnx_file_path, const std::string & output_engine_file_path) +bool TrtCommon::setInputShape(const char * tensor_name, const nvinfer1::Dims & dimensions) { - auto builder = TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); - if (!builder) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder"); + if (!context_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Context is not initialized"); return false; } + auto success = context_->setInputShape(tensor_name, dimensions); + if (!success) { + logger_->log( + nvinfer1::ILogger::Severity::kERROR, "Failed to set input shape for tensor: ", tensor_name); + } + return success; +} - const auto explicitBatch = - 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); +bool TrtCommon::setInputsShapes(const std::vector & dimensions) +{ + bool success = true; + for (std::size_t i = 0, e = dimensions.size(); i < e; i++) { + success &= setInputShape(i, dimensions.at(i)); + } + return success; +} - auto network = - TrtUniquePtr(builder->createNetworkV2(explicitBatch)); - if (!network) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create network"); - return false; +bool TrtCommon::setInputsShapes(const std::unordered_map & dimensions) +{ + bool success = true; + for (auto const & dim : dimensions) { + success &= setInputShape(dim.first, dim.second); } + return success; +} - auto config = TrtUniquePtr(builder->createBuilderConfig()); - if (!config) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder config"); - return false; +bool TrtCommon::setTensor(const int32_t index, void * data, nvinfer1::Dims dimensions) +{ + auto success = setTensorAddress(index, data); + if (dimensions.nbDims > 0) { + success &= setInputShape(index, dimensions); } + return success; +} - int num_available_dla = builder->getNbDLACores(); - if (build_config_->dla_core_id != -1) { - if (num_available_dla > 0) { - std::cout << "###" << num_available_dla << " DLAs are supported! ###" << std::endl; - } else { - std::cout << "###Warning : " - << "No DLA is supported! ###" << std::endl; - } - config->setDefaultDeviceType(nvinfer1::DeviceType::kDLA); - config->setDLACore(build_config_->dla_core_id); -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 - config->setFlag(nvinfer1::BuilderFlag::kPREFER_PRECISION_CONSTRAINTS); -#else - config->setFlag(nvinfer1::BuilderFlag::kSTRICT_TYPES); -#endif - config->setFlag(nvinfer1::BuilderFlag::kGPU_FALLBACK); +bool TrtCommon::setTensor(const char * tensor_name, void * data, nvinfer1::Dims dimensions) +{ + auto success = setTensorAddress(tensor_name, data); + if (dimensions.nbDims > 0) { + success &= setInputShape(tensor_name, dimensions); } - if (precision_ == "fp16" || precision_ == "int8") { - config->setFlag(nvinfer1::BuilderFlag::kFP16); + return success; +} + +bool TrtCommon::setTensors(TensorsVec & tensors) +{ + bool success = true; + for (std::size_t i = 0, e = tensors.size(); i < e; i++) { + success &= setTensor(i, tensors.at(i).first, tensors.at(i).second); } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 - config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, max_workspace_size_); -#else - config->setMaxWorkspaceSize(max_workspace_size_); -#endif + return success; +} - auto parser = TrtUniquePtr(nvonnxparser::createParser(*network, logger_)); - if (!parser->parseFromFile( - onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR))) { - std::cout << "Failed to parse onnx file" << std::endl; - return false; +bool TrtCommon::setTensors(TensorsMap & tensors) +{ + bool success = true; + for (auto const & tensor : tensors) { + success &= setTensor(tensor.first, tensor.second.first, tensor.second.second); } + return success; +} - const int num = network->getNbLayers(); - bool first = build_config_->quantize_first_layer; - bool last = build_config_->quantize_last_layer; - // Partial Quantization - if (precision_ == "int8") { - network->getInput(0)->setDynamicRange(0, 255.0); - for (int i = 0; i < num; i++) { - nvinfer1::ILayer * layer = network->getLayer(i); - auto layer_type = layer->getType(); - std::string name = layer->getName(); - nvinfer1::ITensor * out = layer->getOutput(0); - if (build_config_->clip_value > 0.0) { - std::cout << "Set max value for outputs : " << build_config_->clip_value << " " << name - << std::endl; - out->setDynamicRange(0.0, build_config_->clip_value); - } +std::shared_ptr TrtCommon::getModelProfiler() +{ + return model_profiler_; +} - if (layer_type == nvinfer1::LayerType::kCONVOLUTION) { - if (first) { - layer->setPrecision(nvinfer1::DataType::kHALF); - std::cout << "Set kHALF in " << name << std::endl; - first = false; - } - if (last) { - // cspell: ignore preds - if ( - contain(name, "reg_preds") || contain(name, "cls_preds") || - contain(name, "obj_preds")) { - layer->setPrecision(nvinfer1::DataType::kHALF); - std::cout << "Set kHALF in " << name << std::endl; - } - for (int j = num - 1; j >= 0; j--) { - nvinfer1::ILayer * inner_layer = network->getLayer(j); - auto inner_layer_type = inner_layer->getType(); - std::string inner_name = inner_layer->getName(); - if (inner_layer_type == nvinfer1::LayerType::kCONVOLUTION) { - inner_layer->setPrecision(nvinfer1::DataType::kHALF); - std::cout << "Set kHALF in " << inner_name << std::endl; - break; - } - if (inner_layer_type == nvinfer1::LayerType::kMATRIX_MULTIPLY) { - inner_layer->setPrecision(nvinfer1::DataType::kHALF); - std::cout << "Set kHALF in " << inner_name << std::endl; - break; - } - } - } - } - } +std::shared_ptr TrtCommon::getHostProfiler() +{ + return host_profiler_; +} + +std::shared_ptr TrtCommon::getTrtCommonConfig() +{ + return trt_config_; +} + +std::shared_ptr TrtCommon::getBuilderConfig() +{ + return builder_config_; +} + +std::shared_ptr TrtCommon::getNetwork() +{ + return network_; +} + +std::shared_ptr TrtCommon::getLogger() +{ + return logger_; +} + +bool TrtCommon::enqueueV3(cudaStream_t stream) +{ + if (!context_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Context is not initialized"); + return false; + } + if (trt_config_->profile_per_layer) { + auto inference_start = std::chrono::high_resolution_clock::now(); + auto success = context_->enqueueV3(stream); + auto inference_end = std::chrono::high_resolution_clock::now(); + host_profiler_->reportLayerTime( + "inference_host", + std::chrono::duration(inference_end - inference_start).count()); + return success; } + return context_->enqueueV3(stream); +} - const auto input = network->getInput(0); - const auto input_dims = input->getDimensions(); - const auto input_channel = input_dims.d[1]; - const auto input_height = input_dims.d[2]; - const auto input_width = input_dims.d[3]; - const auto input_batch = input_dims.d[0]; +void TrtCommon::printProfiling() const +{ + logger_->log( + nvinfer1::ILogger::Severity::kINFO, "Host Profiling\n", host_profiler_->toString().c_str()); + logger_->log( + nvinfer1::ILogger::Severity::kINFO, "Model Profiling\n", model_profiler_->toString().c_str()); +} + +std::string TrtCommon::getLayerInformation(nvinfer1::LayerInformationFormat format) +{ + if (!context_ || !engine_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Context or engine are not initialized"); + return {}; + } + auto inspector = std::unique_ptr(engine_->createEngineInspector()); + inspector->setExecutionContext(&(*context_)); + std::string result = inspector->getEngineInformation(format); + return result; +} - if (input_batch > 1) { - batch_config_[0] = input_batch; +bool TrtCommon::initialize() +{ + if (!fs::exists(trt_config_->onnx_path) || trt_config_->onnx_path.extension() != ".onnx") { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Invalid ONNX file path or extension"); + return false; } - if (batch_config_.at(0) > 1 && (batch_config_.at(0) == batch_config_.at(2))) { - // Attention : below API is deprecated in TRT8.4 - builder->setMaxBatchSize(batch_config_.at(2)); - } else { - if (build_config_->profile_per_layer) { - auto profile = builder->createOptimizationProfile(); - profile->setDimensions( - network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kMIN, - nvinfer1::Dims4{batch_config_.at(0), input_channel, input_height, input_width}); - profile->setDimensions( - network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kOPT, - nvinfer1::Dims4{batch_config_.at(1), input_channel, input_height, input_width}); - profile->setDimensions( - network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kMAX, - nvinfer1::Dims4{batch_config_.at(2), input_channel, input_height, input_width}); - config->addOptimizationProfile(profile); - } + builder_ = TrtUniquePtr(nvinfer1::createInferBuilder(*logger_)); + if (!builder_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder"); + return false; } - if (precision_ == "int8" && calibrator_) { - config->setFlag(nvinfer1::BuilderFlag::kINT8); -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 - config->setFlag(nvinfer1::BuilderFlag::kPREFER_PRECISION_CONSTRAINTS); + +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH < 10000 + const auto explicit_batch = + 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + network_ = TrtUniquePtr(builder_->createNetworkV2(explicit_batch)); #else - config->setFlag(nvinfer1::BuilderFlag::kSTRICT_TYPES); + network_ = TrtUniquePtr(builder_->createNetworkV2(0)); #endif - // QAT requires no calibrator. - // assert((calibrator != nullptr) && "Invalid calibrator for INT8 precision"); - config->setInt8Calibrator(calibrator_.get()); + + if (!network_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create network"); + return false; } - if (build_config_->profile_per_layer) { -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 - config->setProfilingVerbosity(nvinfer1::ProfilingVerbosity::kDETAILED); -#else - config->setProfilingVerbosity(nvinfer1::ProfilingVerbosity::kVERBOSE); -#endif + + builder_config_ = TrtUniquePtr(builder_->createBuilderConfig()); + if (!builder_config_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder config"); + return false; + } + + auto num_available_dla = builder_->getNbDLACores(); + if (trt_config_->dla_core_id != -1) { + logger_->log( + nvinfer1::ILogger::Severity::kINFO, "Number of DLAs supported: %d", num_available_dla); + builder_config_->setDefaultDeviceType(nvinfer1::DeviceType::kDLA); + builder_config_->setDLACore(trt_config_->dla_core_id); + builder_config_->setFlag(nvinfer1::BuilderFlag::kPREFER_PRECISION_CONSTRAINTS); + builder_config_->setFlag(nvinfer1::BuilderFlag::kGPU_FALLBACK); } + if (trt_config_->precision == "fp16") { + builder_config_->setFlag(nvinfer1::BuilderFlag::kFP16); + } else if (trt_config_->precision == "int8") { + builder_config_->setFlag(nvinfer1::BuilderFlag::kINT8); + } + builder_config_->setMemoryPoolLimit( + nvinfer1::MemoryPoolType::kWORKSPACE, trt_config_->max_workspace_size); + + parser_ = TrtUniquePtr(nvonnxparser::createParser(*network_, *logger_)); + if (!parser_->parseFromFile( + trt_config_->onnx_path.c_str(), + static_cast(nvinfer1::ILogger::Severity::kERROR))) { + return false; + } + + if (trt_config_->profile_per_layer) { + builder_config_->setProfilingVerbosity(nvinfer1::ProfilingVerbosity::kDETAILED); + } + + return true; +} -#if TENSORRT_VERSION_MAJOR >= 8 - auto plan = - TrtUniquePtr(builder->buildSerializedNetwork(*network, *config)); +bool TrtCommon::buildEngineFromOnnx() +{ + // Build engine + auto plan = TrtUniquePtr( + builder_->buildSerializedNetwork(*network_, *builder_config_)); if (!plan) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create host memory"); + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create host memory"); return false; } engine_ = TrtUniquePtr( runtime_->deserializeCudaEngine(plan->data(), plan->size())); -#else - engine_ = TrtUniquePtr(builder->buildEngineWithConfig(*network, *config)); -#endif if (!engine_) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create engine"); + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create engine"); return false; } - // save engine -#if TENSORRT_VERSION_MAJOR < 8 - auto data = TrtUniquePtr(engine_->serialize()); -#endif + // Save engine std::ofstream file; - file.open(output_engine_file_path, std::ios::binary | std::ios::out); + file.open(trt_config_->engine_path, std::ios::binary | std::ios::out); if (!file.is_open()) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to open engine file"); return false; } -#if TENSORRT_VERSION_MAJOR < 8 - file.write(reinterpret_cast(data->data()), data->size()); -#else - file.write(reinterpret_cast(plan->data()), plan->size()); -#endif - + file.write(reinterpret_cast(plan->data()), plan->size()); // NOLINT file.close(); - return true; -} - -bool TrtCommon::isInitialized() -{ - return is_initialized_; -} - -nvinfer1::Dims TrtCommon::getBindingDimensions(const int32_t index) const -{ -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + (NV_TENSOR_PATCH * 10) >= 8500 - auto const & name = engine_->getIOTensorName(index); - auto dims = context_->getTensorShape(name); - bool const has_runtime_dim = - std::any_of(dims.d, dims.d + dims.nbDims, [](int32_t dim) { return dim == -1; }); + context_ = TrtUniquePtr(engine_->createExecutionContext()); + if (!context_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create context"); + return false; + } - if (has_runtime_dim) { - return dims; - } else { - return context_->getBindingDimensions(index); + if (trt_config_->profile_per_layer) { + context_->setProfiler(&*model_profiler_); } -#else - return context_->getBindingDimensions(index); -#endif -} -int32_t TrtCommon::getNbBindings() -{ - return engine_->getNbBindings(); -} + fs::path json_path = trt_config_->engine_path.replace_extension(".json"); + auto ret = getLayerInformation(nvinfer1::LayerInformationFormat::kJSON); + std::ofstream os(json_path, std::ofstream::trunc); + os << ret << std::flush; + os.close(); -bool TrtCommon::setBindingDimensions(const int32_t index, const nvinfer1::Dims & dimensions) const -{ - return context_->setBindingDimensions(index, dimensions); + return true; } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8500 -bool TrtCommon::enqueueV3(cudaStream_t stream) +bool TrtCommon::loadEngine() { - if (build_config_->profile_per_layer) { - auto inference_start = std::chrono::high_resolution_clock::now(); - - bool ret = context_->enqueueV3(stream); + std::ifstream engine_file(trt_config_->engine_path); + std::stringstream engine_buffer; + engine_buffer << engine_file.rdbuf(); + std::string engine_str = engine_buffer.str(); - auto inference_end = std::chrono::high_resolution_clock::now(); - host_profiler_.reportLayerTime( - "inference", - std::chrono::duration(inference_end - inference_start).count()); - return ret; + engine_ = TrtUniquePtr(runtime_->deserializeCudaEngine( + reinterpret_cast( // NOLINT + engine_str.data()), + engine_str.size())); + if (!engine_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create engine"); + return false; } - return context_->enqueueV3(stream); -} -#endif -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH < 10000 -bool TrtCommon::enqueueV2(void ** bindings, cudaStream_t stream, cudaEvent_t * input_consumed) -{ - if (build_config_->profile_per_layer) { - auto inference_start = std::chrono::high_resolution_clock::now(); - bool ret = context_->enqueueV2(bindings, stream, input_consumed); + context_ = TrtUniquePtr(engine_->createExecutionContext()); + if (!context_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create context"); + return false; + } - auto inference_end = std::chrono::high_resolution_clock::now(); - host_profiler_.reportLayerTime( - "inference", - std::chrono::duration(inference_end - inference_start).count()); - return ret; - } else { - return context_->enqueueV2(bindings, stream, input_consumed); + if (trt_config_->profile_per_layer) { + context_->setProfiler(&*model_profiler_); } + + return true; } -#endif -void TrtCommon::printProfiling() +bool TrtCommon::validateProfileDims() { - std::cout << host_profiler_; - std::cout << std::endl; - std::cout << model_profiler_; + auto success = true; + if (!profile_dims_ || profile_dims_->empty()) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Input profile is empty, skipping validation. If network has dynamic shapes, it might lead " + "to undefined behavior."); + return success; + } + if (engine_->getNbOptimizationProfiles() != 1) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Number of optimization profiles in the engine (%d) is not equal to 1. Selecting the first " + "cached profile.", + engine_->getNbOptimizationProfiles()); + } + + for (const auto & profile_dim : *profile_dims_) { + nvinfer1::Dims min_dims = engine_->getProfileShape( + profile_dim.tensor_name.c_str(), 0, nvinfer1::OptProfileSelector::kMIN); + nvinfer1::Dims opt_dims = engine_->getProfileShape( + profile_dim.tensor_name.c_str(), 0, nvinfer1::OptProfileSelector::kOPT); + nvinfer1::Dims max_dims = engine_->getProfileShape( + profile_dim.tensor_name.c_str(), 0, nvinfer1::OptProfileSelector::kMAX); + ProfileDims profile_from_engine{profile_dim.tensor_name, min_dims, opt_dims, max_dims}; + if (profile_dim != profile_from_engine) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Invalid profile. Current configuration: %s. Cached engine: %s", + profile_dim.toString().c_str(), profile_from_engine.toString().c_str()); + success = false; + } + } + return success; } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 -std::string TrtCommon::getLayerInformation(nvinfer1::LayerInformationFormat format) +bool TrtCommon::validateNetworkIO() { - auto runtime = std::unique_ptr(nvinfer1::createInferRuntime(logger_)); - auto inspector = std::unique_ptr(engine_->createEngineInspector()); - if (context_ != nullptr) { - inspector->setExecutionContext(&(*context_)); + auto success = true; + if (!network_io_ || network_io_->empty()) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Network IO is empty, skipping validation. It might lead to undefined behavior"); + return success; + } + + if (network_io_->size() != static_cast(getNbIOTensors())) { + std::string tensor_names = "[" + std::string(getIOTensorName(0)); + for (int32_t i = 1; i < getNbIOTensors(); ++i) { + tensor_names += ", " + std::string(getIOTensorName(i)); + } + tensor_names += "]"; + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Number of tensors in the engine (%d) does not match number of tensors in the config (%d). " + "Tensors in the built engine: %s", + getNbIOTensors(), network_io_->size(), tensor_names.c_str()); + success = false; + } + for (const auto & io : *network_io_) { + NetworkIO tensor_from_engine{io.tensor_name, getTensorShape(io.tensor_name.c_str())}; + if (io != tensor_from_engine) { + logger_->log( + nvinfer1::ILogger::Severity::kERROR, + "Invalid tensor. Current configuration: %s. Cached engine: %s", io.toString().c_str(), + tensor_from_engine.toString().c_str()); + success = false; + } } - std::string result = inspector->getEngineInformation(format); - return result; + + return success; } -#endif } // namespace tensorrt_common } // namespace autoware diff --git a/perception/autoware_tensorrt_yolox/CMakeLists.txt b/perception/autoware_tensorrt_yolox/CMakeLists.txt index 17b31fc7e8df1..cc0d793151781 100644 --- a/perception/autoware_tensorrt_yolox/CMakeLists.txt +++ b/perception/autoware_tensorrt_yolox/CMakeLists.txt @@ -10,6 +10,9 @@ endif() find_package(autoware_cmake REQUIRED) autoware_package() +# TODO(amadeuszsz): Remove -Wno-deprecated-declarations once removing implicit quantization +add_compile_options(-Wno-deprecated-declarations) + find_package(OpenCV REQUIRED) option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) diff --git a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox.hpp b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox.hpp index 483adfbdf2757..7d49df145b72e 100644 --- a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox.hpp +++ b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox.hpp @@ -18,6 +18,8 @@ #include #include #include +#include +#include #include #include @@ -44,6 +46,13 @@ struct Object using ObjectArray = std::vector; using ObjectArrays = std::vector; +using autoware::tensorrt_common::CalibrationConfig; +using autoware::tensorrt_common::NetworkIOPtr; +using autoware::tensorrt_common::ProfileDimsPtr; +using autoware::tensorrt_common::Profiler; +using autoware::tensorrt_common::TrtCommon; +using autoware::tensorrt_common::TrtCommonConfig; +using autoware::tensorrt_common::TrtConvCalib; struct GridAndStride { @@ -70,31 +79,26 @@ class TrtYoloX public: /** * @brief Construct TrtYoloX. - * @param[in] mode_path ONNX model_path - * @param[in] precision precision for inference + * @param[in] trt_config base trt common configuration * @param[in] num_class classifier-ed num * @param[in] score_threshold threshold for detection * @param[in] nms_threshold threshold for NMS - * @param[in] build_config configuration including precision, calibration method, DLA, remaining - * fp16 for first layer, remaining fp16 for last layer and profiler for builder * @param[in] use_gpu_preprocess whether use cuda gpu for preprocessing - * @param[in] calibration_image_list_file path for calibration files (only require for + * @param[in] gpu_id GPU id for inference + * @param[in] calibration_image_list_path path for calibration files (only require for * quantization) * @param[in] norm_factor scaling factor for preprocess * @param[in] cache_dir unused variable - * @param[in] batch_config configuration for batched execution - * @param[in] max_workspace_size maximum workspace for building TensorRT engine + * @param[in] color_map_path path for colormap for masks + * @param[in] calib_config calibration configuration */ TrtYoloX( - const std::string & model_path, const std::string & precision, const int num_class = 8, - const float score_threshold = 0.3, const float nms_threshold = 0.7, - const autoware::tensorrt_common::BuildConfig build_config = - autoware::tensorrt_common::BuildConfig(), - const bool use_gpu_preprocess = false, const uint8_t gpu_id = 0, - std::string calibration_image_list_file = std::string(), const double norm_factor = 1.0, - [[maybe_unused]] const std::string & cache_dir = "", - const autoware::tensorrt_common::BatchConfig & batch_config = {1, 1, 1}, - const size_t max_workspace_size = (1 << 30), const std::string & color_map_path = ""); + TrtCommonConfig & trt_config, const int num_class = 8, const float score_threshold = 0.3, + const float nms_threshold = 0.7, const bool use_gpu_preprocess = false, + const uint8_t gpu_id = 0, std::string calibration_image_list_path = std::string(), + const double norm_factor = 1.0, [[maybe_unused]] const std::string & cache_dir = "", + const std::string & color_map_path = "", + const CalibrationConfig & calib_config = CalibrationConfig()); /** * @brief Deconstruct TrtYoloX */ @@ -168,6 +172,12 @@ class TrtYoloX cv::Mat & colorized_mask); inline std::vector getColorMap() { return sematic_color_map_; } + /** + * @brief get batch size + * @return batch size + */ + [[nodiscard]] int getBatchSize() const; + private: /** * @brief run preprocess including resizing, letterbox, NHWC2NCHW and toFloat on CPU @@ -266,7 +276,7 @@ class TrtYoloX */ cv::Mat getMaskImageGpu(float * d_prob, nvinfer1::Dims dims, int out_w, int out_h, int b); - std::unique_ptr trt_common_; + std::unique_ptr trt_common_; std::vector input_h_; CudaUniquePtr input_d_; @@ -288,7 +298,7 @@ class TrtYoloX int num_class_; float score_threshold_; float nms_threshold_; - int batch_size_; + int32_t batch_size_; CudaUniquePtrHost out_prob_h_; // flag whether preprocess are performed on GPU diff --git a/perception/autoware_tensorrt_yolox/launch/multiple_yolox.launch.xml b/perception/autoware_tensorrt_yolox/launch/multiple_yolox.launch.xml index a7952b12414b1..606fd1299cf35 100644 --- a/perception/autoware_tensorrt_yolox/launch/multiple_yolox.launch.xml +++ b/perception/autoware_tensorrt_yolox/launch/multiple_yolox.launch.xml @@ -11,34 +11,50 @@ + + + + + + + + + + + + + + + + diff --git a/perception/autoware_tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml b/perception/autoware_tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml index 0096a219c8573..6d40905d78127 100644 --- a/perception/autoware_tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml +++ b/perception/autoware_tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml @@ -1,6 +1,8 @@ + + @@ -15,13 +17,13 @@ - + - + diff --git a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp index 7e2e327bf6f5e..74d8d73ecaed4 100644 --- a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp +++ b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp @@ -155,13 +155,11 @@ std::vector get_seg_colormap(const std::stri namespace autoware::tensorrt_yolox { TrtYoloX::TrtYoloX( - const std::string & model_path, const std::string & precision, const int num_class, - const float score_threshold, const float nms_threshold, - autoware::tensorrt_common::BuildConfig build_config, const bool use_gpu_preprocess, - const uint8_t gpu_id, std::string calibration_image_list_path, const double norm_factor, - [[maybe_unused]] const std::string & cache_dir, - const autoware::tensorrt_common::BatchConfig & batch_config, const size_t max_workspace_size, - const std::string & color_map_path) + TrtCommonConfig & trt_config, const int num_class, const float score_threshold, + const float nms_threshold, const bool use_gpu_preprocess, const uint8_t gpu_id, + std::string calibration_image_list_path, const double norm_factor, + [[maybe_unused]] const std::string & cache_dir, const std::string & color_map_path, + const CalibrationConfig & calib_config) : gpu_id_(gpu_id), is_gpu_initialized_(false) { if (!setCudaDeviceId(gpu_id_)) { @@ -172,13 +170,29 @@ TrtYoloX::TrtYoloX( src_width_ = -1; src_height_ = -1; norm_factor_ = norm_factor; - batch_size_ = batch_config[2]; multitask_ = 0; sematic_color_map_ = get_seg_colormap(color_map_path); stream_ = makeCudaStream(); - if (precision == "int8") { - if (build_config.clip_value <= 0.0) { + trt_common_ = std::make_unique(trt_config); + + const auto network_input_dims = trt_common_->getTensorShape(0); + batch_size_ = network_input_dims.d[0]; + const auto input_channel = network_input_dims.d[1]; + const auto input_height = network_input_dims.d[2]; + const auto input_width = network_input_dims.d[3]; + + auto profile_dims_ptr = std::make_unique>(); + + std::vector profile_dims{ + autoware::tensorrt_common::ProfileDims( + 0, {4, {batch_size_, input_channel, input_height, input_width}}, + {4, {batch_size_, input_channel, input_height, input_width}}, + {4, {batch_size_, input_channel, input_height, input_width}})}; + *profile_dims_ptr = profile_dims; + + if (trt_config.precision == "int8") { + if (calib_config.clip_value <= 0.0) { if (calibration_image_list_path.empty()) { throw std::runtime_error( "calibration_image_list_path should be passed to generate int8 engine " @@ -189,19 +203,17 @@ TrtYoloX::TrtYoloX( calibration_image_list_path = ""; } - int max_batch_size = batch_size_; - nvinfer1::Dims input_dims = autoware::tensorrt_common::get_input_dims(model_path); std::vector calibration_images; if (calibration_image_list_path != "") { calibration_images = loadImageList(calibration_image_list_path, ""); } - tensorrt_yolox::ImageStream stream(max_batch_size, input_dims, calibration_images); - fs::path calibration_table{model_path}; + tensorrt_yolox::ImageStream stream(batch_size_, network_input_dims, calibration_images); + fs::path calibration_table{trt_config.onnx_path}; std::string ext = ""; - if (build_config.calib_type_str == "Entropy") { + if (calib_config.calib_type_str == "Entropy") { ext = "EntropyV2-"; } else if ( - build_config.calib_type_str == "Legacy" || build_config.calib_type_str == "Percentile") { + calib_config.calib_type_str == "Legacy" || calib_config.calib_type_str == "Percentile") { ext = "Legacy-"; } else { ext = "MinMax-"; @@ -209,17 +221,17 @@ TrtYoloX::TrtYoloX( ext += "calibration.table"; calibration_table.replace_extension(ext); - fs::path histogram_table{model_path}; + fs::path histogram_table{trt_config.onnx_path}; ext = "histogram.table"; histogram_table.replace_extension(ext); std::unique_ptr calibrator; - if (build_config.calib_type_str == "Entropy") { + if (calib_config.calib_type_str == "Entropy") { calibrator.reset( new tensorrt_yolox::Int8EntropyCalibrator(stream, calibration_table, norm_factor_)); } else if ( - build_config.calib_type_str == "Legacy" || build_config.calib_type_str == "Percentile") { + calib_config.calib_type_str == "Legacy" || calib_config.calib_type_str == "Percentile") { const double quantile = 0.999999; const double cutoff = 0.999999; calibrator.reset(new tensorrt_yolox::Int8LegacyCalibrator( @@ -228,22 +240,20 @@ TrtYoloX::TrtYoloX( calibrator.reset( new tensorrt_yolox::Int8MinMaxCalibrator(stream, calibration_table, norm_factor_)); } - trt_common_ = std::make_unique( - model_path, precision, std::move(calibrator), batch_config, max_workspace_size, build_config); + if (!trt_common_->setupWithCalibrator( + std::move(calibrator), calib_config, std::move((profile_dims_ptr)))) { + throw std::runtime_error("Failed to setup TensorRT engine with calibrator"); + } } else { - trt_common_ = std::make_unique( - model_path, precision, nullptr, batch_config, max_workspace_size, build_config); - } - trt_common_->setup(); - - if (!trt_common_->isInitialized()) { - return; + if (!trt_common_->setup(std::move(profile_dims_ptr))) { + throw std::runtime_error("Failed to setup TensorRT engine"); + } } // Judge whether decoding output is required // Plain models require decoding, while models with EfficientNMS_TRT module don't. // If getNbBindings == 5, the model contains EfficientNMS_TRT - switch (trt_common_->getNbBindings()) { + switch (trt_common_->getNbIOTensors()) { case 2: // Specified model is plain one. // Bindings are: [input, output] @@ -273,16 +283,16 @@ TrtYoloX::TrtYoloX( */ } // GPU memory allocation - const auto input_dims = trt_common_->getBindingDimensions(0); + const auto input_dims = trt_common_->getTensorShape(0); const auto input_size = std::accumulate(input_dims.d + 1, input_dims.d + input_dims.nbDims, 1, std::multiplies()); if (needs_output_decode_) { - const auto output_dims = trt_common_->getBindingDimensions(1); - input_d_ = autoware::cuda_utils::make_unique(batch_config[2] * input_size); + const auto output_dims = trt_common_->getTensorShape(1); + input_d_ = autoware::cuda_utils::make_unique(batch_size_ * input_size); out_elem_num_ = std::accumulate( output_dims.d + 1, output_dims.d + output_dims.nbDims, 1, std::multiplies()); - out_elem_num_ = out_elem_num_ * batch_config[2]; - out_elem_num_per_batch_ = static_cast(out_elem_num_ / batch_config[2]); + out_elem_num_ = out_elem_num_ * batch_size_; + out_elem_num_per_batch_ = static_cast(out_elem_num_ / batch_size_); out_prob_d_ = autoware::cuda_utils::make_unique(out_elem_num_); out_prob_h_ = autoware::cuda_utils::make_unique_host(out_elem_num_, cudaHostAllocPortable); @@ -298,29 +308,27 @@ TrtYoloX::TrtYoloX( output_strides_ = {8, 16, 32, 4}; } } else { - const auto out_scores_dims = trt_common_->getBindingDimensions(3); + const auto out_scores_dims = trt_common_->getTensorShape(3); max_detections_ = out_scores_dims.d[1]; - input_d_ = autoware::cuda_utils::make_unique(batch_config[2] * input_size); - out_num_detections_d_ = autoware::cuda_utils::make_unique(batch_config[2]); - out_boxes_d_ = - autoware::cuda_utils::make_unique(batch_config[2] * max_detections_ * 4); - out_scores_d_ = autoware::cuda_utils::make_unique(batch_config[2] * max_detections_); - out_classes_d_ = - autoware::cuda_utils::make_unique(batch_config[2] * max_detections_); + input_d_ = autoware::cuda_utils::make_unique(batch_size_ * input_size); + out_num_detections_d_ = autoware::cuda_utils::make_unique(batch_size_); + out_boxes_d_ = autoware::cuda_utils::make_unique(batch_size_ * max_detections_ * 4); + out_scores_d_ = autoware::cuda_utils::make_unique(batch_size_ * max_detections_); + out_classes_d_ = autoware::cuda_utils::make_unique(batch_size_ * max_detections_); } if (multitask_) { // Allocate buffer for segmentation segmentation_out_elem_num_ = 0; for (int m = 0; m < multitask_; m++) { const auto output_dims = - trt_common_->getBindingDimensions(m + 2); // 0 : input, 1 : output for detections + trt_common_->getTensorShape(m + 2); // 0 : input, 1 : output for detections size_t out_elem_num = std::accumulate( output_dims.d + 1, output_dims.d + output_dims.nbDims, 1, std::multiplies()); - out_elem_num = out_elem_num * batch_config[2]; + out_elem_num = out_elem_num * batch_size_; segmentation_out_elem_num_ += out_elem_num; } segmentation_out_elem_num_per_batch_ = - static_cast(segmentation_out_elem_num_ / batch_config[2]); + static_cast(segmentation_out_elem_num_ / batch_size_); segmentation_out_prob_d_ = autoware::cuda_utils::make_unique(segmentation_out_elem_num_); segmentation_out_prob_h_ = autoware::cuda_utils::make_unique_host( @@ -387,7 +395,7 @@ void TrtYoloX::initPreprocessBuffer(int width, int height) src_width_ = width; src_height_ = height; if (use_gpu_preprocess_) { - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); bool const hasRuntimeDim = std::any_of( input_dims.d, input_dims.d + input_dims.nbDims, [](int32_t input_dim) { return input_dim == -1; }); @@ -395,7 +403,9 @@ void TrtYoloX::initPreprocessBuffer(int width, int height) input_dims.d[0] = batch_size_; } if (!image_buf_h_) { - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } scales_.clear(); } const float input_height = static_cast(input_dims.d[2]); @@ -414,7 +424,7 @@ void TrtYoloX::initPreprocessBuffer(int width, int height) size_t argmax_out_elem_num = 0; for (int m = 0; m < multitask_; m++) { const auto output_dims = - trt_common_->getBindingDimensions(m + 2); // 0 : input, 1 : output for detections + trt_common_->getTensorShape(m + 2); // 0 : input, 1 : output for detections const float scale = std::min( output_dims.d[3] / static_cast(width), output_dims.d[2] / static_cast(height)); @@ -441,7 +451,7 @@ void TrtYoloX::printProfiling(void) void TrtYoloX::preprocessGpu(const std::vector & images) { const auto batch_size = images.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; for (const auto & image : images) { @@ -469,7 +479,9 @@ void TrtYoloX::preprocessGpu(const std::vector & images) src_height_ = height; } if (!image_buf_h_) { - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } scales_.clear(); } const float input_height = static_cast(input_dims.d[2]); @@ -495,7 +507,7 @@ void TrtYoloX::preprocessGpu(const std::vector & images) if (multitask_) { for (int m = 0; m < multitask_; m++) { const auto output_dims = - trt_common_->getBindingDimensions(m + 2); // 0: input, 1: output for detections + trt_common_->getTensorShape(m + 2); // 0: input, 1: output for detections const float scale = std::min( output_dims.d[3] / static_cast(image.cols), output_dims.d[2] / static_cast(image.rows)); @@ -530,9 +542,11 @@ void TrtYoloX::preprocessGpu(const std::vector & images) void TrtYoloX::preprocess(const std::vector & images) { const auto batch_size = images.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); std::vector dst_images; @@ -567,9 +581,6 @@ bool TrtYoloX::doInference( if (!setCudaDeviceId(gpu_id_)) { return false; } - if (!trt_common_->isInitialized()) { - return false; - } if (use_gpu_preprocess_) { preprocessGpu(images); @@ -588,7 +599,7 @@ void TrtYoloX::preprocessWithRoiGpu( const std::vector & images, const std::vector & rois) { const auto batch_size = images.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; for (const auto & image : images) { @@ -610,7 +621,9 @@ void TrtYoloX::preprocessWithRoiGpu( src_height_ = height; } if (!image_buf_h_) { - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } } const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); @@ -662,9 +675,11 @@ void TrtYoloX::preprocessWithRoi( const std::vector & images, const std::vector & rois) { const auto batch_size = images.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); std::vector dst_images; @@ -700,7 +715,7 @@ void TrtYoloX::preprocessWithRoi( void TrtYoloX::multiScalePreprocessGpu(const cv::Mat & image, const std::vector & rois) { const auto batch_size = rois.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; @@ -722,7 +737,9 @@ void TrtYoloX::multiScalePreprocessGpu(const cv::Mat & image, const std::vector< src_height_ = height; if (!image_buf_h_) { - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } } const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); @@ -770,9 +787,11 @@ void TrtYoloX::multiScalePreprocessGpu(const cv::Mat & image, const std::vector< void TrtYoloX::multiScalePreprocess(const cv::Mat & image, const std::vector & rois) { const auto batch_size = rois.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); std::vector dst_images; @@ -806,9 +825,6 @@ void TrtYoloX::multiScalePreprocess(const cv::Mat & image, const std::vector & images, ObjectArrays & objects, const std::vector & rois) { - if (!trt_common_->isInitialized()) { - return false; - } if (use_gpu_preprocess_) { preprocessWithRoiGpu(images, rois); } else { @@ -827,9 +843,6 @@ bool TrtYoloX::doInferenceWithRoi( bool TrtYoloX::doMultiScaleInference( const cv::Mat & image, ObjectArrays & objects, const std::vector & rois) { - if (!trt_common_->isInitialized()) { - return false; - } if (use_gpu_preprocess_) { multiScalePreprocessGpu(image, rois); } else { @@ -849,8 +862,10 @@ bool TrtYoloX::feedforward(const std::vector & images, ObjectArrays & o std::vector buffers = { input_d_.get(), out_num_detections_d_.get(), out_boxes_d_.get(), out_scores_d_.get(), out_classes_d_.get()}; - - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); const auto batch_size = images.size(); auto out_num_detections = std::make_unique(batch_size); @@ -903,7 +918,11 @@ bool TrtYoloX::feedforwardAndDecode( if (multitask_) { buffers = {input_d_.get(), out_prob_d_.get(), segmentation_out_prob_d_.get()}; } - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); const auto batch_size = images.size(); @@ -933,7 +952,7 @@ bool TrtYoloX::feedforwardAndDecode( static_cast(segmentation_out_elem_num_ / segmentation_out_elem_num_per_batch_); for (int m = 0; m < multitask_; m++) { const auto output_dims = - trt_common_->getBindingDimensions(m + 2); // 0 : input, 1 : output for detections + trt_common_->getTensorShape(m + 2); // 0 : input, 1 : output for detections size_t out_elem_num = std::accumulate( output_dims.d + 1, output_dims.d + output_dims.nbDims, 1, std::multiplies()); out_elem_num = out_elem_num * batch; @@ -972,7 +991,10 @@ bool TrtYoloX::multiScaleFeedforward(const cv::Mat & image, int batch_size, Obje input_d_.get(), out_num_detections_d_.get(), out_boxes_d_.get(), out_scores_d_.get(), out_classes_d_.get()}; - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); auto out_num_detections = std::make_unique(batch_size); auto out_boxes = std::make_unique(4 * batch_size * max_detections_); @@ -1020,7 +1042,10 @@ bool TrtYoloX::multiScaleFeedforwardAndDecode( const cv::Mat & image, int batch_size, ObjectArrays & objects) { std::vector buffers = {input_d_.get(), out_prob_d_.get()}; - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); CHECK_CUDA_ERROR(cudaMemcpyAsync( out_prob_h_.get(), out_prob_d_.get(), sizeof(float) * out_elem_num_, cudaMemcpyDeviceToHost, @@ -1042,7 +1067,7 @@ void TrtYoloX::decodeOutputs( float * prob, ObjectArray & objects, float scale, cv::Size & img_size) const { ObjectArray proposals; - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); std::vector grid_strides; @@ -1298,4 +1323,9 @@ void TrtYoloX::getColorizedMask( } } +int TrtYoloX::getBatchSize() const +{ + return batch_size_; +} + } // namespace autoware::tensorrt_yolox diff --git a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp index 94a3a37a4d08f..2048279dcf3f1 100644 --- a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp +++ b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp @@ -83,19 +83,18 @@ TrtYoloXNode::TrtYoloXNode(const rclcpp::NodeOptions & node_options) roi_overlay_segment_labels_.ANIMAL = declare_parameter("roi_overlay_segment_label.ANIMAL"); replaceLabelMap(); - autoware::tensorrt_common::BuildConfig build_config( - calibration_algorithm, dla_core_id, quantize_first_layer, quantize_last_layer, - profile_per_layer, clip_value); + TrtCommonConfig trt_config( + model_path, precision, "", (1ULL << 30U), dla_core_id, profile_per_layer); + + CalibrationConfig calib_config( + calibration_algorithm, quantize_first_layer, quantize_last_layer, clip_value); const double norm_factor = 1.0; const std::string cache_dir = ""; - const autoware::tensorrt_common::BatchConfig batch_config{1, 1, 1}; - const size_t max_workspace_size = (1 << 30); trt_yolox_ = std::make_unique( - model_path, precision, label_map_.size(), score_threshold, nms_threshold, build_config, - preprocess_on_gpu, gpu_id, calibration_image_list_path, norm_factor, cache_dir, batch_config, - max_workspace_size, color_map_path); + trt_config, label_map_.size(), score_threshold, nms_threshold, preprocess_on_gpu, gpu_id, + calibration_image_list_path, norm_factor, cache_dir, color_map_path, calib_config); if (!trt_yolox_->isGPUInitialized()) { RCLCPP_ERROR(this->get_logger(), "GPU %d does not exist or is not suitable.", gpu_id); diff --git a/perception/autoware_tensorrt_yolox/src/yolox_single_image_inference_node.cpp b/perception/autoware_tensorrt_yolox/src/yolox_single_image_inference_node.cpp index 243e82c65dab9..63c0ee0cd3e8a 100644 --- a/perception/autoware_tensorrt_yolox/src/yolox_single_image_inference_node.cpp +++ b/perception/autoware_tensorrt_yolox/src/yolox_single_image_inference_node.cpp @@ -46,7 +46,9 @@ class YoloXSingleImageInferenceNode : public rclcpp::Node const auto output_image_path = declare_parameter("output_image_path", p.string() + "_detect" + ext); - auto trt_yolox = std::make_unique(model_path, precision); + auto trt_config = tensorrt_common::TrtCommonConfig(model_path, precision); + + auto trt_yolox = std::make_unique(trt_config); auto image = cv::imread(image_path); tensorrt_yolox::ObjectArrays objects; std::vector masks; diff --git a/perception/autoware_traffic_light_classifier/CMakeLists.txt b/perception/autoware_traffic_light_classifier/CMakeLists.txt index d132577743ab0..181037caebfa5 100644 --- a/perception/autoware_traffic_light_classifier/CMakeLists.txt +++ b/perception/autoware_traffic_light_classifier/CMakeLists.txt @@ -34,7 +34,6 @@ if(NVINFER AND NVONNXPARSER AND NVINFER_PLUGIN) if(CUDA_VERBOSE) message(STATUS "TensorRT is available!") message(STATUS "NVINFER: ${NVINFER}") - message(STATUS "NVPARSERS: ${NVPARSERS}") message(STATUS "NVINFER_PLUGIN: ${NVINFER_PLUGIN}") message(STATUS "NVONNXPARSER: ${NVONNXPARSER}") endif() diff --git a/perception/autoware_traffic_light_classifier/README.md b/perception/autoware_traffic_light_classifier/README.md index 6e720aabc7593..7dcd4a73380bb 100644 --- a/perception/autoware_traffic_light_classifier/README.md +++ b/perception/autoware_traffic_light_classifier/README.md @@ -1,22 +1,33 @@ -# traffic_light_classifier +# autoware_traffic_light_classifier ## Purpose -traffic_light_classifier is a package for classifying traffic light labels using cropped image around a traffic light. This package has two classifier models: `cnn_classifier` and `hsv_classifier`. +`autoware_traffic_light_classifier` is a package for classifying traffic light labels using cropped image around a traffic light. This package has two classifier models: `cnn_classifier` and `hsv_classifier`. ## Inner-workings / Algorithms +If height and width of `~/input/rois` is `0`, color, shape, and confidence of `~/output/traffic_signals` become `UNKNOWN`, `CIRCLE`, and `0.0`. +If `~/input/rois` is judged as backlight, color, shape, and confidence of `~/output/traffic_signals` become `UNKNOWN`, `UNKNOWN`, and `0.0`. + ### cnn_classifier -Traffic light labels are classified by EfficientNet-b1 or MobileNet-v2. -Totally 83400 (58600 for training, 14800 for evaluation and 10000 for test) TIER IV internal images of Japanese traffic lights were used for fine-tuning. -The information of the models is listed here: +Traffic light labels are classified by EfficientNet-b1 or MobileNet-v2. +We trained classifiers for vehicular signals and pedestrian signals separately. +For vehicular signals, a total of 83400 (58600 for training, 14800 for evaluation and 10000 for test) TIER IV internal images of Japanese traffic lights were used for fine-tuning. | Name | Input Size | Test Accuracy | | --------------- | ---------- | ------------- | | EfficientNet-b1 | 128 x 128 | 99.76% | | MobileNet-v2 | 224 x 224 | 99.81% | +For pedestrian signals, a total of 21199 (17860 for training, 2114 for evaluation and 1225 for test) TIER IV internal images of Japanese traffic lights were used for fine-tuning. +The information of the models is listed here: + +| Name | Input Size | Test Accuracy | +| --------------- | ---------- | ------------- | +| EfficientNet-b1 | 128 x 128 | 97.89% | +| MobileNet-v2 | 224 x 224 | 99.10% | + ### hsv_classifier Traffic light colors (green, yellow and red) are classified in HSV model. @@ -52,11 +63,12 @@ These colors and shapes are assigned to the message as follows: ### Node Parameters -| Name | Type | Description | -| --------------------- | ----- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `classifier_type` | int | if the value is `1`, cnn_classifier is used | -| `data_path` | str | packages data and artifacts directory path | -| `backlight_threshold` | float | If the intensity get grater than this overwrite with UNKNOWN in corresponding RoI. Note that, if the value is much higher, the node only overwrites in the harsher backlight situations. Therefore, If you wouldn't like to use this feature set this value to `1.0`. The value can be `[0.0, 1.0]`. The confidence of overwritten signal is set to `0.0`. | +| Name | Type | Description | +| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `classifier_type` | int | If the value is `1`, cnn_classifier is used | +| `data_path` | str | Packages data and artifacts directory path | +| `backlight_threshold` | double | If the intensity of light is grater than this threshold, the color and shape of the corresponding ROI will be overwritten with UNKNOWN, and the confidence of the overwritten signal will be set to `0.0`. The value should be set in the range of `[0.0, 1.0]`. If you wouldn't like to use this feature, please set it to `1.0`. | +| `classify_traffic_light_type` | int | If the value is `0`, vehicular signals are classified. If the value is `1`, pedestrian signals are classified. | ### Core Parameters diff --git a/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.cpp b/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.cpp index f9bf2509aa2ae..d47cb1500fffd 100644 --- a/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.cpp +++ b/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.cpp @@ -51,13 +51,10 @@ CNNClassifier::CNNClassifier(rclcpp::Node * node_ptr) : node_ptr_(node_ptr) } readLabelfile(label_file_path, labels_); - nvinfer1::Dims input_dim = autoware::tensorrt_common::get_input_dims(model_file_path); - assert(input_dim.d[0] > 0); - batch_size_ = input_dim.d[0]; - autoware::tensorrt_common::BatchConfig batch_config{batch_size_, batch_size_, batch_size_}; classifier_ = std::make_unique( - model_file_path, precision, batch_config, mean_, std_); + model_file_path, precision, mean_, std_); + batch_size_ = classifier_->getBatchSize(); if (node_ptr_->declare_parameter("build_only", false)) { RCLCPP_INFO(node_ptr_->get_logger(), "TensorRT engine is built and shutdown node."); rclcpp::shutdown(); diff --git a/perception/autoware_traffic_light_fine_detector/CMakeLists.txt b/perception/autoware_traffic_light_fine_detector/CMakeLists.txt index 80d6e43c098ed..dffbab0e77681 100644 --- a/perception/autoware_traffic_light_fine_detector/CMakeLists.txt +++ b/perception/autoware_traffic_light_fine_detector/CMakeLists.txt @@ -39,7 +39,6 @@ if(NVINFER AND NVONNXPARSER AND NVINFER_PLUGIN) if(CUDA_VERBOSE) message(STATUS "TensorRT is available!") message(STATUS "NVINFER: ${NVINFER}") - message(STATUS "NVPARSERS: ${NVPARSERS}") message(STATUS "NVINFER_PLUGIN: ${NVINFER_PLUGIN}") message(STATUS "NVONNXPARSER: ${NVONNXPARSER}") endif() diff --git a/perception/autoware_traffic_light_fine_detector/README.md b/perception/autoware_traffic_light_fine_detector/README.md index 35b55c84aa087..df9f297422fce 100644 --- a/perception/autoware_traffic_light_fine_detector/README.md +++ b/perception/autoware_traffic_light_fine_detector/README.md @@ -1,8 +1,8 @@ -# traffic_light_fine_detector +# autoware_traffic_light_fine_detector ## Purpose -It is a package for traffic light detection using YoloX-s. +It is a package for traffic light detection using YOLOX-s. ## Training Information @@ -21,17 +21,18 @@ Please visit [autoware-documentation](https://github.com/autowarefoundation/auto ## Inner-workings / Algorithms -Based on the camera image and the global ROI array detected by `map_based_detection` node, a CNN-based detection method enables highly accurate traffic light detection. +Based on the camera image and the global ROI array detected by `map_based_detector` node, a CNN-based detection method enables highly accurate traffic light detection. If can not detect traffic light, x_offset, y_offset, height and width of output ROI become `0`. +ROIs detected from YOLOX will be selected by a combination of `expect/rois`. At this time, evaluate the whole as ROIs, not just the ROI alone. ## Inputs / Outputs ### Input -| Name | Type | Description | -| --------------- | -------------------------------------------------- | ------------------------------------------------------------------- | -| `~/input/image` | `sensor_msgs/Image` | The full size camera image | -| `~/input/rois` | `tier4_perception_msgs::msg::TrafficLightRoiArray` | The array of ROIs detected by map_based_detector | -| `~/expect/rois` | `tier4_perception_msgs::msg::TrafficLightRoiArray` | The array of ROIs detected by map_based_detector without any offset | +| Name | Type | Description | +| --------------- | -------------------------------------------------- | -------------------------------------------------------------------------------------------------------------- | +| `~/input/image` | `sensor_msgs::msg::Image` | The full size camera image | +| `~/input/rois` | `tier4_perception_msgs::msg::TrafficLightRoiArray` | The array of ROIs detected by map_based_detector | +| `~/expect/rois` | `tier4_perception_msgs::msg::TrafficLightRoiArray` | The array of ROIs detected by map_based_detector without any offset, used to select the best detection results | ### Output @@ -56,7 +57,7 @@ Based on the camera image and the global ROI array detected by `map_based_detect | `data_path` | string | "$(env HOME)/autoware_data" | packages data and artifacts directory path | | `fine_detector_model_path` | string | "" | The onnx file name for yolo model | | `fine_detector_label_path` | string | "" | The label file with label names for detected objects written on it | -| `fine_detector_precision` | string | "fp32" | The inference mode: "fp32", "fp16" | +| `fine_detector_precision` | string | "fp16" | The inference mode: "fp32", "fp16" | | `approximate_sync` | bool | false | Flag for whether to ues approximate sync policy | | `gpu_id` | integer | 0 | ID for the selecting CUDA GPU device | diff --git a/perception/autoware_traffic_light_fine_detector/config/traffic_light_fine_detector.param.yaml b/perception/autoware_traffic_light_fine_detector/config/traffic_light_fine_detector.param.yaml index 36d4413472a0b..b647729a27de5 100644 --- a/perception/autoware_traffic_light_fine_detector/config/traffic_light_fine_detector.param.yaml +++ b/perception/autoware_traffic_light_fine_detector/config/traffic_light_fine_detector.param.yaml @@ -5,4 +5,5 @@ fine_detector_precision: fp16 fine_detector_score_thresh: 0.3 fine_detector_nms_thresh: 0.65 + approximate_sync: false gpu_id: 0 diff --git a/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp index 37ffca4a9526c..bc3d29ce456a8 100644 --- a/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp +++ b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp @@ -61,7 +61,7 @@ TrafficLightFineDetectorNode::TrafficLightFineDetectorNode(const rclcpp::NodeOpt std::string model_path = declare_parameter("fine_detector_model_path", ""); std::string label_path = declare_parameter("fine_detector_label_path", ""); - std::string precision = declare_parameter("fine_detector_precision", "fp32"); + std::string precision = declare_parameter("fine_detector_precision", "fp16"); const uint8_t gpu_id = declare_parameter("gpu_id", 0); // Objects with a score lower than this value will be ignored. // This threshold will be ignored if specified model contains EfficientNMS_TRT module in it @@ -75,21 +75,18 @@ TrafficLightFineDetectorNode::TrafficLightFineDetectorNode(const rclcpp::NodeOpt RCLCPP_ERROR(this->get_logger(), "Could not find tlr id"); } - const autoware::tensorrt_common::BuildConfig build_config = - autoware::tensorrt_common::BuildConfig("MinMax", -1, false, false, false, 0.0); - const bool cuda_preprocess = true; const std::string calib_image_list = ""; const double scale = 1.0; const std::string cache_dir = ""; - nvinfer1::Dims input_dim = autoware::tensorrt_common::get_input_dims(model_path); - assert(input_dim.d[0] > 0); - batch_size_ = input_dim.d[0]; - const autoware::tensorrt_common::BatchConfig batch_config{batch_size_, batch_size_, batch_size_}; + + auto trt_config = autoware::tensorrt_common::TrtCommonConfig(model_path, precision); trt_yolox_ = std::make_unique( - model_path, precision, num_class, score_thresh_, nms_threshold, build_config, cuda_preprocess, - gpu_id, calib_image_list, scale, cache_dir, batch_config); + trt_config, num_class, score_thresh_, nms_threshold, cuda_preprocess, gpu_id, calib_image_list, + scale, cache_dir); + + batch_size_ = trt_yolox_->getBatchSize(); if (!trt_yolox_->isGPUInitialized()) { RCLCPP_ERROR(this->get_logger(), "GPU %d does not exist or is not suitable.", gpu_id); diff --git a/perception/autoware_traffic_light_map_based_detector/README.md b/perception/autoware_traffic_light_map_based_detector/README.md index 8a59db19ae64d..6ef54d448b415 100644 --- a/perception/autoware_traffic_light_map_based_detector/README.md +++ b/perception/autoware_traffic_light_map_based_detector/README.md @@ -1,4 +1,4 @@ -# The `autoware_traffic_light_map_based_detector` Package +# autoware_traffic_light_map_based_detector ## Overview @@ -9,34 +9,36 @@ Calibration and vibration errors can be entered as parameters, and the size of t ![traffic_light_map_based_detector_result](./docs/traffic_light_map_based_detector_result.svg) If the node receives route information, it only looks at traffic lights on that route. -If the node receives no route information, it looks at a radius of 200 meters and the angle between the traffic light and the camera is less than 40 degrees. +If the node receives no route information, it looks at them within a radius of `max_detection_range` and the angle between the traffic light and the camera is less than `car_traffic_light_max_angle_range` or `pedestrian_traffic_light_max_angle_range`. ## Input topics -| Name | Type | Description | -| -------------------- | ------------------------------------- | ----------------------- | -| `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | -| `~input/camera_info` | sensor_msgs::CameraInfo | target camera parameter | -| `~input/route` | autoware_planning_msgs::LaneletRoute | optional: route | +| Name | Type | Description | +| --------------------- | ----------------------------------------- | ----------------------- | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/camera_info` | sensor_msgs::msg::CameraInfo | target camera parameter | +| `~/input/route` | autoware_planning_msgs::msg::LaneletRoute | optional: route | ## Output topics -| Name | Type | Description | -| ---------------- | ------------------------------------------- | -------------------------------------------------------------------- | -| `~output/rois` | tier4_perception_msgs::TrafficLightRoiArray | location of traffic lights in image corresponding to the camera info | -| `~expect/rois` | tier4_perception_msgs::TrafficLightRoiArray | location of traffic lights in image without any offset | -| `~debug/markers` | visualization_msgs::MarkerArray | visualization to debug | +| Name | Type | Description | +| ----------------- | ------------------------------------------------ | ------------------------------------------------------------------------- | +| `~/output/rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | location of traffic lights in image corresponding to the camera info | +| `~/expect/rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | location of traffic lights in image without any offset | +| `~/debug/markers` | visualization_msgs::msg::MarkerArray | markers which show a line that combines from camera to each traffic light | ## Node parameters -| Parameter | Type | Description | -| ---------------------- | ------ | --------------------------------------------------------------------- | -| `max_vibration_pitch` | double | Maximum error in pitch direction. If -5~+5, it will be 10. | -| `max_vibration_yaw` | double | Maximum error in yaw direction. If -5~+5, it will be 10. | -| `max_vibration_height` | double | Maximum error in height direction. If -5~+5, it will be 10. | -| `max_vibration_width` | double | Maximum error in width direction. If -5~+5, it will be 10. | -| `max_vibration_depth` | double | Maximum error in depth direction. If -5~+5, it will be 10. | -| `max_detection_range` | double | Maximum detection range in meters. Must be positive | -| `min_timestamp_offset` | double | Minimum timestamp offset when searching for corresponding tf | -| `max_timestamp_offset` | double | Maximum timestamp offset when searching for corresponding tf | -| `timestamp_sample_len` | double | sampling length between min_timestamp_offset and max_timestamp_offset | +| Parameter | Type | Description | +| ------------------------------------------ | ------ | ----------------------------------------------------------------------------------------------- | +| `max_vibration_pitch` | double | Maximum error in pitch direction. If -5~+5, it will be 10. | +| `max_vibration_yaw` | double | Maximum error in yaw direction. If -5~+5, it will be 10. | +| `max_vibration_height` | double | Maximum error in height direction. If -5~+5, it will be 10. | +| `max_vibration_width` | double | Maximum error in width direction. If -5~+5, it will be 10. | +| `max_vibration_depth` | double | Maximum error in depth direction. If -5~+5, it will be 10. | +| `max_detection_range` | double | Maximum detection range in meters. Must be positive. | +| `min_timestamp_offset` | double | Minimum timestamp offset when searching for corresponding tf. | +| `max_timestamp_offset` | double | Maximum timestamp offset when searching for corresponding tf. | +| `timestamp_sample_len` | double | Sampling length between min_timestamp_offset and max_timestamp_offset. | +| `car_traffic_light_max_angle_range` | double | Maximum angle between the vehicular traffic light and the camera in degrees. Must be positive. | +| `pedestrian_traffic_light_max_angle_range` | double | Maximum angle between the pedestrian traffic light and the camera in degrees. Must be positive. | diff --git a/perception/autoware_traffic_light_multi_camera_fusion/README.md b/perception/autoware_traffic_light_multi_camera_fusion/README.md index f7ee294cda147..b54c623f5d750 100644 --- a/perception/autoware_traffic_light_multi_camera_fusion/README.md +++ b/perception/autoware_traffic_light_multi_camera_fusion/README.md @@ -1,29 +1,36 @@ -# The `traffic_light_multi_camera_fusion` Package +# autoware_traffic_light_multi_camera_fusion ## Overview -`traffic_light_multi_camera_fusion` performs traffic light signal fusion which can be summarized as the following two tasks: +`autoware_traffic_light_multi_camera_fusion` performs traffic light signal fusion which can be summarized as the following two tasks: -1. Multi-Camera-Fusion: performed on single traffic light signal detected by different cameras. -2. Group-Fusion: performed on traffic light signals within the same group, which means traffic lights sharing the same regulatory element id defined in lanelet2 map. +1. Multi-Camera-Fusion: fusion each traffic light signal detected by different cameras. +2. Group-Fusion: Fusion each traffic light signal within the same group, which means traffic lights share the same regulatory element ID defined in lanelet2 map. + +The fusion method is below. + +1. Use the results of the new timestamp if the results are from the same sensor +2. Use the results that are not `elements.size() == 1 && color == UNKNOWN && shape == UNKNOWN` +3. Use the results that each vertex of ROI is not at the edge of the image +4. Use the results of high confidence ## Input topics For every camera, the following three topics are subscribed: -| Name | Type | Description | -| -------------------------------------- | ---------------------------------------------- | --------------------------------------------------- | -| `~//camera_info` | sensor_msgs::CameraInfo | camera info from traffic_light_map_based_detector | -| `~//rois` | tier4_perception_msgs::TrafficLightRoiArray | detection roi from traffic_light_fine_detector | -| `~//traffic_signals` | tier4_perception_msgs::TrafficLightSignalArray | classification result from traffic_light_classifier | +| Name | Type | Description | +| ----------------------------------------------------- | ------------------------------------------------ | ------------------------------------- | +| `~//camera_info` | sensor_msgs::msg::CameraInfo | camera info from map_based_detector | +| `~//detection/rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | detection roi from fine_detector | +| `~//classification/traffic_signals` | tier4_perception_msgs::msg::TrafficLightArray | classification result from classifier | You don't need to configure these topics manually. Just provide the `camera_namespaces` parameter and the node will automatically extract the `` and create the subscribers. ## Output topics -| Name | Type | Description | -| -------------------------- | ------------------------------------------------- | ---------------------------------- | -| `~/output/traffic_signals` | autoware_perception_msgs::TrafficLightSignalArray | traffic light signal fusion result | +| Name | Type | Description | +| -------------------------- | ----------------------------------------------------- | ---------------------------------- | +| `~/output/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | traffic light signal fusion result | ## Node parameters diff --git a/perception/autoware_traffic_light_occlusion_predictor/README.md b/perception/autoware_traffic_light_occlusion_predictor/README.md index bc57dbea76c97..5d2e320dfc72a 100644 --- a/perception/autoware_traffic_light_occlusion_predictor/README.md +++ b/perception/autoware_traffic_light_occlusion_predictor/README.md @@ -1,8 +1,10 @@ -# The `autoware_traffic_light_occlusion_predictor` Package +# autoware_traffic_light_occlusion_predictor ## Overview `autoware_traffic_light_occlusion_predictor` receives the detected traffic lights rois and calculates the occlusion ratios of each roi with point cloud. +If that rois is judged as occlusion, color, shape, and confidence of `~/output/traffic_signals` become `UNKNOWN`, `UNKNOWN`, and `0.0`. +This node publishes when each car and pedestrian `traffic_signals` is arrived and processed. For each traffic light roi, hundreds of pixels would be selected and projected into the 3D space. Then from the camera point of view, the number of projected pixels that are occluded by the point cloud is counted and used for calculating the occlusion ratio for the roi. As shown in follow image, the red pixels are occluded and the occlusion ratio is the number of red pixels divided by the total pixel numbers. @@ -12,18 +14,20 @@ If no point cloud is received or all point clouds have very large stamp differen ## Input topics -| Name | Type | Description | -| -------------------- | ---------------------------------------------- | ------------------------ | -| `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | -| `~/input/rois` | autoware_perception_msgs::TrafficLightRoiArray | traffic light detections | -| `~input/camera_info` | sensor_msgs::CameraInfo | target camera parameter | -| `~/input/cloud` | sensor_msgs::PointCloud2 | LiDAR point cloud | +| Name | Type | Description | +| ------------------------------------ | ------------------------------------------------ | -------------------------------- | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/car/traffic_signals` | tier4_perception_msgs::msg::TrafficLightArray | vehicular traffic light signals | +| `~/input/pedestrian/traffic_signals` | tier4_perception_msgs::msg::TrafficLightArray | pedestrian traffic light signals | +| `~/input/rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | traffic light detections | +| `~/input/camera_info` | sensor_msgs::msg::CameraInfo | target camera parameter | +| `~/input/cloud` | sensor_msgs::msg::PointCloud2 | LiDAR point cloud | ## Output topics -| Name | Type | Description | -| -------------------- | ---------------------------------------------------- | ---------------------------- | -| `~/output/occlusion` | autoware_perception_msgs::TrafficLightOcclusionArray | occlusion ratios of each roi | +| Name | Type | Description | +| -------------------------- | --------------------------------------------- | -------------------------------------------------------------- | +| `~/output/traffic_signals` | tier4_perception_msgs::msg::TrafficLightArray | traffic light signals which occluded image results overwritten | ## Node parameters @@ -34,3 +38,4 @@ If no point cloud is received or all point clouds have very large stamp differen | `max_valid_pt_dist` | double | The points within this distance would be used for calculation | | `max_image_cloud_delay` | double | The maximum delay between LiDAR point cloud and camera image | | `max_wait_t` | double | The maximum time waiting for the LiDAR point cloud | +| `max_occlusion_ratio` | int | The maximum occlusion ratio for setting signal as unknown | diff --git a/planning/README.md b/planning/README.md index ccf8288df3911..3fadafe54ed4c 100644 --- a/planning/README.md +++ b/planning/README.md @@ -18,7 +18,7 @@ Enabling and disabling modules involves managing settings in key configuration a ### Key Files for Configuration -The `default_preset.yaml` file acts as the primary configuration file, where planning modules can be disable or enabled. Furthermore, users can also set the type of motion planner across various motion planners. For example: +The `default_preset.yaml` file acts as the primary configuration file, where planning modules can be disabled or enabled. Furthermore, users can also set the type of motion planner across various motion planners. For example: - `launch_avoidance_module`: Set to `true` to enable the avoidance module, or `false` to disable it. - `motion_stop_planner_type`: Set `default` to either `obstacle_stop_planner` or `obstacle_cruise_planner`. @@ -35,7 +35,7 @@ The [launch files](https://github.com/autowarefoundation/autoware.universe/tree/ corresponds to launch_avoidance_module from `default_preset.yaml`. -### Parameters configuration +### Parameter Configuration There are multiple parameters available for configuration, and users have the option to modify them in [here](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning). It's important to note that not all parameters are adjustable via `rqt_reconfigure`. To ensure the changes are effective, modify the parameters and then restart Autoware. Additionally, detailed information about each parameter is available in the corresponding documents under the planning tab. @@ -43,7 +43,7 @@ There are multiple parameters available for configuration, and users have the op This guide outlines the steps for integrating your custom module into Autoware: -- Add your modules to the `default_preset.yaml` file. For example +- Add your modules to the `default_preset.yaml` file. For example: ```yaml - arg: @@ -51,7 +51,7 @@ This guide outlines the steps for integrating your custom module into Autoware: default: "true" ``` -- Incorporate your modules into the [launcher](https://github.com/autowarefoundation/autoware.universe/tree/main/launch/tier4_planning_launch/launch/scenario_planning). For example in [behavior_planning.launch.xml](https://github.com/autowarefoundation/autoware.universe/blob/main/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml): +- Incorporate your modules into the [launcher](https://github.com/autowarefoundation/autoware.universe/tree/main/launch/tier4_planning_launch/launch/scenario_planning). For example, in [behavior_planning.launch.xml](https://github.com/autowarefoundation/autoware.universe/blob/main/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml): ```xml @@ -63,14 +63,14 @@ This guide outlines the steps for integrating your custom module into Autoware: /> ``` -- If applicable, place your parameter folder within the appropriate existing parameter folder. For example [intersection_module's parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml) is in [behavior_velocity_planner](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner). -- Insert the path of your parameters in the [tier4_planning_component.launch.xml](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/launch/components/tier4_planning_component.launch.xml). For example `behavior_velocity_planner_intersection_module_param_path` is used. +- If applicable, place your parameter folder within the appropriate existing parameter folder. For example, [intersection_module's parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml) are in [behavior_velocity_planner](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner). +- Insert the path of your parameters in the [tier4_planning_component.launch.xml](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/launch/components/tier4_planning_component.launch.xml). For example, `behavior_velocity_planner_intersection_module_param_path` is used. ```xml ``` -- Define your parameter path variable within the corresponding launcher. For example in [behavior_planning.launch.xml](https://github.com/autowarefoundation/autoware.universe/blob/04aa54bf5fb0c88e70198ca74b9ac343cc3457bf/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml#L191) +- Define your parameter path variable within the corresponding launcher. For example, in [behavior_planning.launch.xml](https://github.com/autowarefoundation/autoware.universe/blob/04aa54bf5fb0c88e70198ca74b9ac343cc3457bf/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml#L191) ```xml @@ -82,7 +82,7 @@ This guide outlines the steps for integrating your custom module into Autoware: ## Join Our Community-Driven Effort -Autoware thrives on community collaboration. Every contribution, big or small, is invaluable to us. Whether it's reporting bugs, suggesting improvements, offering new ideas, or anything else you can think of – we welcome it all with open arms. +Autoware thrives on community collaboration. Every contribution, big or small, is invaluable to us. Whether it's reporting bugs, suggesting improvements, offering new ideas, or anything else you can think of – we welcome all contributions with open arms. ### How to Contribute? @@ -105,7 +105,7 @@ Interested in joining our meetings? We’d love to have you! For more informatio Occasionally, we publish papers specific to the Planning Component in Autoware. We encourage you to explore these publications and find valuable insights for your work. If you find them useful and incorporate any of our methodologies or algorithms in your projects, citing our papers would be immensely helpful. This support allows us to reach a broader audience and continue contributing to the field. -If you use the Jerk Constrained Velocity Planning algorithm in [Motion Velocity Smoother](./autoware_velocity_smoother/README.md) module in the Planning Component, we kindly request you to cite the relevant paper. +If you use the Jerk Constrained Velocity Planning algorithm in the [Motion Velocity Smoother](./autoware_velocity_smoother/README.md) module in the Planning Component, we kindly request you cite the relevant paper. diff --git a/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp b/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp index 4fed5bcb6cff3..b6749117a6177 100644 --- a/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp +++ b/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp @@ -58,10 +58,10 @@ #include #include +#include #include #include #include -#include #include #include @@ -99,7 +99,8 @@ class CostmapGenerator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_costmap_; rclcpp::Publisher::SharedPtr pub_occupancy_grid_; rclcpp::Publisher::SharedPtr pub_processing_time_; - rclcpp::Publisher::SharedPtr pub_processing_time_ms_; + rclcpp::Publisher::SharedPtr + pub_processing_time_ms_; rclcpp::Subscription::SharedPtr sub_lanelet_bin_map_; autoware::universe_utils::InterProcessPollingSubscriber diff --git a/planning/autoware_costmap_generator/src/costmap_generator.cpp b/planning/autoware_costmap_generator/src/costmap_generator.cpp index 081a6ead91cda..70b8051bcdc12 100644 --- a/planning/autoware_costmap_generator/src/costmap_generator.cpp +++ b/planning/autoware_costmap_generator/src/costmap_generator.cpp @@ -176,7 +176,8 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) create_publisher("processing_time", 1); time_keeper_ = std::make_shared(pub_processing_time_); pub_processing_time_ms_ = - this->create_publisher("~/debug/processing_time_ms", 1); + this->create_publisher( + "~/debug/processing_time_ms", 1); // Timer const auto period_ns = rclcpp::Rate(param_->update_rate).period(); @@ -269,7 +270,7 @@ void CostmapGenerator::onTimer() if (!isActive()) { // Publish ProcessingTime - 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 = stop_watch.toc(); pub_processing_time_ms_->publish(processing_time_msg); @@ -485,7 +486,7 @@ void CostmapGenerator::publishCostmap( pub_costmap_->publish(*out_gridmap_msg); // Publish ProcessingTime - 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 = stop_watch.toc(); pub_processing_time_ms_->publish(processing_time_msg); diff --git a/planning/autoware_external_velocity_limit_selector/CMakeLists.txt b/planning/autoware_external_velocity_limit_selector/CMakeLists.txt index ca758d1262b48..aa8d07d91135e 100644 --- a/planning/autoware_external_velocity_limit_selector/CMakeLists.txt +++ b/planning/autoware_external_velocity_limit_selector/CMakeLists.txt @@ -21,6 +21,13 @@ rclcpp_components_register_node(external_velocity_limit_selector_node EXECUTABLE external_velocity_limit_selector ) +if(BUILD_TESTING) + add_launch_test( + test/test_external_velocity_limit_selector_node_launch.py + TIMEOUT "30" + ) +endif() + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/planning/autoware_external_velocity_limit_selector/package.xml b/planning/autoware_external_velocity_limit_selector/package.xml index b75a4fab72d7c..5bfd3fa936d50 100644 --- a/planning/autoware_external_velocity_limit_selector/package.xml +++ b/planning/autoware_external_velocity_limit_selector/package.xml @@ -28,6 +28,7 @@ ament_lint_auto autoware_lint_common + ros_testing ament_cmake diff --git a/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp index cf42763fc6b60..cc2b134900a31 100644 --- a/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp +++ b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp @@ -14,7 +14,6 @@ #include "autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp" -#include #include #include #include diff --git a/planning/autoware_external_velocity_limit_selector/test/test_external_velocity_limit_selector_node_launch.py b/planning/autoware_external_velocity_limit_selector/test/test_external_velocity_limit_selector_node_launch.py new file mode 100644 index 0000000000000..1c7e882fbc81f --- /dev/null +++ b/planning/autoware_external_velocity_limit_selector/test/test_external_velocity_limit_selector_node_launch.py @@ -0,0 +1,178 @@ +#!/usr/bin/env python3 + +# Copyright 2024 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import AnyLaunchDescriptionSource +from launch.logging import get_logger +import launch_testing +import pytest +from rcl_interfaces.msg import Parameter +from rcl_interfaces.msg import ParameterType +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters +import rclpy +import rclpy.qos +from tier4_planning_msgs.msg import VelocityLimit +from tier4_planning_msgs.msg import VelocityLimitClearCommand + +logger = get_logger(__name__) + + +@pytest.mark.launch_test +def generate_test_description(): + test_external_velocity_limit_selector_launch_file = os.path.join( + get_package_share_directory("autoware_external_velocity_limit_selector"), + "launch", + "external_velocity_limit_selector.launch.xml", + ) + external_velocity_limit_selector = IncludeLaunchDescription( + AnyLaunchDescriptionSource(test_external_velocity_limit_selector_launch_file), + ) + + return launch.LaunchDescription( + [ + external_velocity_limit_selector, + # Start tests right away - no need to wait for anything + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestExternalVelocityLimitSelector(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def velocity_limit_callback(self, msg): + self.msg_buffer_ = msg + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + qos = rclpy.qos.QoSProfile(depth=1, durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL) + self.pub_api_limit_ = self.test_node.create_publisher( + VelocityLimit, "/planning/scenario_planning/max_velocity_default", qos + ) + self.pub_internal_limit_ = self.test_node.create_publisher( + VelocityLimit, "/planning/scenario_planning/max_velocity_candidates", qos + ) + self.pub_clear_limit_ = self.test_node.create_publisher( + VelocityLimitClearCommand, "/planning/scenario_planning/clear_velocity_limit", qos + ) + self.msg_buffer_ = None + self.velocity_limit_output_ = None + self.test_node.create_subscription( + VelocityLimit, + "/planning/scenario_planning/max_velocity", + self.velocity_limit_callback, + 1, + ) + + def wait_for_output(self): + while not self.msg_buffer_: + rclpy.spin_once(self.test_node, timeout_sec=0.1) + self.velocity_limit_output_ = self.msg_buffer_ + self.msg_buffer_ = None + + def tearDown(self): + self.test_node.destroy_node() + + def update_max_vel_param(self, max_vel): + set_params_client = self.test_node.create_client( + SetParameters, "/external_velocity_limit_selector/set_parameters" + ) + while not set_params_client.wait_for_service(timeout_sec=1.0): + continue + set_params_request = SetParameters.Request() + set_params_request.parameters = [ + Parameter( + name="max_vel", + value=ParameterValue(type=ParameterType.PARAMETER_DOUBLE, double_value=max_vel), + ), + ] + future = set_params_client.call_async(set_params_request) + rclpy.spin_until_future_complete(self.test_node, future) + + if future.result() is None: + self.test_node.get_logger().error( + "Exception while calling service: %r" % future.exception() + ) + raise self.failureException("setting of initial parameters failed") + + @staticmethod + def make_velocity_limit_msg(vel): + msg = VelocityLimit() + msg.use_constraints = False + msg.max_velocity = vel + return msg + + def test_external_velocity_limit_selector_node(self): + self.update_max_vel_param(15.0) + # clear velocity limit to trigger first output + clear_cmd = VelocityLimitClearCommand(command=True) + self.pub_clear_limit_.publish(clear_cmd) + self.wait_for_output() + # velocity limit is 0 before any limit is set + self.assertEqual(self.velocity_limit_output_.max_velocity, 0.0) + + # Send velocity limits + # new API velocity limit: higher than the node param -> limit is set to the param value + api_limit = self.make_velocity_limit_msg(20.0) + self.pub_api_limit_.publish(api_limit) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 15.0) + # new API velocity limit + api_limit = self.make_velocity_limit_msg(10.0) + self.pub_api_limit_.publish(api_limit) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 10.0) + # new INTERNAL velocity limit + internal_limit = self.make_velocity_limit_msg(5.0) + self.pub_internal_limit_.publish(internal_limit) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 5.0) + # CLEAR: back to API velocity limit + self.pub_clear_limit_.publish(clear_cmd) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 10.0) + # lower the max_vel node parameter + self.update_max_vel_param(2.5) + self.pub_clear_limit_.publish(clear_cmd) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 2.5) + # velocity limit set by internal limit is no longer applied since above max_vel parameter + internal_limit = self.make_velocity_limit_msg(5.0) + self.pub_internal_limit_.publish(internal_limit) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 2.5) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp index 1f5a52ca040a0..bd431372b70dc 100644 --- a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp +++ b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp @@ -39,13 +39,13 @@ #include #include +#include #include #include #include #include #include #include -#include #include #ifdef ROS_DISTRO_GALACTIC @@ -112,7 +112,8 @@ class FreespacePlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_pose_array_pub_; rclcpp::Publisher::SharedPtr debug_partial_pose_array_pub_; rclcpp::Publisher::SharedPtr parking_state_pub_; - rclcpp::Publisher::SharedPtr processing_time_pub_; + rclcpp::Publisher::SharedPtr + processing_time_pub_; rclcpp::Subscription::SharedPtr route_sub_; diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp index a937114e653c6..2749408bedb7b 100644 --- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp @@ -95,8 +95,8 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti debug_pose_array_pub_ = create_publisher("~/debug/pose_array", qos); debug_partial_pose_array_pub_ = create_publisher("~/debug/partial_pose_array", qos); parking_state_pub_ = create_publisher("is_completed", qos); - processing_time_pub_ = - create_publisher("~/debug/processing_time_ms", 1); + processing_time_pub_ = create_publisher( + "~/debug/processing_time_ms", 1); } // TF @@ -362,7 +362,7 @@ void FreespacePlannerNode::onTimer() is_new_parking_cycle_ = false; // Publish ProcessingTime - 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 = stop_watch.toc(); processing_time_pub_->publish(processing_time_msg); diff --git a/planning/autoware_mission_planner/README.md b/planning/autoware_mission_planner/README.md index b5993d0106add..720ecd976f65f 100644 --- a/planning/autoware_mission_planner/README.md +++ b/planning/autoware_mission_planner/README.md @@ -53,6 +53,7 @@ It distributes route requests and planning results according to current MRM oper | `input/vector_map` | autoware_map_msgs/msg/LaneletMapBin | vector map of Lanelet2 | | `input/modified_goal` | geometry_msgs/PoseWithUuidStamped | modified goal pose | | `input/operation_mode_state` | autoware_adapi_v1_msgs/OperationModeState | operation mode state | +| `input/odometry` | nav_msgs/msg/Odometry | vehicle odometry | ### Publications diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp index ef6ae1662dcee..823771e173c48 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ #define AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ +#include "autoware/motion_utils/factor/planning_factor_interface.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/obstacle_cruise_planner/common_structs.hpp" #include "autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp" @@ -47,15 +48,15 @@ class PlannerInterface rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr) - : longitudinal_info_(longitudinal_info), + : planning_factor_interface_{std::make_unique( + &node, "obstacle_cruise_planner")}, + longitudinal_info_(longitudinal_info), vehicle_info_(vehicle_info), ego_nearest_param_(ego_nearest_param), debug_data_ptr_(debug_data_ptr), slow_down_param_(SlowDownParam(node)), stop_param_(StopParam(node, longitudinal_info)) { - velocity_factors_pub_ = - node.create_publisher("/planning/velocity_factors/obstacle_cruise", 1); stop_speed_exceeded_pub_ = node.create_publisher("~/output/stop_speed_exceeded", 1); metrics_pub_ = node.create_publisher("~/metrics", 10); @@ -101,6 +102,7 @@ class PlannerInterface const std::optional & stop_pose = std::nullopt, const std::optional & stop_obstacle = std::nullopt); void publishMetrics(const rclcpp::Time & current_time); + void publishPlanningFactors() { planning_factor_interface_->publish(); } void clearMetrics(); void onParam(const std::vector & parameters) @@ -128,6 +130,8 @@ class PlannerInterface double getSafeDistanceMargin() const { return longitudinal_info_.safe_distance_margin; } protected: + std::unique_ptr planning_factor_interface_; + // Parameters bool enable_debug_info_{false}; bool enable_calculation_time_info_{false}; @@ -145,7 +149,6 @@ class PlannerInterface stop_watch_; // Publishers - rclcpp::Publisher::SharedPtr velocity_factors_pub_; rclcpp::Publisher::SharedPtr stop_speed_exceeded_pub_; rclcpp::Publisher::SharedPtr metrics_pub_; diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp index 04badd2a956ef..31344903b6809 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp @@ -17,8 +17,6 @@ #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "autoware_adapi_v1_msgs/msg/planning_behavior.hpp" -#include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp" #include "autoware_perception_msgs/msg/predicted_object.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -39,9 +37,6 @@ #include using autoware::vehicle_info_utils::VehicleInfo; -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::VelocityFactor; -using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp index d929647d9bcd0..ebabd96a54608 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp @@ -94,9 +94,6 @@ size_t getIndexWithLongitudinalOffset( return 0; } -VelocityFactorArray makeVelocityFactorArray( - const rclcpp::Time & time, const std::string & behavior = PlanningBehavior::ROUTE_OBSTACLE, - const std::optional pose = std::nullopt); } // namespace obstacle_cruise_utils #endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index 8583d0639e5ce..7c6d0b286c9b0 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -18,7 +18,6 @@ ament_cmake_auto autoware_cmake - autoware_adapi_v1_msgs autoware_interpolation autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 6931ff58e8457..4242f0d559946 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -726,6 +726,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // 8. Publish debug data published_time_publisher_->publish_if_subscribed(trajectory_pub_, output_traj.header.stamp); planner_ptr_->publishMetrics(now()); + planner_ptr_->publishPlanningFactors(); publishDebugMarker(); publishDebugInfo(); objects_of_interest_marker_interface_.publishMarkerArray(); diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 544597f05ff75..20b564addf149 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -335,9 +335,10 @@ std::vector PIDBasedPlanner::planCruise( debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); debug_data_ptr_->cruise_metrics = makeMetrics("PIDBasedPlanner", "cruise", planner_data); - velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray( - planner_data.current_time, PlanningBehavior::ADAPTIVE_CRUISE, - stop_traj_points.at(wall_idx).pose)); + planning_factor_interface_->add( + stop_traj_points, planner_data.ego_pose, stop_traj_points.at(wall_idx).pose, + tier4_planning_msgs::msg::PlanningFactor::NONE, + tier4_planning_msgs::msg::SafetyFactorArray{}); } // do cruise planning diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index b08bb10ef69cf..b32d7f3a1bcbb 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -182,8 +182,6 @@ std::vector PlannerInterface::generateStopTrajectory( : std::abs(vehicle_info_.min_longitudinal_offset_m); if (stop_obstacles.empty()) { - velocity_factors_pub_->publish( - obstacle_cruise_utils::makeVelocityFactorArray(planner_data.current_time)); // delete marker const auto markers = autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); @@ -336,8 +334,10 @@ std::vector PlannerInterface::generateStopTrajectory( // Publish Stop Reason const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; - velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray( - planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE, stop_pose)); + planning_factor_interface_->add( + output_traj_points, planner_data.ego_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}); // Store stop reason debug data debug_data_ptr_->stop_metrics = makeMetrics("PlannerInterface", "stop", planner_data, stop_pose, *determined_stop_obstacle); @@ -590,10 +590,14 @@ std::vector PlannerInterface::generateSlowDownTrajectory( const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down", planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward); - velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray( - planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE, - slow_down_traj_points.at(slow_down_wall_idx).pose)); autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); + planning_factor_interface_->add( + slow_down_traj_points, planner_data.ego_pose, + slow_down_traj_points.at(*slow_down_start_idx).pose, + slow_down_traj_points.at(*slow_down_end_idx).pose, + tier4_planning_msgs::msg::PlanningFactor::SLOW_DOWN, + tier4_planning_msgs::msg::SafetyFactorArray{}, planner_data.is_driving_forward, + stable_slow_down_vel); } // add debug virtual wall diff --git a/planning/autoware_obstacle_cruise_planner/src/utils.cpp b/planning/autoware_obstacle_cruise_planner/src/utils.cpp index 82f4e6978181f..b27def0bfcbe5 100644 --- a/planning/autoware_obstacle_cruise_planner/src/utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/utils.cpp @@ -118,25 +118,4 @@ std::vector getClosestStopObstacles(const std::vector pose) -{ - VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = time; - - if (pose) { - using distance_type = VelocityFactor::_distance_type; - VelocityFactor velocity_factor; - velocity_factor.behavior = behavior; - velocity_factor.pose = pose.value(); - velocity_factor.distance = std::numeric_limits::quiet_NaN(); - velocity_factor.status = VelocityFactor::UNKNOWN; - velocity_factor.detail = std::string(); - velocity_factor_array.factors.push_back(velocity_factor); - } - return velocity_factor_array; -} - } // namespace obstacle_cruise_utils diff --git a/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp b/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp index 93526518306af..62b9ae124f555 100644 --- a/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp @@ -45,12 +45,13 @@ namespace autoware::motion_planning { ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode( rclcpp::Node * node, const double base_link2front) -: node_(node), base_link2front_(base_link2front) +: node_(node), + base_link2front_(base_link2front), + planning_factor_interface_{std::make_unique( + node, "obstacle_stop_planner")} { virtual_wall_pub_ = node_->create_publisher("~/virtual_wall", 1); debug_viz_pub_ = node_->create_publisher("~/debug/marker", 1); - velocity_factor_pub_ = - node_->create_publisher("/planning/velocity_factors/obstacle_stop", 1); pub_debug_values_ = node_->create_publisher("~/obstacle_stop/debug_values", 1); } @@ -189,8 +190,13 @@ void ObstacleStopPlannerDebugNode::publish() debug_viz_pub_->publish(visualization_msg); /* publish stop reason for autoware api */ - const auto velocity_factor_msg = makeVelocityFactorArray(); - velocity_factor_pub_->publish(velocity_factor_msg); + if (stop_pose_ptr_) { + planning_factor_interface_->add( + std::numeric_limits::quiet_NaN(), *stop_pose_ptr_, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); + } + planning_factor_interface_->publish(); // publish debug values Float32MultiArrayStamped debug_msg{}; @@ -492,23 +498,4 @@ MarkerArray ObstacleStopPlannerDebugNode::makeVisualizationMarker() return msg; } -VelocityFactorArray ObstacleStopPlannerDebugNode::makeVelocityFactorArray() -{ - VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = node_->now(); - - if (stop_pose_ptr_) { - using distance_type = VelocityFactor::_distance_type; - VelocityFactor velocity_factor; - velocity_factor.behavior = PlanningBehavior::ROUTE_OBSTACLE; - velocity_factor.pose = *stop_pose_ptr_; - velocity_factor.distance = std::numeric_limits::quiet_NaN(); - velocity_factor.status = VelocityFactor::UNKNOWN; - velocity_factor.detail = std::string(); - velocity_factor_array.factors.push_back(velocity_factor); - } - return velocity_factor_array; -} - } // namespace autoware::motion_planning diff --git a/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp b/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp index d13e775e3d683..dd9e5e3af1b3d 100644 --- a/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp +++ b/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp @@ -14,10 +14,9 @@ #ifndef DEBUG_MARKER_HPP_ #define DEBUG_MARKER_HPP_ +#include #include -#include -#include #include #include #include @@ -42,9 +41,6 @@ using std_msgs::msg::Header; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::VelocityFactor; -using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using tier4_debug_msgs::msg::Float32MultiArrayStamped; enum class PolygonType : int8_t { Vehicle = 0, Collision, SlowDownRange, SlowDown, Obstacle }; @@ -109,7 +105,6 @@ class ObstacleStopPlannerDebugNode bool pushObstaclePoint(const pcl::PointXYZ & obstacle_point, const PointType & type); MarkerArray makeVirtualWallMarker(); MarkerArray makeVisualizationMarker(); - VelocityFactorArray makeVelocityFactorArray(); void setDebugValues(const DebugValues::TYPE type, const double val) { @@ -120,11 +115,12 @@ class ObstacleStopPlannerDebugNode private: rclcpp::Publisher::SharedPtr virtual_wall_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Publisher::SharedPtr velocity_factor_pub_; rclcpp::Publisher::SharedPtr pub_debug_values_; rclcpp::Node * node_; double base_link2front_; + std::unique_ptr planning_factor_interface_; + std::shared_ptr stop_pose_ptr_; std::shared_ptr target_stop_pose_ptr_; std::shared_ptr slow_down_start_pose_ptr_; diff --git a/planning/autoware_remaining_distance_time_calculator/CMakeLists.txt b/planning/autoware_remaining_distance_time_calculator/CMakeLists.txt index d29a153a0ce5d..31fc4b1572f33 100644 --- a/planning/autoware_remaining_distance_time_calculator/CMakeLists.txt +++ b/planning/autoware_remaining_distance_time_calculator/CMakeLists.txt @@ -4,10 +4,22 @@ project(autoware_remaining_distance_time_calculator) find_package(autoware_cmake REQUIRED) autoware_package() +generate_parameter_library(remaining_distance_time_calculator_parameters + param/remaining_distance_time_calculator_parameters.yaml +) + ament_auto_add_library(${PROJECT_NAME} SHARED src/remaining_distance_time_calculator_node.cpp ) +target_link_libraries(${PROJECT_NAME} + remaining_distance_time_calculator_parameters +) + +target_compile_options(${PROJECT_NAME} PRIVATE + -Wno-error=deprecated-declarations +) + rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "autoware::remaining_distance_time_calculator::RemainingDistanceTimeCalculatorNode" EXECUTABLE ${PROJECT_NAME}_node diff --git a/planning/autoware_remaining_distance_time_calculator/README.md b/planning/autoware_remaining_distance_time_calculator/README.md index 694c6764de91c..c227b73b87448 100644 --- a/planning/autoware_remaining_distance_time_calculator/README.md +++ b/planning/autoware_remaining_distance_time_calculator/README.md @@ -8,6 +8,7 @@ This package aims to provide mission remaining distance and remaining time calcu - The calculations are activated once we have a route planned for a mission in Autoware. - The calculations are triggered timely based on the `update_rate` parameter. +- The calculations are skipped if the scenario is PARKING, and the remaining time and distance values are set to 0.0. ### Module Parameters diff --git a/planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml b/planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml index cfb456c57ca41..be7898bdd8913 100644 --- a/planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml +++ b/planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml @@ -4,6 +4,7 @@ + @@ -13,6 +14,7 @@ + diff --git a/planning/autoware_remaining_distance_time_calculator/package.xml b/planning/autoware_remaining_distance_time_calculator/package.xml index 924e6d62d6aba..568f677a0c13d 100644 --- a/planning/autoware_remaining_distance_time_calculator/package.xml +++ b/planning/autoware_remaining_distance_time_calculator/package.xml @@ -18,6 +18,7 @@ autoware_route_handler autoware_test_utils autoware_universe_utils + generate_parameter_library geometry_msgs nav_msgs rclcpp diff --git a/planning/autoware_remaining_distance_time_calculator/param/remaining_distance_time_calculator_parameters.yaml b/planning/autoware_remaining_distance_time_calculator/param/remaining_distance_time_calculator_parameters.yaml new file mode 100644 index 0000000000000..0f1b1b9efbb6d --- /dev/null +++ b/planning/autoware_remaining_distance_time_calculator/param/remaining_distance_time_calculator_parameters.yaml @@ -0,0 +1,4 @@ +remaining_distance_time_calculator: + update_rate: + type: double + description: Timer callback period. [Hz] diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp index 383e85731604e..74f3f2f43077d 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp @@ -37,6 +37,7 @@ RemainingDistanceTimeCalculatorNode::RemainingDistanceTimeCalculatorNode( : Node("remaining_distance_time_calculator", options), is_graph_ready_{false}, has_received_route_{false}, + has_received_scenario_{false}, velocity_limit_{99.99}, remaining_distance_{0.0}, remaining_time_{0.0} @@ -57,14 +58,18 @@ RemainingDistanceTimeCalculatorNode::RemainingDistanceTimeCalculatorNode( sub_planning_velocity_ = create_subscription( "/planning/scenario_planning/current_max_velocity", qos_transient_local, std::bind(&RemainingDistanceTimeCalculatorNode::on_velocity_limit, this, _1)); + sub_scenario_ = this->create_subscription( + "~/input/scenario", 1, std::bind(&RemainingDistanceTimeCalculatorNode::on_scenario, this, _1)); pub_mission_remaining_distance_time_ = create_publisher( "~/output/mission_remaining_distance_time", rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable()); - node_param_.update_rate = declare_parameter("update_rate"); + param_listener_ = std::make_shared<::remaining_distance_time_calculator::ParamListener>( + this->get_node_parameters_interface()); + const auto param = param_listener_->get_params(); - const auto period_ns = rclcpp::Rate(node_param_.update_rate).period(); + const auto period_ns = rclcpp::Rate(param.update_rate).period(); timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&RemainingDistanceTimeCalculatorNode::on_timer, this)); } @@ -100,9 +105,25 @@ void RemainingDistanceTimeCalculatorNode::on_velocity_limit( } } +void RemainingDistanceTimeCalculatorNode::on_scenario( + const tier4_planning_msgs::msg::Scenario::ConstSharedPtr & msg) +{ + scenario_ = msg; + has_received_scenario_ = true; +} + void RemainingDistanceTimeCalculatorNode::on_timer() { - if (is_graph_ready_ && has_received_route_) { + if (!has_received_scenario_) { + return; + } + + // check if the scenario is parking or not + if (scenario_->current_scenario == tier4_planning_msgs::msg::Scenario::PARKING) { + remaining_distance_ = 0.0; + remaining_time_ = 0.0; + publish_mission_remaining_distance_time(); + } else if (is_graph_ready_ && has_received_route_) { calculate_remaining_distance(); calculate_remaining_time(); publish_mission_remaining_distance_time(); diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp index d6c900af4dfed..8a38941d383ee 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -24,6 +25,7 @@ #include #include #include +#include #include #include @@ -38,11 +40,6 @@ namespace autoware::remaining_distance_time_calculator { -struct NodeParam -{ - double update_rate{0.0}; -}; - class RemainingDistanceTimeCalculatorNode : public rclcpp::Node { public: @@ -59,6 +56,7 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_odometry_; rclcpp::Subscription::SharedPtr sub_planning_velocity_; + rclcpp::Subscription::SharedPtr sub_scenario_; rclcpp::Publisher::SharedPtr pub_mission_remaining_distance_time_; @@ -75,14 +73,16 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node geometry_msgs::msg::Pose current_vehicle_pose_; geometry_msgs::msg::Vector3 current_vehicle_velocity_; geometry_msgs::msg::Pose goal_pose_; + tier4_planning_msgs::msg::Scenario::ConstSharedPtr scenario_; bool has_received_route_; + bool has_received_scenario_; double velocity_limit_; double remaining_distance_; double remaining_time_; // Parameter - NodeParam node_param_; + std::shared_ptr<::remaining_distance_time_calculator::ParamListener> param_listener_; // Callbacks void on_timer(); @@ -90,6 +90,7 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node void on_route(const LaneletRoute::ConstSharedPtr & msg); void on_map(const HADMapBin::ConstSharedPtr & msg); void on_velocity_limit(const VelocityLimit::ConstSharedPtr & msg); + void on_scenario(const tier4_planning_msgs::msg::Scenario::ConstSharedPtr & msg); /** * @brief calculate mission remaining distance diff --git a/planning/autoware_rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp index 47bc278c30564..7fcd0596b6b32 100644 --- a/planning/autoware_rtc_interface/src/rtc_interface.cpp +++ b/planning/autoware_rtc_interface/src/rtc_interface.cpp @@ -80,9 +80,7 @@ Module getModuleType(const std::string & module_name) module.type = Module::OCCLUSION_SPOT; } else if (module_name == "stop_line") { module.type = Module::NONE; - } else if (module_name == "traffic_light") { - module.type = Module::TRAFFIC_LIGHT; - } else if (module_name == "virtual_traffic_light") { + } else if (module_name == "traffic_light" || module_name == "virtual_traffic_light") { module.type = Module::TRAFFIC_LIGHT; } else if (module_name == "lane_change_left") { module.type = Module::LANE_CHANGE_LEFT; diff --git a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp index 244876ee2cb22..7e215fcfef8dd 100644 --- a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp +++ b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp @@ -19,13 +19,13 @@ #include #include +#include #include #include #include #include #include #include -#include #include #include @@ -97,7 +97,8 @@ class ScenarioSelectorNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_parking_trajectory_; rclcpp::Publisher::SharedPtr pub_trajectory_; rclcpp::Publisher::SharedPtr pub_scenario_; - rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr + pub_processing_time_; // polling subscribers universe_utils::InterProcessPollingSubscriber< diff --git a/planning/autoware_scenario_selector/src/node.cpp b/planning/autoware_scenario_selector/src/node.cpp index 4bb64e27368d8..91e86bfea6dba 100644 --- a/planning/autoware_scenario_selector/src/node.cpp +++ b/planning/autoware_scenario_selector/src/node.cpp @@ -156,9 +156,8 @@ std::string ScenarioSelectorNode::selectScenarioByPosition() return tier4_planning_msgs::msg::Scenario::LANEDRIVING; } else if (is_in_parking_lot) { return tier4_planning_msgs::msg::Scenario::PARKING; - } else { - return tier4_planning_msgs::msg::Scenario::LANEDRIVING; } + return tier4_planning_msgs::msg::Scenario::LANEDRIVING; } if (current_scenario_ == tier4_planning_msgs::msg::Scenario::LANEDRIVING) { @@ -391,7 +390,7 @@ void ScenarioSelectorNode::onTimer() pub_scenario_->publish(scenario); // Publish ProcessingTime - 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 = stop_watch.toc(); pub_processing_time_->publish(processing_time_msg); @@ -491,8 +490,8 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti this, get_clock(), period_ns, std::bind(&ScenarioSelectorNode::onTimer, this)); published_time_publisher_ = std::make_unique(this); - pub_processing_time_ = - this->create_publisher("~/debug/processing_time_ms", 1); + pub_processing_time_ = this->create_publisher( + "~/debug/processing_time_ms", 1); } } // namespace autoware::scenario_selector diff --git a/planning/autoware_static_centerline_generator/CHANGELOG.rst b/planning/autoware_static_centerline_generator/CHANGELOG.rst deleted file mode 100644 index 0338cdc8b66bb..0000000000000 --- a/planning/autoware_static_centerline_generator/CHANGELOG.rst +++ /dev/null @@ -1,287 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_static_centerline_generator -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix(static_centerline_generator): several bug fixes (`#9426 `_) - * fix: dependent packages - * feat: use steer angle, use warn for steer angle failure, calc curvature dicontinuously - * fix cppcheck - --------- -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - planning (`#9570 `_) -* refactor(global_parameter_loader): prefix package and namespace with autoware (`#9303 `_) -* feat!: replace tier4_map_msgs with autoware_map_msgs for MapProjectorInfo (`#9392 `_) -* refactor(map_loader)!: prefix package and namespace with autoware (`#8927 `_) - * make lanelet2_map_visualization independent - * remove unused files - * remove unused package - * fix package name - * add autoware\_ prefix - * add autoware to exec name - * add autoware prefix - * removed unnecessary dependency - --------- -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix(static_centerline_generator): map_tf_generator package name needs update (`#9383 `_) - fix map_tf_generator name in autoware_static_centerline_generator.launch -* refactor(map_loader)!: prefix package and namespace with autoware (`#8927 `_) - * make lanelet2_map_visualization independent - * remove unused files - * remove unused package - * fix package name - * add autoware\_ prefix - * add autoware to exec name - * add autoware prefix - * removed unnecessary dependency - --------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Masaki Baba, Ryohsuke Mitsudome, Takayuki Murooka, Yutaka Kondo, Zhanhong Yan - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(osqp_interface): added autoware prefix to osqp_interface (`#8958 `_) -* refactor(autoware_interpolation): prefix package and namespace with autoware (`#8088 `_) - Co-authored-by: kosuke55 -* fix(other_planning_packages): align the parameters with launcher (`#8793 `_) - * parameters in planning/others aligned - * update json - --------- -* refactor(map_projection_loader)!: prefix package and namespace with autoware (`#8420 `_) - * add autoware\_ prefix - * add autoware\_ prefix - --------- - Co-authored-by: SakodaShintaro -* fix(autoware_static_centerline_generator): fix unusedFunction (`#8647 `_) - * fix:unusedFunction - * fix:unusedFunction - * fix:compile error - --------- -* refactor(geography_utils): prefix package and namespace with autoware (`#7790 `_) - * refactor(geography_utils): prefix package and namespace with autoware - * move headers to include/autoware/ - --------- -* fix(autoware_static_centerline_generator): fix funcArgNamesDifferent (`#8019 `_) - fix:funcArgNamesDifferent -* fix(static_centerline_generator): save_map only once (`#7770 `_) -* refactor(static_centerline_optimizer): clean up the code (`#7756 `_) -* feat: add `autoware\_` prefix to `lanelet2_extension` (`#7640 `_) -* feat(static_centerline_generator): organize AUTO/GUI/VMB modes (`#7432 `_) -* refactor(universe_utils/motion_utils)!: add autoware namespace (`#7594 `_) -* refactor(motion_utils)!: add autoware prefix and include dir (`#7539 `_) - refactor(motion_utils): add autoware prefix and include dir -* feat(autoware_universe_utils)!: rename from tier4_autoware_utils (`#7538 `_) - Co-authored-by: kosuke55 -* refactor(route_handler)!: rename to include/autoware/{package_name} (`#7530 `_) - refactor(route_handler)!: rename to include/autoware/{package_name} -* feat(map_loader): add waypoints flag (`#7480 `_) - * feat(map_loader): handle centelrine and waypoints - * update README - * fix doc - * update schema - * fix - * fix - --------- -* feat(path_optimizer): rename to include/autoware/{package_name} (`#7529 `_) -* feat(path_smoother): rename to include/autoware/{package_name} (`#7527 `_) - * feat(path_smoother): rename to include/autoware/{package_name} - * fix - --------- -* refactor(behaivor_path_planner)!: rename to include/autoware/{package_name} (`#7522 `_) - * refactor(behavior_path_planner)!: make autoware dir in include - * refactor(start_planner): make autoware include dir - * refactor(goal_planner): make autoware include dir - * sampling planner module - * fix sampling planner build - * dynamic_avoidance - * lc - * side shift - * autoware_behavior_path_static_obstacle_avoidance_module - * autoware_behavior_path_planner_common - * make behavior_path dir - * pre-commit - * fix pre-commit - * fix build - --------- -* feat(mission_planner): rename to include/autoware/{package_name} (`#7513 `_) - * feat(mission_planner): rename to include/autoware/{package_name} - * feat(mission_planner): rename to include/autoware/{package_name} - * feat(mission_planner): rename to include/autoware/{package_name} - --------- -* fix(static_centerline_generator): fix dependency (`#7442 `_) - * fix: deps - * fix: package name - * fix: package name - --------- -* refactor(route_handler): route handler add autoware prefix (`#7341 `_) - * rename route handler package - * update packages dependencies - * update include guards - * update includes - * put in autoware namespace - * fix formats - * keep header and source file name as before - --------- -* refactor(mission_planner)!: add autoware prefix and namespace (`#7414 `_) - * refactor(mission_planner)!: add autoware prefix and namespace - * fix svg - --------- -* refactor(vehicle_info_utils)!: prefix package and namespace with autoware (`#7353 `_) - * chore(autoware_vehicle_info_utils): rename header - * chore(bpp-common): vehicle info - * chore(path_optimizer): vehicle info - * chore(velocity_smoother): vehicle info - * chore(bvp-common): vehicle info - * chore(static_centerline_generator): vehicle info - * chore(obstacle_cruise_planner): vehicle info - * chore(obstacle_velocity_limiter): vehicle info - * chore(mission_planner): vehicle info - * chore(obstacle_stop_planner): vehicle info - * chore(planning_validator): vehicle info - * chore(surround_obstacle_checker): vehicle info - * chore(goal_planner): vehicle info - * chore(start_planner): vehicle info - * chore(control_performance_analysis): vehicle info - * chore(lane_departure_checker): vehicle info - * chore(predicted_path_checker): vehicle info - * chore(vehicle_cmd_gate): vehicle info - * chore(obstacle_collision_checker): vehicle info - * chore(operation_mode_transition_manager): vehicle info - * chore(mpc): vehicle info - * chore(control): vehicle info - * chore(common): vehicle info - * chore(perception): vehicle info - * chore(evaluator): vehicle info - * chore(freespace): vehicle info - * chore(planning): vehicle info - * chore(vehicle): vehicle info - * chore(simulator): vehicle info - * chore(launch): vehicle info - * chore(system): vehicle info - * chore(sensing): vehicle info - * fix(autoware_joy_controller): remove unused deps - --------- -* refactor(path_smoother)!: prefix package and namespace with autoware (`#7381 `_) - * git mv - * fix - * fix launch - * rever a part of prefix - * fix test - * fix - * fix static_centerline_optimizer - * fix - --------- -* refactor(path_optimizer, velocity_smoother)!: prefix package and namespace with autoware (`#7354 `_) - * chore(autoware_velocity_smoother): update namespace - * chore(autoware_path_optimizer): update namespace - --------- -* chore(bpp): add prefix `autoware\_` (`#7288 `_) - * chore(common): rename package - * fix(static_obstacle_avoidance): fix header - * fix(dynamic_obstacle_avoidance): fix header - * fix(side_shift): fix header - * fix(sampling_planner): fix header - * fix(start_planner): fix header - * fix(goal_planner): fix header - * fix(lane_change): fix header - * fix(external_lane_change): fix header - * fix(AbLC): fix header - * fix(bpp-node): fix header - * fix(static_centerline_generator): fix header - * fix(.pages): update link - --------- -* feat!: replace autoware_auto_msgs with autoware_msgs for planning modules (`#7246 `_) - Co-authored-by: Cynthia Liu - Co-authored-by: NorahXiong - Co-authored-by: beginningfan -* chore(autoware_velocity_smoother, autoware_path_optimizer): rename packages (`#7202 `_) - * chore(autoware_path_optimizer): rename package and namespace - * chore(autoware_static_centerline_generator): rename package and namespace - * chore: update module name - * chore(autoware_velocity_smoother): rename package and namespace - * chore(tier4_planning_launch): update module name - * chore: update module name - * fix: test - * fix: test - * fix: test - --------- -* refactor(behavior_velocity_planner)!: prefix package and namespace with autoware\_ (`#6693 `_) -* fix(autoware_static_centerline_generator): update the centerline correctly with map projector (`#6825 `_) - * fix(static_centerline_generator): fixed the bug of offset lat/lon values - * fix typo - --------- -* fix(autoware_static_centerline_generator): remove prefix from topics and node names (`#7028 `_) -* build(static_centerline_generator): prefix package and namespace with autoware\_ (`#6817 `_) - * build(static_centerline_generator): prefix package and namespace with autoware\_ - * style(pre-commit): autofix - * build: fix CMake target - * build(autoware_static_centerline_generator): more renames - * style(pre-commit): autofix - * build(autoware_static_centerline_generator): fix namespace - * fix(autoware_static_centerline_generator): fix clang-tidy issues - * style(pre-commit): autofix - * style(pre-commit): autofix - * fix(autoware_static_centerline_generator): fix clang-tidy issues - * fix(autoware_static_centerline_generator): fix build issues - * fix(autoware_static_centerline_generator): fix build issues - * style(pre-commit): autofix - * fix(autoware_static_centerline_optimizer): fix clang-tidy issues - * style(pre-commit): autofix - * build: fix build errors - * fix: remove else statements after return - * fix(autoware_static_centerline_generator): fix clang-tidy issues - * style(pre-commit): autofix - * revert changes for static_centerline_generator - * fix(autoware_static_centerline_generator): add autoware\_ prefix - * style(pre-commit): autofix - * fix(autoware_static_centerline_generator): fix filenames - * fix(autoware_static_centerline_generator): fix namespaces - * style(pre-commit): autofix - * fix: added prefix to missing strings - * refactor(autoware_static_centerline_generator): move header files to src - * refactor(autoware_static_centerline_generator): fix include paths - * style(pre-commit): autofix - * refactor(autoware_static_centerline_generator): rename base folder - * Update planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml - Co-authored-by: M. Fatih Cırıt - * build(autoware_static_centerline_generator): fix include in CMake - * build(autoware_static_centerline_generator): fix missing includes - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> - Co-authored-by: M. Fatih Cırıt -* Contributors: Esteve Fernandez, Kosuke Takeuchi, Masaki Baba, Ryohsuke Mitsudome, Satoshi OTA, Takayuki Murooka, Yutaka Kondo, Zhe Shen, kobayu858, mkquda - -0.26.0 (2024-04-03) -------------------- diff --git a/planning/autoware_static_centerline_generator/CMakeLists.txt b/planning/autoware_static_centerline_generator/CMakeLists.txt deleted file mode 100644 index 261e8beb0022f..0000000000000 --- a/planning/autoware_static_centerline_generator/CMakeLists.txt +++ /dev/null @@ -1,66 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_static_centerline_generator) - -find_package(autoware_cmake REQUIRED) - -find_package(builtin_interfaces REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(std_msgs REQUIRED) - -rosidl_generate_interfaces( - autoware_static_centerline_generator - "srv/LoadMap.srv" - "srv/PlanRoute.srv" - "srv/PlanPath.srv" - "msg/PointsWithLaneId.msg" - DEPENDENCIES builtin_interfaces geometry_msgs -) - -autoware_package() - -ament_auto_add_executable(main - src/main.cpp - src/static_centerline_generator_node.cpp - src/centerline_source/optimization_trajectory_based_centerline.cpp - src/centerline_source/bag_ego_trajectory_based_centerline.cpp - src/utils.cpp -) - -if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(main - autoware_static_centerline_generator "rosidl_typesupport_cpp") -else() - rosidl_get_typesupport_target( - cpp_typesupport_target autoware_static_centerline_generator "rosidl_typesupport_cpp") - target_link_libraries(main "${cpp_typesupport_target}") -endif() - -ament_auto_package( - INSTALL_TO_SHARE - launch - config - rviz -) - -target_include_directories(main PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/src -) - -if(BUILD_TESTING) - add_launch_test( - test/test_static_centerline_generator.test.py - TIMEOUT "30" - ) - install(DIRECTORY - test/data/ - DESTINATION share/${PROJECT_NAME}/test/data/ - ) -endif() - -install(PROGRAMS - scripts/app.py - scripts/centerline_updater_helper.py - scripts/show_lanelet2_map_diff.py - DESTINATION lib/${PROJECT_NAME} -) diff --git a/planning/autoware_static_centerline_generator/README.md b/planning/autoware_static_centerline_generator/README.md deleted file mode 100644 index 426d5487cf0cb..0000000000000 --- a/planning/autoware_static_centerline_generator/README.md +++ /dev/null @@ -1,83 +0,0 @@ -# Static Centerline Generator - -## Purpose - -This package statically calculates the centerline satisfying path footprints inside the drivable area. - -On narrow-road driving, the default centerline, which is the middle line between lanelets' right and left boundaries, often causes path footprints outside the drivable area. -To make path footprints inside the drivable area, we use online path shape optimization by [the autoware_path_optimizer package](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/autoware_path_optimizer/). - -Instead of online path shape optimization, we introduce static centerline optimization. -With this static centerline optimization, we have following advantages. - -- We can see the optimized centerline shape in advance. - - With the default autoware, path shape is not determined until the vehicle drives there. - - This enables offline path shape evaluation. -- We do not have to calculate a heavy and sometimes unstable path optimization since the path footprints are already inside the drivable area. - -## Use cases - -There are two interfaces to communicate with the centerline optimizer. - -### Vector Map Builder Interface - -Note: This function of Vector Map Builder has not been released. Please wait for a while. -Currently there is no documentation about Vector Map Builder's operation for this function. - -The optimized centerline can be generated from Vector Map Builder's operation. - -We can run - -- path planning server -- http server to connect path planning server and Vector Map Builder - -with the following command by designating `` - -```sh -ros2 launch autoware_static_centerline_generator run_planning_server.launch.xml vehicle_model:= -``` - -FYI, port ID of the http server is 4010 by default. - -### Command Line Interface - -The optimized centerline can be generated from the command line interface by designating - -- `` -- `` (not mandatory) -- `` -- `` -- `` - -```sh -ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml run_backgrond:=false lanelet2_input_file_path:= lanelet2_output_file_path:= start_lanelet_id:= end_lanelet_id:= vehicle_model:= -``` - -The default output map path containing the optimized centerline locates `/tmp/lanelet2_map.osm`. -If you want to change the output map path, you can remap the path by designating ``. - -## Visualization - -When launching the path planning server, rviz is launched as well as follows. -![rviz](./media/rviz.png) - -- The yellow footprints are the original ones from the osm map file. - - FYI: Footprints are generated based on the centerline and vehicle size. -- The red footprints are the optimized ones. -- The gray area is the drivable area. -- You can see that the red footprints are inside the drivable area although the yellow ones are outside. - -### Unsafe footprints - -Sometimes the optimized centerline footprints are close to the lanes' boundaries. -We can check how close they are with `unsafe footprints` marker as follows. - -Footprints' color depends on its distance to the boundaries, and text expresses its distance. - -![rviz](./media/unsafe_footprints.png) - -By default, footprints' color is - -- when the distance is less than 0.1 [m] : red -- when the distance is less than 0.2 [m] : green -- when the distance is less than 0.3 [m] : blue diff --git a/planning/autoware_static_centerline_generator/config/common.param.yaml b/planning/autoware_static_centerline_generator/config/common.param.yaml deleted file mode 100644 index 6c547c8cd83dc..0000000000000 --- a/planning/autoware_static_centerline_generator/config/common.param.yaml +++ /dev/null @@ -1,17 +0,0 @@ -/**: - ros__parameters: - # constraints param for normal driving - max_vel: 11.1 # max velocity limit [m/s] - - normal: - min_acc: -1.0 # min deceleration [m/ss] - max_acc: 1.0 # max acceleration [m/ss] - min_jerk: -1.0 # min jerk [m/sss] - max_jerk: 1.0 # max jerk [m/sss] - - # constraints to be observed - limit: - min_acc: -2.5 # min deceleration limit [m/ss] - max_acc: 1.0 # max acceleration limit [m/ss] - min_jerk: -1.5 # min jerk limit [m/sss] - max_jerk: 1.5 # max jerk limit [m/sss] diff --git a/planning/autoware_static_centerline_generator/config/nearest_search.param.yaml b/planning/autoware_static_centerline_generator/config/nearest_search.param.yaml deleted file mode 100644 index eb6709764ce3e..0000000000000 --- a/planning/autoware_static_centerline_generator/config/nearest_search.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - # ego - ego_nearest_dist_threshold: 3.0 # [m] - ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] diff --git a/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml b/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml deleted file mode 100644 index 060590803428a..0000000000000 --- a/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml +++ /dev/null @@ -1,9 +0,0 @@ -/**: - ros__parameters: - marker_color: ["FF0000", "FF5A00", "FFFF00"] - marker_color_dist_thresh : [0.1, 0.2, 0.3] - output_trajectory_interval: 1.0 - - validation: - dist_threshold_to_road_border: 0.0 - max_steer_angle_margin: 0.0 # [rad] NOTE: Positive value makes max steer angle threshold to decrease. diff --git a/planning/autoware_static_centerline_generator/config/vehicle_info.param.yaml b/planning/autoware_static_centerline_generator/config/vehicle_info.param.yaml deleted file mode 100644 index 8941b92b4e78e..0000000000000 --- a/planning/autoware_static_centerline_generator/config/vehicle_info.param.yaml +++ /dev/null @@ -1,12 +0,0 @@ -/**: - ros__parameters: - wheel_radius: 0.39 - wheel_width: 0.42 - wheel_base: 2.74 # between front wheel center and rear wheel center - wheel_tread: 1.63 # between left wheel center and right wheel center - front_overhang: 1.0 # between front wheel center and vehicle front - rear_overhang: 1.03 # between rear wheel center and vehicle rear - left_overhang: 0.1 # between left wheel center and vehicle left - right_overhang: 0.1 # between right wheel center and vehicle right - vehicle_height: 2.5 - max_steer_angle: 0.70 # [rad] diff --git a/planning/autoware_static_centerline_generator/launch/run_planning_server.launch.xml b/planning/autoware_static_centerline_generator/launch/run_planning_server.launch.xml deleted file mode 100644 index cb368ca336316..0000000000000 --- a/planning/autoware_static_centerline_generator/launch/run_planning_server.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml deleted file mode 100644 index 0590009ab378c..0000000000000 --- a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml +++ /dev/null @@ -1,91 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/planning/autoware_static_centerline_generator/media/rviz.png b/planning/autoware_static_centerline_generator/media/rviz.png deleted file mode 100644 index e76ede78294f0..0000000000000 Binary files a/planning/autoware_static_centerline_generator/media/rviz.png and /dev/null differ diff --git a/planning/autoware_static_centerline_generator/media/unsafe_footprints.png b/planning/autoware_static_centerline_generator/media/unsafe_footprints.png deleted file mode 100644 index b4404f4cfa89b..0000000000000 Binary files a/planning/autoware_static_centerline_generator/media/unsafe_footprints.png and /dev/null differ diff --git a/planning/autoware_static_centerline_generator/msg/PointsWithLaneId.msg b/planning/autoware_static_centerline_generator/msg/PointsWithLaneId.msg deleted file mode 100644 index 97cd0355df831..0000000000000 --- a/planning/autoware_static_centerline_generator/msg/PointsWithLaneId.msg +++ /dev/null @@ -1,2 +0,0 @@ -int64 lane_id -geometry_msgs/Point[] points diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml deleted file mode 100644 index cea18e5605921..0000000000000 --- a/planning/autoware_static_centerline_generator/package.xml +++ /dev/null @@ -1,57 +0,0 @@ - - - - autoware_static_centerline_generator - 0.40.0 - The autoware_static_centerline_generator package - Takayuki Murooka - Kosuke Takeuchi - Apache License 2.0 - - Takayuki Murooka - - ament_cmake_auto - autoware_cmake - - rosidl_default_generators - - autoware_behavior_path_planner_common - autoware_geography_utils - autoware_global_parameter_loader - autoware_interpolation - autoware_lanelet2_extension - autoware_lanelet2_map_visualizer - autoware_map_loader - autoware_map_msgs - autoware_map_projection_loader - autoware_map_tf_generator - autoware_mission_planner - autoware_motion_utils - autoware_osqp_interface - autoware_path_optimizer - autoware_path_smoother - autoware_perception_msgs - autoware_planning_msgs - autoware_route_handler - autoware_universe_utils - autoware_vehicle_info_utils - geometry_msgs - rclcpp - rclcpp_components - - python3-flask-cors - rosidl_default_runtime - - ament_cmake_gmock - ament_lint_auto - autoware_behavior_path_planner - autoware_behavior_velocity_planner - autoware_lint_common - ros_testing - - rosidl_interface_packages - - - ament_cmake - - diff --git a/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz b/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz deleted file mode 100644 index f1bd110783009..0000000000000 --- a/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz +++ /dev/null @@ -1,459 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.550000011920929 - Tree Height: 386 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Dummy Car1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" - - Class: rviz_plugins::AutowareStatePanel - Name: AutowareStatePanel -Visualization Manager: - Class: "" - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: false - Enabled: false - Name: System - - Class: rviz_common/Group - Displays: - - Alpha: 0.20000000298023224 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 28.71826171875 - Min Value: -7.4224700927734375 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 237 - Min Color: 211; 215; 207 - Min Intensity: 0 - Name: PointCloudMap - Position Transformer: XYZ - Selectable: false - Size (Pixels): 1 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/pointcloud_map - Use Fixed Frame: true - Use rainbow: false - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Lanelet2VectorMap - Namespaces: - center_lane_line: true - center_line_arrows: true - lane_start_bound: false - lanelet_id: false - left_lane_bound: true - right_lane_bound: true - road_lanelets: false - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/vector_map_marker - Value: true - Enabled: true - Name: Map - - Class: rviz_plugins/PathWithLaneId - Color Border Vel Max: 3 - Enabled: false - Name: Raw Centerline - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_generator/input_centerline - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: true - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View LaneId: - Scale: 0.10000000149011612 - Value: false - View Path: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Value: false - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: Optimized Centerline - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_generator/output/centerline - Value: true - View Footprint: - Alpha: 0.5 - Color: 0; 255; 0 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: true - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Value: false - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: Raw Centerline (Path type) - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_generator/debug/raw_centerline - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: true - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MarkerArray - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_generator/output/validation_results - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Debug Markers - Namespaces: - curvature: false - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_generator/debug/markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_generator/debug/ego_footprint_bounds - Value: true - Enabled: true - Name: debug - Enabled: true - Global Options: - Background Color: 10; 10; 10 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/goal - - Acceleration: 0 - Class: rviz_plugins/PedestrianInitialPoseTool - Interactive: false - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 1 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/CarInitialPoseTool - H vehicle height: 2 - Interactive: false - L vehicle length: 4 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 8 - W vehicle width: 1.7999999523162842 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/BusInitialPoseTool - H vehicle height: 3.5 - Interactive: false - L vehicle length: 10.5 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - W vehicle width: 2.5 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Class: rviz_plugins/MissionCheckpointTool - Pose Topic: /planning/mission_planning/checkpoint - Theta std deviation: 0.2617993950843811 - X std deviation: 0.5 - Y std deviation: 0.5 - Z position: 0 - - Class: rviz_plugins/DeleteAllObjectsTool - Pose Topic: /simulation/dummy_perception_publisher/object_info - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz_default_plugins) - X: 0 - Y: 0 - Saved: - - Class: rviz_default_plugins/ThirdPersonFollower - Distance: 18 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: ThirdPersonFollower - Near Clip Distance: 0.009999999776482582 - Pitch: 0.20000000298023224 - Target Frame: base_link - Value: ThirdPersonFollower (rviz) - Yaw: 3.141592025756836 - - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: TopDownOrtho - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 -Window Geometry: - AutowareStatePanel: - collapsed: false - Displays: - collapsed: false - Height: 1043 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 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 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1920 - X: 0 - Y: 0 diff --git a/planning/autoware_static_centerline_generator/scripts/app.py b/planning/autoware_static_centerline_generator/scripts/app.py deleted file mode 100755 index 08bff8b80dcb7..0000000000000 --- a/planning/autoware_static_centerline_generator/scripts/app.py +++ /dev/null @@ -1,178 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2022 Tier IV, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import json -import uuid - -from autoware_static_centerline_generator.srv import LoadMap -from autoware_static_centerline_generator.srv import PlanPath -from autoware_static_centerline_generator.srv import PlanRoute -from flask import Flask -from flask import jsonify -from flask import request -from flask_cors import CORS -import rclpy -from rclpy.node import Node - -rclpy.init() -node = Node("static_centerline_generator_http_server") - -app = Flask(__name__) -CORS(app) - - -def create_client(service_type, server_name): - # create client - cli = node.create_client(service_type, server_name) - while not cli.wait_for_service(timeout_sec=1.0): - print("{} service not available, waiting again...".format(server_name)) - return cli - - -@app.route("/map", methods=["POST"]) -def get_map(): - data = request.get_json() - - map_uuid = str(uuid.uuid4()) - global map_id - map_id = map_uuid - - # create client - cli = create_client(LoadMap, "/planning/static_centerline_generator/load_map") - - # request map loading - req = LoadMap.Request(map=data["map"]) - future = cli.call_async(req) - - # get result - rclpy.spin_until_future_complete(node, future) - res = future.result() - - # error handling - if res.message == "InvalidMapFormat": - return jsonify(code=res.message, message="Map format is invalid."), 400 - elif res.message != "": - return ( - jsonify( - code="InternalServerError", - message="Error occurred on the server. Please check the terminal.", - ), - 500, - ) - - return map_uuid - - -@app.route("/planned_route", methods=["GET"]) -def post_planned_route(): - args = request.args.to_dict() - global map_id - if map_id != args.get("map_id"): - # TODO(murooka) error handling for map_id mismatch - print("map_id is not correct.") - - # create client - cli = create_client(PlanRoute, "/planning/static_centerline_generator/plan_route") - - # request route planning - req = PlanRoute.Request( - start_lane_id=int(args.get("start_lane_id")), end_lane_id=int(args.get("end_lane_id")) - ) - future = cli.call_async(req) - - # get result - rclpy.spin_until_future_complete(node, future) - res = future.result() - - # error handling - if res.message == "MapNotFound": - return jsonify(code=res.message, message="Map is missing."), 404 - elif res.message == "RouteNotFound": - return jsonify(code=res.message, message="Planning route failed."), 404 - elif res.message != "": - return ( - jsonify( - code="InternalServerError", - message="Error occurred on the server. Please check the terminal.", - ), - 500, - ) - - return json.dumps(tuple(res.lane_ids)) - - -@app.route("/planned_path", methods=["GET"]) -def post_planned_path(): - args = request.args.to_dict() - global map_id - if map_id != args.get("map_id"): - # TODO(murooka) error handling for map_id mismatch - print("map_id is not correct.") - - # create client - cli = create_client(PlanPath, "/planning/static_centerline_generator/plan_path") - - # request path planning - route_lane_ids = [eval(i) for i in request.args.getlist("route[]")] - req = PlanPath.Request(route=route_lane_ids) - future = cli.call_async(req) - - # get result - rclpy.spin_until_future_complete(node, future) - res = future.result() - - # error handling - if res.message == "MapNotFound": - return jsonify(code=res.message, message="Map is missing."), 404 - elif res.message == "LaneletsNotConnected": - return ( - jsonify( - code=res.message, - message="Lanelets are not connected.", - object_ids=tuple(res.unconnected_lane_ids), - ), - 400, - ) - elif res.message != "": - return ( - jsonify( - code="InternalServerError", - message="Error occurred on the server. Please check the terminal.", - ), - 500, - ) - - # create output json - result_json = [] - for points_with_lane_id in res.points_with_lane_ids: - current_lane_points = [] - for geom_point in points_with_lane_id.points: - point = {"x": geom_point.x, "y": geom_point.y, "z": geom_point.z} - current_lane_points.append(point) - - current_result_json = {} - current_result_json["lane_id"] = int(points_with_lane_id.lane_id) - current_result_json["points"] = current_lane_points - - result_json.append(current_result_json) - - return json.dumps(tuple(result_json)) - - -if __name__ == "__main__": - app.debug = True - app.secret_key = "tmp_secret_key" - app.run(host="localhost", port=4010) diff --git a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py deleted file mode 100755 index f3d908713361d..0000000000000 --- a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py +++ /dev/null @@ -1,207 +0,0 @@ -#!/bin/env python3 - -# Copyright 2024 TIER IV, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import sys -import time - -from PyQt5 import QtCore -from PyQt5.QtWidgets import QApplication -from PyQt5.QtWidgets import QGroupBox -from PyQt5.QtWidgets import QMainWindow -from PyQt5.QtWidgets import QPushButton -from PyQt5.QtWidgets import QSizePolicy -from PyQt5.QtWidgets import QSlider -from PyQt5.QtWidgets import QVBoxLayout -from PyQt5.QtWidgets import QWidget -from autoware_planning_msgs.msg import Trajectory -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSProfile -from std_msgs.msg import Empty -from std_msgs.msg import Float32 -from std_msgs.msg import Int32 - - -class CenterlineUpdaterWidget(QMainWindow): - def __init__(self): - super(self.__class__, self).__init__() - self.setupUI() - - def setupUI(self): - self.setObjectName("MainWindow") - self.resize(480, 120) - self.setWindowFlags(QtCore.Qt.WindowStaysOnTopHint) - - central_widget = QWidget(self) - self.grid_layout = QVBoxLayout(central_widget) - self.grid_layout.setContentsMargins(10, 10, 10, 10) - - # slide of the trajectory's start and end index - self.traj_start_index_slider = QSlider(QtCore.Qt.Horizontal) - self.traj_end_index_slider = QSlider(QtCore.Qt.Horizontal) - self.min_traj_start_index = 0 - self.max_traj_end_index = None - - # Layout: Range of Centerline - centerline_vertical_box = QVBoxLayout(self) - centerline_vertical_box.addWidget(self.traj_start_index_slider) - centerline_vertical_box.addWidget(self.traj_end_index_slider) - centerline_group = QGroupBox("Centerline") - centerline_group.setLayout(centerline_vertical_box) - self.grid_layout.addWidget(centerline_group) - - """ - # 2. Road Boundary - road_boundary_group = QGroupBox("Road Boundary") - road_boundary_vertical_box = QVBoxLayout(self) - road_boundary_group.setLayout(road_boundary_vertical_box) - self.grid_layout.addWidget(road_boundary_group) - - # 2.1. Slider - self.road_boundary_lateral_margin_slider = QSlider(QtCore.Qt.Horizontal) - road_boundary_vertical_box.addWidget(self.road_boundary_lateral_margin_slider) - self.road_boundary_lateral_margin_ratio = 10 - self.road_boundary_lateral_margin_slider.setMinimum(0) - self.road_boundary_lateral_margin_slider.setMaximum( - 5 * self.road_boundary_lateral_margin_ratio - ) - road_boundary_vertical_box.addWidget(QPushButton("reset")) - """ - - # 3. General - general_group = QGroupBox("General") - general_vertical_box = QVBoxLayout(self) - general_group.setLayout(general_vertical_box) - self.grid_layout.addWidget(general_group) - - # 3.1. Validate Centerline - self.validate_button = QPushButton("validate") - self.validate_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) - general_vertical_box.addWidget(self.validate_button) - - # 3.2. Save Map - self.save_map_button = QPushButton("save map") - self.save_map_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) - general_vertical_box.addWidget(self.save_map_button) - - self.setCentralWidget(central_widget) - - def initWithEndIndex(self, max_traj_end_index): - self.max_traj_end_index = max_traj_end_index - - # initialize sliders - self.traj_start_index_slider.setMinimum(self.min_traj_start_index) - self.traj_start_index_slider.setMaximum(self.max_traj_end_index) - self.traj_start_index_slider.setValue(self.min_traj_start_index) - - self.traj_end_index_slider.setMinimum(self.min_traj_start_index) - self.traj_end_index_slider.setMaximum(self.max_traj_end_index) - self.traj_end_index_slider.setValue(self.max_traj_end_index) - - -class CenterlineUpdaterHelper(Node): - def __init__(self): - super().__init__("centerline_updater_helper") - # Qt - self.widget = CenterlineUpdaterWidget() - self.widget.show() - self.widget.save_map_button.clicked.connect(self.onSaveMapButtonPushed) - self.widget.validate_button.clicked.connect(self.onValidateButtonPushed) - self.widget.traj_start_index_slider.valueChanged.connect(self.onStartIndexChanged) - self.widget.traj_end_index_slider.valueChanged.connect(self.onEndIndexChanged) - """ - self.widget.road_boundary_lateral_margin_slider.valueChanged.connect( - self.onRoadBoundaryLateralMargin - ) - """ - - # ROS - self.pub_save_map = self.create_publisher(Empty, "/static_centerline_generator/save_map", 1) - self.pub_validate = self.create_publisher(Empty, "/static_centerline_generator/validate", 1) - self.pub_traj_start_index = self.create_publisher( - Int32, "/static_centerline_generator/traj_start_index", 1 - ) - self.pub_traj_end_index = self.create_publisher( - Int32, "/static_centerline_generator/traj_end_index", 1 - ) - self.pub_road_boundary_lateral_margin = self.create_publisher( - Float32, "/static_centerline_generator/road_boundary_lateral_margin", 1 - ) - - transient_local_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) - self.sub_whole_centerline = self.create_subscription( - Trajectory, - "/static_centerline_generator/output/whole_centerline", - self.onWholeCenterline, - transient_local_qos, - ) - - while self.widget.max_traj_end_index is None: - rclpy.spin_once(self, timeout_sec=0.01) - time.sleep(0.1) - - def onWholeCenterline(self, whole_centerline): - self.widget.initWithEndIndex(len(whole_centerline.points) - 1) - - def onSaveMapButtonPushed(self, event): - msg = Empty() - self.pub_save_map.publish(msg) - - # NOTE: After saving the map, the generated centerline is written - # in original_map_ptr_ in static_centerline_generator_node. - # When saving the map again, another centerline is written without - # removing the previous centerline. - # Therefore, saving the map can be called only once. - self.widget.save_map_button.setEnabled(False) - - def onValidateButtonPushed(self, event): - msg = Empty() - self.pub_validate.publish(msg) - - def onStartIndexChanged(self, event): - msg = Int32() - msg.data = self.widget.traj_start_index_slider.value() - self.pub_traj_start_index.publish(msg) - - def onEndIndexChanged(self, event): - msg = Int32() - msg.data = self.widget.traj_end_index_slider.value() - self.pub_traj_end_index.publish(msg) - - def onRoadBoundaryLateralMargin(self, event): - msg = Float32() - msg.data = ( - self.widget.road_boundary_lateral_margin_slider.value() - / self.widget.road_boundary_lateral_margin_ratio - ) - self.pub_road_boundary_lateral_margin.publish(msg) - - -def main(args=None): - app = QApplication(sys.argv) - - rclpy.init(args=args) - node = CenterlineUpdaterHelper() - - while True: - app.processEvents() - rclpy.spin_once(node, timeout_sec=0.01) - - -if __name__ == "__main__": - main() diff --git a/planning/autoware_static_centerline_generator/scripts/show_lanelet2_map_diff.py b/planning/autoware_static_centerline_generator/scripts/show_lanelet2_map_diff.py deleted file mode 100755 index 00d06ca2213d1..0000000000000 --- a/planning/autoware_static_centerline_generator/scripts/show_lanelet2_map_diff.py +++ /dev/null @@ -1,139 +0,0 @@ -#!/bin/env python3 - -# Copyright 2024 TIER IV, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import argparse -from decimal import Decimal -import os -import subprocess -import xml.etree.ElementTree as ET - - -def sort_attributes(root): - for shallow_element in root: - # sort nodes - attrib = shallow_element.attrib - if len(attrib) > 1: - attributes = sorted(attrib.items()) - attrib.clear() - attrib.update(attributes) - if shallow_element.tag == "relation": - pass - - # sort the element in the tag - for deep_element in shallow_element: - attrib = deep_element.attrib - if len(attrib) > 1: - # adjust attribute order, e.g. by sorting - attributes = sorted(attrib.items()) - attrib.clear() - attrib.update(attributes) - - # sort tags - sorted_shallow_element = sorted(shallow_element, key=lambda x: x.items()) - if len(shallow_element) != 0: - # NOTE: This "tail" is related to the indent of the next tag - first_tail = shallow_element[0].tail - last_tail = shallow_element[-1].tail - for idx, val_shallow_element in enumerate(sorted_shallow_element): - shallow_element[idx] = val_shallow_element - if idx == len(shallow_element) - 1: - shallow_element[idx].tail = last_tail - else: - shallow_element[idx].tail = first_tail - - -def remove_diff_to_ignore(osm_root): - decimal_precision = 11 # for lat/lon values - - # remove attributes of the osm tag - osm_root.attrib.clear() - - # remove the MetaInfo tag generated by Vector Map Builder - if osm_root[0].tag == "MetaInfo": - osm_root.remove(osm_root[0]) - - # remove unnecessary attributes for diff - for osm_child in osm_root: - if osm_child.tag == "osm": - osm_child.attrib.pop("osm") - if "visible" in osm_child.attrib and osm_child.attrib["visible"]: - osm_child.attrib.pop("visible") - if "version" in osm_child.attrib and osm_child.attrib["version"]: - osm_child.attrib.pop("version") - if "action" in osm_child.attrib and osm_child.attrib["action"] == "modify": - osm_child.attrib.pop("action") - if "lat" in osm_child.attrib: - osm_child.attrib["lat"] = str( - Decimal(float(osm_child.attrib["lat"])).quantize(Decimal(f"1e-{decimal_precision}")) - ) - if "lon" in osm_child.attrib: - osm_child.attrib["lon"] = str( - Decimal(float(osm_child.attrib["lon"])).quantize(Decimal(f"1e-{decimal_precision}")) - ) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument( - "-s", "--sort-attributes", action="store_true", help="Sort attributes of LL2 maps" - ) - parser.add_argument( - "-i", - "--ignore-minor-attributes", - action="store_true", - help="Ignore minor attributes of LL2 maps which does not change the map's behavior", - ) - args = parser.parse_args() - - original_osm_file_name = "/tmp/autoware_static_centerline_generator/input/lanelet2_map.osm" - modified_osm_file_name = "/tmp/autoware_static_centerline_generator/output/lanelet2_map.osm" - - # load LL2 maps - original_osm_tree = ET.parse(original_osm_file_name) - original_osm_root = original_osm_tree.getroot() - modified_osm_tree = ET.parse(modified_osm_file_name) - modified_osm_root = modified_osm_tree.getroot() - - # sort attributes - if args.sort_attributes: - sort_attributes(modified_osm_root) - sort_attributes(original_osm_root) - - # ignore minor attributes - if args.ignore_minor_attributes: - remove_diff_to_ignore(original_osm_root) - remove_diff_to_ignore(modified_osm_root) - - # write LL2 maps - output_dir_path = "/tmp/autoware_static_centerline_generator/show_lanelet2_map_diff/" - os.makedirs(output_dir_path + "original/", exist_ok=True) - os.makedirs(output_dir_path + "modified/", exist_ok=True) - - original_osm_tree.write(output_dir_path + "original/lanelet2_map.osm") - modified_osm_tree.write(output_dir_path + "modified/lanelet2_map.osm") - - # show diff - print("[INFO] Show diff of following LL2 maps") - print(f" {output_dir_path + 'original/lanelet2_map.osm'}") - print(f" {output_dir_path + 'modified/lanelet2_map.osm'}") - subprocess.run( - [ - "diff", - output_dir_path + "original/lanelet2_map.osm", - output_dir_path + "modified/lanelet2_map.osm", - "--color", - ] - ) diff --git a/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh b/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh deleted file mode 100755 index c170e24e475d9..0000000000000 --- a/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh +++ /dev/null @@ -1,5 +0,0 @@ -#!/bin/bash - -ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" mode:="GUI" lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory_turn-right.db3" vehicle_model:=lexus - -# ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" mode:="GUI" lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory_turn-left-right.db3" vehicle_model:=lexus diff --git a/planning/autoware_static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh b/planning/autoware_static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh deleted file mode 100755 index 488091989d1fa..0000000000000 --- a/planning/autoware_static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="optimization_trajectory_base" run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" start_lanelet_id:=125 end_lanelet_id:=132 vehicle_model:=lexus diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp deleted file mode 100644 index dc950f190652b..0000000000000 --- a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp +++ /dev/null @@ -1,86 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "centerline_source/bag_ego_trajectory_based_centerline.hpp" - -#include "rclcpp/serialization.hpp" -#include "rosbag2_cpp/reader.hpp" -#include "static_centerline_generator_node.hpp" - -#include - -#include -#include -#include - -namespace autoware::static_centerline_generator -{ -std::vector generate_centerline_with_bag(rclcpp::Node & node) -{ - const auto bag_filename = node.declare_parameter("bag_filename"); - - // open rosbag - rosbag2_cpp::Reader bag_reader; - bag_reader.open(bag_filename); - - // extract 2D position of ego's trajectory from rosbag - rclcpp::Serialization bag_serialization; - std::vector centerline_traj_points; - while (bag_reader.has_next()) { - const rosbag2_storage::SerializedBagMessageSharedPtr msg = bag_reader.read_next(); - - if (msg->topic_name != "/localization/kinematic_state") { - continue; - } - - rclcpp::SerializedMessage serialized_msg(*msg->serialized_data); - const auto ros_msg = std::make_shared(); - - bag_serialization.deserialize_message(&serialized_msg, ros_msg.get()); - - if (!centerline_traj_points.empty()) { - constexpr double epsilon = 1e-1; - if ( - std::abs(centerline_traj_points.back().pose.position.x - ros_msg->pose.pose.position.x) < - epsilon && - std::abs(centerline_traj_points.back().pose.position.y - ros_msg->pose.pose.position.y) < - epsilon) { - continue; - } - } - TrajectoryPoint centerline_traj_point; - centerline_traj_point.pose.position = ros_msg->pose.pose.position; - centerline_traj_points.push_back(centerline_traj_point); - } - - RCLCPP_INFO(node.get_logger(), "Extracted centerline from the bag."); - - // calculate rough orientation of centerline trajectory points - for (size_t i = 0; i < centerline_traj_points.size(); ++i) { - if (i == centerline_traj_points.size() - 1) { - if (i != 0) { - centerline_traj_points.at(i).pose.orientation = - centerline_traj_points.at(i - 1).pose.orientation; - } - } else { - const double yaw_angle = autoware::universe_utils::calcAzimuthAngle( - centerline_traj_points.at(i).pose.position, centerline_traj_points.at(i + 1).pose.position); - centerline_traj_points.at(i).pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(yaw_angle); - } - } - - return centerline_traj_points; -} -} // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp deleted file mode 100644 index 2275f88184c6f..0000000000000 --- a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp +++ /dev/null @@ -1,27 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ -#define CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ - -#include "rclcpp/rclcpp.hpp" -#include "type_alias.hpp" - -#include - -namespace autoware::static_centerline_generator -{ -std::vector generate_centerline_with_bag(rclcpp::Node & node); -} // namespace autoware::static_centerline_generator -#endif // CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp deleted file mode 100644 index 51725fb8a3799..0000000000000 --- a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ /dev/null @@ -1,187 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "centerline_source/optimization_trajectory_based_centerline.hpp" - -#include "autoware/motion_utils/resample/resample.hpp" -#include "autoware/motion_utils/trajectory/conversion.hpp" -#include "autoware/path_optimizer/node.hpp" -#include "autoware/path_smoother/elastic_band_smoother.hpp" -#include "autoware/universe_utils/ros/parameter.hpp" -#include "static_centerline_generator_node.hpp" -#include "utils.hpp" - -#include -#include - -namespace autoware::static_centerline_generator -{ -namespace -{ -rclcpp::NodeOptions create_node_options() -{ - return rclcpp::NodeOptions{}; -} - -Path convert_to_path(const PathWithLaneId & path_with_lane_id) -{ - Path path; - path.header = path_with_lane_id.header; - path.left_bound = path_with_lane_id.left_bound; - path.right_bound = path_with_lane_id.right_bound; - for (const auto & point : path_with_lane_id.points) { - path.points.push_back(point.point); - } - - return path; -} - -Trajectory convert_to_trajectory(const Path & path) -{ - Trajectory traj; - for (const auto & point : path.points) { - TrajectoryPoint traj_point; - traj_point.pose = point.pose; - traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; - traj_point.lateral_velocity_mps = point.lateral_velocity_mps; - traj_point.heading_rate_rps = point.heading_rate_rps; - - traj.points.push_back(traj_point); - } - return traj; -} -} // namespace - -OptimizationTrajectoryBasedCenterline::OptimizationTrajectoryBasedCenterline(rclcpp::Node & node) -{ - pub_raw_path_with_lane_id_ = - node.create_publisher("input_centerline", utils::create_transient_local_qos()); - pub_raw_path_ = - node.create_publisher("debug/raw_centerline", utils::create_transient_local_qos()); -} - -std::vector -OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( - rclcpp::Node & node, const RouteHandler & route_handler, - const std::vector & route_lane_ids) -{ - const auto route_lanelets = utils::get_lanelets_from_ids(route_handler, route_lane_ids); - - // optimize centerline inside the lane - const auto start_center_pose = utils::get_center_pose(route_handler, route_lane_ids.front()); - - // get ego nearest search parameters and resample interval in behavior_path_planner - const double ego_nearest_dist_threshold = - autoware::universe_utils::getOrDeclareParameter(node, "ego_nearest_dist_threshold"); - const double ego_nearest_yaw_threshold = - autoware::universe_utils::getOrDeclareParameter(node, "ego_nearest_yaw_threshold"); - const double behavior_path_interval = - autoware::universe_utils::getOrDeclareParameter(node, "output_path_interval"); - const double behavior_vel_interval = - autoware::universe_utils::getOrDeclareParameter(node, "behavior_output_path_interval"); - - // extract path with lane id from lanelets - const auto raw_path_with_lane_id = [&]() { - const auto non_resampled_path_with_lane_id = utils::get_path_with_lane_id( - route_handler, route_lanelets, start_center_pose, ego_nearest_dist_threshold, - ego_nearest_yaw_threshold); - return autoware::motion_utils::resamplePath( - non_resampled_path_with_lane_id, behavior_path_interval); - }(); - pub_raw_path_with_lane_id_->publish(raw_path_with_lane_id); - RCLCPP_INFO(node.get_logger(), "Calculated raw path with lane id and published."); - - // convert path with lane id to path - const auto raw_path = [&]() { - const auto non_resampled_path = convert_to_path(raw_path_with_lane_id); - return autoware::motion_utils::resamplePath(non_resampled_path, behavior_vel_interval); - }(); - pub_raw_path_->publish(raw_path); - RCLCPP_INFO(node.get_logger(), "Converted to path and published."); - - // smooth trajectory and road collision avoidance - const auto optimized_traj_points = optimize_trajectory(raw_path); - RCLCPP_INFO( - node.get_logger(), - "Smoothed trajectory and made it collision free with the road and published."); - - return optimized_traj_points; -} - -std::vector OptimizationTrajectoryBasedCenterline::optimize_trajectory( - const Path & raw_path) const -{ - // convert to trajectory points - const auto raw_traj_points = [&]() { - const auto raw_traj = convert_to_trajectory(raw_path); - return autoware::motion_utils::convertToTrajectoryPointArray(raw_traj); - }(); - - // create an instance of elastic band and model predictive trajectory. - const auto eb_path_smoother_ptr = - autoware::path_smoother::ElasticBandSmoother(create_node_options()).getElasticBandSmoother(); - const auto mpt_optimizer_ptr = - autoware::path_optimizer::PathOptimizer(create_node_options()).getMPTOptimizer(); - - // NOTE: The optimization is executed every valid_optimized_traj_points_num points. - constexpr int valid_optimized_traj_points_num = 10; - const int traj_segment_num = raw_traj_points.size() / valid_optimized_traj_points_num; - - // NOTE: num_initial_optimization exists to make the both optimizations stable since they may use - // warm start. - constexpr int num_initial_optimization = 2; - - std::vector whole_optimized_traj_points; - for (int virtual_ego_pose_idx = -num_initial_optimization; - virtual_ego_pose_idx < traj_segment_num; ++virtual_ego_pose_idx) { - // calculate virtual ego pose for the optimization - constexpr int virtual_ego_pose_offset_idx = 1; - const auto virtual_ego_pose = - raw_traj_points - .at( - valid_optimized_traj_points_num * std::max(virtual_ego_pose_idx, 0) + - virtual_ego_pose_offset_idx) - .pose; - - // smooth trajectory by elastic band in the autoware_path_smoother package - const auto smoothed_traj_points = - eb_path_smoother_ptr->smoothTrajectory(raw_traj_points, virtual_ego_pose); - - // road collision avoidance by model predictive trajectory in the autoware_path_optimizer - // package - const autoware::path_optimizer::PlannerData planner_data{ - raw_path.header, smoothed_traj_points, raw_path.left_bound, raw_path.right_bound, - virtual_ego_pose}; - const auto optimized_traj_points = mpt_optimizer_ptr->optimizeTrajectory(planner_data); - - // connect the previously and currently optimized trajectory points - for (size_t j = 0; j < whole_optimized_traj_points.size(); ++j) { - const double dist = autoware::universe_utils::calcDistance2d( - whole_optimized_traj_points.at(j), optimized_traj_points.front()); - if (dist < 0.5) { - const std::vector extracted_whole_optimized_traj_points{ - whole_optimized_traj_points.begin(), - whole_optimized_traj_points.begin() + std::max(j, 1UL) - 1}; - whole_optimized_traj_points = extracted_whole_optimized_traj_points; - break; - } - } - for (size_t j = 0; j < optimized_traj_points.size(); ++j) { - whole_optimized_traj_points.push_back(optimized_traj_points.at(j)); - } - } - - return whole_optimized_traj_points; -} -} // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp deleted file mode 100644 index 88def6fa7bd4c..0000000000000 --- a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT -#define CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT - -#include "rclcpp/rclcpp.hpp" -#include "type_alias.hpp" - -#include - -namespace autoware::static_centerline_generator -{ -class OptimizationTrajectoryBasedCenterline -{ -public: - OptimizationTrajectoryBasedCenterline() = default; - explicit OptimizationTrajectoryBasedCenterline(rclcpp::Node & node); - std::vector generate_centerline_with_optimization( - rclcpp::Node & node, const RouteHandler & route_handler, - const std::vector & route_lane_ids); - -private: - std::vector optimize_trajectory(const Path & raw_path) const; - - rclcpp::Publisher::SharedPtr pub_raw_path_with_lane_id_{nullptr}; - rclcpp::Publisher::SharedPtr pub_raw_path_{nullptr}; -}; -} // namespace autoware::static_centerline_generator -// clang-format off -#endif // CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT -// clang-format on diff --git a/planning/autoware_static_centerline_generator/src/main.cpp b/planning/autoware_static_centerline_generator/src/main.cpp deleted file mode 100644 index 811d57c6036ef..0000000000000 --- a/planning/autoware_static_centerline_generator/src/main.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "static_centerline_generator_node.hpp" - -#include -#include - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - // initialize node - rclcpp::NodeOptions node_options; - auto node = - std::make_shared( - node_options); - - // get ros parameter - const auto mode = node->declare_parameter("mode"); - - // process - if (mode == "AUTO") { - node->generate_centerline(); - node->validate(); - node->save_map(); - } else if (mode == "GUI") { - node->generate_centerline(); - } else if (mode == "VMB") { - // Do nothing - } else { - throw std::invalid_argument("The `mode` is invalid."); - } - - // NOTE: spin node to keep showing debug path/trajectory in rviz with transient local - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp deleted file mode 100644 index f172e3e0cb1cd..0000000000000 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ /dev/null @@ -1,774 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "static_centerline_generator_node.hpp" - -#include "autoware/map_loader/lanelet2_map_loader_node.hpp" -#include "autoware/map_projection_loader/load_info_from_lanelet2_map.hpp" -#include "autoware/map_projection_loader/map_projection_loader.hpp" -#include "autoware/motion_utils/resample/resample.hpp" -#include "autoware/motion_utils/trajectory/conversion.hpp" -#include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/parameter.hpp" -#include "autoware_lanelet2_extension/utility/message_conversion.hpp" -#include "autoware_lanelet2_extension/utility/query.hpp" -#include "autoware_lanelet2_extension/utility/utilities.hpp" -#include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" -#include "centerline_source/bag_ego_trajectory_based_centerline.hpp" -#include "type_alias.hpp" -#include "utils.hpp" - -#include -#include -#include -#include - -#include "std_msgs/msg/empty.hpp" -#include "std_msgs/msg/float32.hpp" -#include "std_msgs/msg/int32.hpp" - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define RESET_TEXT "\x1B[0m" -#define RED_TEXT "\x1B[31m" -#define YELLOW_TEXT "\x1b[33m" -#define BOLD_TEXT "\x1B[1m" - -namespace autoware::static_centerline_generator -{ -namespace -{ -std::vector get_lane_ids_from_route(const LaneletRoute & route) -{ - std::vector lane_ids; - for (const auto & segment : route.segments) { - const auto & target_lanelet_id = segment.preferred_primitive.id; - lane_ids.push_back(target_lanelet_id); - } - - return lane_ids; -} - -lanelet::BasicPoint2d convert_to_lanelet_point(const geometry_msgs::msg::Point & geom_point) -{ - lanelet::BasicPoint2d point(geom_point.x, geom_point.y); - return point; -} - -LinearRing2d create_vehicle_footprint( - const geometry_msgs::msg::Pose & pose, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0) -{ - const auto & i = vehicle_info; - - const double x_front = i.front_overhang_m + i.wheel_base_m + margin; - const double x_rear = -(i.rear_overhang_m + margin); - const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + margin; - const double y_right = -(i.wheel_tread_m / 2.0 + i.right_overhang_m + margin); - - std::vector geom_points; - geom_points.push_back( - autoware::universe_utils::calcOffsetPose(pose, x_front, y_left, 0.0).position); - geom_points.push_back( - autoware::universe_utils::calcOffsetPose(pose, x_front, y_right, 0.0).position); - geom_points.push_back( - autoware::universe_utils::calcOffsetPose(pose, x_rear, y_right, 0.0).position); - geom_points.push_back( - autoware::universe_utils::calcOffsetPose(pose, x_rear, y_left, 0.0).position); - - LinearRing2d footprint; - for (const auto & geom_point : geom_points) { - footprint.push_back(Point2d{geom_point.x, geom_point.y}); - } - footprint.push_back(footprint.back()); - - boost::geometry::correct(footprint); - - return footprint; -} - -geometry_msgs::msg::Pose get_text_pose( - const geometry_msgs::msg::Pose & pose, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) -{ - const auto & i = vehicle_info; - - const double x_front = i.front_overhang_m + i.wheel_base_m; - const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + 0.5; - - return autoware::universe_utils::calcOffsetPose(pose, x_front, y_left, 0.0); -} - -std::array convert_hex_string_to_decimal(const std::string & hex_str_color) -{ - unsigned int hex_int_color; - std::istringstream iss(hex_str_color); - iss >> std::hex >> hex_int_color; - - unsigned int unit = 16 * 16; - unsigned int b = hex_int_color % unit; - unsigned int g = (hex_int_color - b) / unit % unit; - unsigned int r = (hex_int_color - g * unit - b) / unit / unit; - - return std::array{r / 255.0, g / 255.0, b / 255.0}; -} - -std::vector check_lanelet_connection( - const RouteHandler & route_handler, const lanelet::ConstLanelets & route_lanelets) -{ - std::vector unconnected_lane_ids; - - for (size_t i = 0; i < route_lanelets.size() - 1; ++i) { - const auto next_lanelets = route_handler.getNextLanelets(route_lanelets.at(i)); - - const bool is_connected = - std::find_if(next_lanelets.begin(), next_lanelets.end(), [&](const auto & next_lanelet) { - return next_lanelet.id() == route_lanelets.at(i + 1).id(); - }) != next_lanelets.end(); - if (!is_connected) { - unconnected_lane_ids.push_back(route_lanelets.at(i).id()); - } - } - - return unconnected_lane_ids; -} - -std_msgs::msg::Header create_header(const rclcpp::Time & now) -{ - std_msgs::msg::Header header; - header.frame_id = "map"; - header.stamp = now; - return header; -} - -std::vector resample_trajectory_points( - const std::vector & input_traj_points, const double resample_interval) -{ - // resample and calculate trajectory points' orientation - const auto input_traj = autoware::motion_utils::convertToTrajectory(input_traj_points); - auto resampled_input_traj = - autoware::motion_utils::resampleTrajectory(input_traj, resample_interval); - return autoware::motion_utils::convertToTrajectoryPointArray(resampled_input_traj); -} - -std::vector convertToGeometryPoints(const LineString2d & lanelet_points) -{ - std::vector points; - for (const auto & lanelet_point : lanelet_points) { - geometry_msgs::msg::Point point; - point.x = lanelet_point.x(); - point.y = lanelet_point.y(); - points.push_back(point); - } - return points; -} -} // namespace - -StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( - const rclcpp::NodeOptions & node_options) -: Node("static_centerline_generator", node_options) -{ - // publishers - pub_map_bin_ = - create_publisher("lanelet2_map_topic", utils::create_transient_local_qos()); - pub_whole_centerline_ = - create_publisher("~/output/whole_centerline", utils::create_transient_local_qos()); - pub_centerline_ = - create_publisher("~/output/centerline", utils::create_transient_local_qos()); - - // debug publishers - pub_validation_results_ = - create_publisher("~/validation_results", utils::create_transient_local_qos()); - pub_debug_markers_ = - create_publisher("~/debug/markers", utils::create_transient_local_qos()); - - pub_debug_ego_footprint_bounds_ = create_publisher( - "~/debug/ego_footprint_bounds", utils::create_transient_local_qos()); - - // subscribers - sub_footprint_margin_for_road_bound_ = create_subscription( - "/static_centerline_generator/road_boundary_lateral_margin", rclcpp::QoS{1}, - [this](const std_msgs::msg::Float32 & msg) { footprint_margin_for_road_bound_ = msg.data; }); - sub_traj_start_index_ = create_subscription( - "/static_centerline_generator/traj_start_index", rclcpp::QoS{1}, - [this](const std_msgs::msg::Int32 & msg) { - if (centerline_handler_.update_start_index(msg.data)) { - visualize_selected_centerline(); - } - }); - sub_traj_end_index_ = create_subscription( - "/static_centerline_generator/traj_end_index", rclcpp::QoS{1}, - [this](const std_msgs::msg::Int32 & msg) { - if (centerline_handler_.update_end_index(msg.data)) { - visualize_selected_centerline(); - } - }); - sub_save_map_ = create_subscription( - "/static_centerline_generator/save_map", rclcpp::QoS{1}, - [this]([[maybe_unused]] const std_msgs::msg::Empty & msg) { - if (!centerline_handler_.is_valid()) { - return; - } - save_map(); - }); - sub_validate_ = create_subscription( - "/static_centerline_generator/validate", rclcpp::QoS{1}, - [this]([[maybe_unused]] const std_msgs::msg::Empty & msg) { validate(); }); - - // services - callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - srv_load_map_ = create_service( - "/planning/static_centerline_generator/load_map", - std::bind( - &StaticCenterlineGeneratorNode::on_load_map, this, std::placeholders::_1, - std::placeholders::_2), - rmw_qos_profile_services_default, callback_group_); - srv_plan_route_ = create_service( - "/planning/static_centerline_generator/plan_route", - std::bind( - &StaticCenterlineGeneratorNode::on_plan_route, this, std::placeholders::_1, - std::placeholders::_2), - rmw_qos_profile_services_default, callback_group_); - srv_plan_path_ = create_service( - "/planning/static_centerline_generator/plan_path", - std::bind( - &StaticCenterlineGeneratorNode::on_plan_path, this, std::placeholders::_1, - std::placeholders::_2), - rmw_qos_profile_services_default, callback_group_); - - // vehicle info - vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); - - centerline_source_ = [&]() { - const auto centerline_source_param = declare_parameter("centerline_source"); - if (centerline_source_param == "optimization_trajectory_base") { - optimization_trajectory_based_centerline_ = OptimizationTrajectoryBasedCenterline(*this); - return CenterlineSource::OptimizationTrajectoryBase; - } else if (centerline_source_param == "bag_ego_trajectory_base") { - return CenterlineSource::BagEgoTrajectoryBase; - } - throw std::logic_error( - "The centerline source is not supported in autoware_static_centerline_generator."); - }(); -} - -void StaticCenterlineGeneratorNode::visualize_selected_centerline() -{ - // publish selected centerline - const auto selected_centerline = centerline_handler_.get_selected_centerline(); - pub_centerline_->publish( - autoware::motion_utils::convertToTrajectory(selected_centerline, create_header(this->now()))); - - // delete markers for validation - pub_validation_results_->publish(utils::create_delete_all_marker_array({}, now())); - pub_debug_markers_->publish(utils::create_delete_all_marker_array( - {"unsafe_footprints", "unsafe_footprints_distance"}, now())); - pub_debug_ego_footprint_bounds_->publish( - utils::create_delete_all_marker_array({"road_bounds"}, now())); -} - -void StaticCenterlineGeneratorNode::generate_centerline() -{ - // declare planning setting parameters - const auto lanelet2_input_file_path = declare_parameter("lanelet2_input_file_path"); - - // process - load_map(lanelet2_input_file_path); - const auto whole_centerline_with_route = generate_whole_centerline_with_route(); - centerline_handler_ = CenterlineHandler(whole_centerline_with_route); - - visualize_selected_centerline(); -} - -CenterlineWithRoute StaticCenterlineGeneratorNode::generate_whole_centerline_with_route() -{ - if (!route_handler_ptr_) { - RCLCPP_ERROR(get_logger(), "Route handler is not ready. Return empty trajectory."); - return CenterlineWithRoute{}; - } - - // generate centerline with route - auto centerline_with_route = [&]() { - if (centerline_source_ == CenterlineSource::OptimizationTrajectoryBase) { - const lanelet::Id start_lanelet_id = declare_parameter("start_lanelet_id"); - const lanelet::Id end_lanelet_id = declare_parameter("end_lanelet_id"); - const auto route_lane_ids = plan_route_by_lane_ids(start_lanelet_id, end_lanelet_id); - const auto optimized_centerline = - optimization_trajectory_based_centerline_.generate_centerline_with_optimization( - *this, *route_handler_ptr_, route_lane_ids); - return CenterlineWithRoute{optimized_centerline, route_lane_ids}; - } else if (centerline_source_ == CenterlineSource::BagEgoTrajectoryBase) { - const auto bag_centerline = generate_centerline_with_bag(*this); - const auto route_lane_ids = - plan_route(bag_centerline.front().pose, bag_centerline.back().pose); - return CenterlineWithRoute{bag_centerline, route_lane_ids}; - } - throw std::logic_error( - "The centerline source is not supported in autoware_static_centerline_generator."); - }(); - - // resample - const double output_trajectory_interval = declare_parameter("output_trajectory_interval"); - centerline_with_route.centerline = - resample_trajectory_points(centerline_with_route.centerline, output_trajectory_interval); - - pub_whole_centerline_->publish(autoware::motion_utils::convertToTrajectory( - centerline_with_route.centerline, create_header(this->now()))); - - return centerline_with_route; -} - -void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_file_path) -{ - // copy the input LL2 map to the temporary file for debugging - const std::string debug_input_file_dir{"/tmp/autoware_static_centerline_generator/input/"}; - std::filesystem::create_directories(debug_input_file_dir); - std::filesystem::copy( - lanelet2_input_file_path, debug_input_file_dir + "lanelet2_map.osm", - std::filesystem::copy_options::overwrite_existing); - - // load map by the map_loader package - map_bin_ptr_ = [&]() -> LaneletMapBin::ConstSharedPtr { - // load map - map_projector_info_ = std::make_unique( - autoware::map_projection_loader::load_info_from_lanelet2_map(lanelet2_input_file_path)); - const auto map_ptr = autoware::map_loader::Lanelet2MapLoaderNode::load_map( - lanelet2_input_file_path, *map_projector_info_); - if (!map_ptr) { - return nullptr; - } - - // NOTE: The original map is stored here since the centerline will be added to all the - // lanelet when lanelet::utils::overwriteLaneletCenterline is called. - original_map_ptr_ = autoware::map_loader::Lanelet2MapLoaderNode::load_map( - lanelet2_input_file_path, *map_projector_info_); - - // overwrite more dense centerline - // NOTE: overwriteLaneletsCenterlineWithWaypoints is used only in real time calculation. - lanelet::utils::overwriteLaneletsCenterline(map_ptr, 5.0, false); - - // create map bin msg - const auto map_bin_msg = autoware::map_loader::Lanelet2MapLoaderNode::create_map_bin_msg( - map_ptr, lanelet2_input_file_path, now()); - - return std::make_shared(map_bin_msg); - }(); - - // check if map_bin_ptr_ is not null pointer - if (!map_bin_ptr_) { - RCLCPP_ERROR(get_logger(), "Loading map failed"); - return; - } - RCLCPP_INFO(get_logger(), "Loaded map."); - - // publish map bin msg - pub_map_bin_->publish(*map_bin_ptr_); - RCLCPP_INFO(get_logger(), "Published map."); - - // create route_handler - route_handler_ptr_ = std::make_shared(); - route_handler_ptr_->setMap(*map_bin_ptr_); -} - -void StaticCenterlineGeneratorNode::on_load_map( - const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response) -{ - const std::string tmp_lanelet2_input_file_path = "/tmp/input_lanelet2_map.osm"; - - // save map file temporarily since load map's input must be a file - std::ofstream map_writer; - map_writer.open(tmp_lanelet2_input_file_path, std::ios::out); - map_writer << request->map; - map_writer.close(); - - // load map from the saved map file - load_map(tmp_lanelet2_input_file_path); - - if (map_bin_ptr_) { - return; - } - - response->message = "InvalidMapFormat"; -} - -std::vector StaticCenterlineGeneratorNode::plan_route_by_lane_ids( - const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id) -{ - if (!route_handler_ptr_) { - RCLCPP_ERROR(get_logger(), "Map or route handler is not ready. Return empty lane ids."); - return std::vector{}; - } - - const auto start_center_pose = utils::get_center_pose(*route_handler_ptr_, start_lanelet_id); - const auto end_center_pose = utils::get_center_pose(*route_handler_ptr_, end_lanelet_id); - return plan_route(start_center_pose, end_center_pose); -} - -std::vector StaticCenterlineGeneratorNode::plan_route( - const geometry_msgs::msg::Pose & start_center_pose, - const geometry_msgs::msg::Pose & end_center_pose) -{ - if (!map_bin_ptr_) { - RCLCPP_ERROR(get_logger(), "Map or route handler is not ready. Return empty lane ids."); - return std::vector{}; - } - - // plan route by the mission_planner package - const auto route = [&]() { - // calculate check points - RCLCPP_INFO(get_logger(), "Calculated check points."); - const auto check_points = - std::vector{start_center_pose, end_center_pose}; - - // create mission_planner plugin - auto plugin_loader = pluginlib::ClassLoader( - "autoware_mission_planner", "autoware::mission_planner::PlannerPlugin"); - auto mission_planner = - plugin_loader.createSharedInstance("autoware::mission_planner::lanelet2::DefaultPlanner"); - - // initialize mission_planner - auto node = rclcpp::Node("mission_planner"); - mission_planner->initialize(&node, map_bin_ptr_); - - // plan route - const auto route = mission_planner->plan(check_points); - - return route; - }(); - - // get lanelets - const auto route_lane_ids = get_lane_ids_from_route(route); - - std::string route_lane_ids_str = ""; - for (const lanelet::Id route_lane_id : route_lane_ids) { - route_lane_ids_str += std::to_string(route_lane_id) + ","; - } - RCLCPP_INFO_STREAM(get_logger(), "Planned route. (" << route_lane_ids_str << ")"); - - return route_lane_ids; -} - -void StaticCenterlineGeneratorNode::on_plan_route( - const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response) -{ - if (!map_bin_ptr_ || !route_handler_ptr_) { - response->message = "MapNotFound"; - RCLCPP_ERROR(get_logger(), "Map is not ready."); - return; - } - - const lanelet::Id start_lanelet_id = request->start_lane_id; - const lanelet::Id end_lanelet_id = request->end_lane_id; - - // plan route - const auto route_lane_ids = plan_route_by_lane_ids(start_lanelet_id, end_lanelet_id); - const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - - // extract lane ids - std::vector lane_ids; - for (const auto & lanelet : route_lanelets) { - lane_ids.push_back(lanelet.id()); - } - - // check calculation result - if (lane_ids.empty()) { - response->message = "RouteNotFound"; - RCLCPP_ERROR(get_logger(), "Route planning failed."); - return; - } - - // set response - response->lane_ids = lane_ids; -} - -void StaticCenterlineGeneratorNode::on_plan_path( - const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response) -{ - if (!route_handler_ptr_) { - response->message = "MapNotFound"; - RCLCPP_ERROR(get_logger(), "Route handler is not ready."); - return; - } - - // get lanelets from route lane ids - const auto route_lane_ids = request->route; - const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - - // check if input route lanelets are connected to each other. - const auto unconnected_lane_ids = check_lanelet_connection(*route_handler_ptr_, route_lanelets); - if (!unconnected_lane_ids.empty()) { - response->message = "LaneletsNotConnected"; - response->unconnected_lane_ids = unconnected_lane_ids; - RCLCPP_ERROR(get_logger(), "Lanelets are not connected."); - return; - } - - // plan path - const auto optimized_traj_points = - optimization_trajectory_based_centerline_.generate_centerline_with_optimization( - *this, *route_handler_ptr_, route_lane_ids); - - // check calculation result - if (optimized_traj_points.empty()) { - response->message = "PathNotFound"; - RCLCPP_ERROR(get_logger(), "Path planning failed."); - return; - } - - centerline_handler_ = - CenterlineHandler(CenterlineWithRoute{optimized_traj_points, route_lane_ids}); - - // publish unsafe_footprints - validate(); - - // create output data - auto target_traj_point = optimized_traj_points.cbegin(); - bool is_end_lanelet = false; - for (const auto & lanelet : route_lanelets) { - std::vector current_lanelet_points; - - // check if target point is inside the lanelet - while (lanelet::geometry::inside( - lanelet, convert_to_lanelet_point(target_traj_point->pose.position))) { - // memorize points inside the lanelet - current_lanelet_points.push_back(target_traj_point->pose.position); - target_traj_point++; - - if (target_traj_point == optimized_traj_points.cend()) { - is_end_lanelet = true; - break; - } - } - - if (!current_lanelet_points.empty()) { - // register points with lane_id - autoware_static_centerline_generator::msg::PointsWithLaneId points_with_lane_id; - points_with_lane_id.lane_id = lanelet.id(); - points_with_lane_id.points = current_lanelet_points; - response->points_with_lane_ids.push_back(points_with_lane_id); - } - - if (is_end_lanelet) { - break; - } - } - - // empty string if error did not occur - response->message = ""; -} - -void StaticCenterlineGeneratorNode::validate() -{ - std::cerr << std::endl - << "############################################## Validation Results " - "##############################################" - << std::endl; - - const auto & centerline = centerline_handler_.get_selected_centerline(); - const auto & route_lane_ids = centerline_handler_.get_route_lane_ids(); - const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - - const double dist_thresh_to_road_border = - getRosParameter("validation.dist_threshold_to_road_border"); - const double max_steer_angle_margin = - getRosParameter("validation.max_steer_angle_margin"); - - // calculate color for distance to road border - const auto dist_thresh_vec = getRosParameter>("marker_color_dist_thresh"); - const auto marker_color_vec = getRosParameter>("marker_color"); - const auto get_marker_color = [&](const double dist) -> boost::optional> { - for (size_t i = 0; i < dist_thresh_vec.size(); ++i) { - const double dist_thresh = dist_thresh_vec.at(i); - if (dist < dist_thresh) { - return convert_hex_string_to_decimal(marker_color_vec.at(i)); - } - } - return boost::none; - }; - - // create right/left bound - LineString2d lanelet_right_bound; - LineString2d lanelet_left_bound; - for (const auto & lanelet : route_lanelets) { - for (const auto & point : lanelet.rightBound()) { - boost::geometry::append(lanelet_right_bound, Point2d(point.x(), point.y())); - } - for (const auto & point : lanelet.leftBound()) { - boost::geometry::append(lanelet_left_bound, Point2d(point.x(), point.y())); - } - } - - // calculate curvature - const auto curvature_vec = autoware::motion_utils::calcCurvature(centerline); - const double steer_angle_threshold = vehicle_info_.max_steer_angle_rad - max_steer_angle_margin; - - // calculate the distance between footprint and right/left bounds - MarkerArray marker_array; - double min_dist = std::numeric_limits::max(); - double max_curvature = std::numeric_limits::min(); - for (size_t i = 0; i < centerline.size(); ++i) { - const auto & traj_point = centerline.at(i); - - const auto footprint_poly = create_vehicle_footprint(traj_point.pose, vehicle_info_); - - const double dist_to_right = boost::geometry::distance(footprint_poly, lanelet_right_bound); - const double dist_to_left = boost::geometry::distance(footprint_poly, lanelet_left_bound); - const double min_dist_to_bound = std::min(dist_to_right, dist_to_left); - - if (min_dist_to_bound < min_dist) { - min_dist = min_dist_to_bound; - } - - // create marker - const auto marker_color_opt = get_marker_color(min_dist_to_bound); - const auto text_pose = get_text_pose(traj_point.pose, vehicle_info_); - if (marker_color_opt) { - const auto & marker_color = marker_color_opt.get(); - - // add footprint marker - const auto footprint_marker = utils::create_footprint_marker( - footprint_poly, 0.05, marker_color.at(0), marker_color.at(1), marker_color.at(2), 0.7, - now(), i); - marker_array.markers.push_back(footprint_marker); - - // add text of distance to bounds marker - const auto text_marker = utils::create_text_marker( - "unsafe_footprints_distance", text_pose, min_dist_to_bound, marker_color.at(0), - marker_color.at(1), marker_color.at(2), 0.999, now(), i); - marker_array.markers.push_back(text_marker); - } - - const double curvature = curvature_vec.at(i); - const auto text_marker = - utils::create_text_marker("curvature", text_pose, curvature, 0.05, 0.05, 0.0, 0.9, now(), i); - marker_array.markers.push_back(text_marker); - - if (max_curvature < std::abs(curvature)) { - max_curvature = std::abs(curvature); - } - } - const double max_steer_angle = vehicle_info_.calcSteerAngleFromCurvature(max_curvature); - - // publish road boundaries - const auto left_bound = convertToGeometryPoints(lanelet_left_bound); - const auto right_bound = convertToGeometryPoints(lanelet_right_bound); - - marker_array.markers.push_back( - utils::create_points_marker("left_bound", left_bound, 0.05, 0.0, 0.6, 0.8, 0.8, now())); - marker_array.markers.push_back( - utils::create_points_marker("right_bound", right_bound, 0.05, 0.0, 0.6, 0.8, 0.8, now())); - pub_debug_markers_->publish(marker_array); - - // show the validation results - // 1. distance from footprints to road boundaries - const bool are_footprints_inside_lanelets = [&]() { - std::cerr << "1. Footprints inside Lanelets:" << std::endl; - if (dist_thresh_to_road_border < min_dist) { - std::cerr << " The generated centerline is inside the lanelet. (threshold:" - << dist_thresh_to_road_border << "[m] < actual:" << min_dist << "[m])" << std::endl - << " Passed." << std::endl; - return true; - } - std::cerr << RED_TEXT - << " The generated centerline is outside the lanelet. (actual:" << min_dist - << "[m] <= threshold:" << dist_thresh_to_road_border << "[m])" << std::endl - << " Failed." << RESET_TEXT << std::endl; - return false; - }(); - // 2. centerline's curvature - std::cerr << "2. Curvature:" << std::endl; - const bool is_curvature_low = - true; // always tre for now since the curvature is just estimated and not enough precise. - if (max_steer_angle < steer_angle_threshold) { - std::cerr << " The generated centerline has no high steer angle. (estimated:" - << autoware::universe_utils::rad2deg(max_steer_angle) - << "[deg] < threshold:" << autoware::universe_utils::rad2deg(steer_angle_threshold) - << "[deg])" << std::endl - << " Passed." << std::endl; - } else { - std::cerr << YELLOW_TEXT << " The generated centerline has a too high steer angle. (threshold:" - << autoware::universe_utils::rad2deg(steer_angle_threshold) - << "[deg] <= estimated:" << autoware::universe_utils::rad2deg(max_steer_angle) - << "[deg])" << std::endl - << " However, the estimated steer angle is not enough precise, so the result is " - "conditional pass." - << std::endl - << " Conditionally Passed." << RESET_TEXT << std::endl; - } - // 3. result - std::cerr << std::endl << BOLD_TEXT << "Result:" << RESET_TEXT << std::endl; - if (are_footprints_inside_lanelets && is_curvature_low) { - std::cerr << BOLD_TEXT << " Passed!" << RESET_TEXT << std::endl; - } else { - std::cerr << BOLD_TEXT << RED_TEXT << " Failed!" << RESET_TEXT << std::endl; - } - - std::cerr << "###################################################################################" - "#############################" - << std::endl - << std::endl; - RCLCPP_INFO(get_logger(), "Validated the generated centerline."); -} - -void StaticCenterlineGeneratorNode::save_map() -{ - if (!route_handler_ptr_) { - return; - } - - const auto & centerline = centerline_handler_.get_selected_centerline(); - const auto & route_lane_ids = centerline_handler_.get_route_lane_ids(); - - const auto lanelet2_output_file_path = getRosParameter("lanelet2_output_file_path"); - - const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - - // update centerline in map - utils::update_centerline(original_map_ptr_, route_lanelets, centerline); - RCLCPP_INFO(get_logger(), "Updated centerline in map."); - - // save map with modified center line - std::filesystem::create_directory("/tmp/autoware_static_centerline_generator"); - const auto map_projector = - autoware::geography_utils::get_lanelet2_projector(*map_projector_info_); - lanelet::write(lanelet2_output_file_path, *original_map_ptr_, *map_projector); - RCLCPP_INFO( - get_logger(), "Saved map in %s", "/tmp/autoware_static_centerline_generator/lanelet2_map.osm"); - - // copy the output LL2 map to the temporary file for debugging - const std::string debug_output_file_dir{"/tmp/autoware_static_centerline_generator/output/"}; - std::filesystem::create_directories(debug_output_file_dir); - std::filesystem::copy( - lanelet2_output_file_path, debug_output_file_dir + "lanelet2_map.osm", - std::filesystem::copy_options::overwrite_existing); -} -} // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp deleted file mode 100644 index c591dcfc73bd8..0000000000000 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp +++ /dev/null @@ -1,187 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef STATIC_CENTERLINE_GENERATOR_NODE_HPP_ -#define STATIC_CENTERLINE_GENERATOR_NODE_HPP_ - -#include "autoware/universe_utils/ros/parameter.hpp" -#include "autoware_static_centerline_generator/srv/load_map.hpp" -#include "autoware_static_centerline_generator/srv/plan_path.hpp" -#include "autoware_static_centerline_generator/srv/plan_route.hpp" -#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "centerline_source/optimization_trajectory_based_centerline.hpp" -#include "rclcpp/rclcpp.hpp" -#include "type_alias.hpp" - -#include "autoware_map_msgs/msg/map_projector_info.hpp" -#include "std_msgs/msg/empty.hpp" -#include "std_msgs/msg/float32.hpp" -#include "std_msgs/msg/int32.hpp" - -#include -#include -#include -#include - -namespace autoware::static_centerline_generator -{ -using autoware_map_msgs::msg::MapProjectorInfo; -using autoware_static_centerline_generator::srv::LoadMap; -using autoware_static_centerline_generator::srv::PlanPath; -using autoware_static_centerline_generator::srv::PlanRoute; - -struct CenterlineWithRoute -{ - std::vector centerline{}; - std::vector route_lane_ids{}; -}; -struct CenterlineHandler -{ - CenterlineHandler() = default; - explicit CenterlineHandler(const CenterlineWithRoute & centerline_with_route) - : whole_centerline_with_route(centerline_with_route), - start_index(0), - end_index(centerline_with_route.centerline.size() - 1) - { - } - std::vector get_selected_centerline() const - { - if (!whole_centerline_with_route) { - return std::vector{}; - } - const auto & centerline_begin = whole_centerline_with_route->centerline.begin(); - return std::vector( - centerline_begin + start_index, centerline_begin + end_index + 1); - } - std::vector get_route_lane_ids() const - { - if (!whole_centerline_with_route) { - return std::vector{}; - } - return whole_centerline_with_route->route_lane_ids; - } - bool is_valid() const { return whole_centerline_with_route && start_index < end_index; } - bool update_start_index(const int arg_start_index) - { - if (whole_centerline_with_route && arg_start_index < end_index) { - start_index = arg_start_index; - return true; - } - return false; - } - bool update_end_index(const int arg_end_index) - { - if (whole_centerline_with_route && start_index < arg_end_index) { - end_index = arg_end_index; - return true; - } - return false; - } - - std::optional whole_centerline_with_route{std::nullopt}; - int start_index{}; - int end_index{}; -}; - -struct RoadBounds -{ - std::vector left_bound; - std::vector right_bound; -}; - -class StaticCenterlineGeneratorNode : public rclcpp::Node -{ -public: - explicit StaticCenterlineGeneratorNode(const rclcpp::NodeOptions & node_options); - void generate_centerline(); - void validate(); - void save_map(); - -private: - // load map - void load_map(const std::string & lanelet2_input_file_path); - void on_load_map( - const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response); - - // plan route - std::vector plan_route_by_lane_ids( - const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id); - std::vector plan_route( - const geometry_msgs::msg::Pose & start_center_pose, - const geometry_msgs::msg::Pose & end_center_pose); - - void on_plan_route( - const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response); - - // plan centerline - CenterlineWithRoute generate_whole_centerline_with_route(); - std::vector get_route_lane_ids_from_points( - const std::vector & points); - void on_plan_path( - const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response); - - void visualize_selected_centerline(); - - // parameter - template - T getRosParameter(const std::string & param_name) - { - return autoware::universe_utils::getOrDeclareParameter(*this, param_name); - } - - lanelet::LaneletMapPtr original_map_ptr_{nullptr}; - LaneletMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; - std::shared_ptr route_handler_ptr_{nullptr}; - std::unique_ptr map_projector_info_{nullptr}; - - CenterlineHandler centerline_handler_; - - float footprint_margin_for_road_bound_{0.0}; - - enum class CenterlineSource { - OptimizationTrajectoryBase = 0, - BagEgoTrajectoryBase, - }; - CenterlineSource centerline_source_; - OptimizationTrajectoryBasedCenterline optimization_trajectory_based_centerline_; - - // publisher - rclcpp::Publisher::SharedPtr pub_map_bin_{nullptr}; - rclcpp::Publisher::SharedPtr pub_whole_centerline_{nullptr}; - rclcpp::Publisher::SharedPtr pub_centerline_{nullptr}; - rclcpp::Publisher::SharedPtr pub_validation_results_{nullptr}; - rclcpp::Publisher::SharedPtr pub_debug_ego_footprint_bounds_{nullptr}; - rclcpp::Publisher::SharedPtr pub_debug_markers_{nullptr}; - - // subscriber - rclcpp::Subscription::SharedPtr sub_traj_start_index_; - rclcpp::Subscription::SharedPtr sub_traj_end_index_; - rclcpp::Subscription::SharedPtr sub_save_map_; - rclcpp::Subscription::SharedPtr sub_validate_; - rclcpp::Subscription::SharedPtr sub_traj_resample_interval_; - rclcpp::Subscription::SharedPtr sub_footprint_margin_for_road_bound_; - - // service - rclcpp::Service::SharedPtr srv_load_map_; - rclcpp::Service::SharedPtr srv_plan_route_; - rclcpp::Service::SharedPtr srv_plan_path_; - - // callback group for service - rclcpp::CallbackGroup::SharedPtr callback_group_; - - // vehicle info - autoware::vehicle_info_utils::VehicleInfo vehicle_info_; -}; -} // namespace autoware::static_centerline_generator -#endif // STATIC_CENTERLINE_GENERATOR_NODE_HPP_ diff --git a/planning/autoware_static_centerline_generator/src/type_alias.hpp b/planning/autoware_static_centerline_generator/src/type_alias.hpp deleted file mode 100644 index 2b7b99bfe81be..0000000000000 --- a/planning/autoware_static_centerline_generator/src/type_alias.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef TYPE_ALIAS_HPP_ -#define TYPE_ALIAS_HPP_ - -#include "autoware/route_handler/route_handler.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" - -#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" -#include "autoware_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_planning_msgs/msg/lanelet_route.hpp" -#include "autoware_planning_msgs/msg/path.hpp" -#include "autoware_planning_msgs/msg/path_point.hpp" -#include "autoware_planning_msgs/msg/trajectory.hpp" -#include "autoware_planning_msgs/msg/trajectory_point.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - -namespace autoware::static_centerline_generator -{ -using autoware::route_handler::RouteHandler; -using autoware::universe_utils::LinearRing2d; -using autoware::universe_utils::LineString2d; -using autoware::universe_utils::Point2d; -using autoware_map_msgs::msg::LaneletMapBin; -using autoware_perception_msgs::msg::PredictedObjects; -using autoware_planning_msgs::msg::LaneletRoute; -using autoware_planning_msgs::msg::Path; -using autoware_planning_msgs::msg::PathPoint; -using autoware_planning_msgs::msg::Trajectory; -using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_planning_msgs::msg::PathWithLaneId; -using visualization_msgs::msg::Marker; -using visualization_msgs::msg::MarkerArray; -} // namespace autoware::static_centerline_generator - -#endif // TYPE_ALIAS_HPP_ diff --git a/planning/autoware_static_centerline_generator/src/utils.cpp b/planning/autoware_static_centerline_generator/src/utils.cpp deleted file mode 100644 index f84fe79cec288..0000000000000 --- a/planning/autoware_static_centerline_generator/src/utils.cpp +++ /dev/null @@ -1,263 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "utils.hpp" - -#include "autoware/behavior_path_planner_common/data_manager.hpp" -#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/ros/marker_helper.hpp" - -#include -#include - -#include -#include -#include -#include -#include - -namespace autoware::static_centerline_generator -{ -namespace -{ -nav_msgs::msg::Odometry::ConstSharedPtr convert_to_odometry(const geometry_msgs::msg::Pose & pose) -{ - auto odometry_ptr = std::make_shared(); - odometry_ptr->pose.pose = pose; - return odometry_ptr; -} - -lanelet::Point3d createPoint3d(const double x, const double y, const double z) -{ - lanelet::Point3d point(lanelet::utils::getId()); - - // position - point.x() = x; - point.y() = y; - point.z() = z; - - // attributes - point.setAttribute("local_x", x); - point.setAttribute("local_y", y); - // NOTE: It seems that the attribute "ele" is assigned automatically. - - return point; -} -} // namespace - -namespace utils -{ -rclcpp::QoS create_transient_local_qos() -{ - return rclcpp::QoS{1}.transient_local(); -} - -lanelet::ConstLanelets get_lanelets_from_ids( - const RouteHandler & route_handler, const std::vector & lane_ids) -{ - lanelet::ConstLanelets lanelets; - for (const lanelet::Id lane_id : lane_ids) { - const auto lanelet = route_handler.getLaneletsFromId(lane_id); - lanelets.push_back(lanelet); - } - return lanelets; -} - -geometry_msgs::msg::Pose get_center_pose( - const RouteHandler & route_handler, const size_t lanelet_id) -{ - // get middle idx of the lanelet - const auto lanelet = route_handler.getLaneletsFromId(lanelet_id); - const auto center_line = lanelet.centerline(); - const size_t middle_point_idx = std::floor(center_line.size() / 2.0); - - // get middle position of the lanelet - geometry_msgs::msg::Point middle_pos; - middle_pos.x = center_line[middle_point_idx].x(); - middle_pos.y = center_line[middle_point_idx].y(); - middle_pos.z = center_line[middle_point_idx].z(); - - // get next middle position of the lanelet - geometry_msgs::msg::Point next_middle_pos; - next_middle_pos.x = center_line[middle_point_idx + 1].x(); - next_middle_pos.y = center_line[middle_point_idx + 1].y(); - next_middle_pos.z = center_line[middle_point_idx + 1].z(); - - // calculate middle pose - geometry_msgs::msg::Pose middle_pose; - middle_pose.position = middle_pos; - const double yaw = autoware::universe_utils::calcAzimuthAngle(middle_pos, next_middle_pos); - middle_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); - - return middle_pose; -} - -PathWithLaneId get_path_with_lane_id( - const RouteHandler & route_handler, const lanelet::ConstLanelets lanelets, - const geometry_msgs::msg::Pose & start_pose, const double ego_nearest_dist_threshold, - const double ego_nearest_yaw_threshold) -{ - // get center line - constexpr double s_start = 0.0; - constexpr double s_end = std::numeric_limits::max(); - auto path_with_lane_id = route_handler.getCenterLinePath(lanelets, s_start, s_end); - path_with_lane_id.header.frame_id = "map"; - - // create planner data - auto planner_data = std::make_shared(); - planner_data->route_handler = std::make_shared(route_handler); - planner_data->self_odometry = convert_to_odometry(start_pose); - planner_data->parameters.ego_nearest_dist_threshold = ego_nearest_dist_threshold; - planner_data->parameters.ego_nearest_yaw_threshold = ego_nearest_yaw_threshold; - - // generate drivable area and store it in path with lane id - constexpr double vehicle_length = 0.0; - const auto drivable_lanes = behavior_path_planner::utils::generateDrivableLanes(lanelets); - behavior_path_planner::utils::generateDrivableArea( - path_with_lane_id, drivable_lanes, false, false, vehicle_length, planner_data); - - return path_with_lane_id; -} - -void update_centerline( - lanelet::LaneletMapPtr lanelet_map_ptr, const lanelet::ConstLanelets & lanelets, - const std::vector & new_centerline) -{ - // get lanelet as reference to update centerline - lanelet::Lanelets lanelets_ref; - for (const auto & lanelet : lanelets) { - for (auto & lanelet_ref : lanelet_map_ptr->laneletLayer) { - if (lanelet_ref.id() == lanelet.id()) { - lanelets_ref.push_back(lanelet_ref); - } - } - } - - // store new centerline in lanelets - size_t lanelet_idx = 0; - lanelet::LineString3d centerline(lanelet::utils::getId()); - for (size_t traj_idx = 0; traj_idx < new_centerline.size(); ++traj_idx) { - const auto & traj_pos = new_centerline.at(traj_idx).pose.position; - - for (; lanelet_idx < lanelets_ref.size(); ++lanelet_idx) { - auto & lanelet_ref = lanelets_ref.at(lanelet_idx); - - const lanelet::BasicPoint2d point(traj_pos.x, traj_pos.y); - // TODO(murooka) This does not work with L-crank map. - const bool is_inside = lanelet::geometry::inside(lanelet_ref, point); - if (is_inside) { - const auto center_point = createPoint3d(traj_pos.x, traj_pos.y, traj_pos.z); - - // set center point - centerline.push_back(center_point); - lanelet_map_ptr->add(center_point); - break; - } - - if (!centerline.empty()) { - // set centerline - lanelet_map_ptr->add(centerline); - lanelet_ref.setCenterline(centerline); - - // prepare new centerline - centerline = lanelet::LineString3d(lanelet::utils::getId()); - } - } - - if (traj_idx == new_centerline.size() - 1 && !centerline.empty()) { - auto & lanelet_ref = lanelets_ref.at(lanelet_idx); - - // set centerline - lanelet_map_ptr->add(centerline); - lanelet_ref.setCenterline(centerline); - } - } -} - -Marker create_footprint_marker( - const LinearRing2d & footprint_poly, const double width, const double r, const double g, - const double b, const double a, const rclcpp::Time & now, const size_t idx) -{ - auto marker = autoware::universe_utils::createDefaultMarker( - "map", rclcpp::Clock().now(), "unsafe_footprints", idx, - visualization_msgs::msg::Marker::LINE_STRIP, - autoware::universe_utils::createMarkerScale(width, 0.0, 0.0), - autoware::universe_utils::createMarkerColor(r, g, b, a)); - marker.header.stamp = now; - // TODO(murooka) Ideally, the following is unnecessary for the topic of transient local. - marker.lifetime = rclcpp::Duration(0, 0); - - for (const auto & point : footprint_poly) { - geometry_msgs::msg::Point geom_point; - geom_point.x = point.x(); - geom_point.y = point.y(); - // geom_point.z = point.z(); - - marker.points.push_back(geom_point); - } - marker.points.push_back(marker.points.front()); - return marker; -} - -Marker create_text_marker( - const std::string & ns, const geometry_msgs::msg::Pose & pose, const double value, const double r, - const double g, const double b, const double a, const rclcpp::Time & now, const size_t idx) -{ - auto marker = autoware::universe_utils::createDefaultMarker( - "map", rclcpp::Clock().now(), ns, idx, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - autoware::universe_utils::createMarkerScale(0.5, 0.5, 0.5), - autoware::universe_utils::createMarkerColor(r, g, b, a)); - marker.pose = pose; - marker.header.stamp = now; - marker.lifetime = rclcpp::Duration(0, 0); - - std::stringstream ss; - ss << std::setprecision(2) << value; - marker.text = ss.str(); - - return marker; -} - -Marker create_points_marker( - const std::string & ns, const std::vector & points, const double width, - const double r, const double g, const double b, const double a, const rclcpp::Time & now) -{ - auto marker = autoware::universe_utils::createDefaultMarker( - "map", now, ns, 1, Marker::LINE_STRIP, - autoware::universe_utils::createMarkerScale(width, 0.0, 0.0), - autoware::universe_utils::createMarkerColor(r, g, b, a)); - marker.lifetime = rclcpp::Duration(0, 0); - marker.points = points; - return marker; -} - -MarkerArray create_delete_all_marker_array( - const std::vector & ns_vec, const rclcpp::Time & now) -{ - Marker marker; - marker.header.stamp = now; - marker.action = visualization_msgs::msg::Marker::DELETEALL; - - MarkerArray marker_array; - for (const auto & ns : ns_vec) { - marker.ns = ns; - marker_array.markers.push_back(marker); - } - - return marker_array; -} - -} // namespace utils -} // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/utils.hpp b/planning/autoware_static_centerline_generator/src/utils.hpp deleted file mode 100644 index f4d15d3dcfd4f..0000000000000 --- a/planning/autoware_static_centerline_generator/src/utils.hpp +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef UTILS_HPP_ -#define UTILS_HPP_ - -#include "autoware/route_handler/route_handler.hpp" -#include "type_alias.hpp" - -#include - -#include -#include -#include -#include - -namespace autoware::static_centerline_generator -{ -namespace utils -{ -rclcpp::QoS create_transient_local_qos(); - -lanelet::ConstLanelets get_lanelets_from_ids( - const RouteHandler & route_handler, const std::vector & lane_ids); - -geometry_msgs::msg::Pose get_center_pose( - const RouteHandler & route_handler, const size_t lanelet_id); - -PathWithLaneId get_path_with_lane_id( - const RouteHandler & route_handler, const lanelet::ConstLanelets lanelets, - const geometry_msgs::msg::Pose & start_pose, const double ego_nearest_dist_threshold, - const double ego_nearest_yaw_threshold); - -void update_centerline( - lanelet::LaneletMapPtr lanelet_map_ptr, const lanelet::ConstLanelets & lanelets, - const std::vector & new_centerline); - -Marker create_footprint_marker( - const LinearRing2d & footprint_poly, const double width, const double r, const double g, - const double b, const double a, const rclcpp::Time & now, const size_t idx); - -Marker create_text_marker( - const std::string & ns, const geometry_msgs::msg::Pose & pose, const double value, const double r, - const double g, const double b, const double a, const rclcpp::Time & now, const size_t idx); - -Marker create_points_marker( - const std::string & ns, const std::vector & points, const double width, - const double r, const double g, const double b, const double a, const rclcpp::Time & now); - -MarkerArray create_delete_all_marker_array( - const std::vector & ns_vec, const rclcpp::Time & now); - -} // namespace utils -} // namespace autoware::static_centerline_generator - -#endif // UTILS_HPP_ diff --git a/planning/autoware_static_centerline_generator/srv/LoadMap.srv b/planning/autoware_static_centerline_generator/srv/LoadMap.srv deleted file mode 100644 index 02142e60c0035..0000000000000 --- a/planning/autoware_static_centerline_generator/srv/LoadMap.srv +++ /dev/null @@ -1,3 +0,0 @@ -string map ---- -string message diff --git a/planning/autoware_static_centerline_generator/srv/PlanPath.srv b/planning/autoware_static_centerline_generator/srv/PlanPath.srv deleted file mode 100644 index 3a8d0321ffb9a..0000000000000 --- a/planning/autoware_static_centerline_generator/srv/PlanPath.srv +++ /dev/null @@ -1,6 +0,0 @@ -uint32 map_id -int64[] route ---- -autoware_static_centerline_generator/PointsWithLaneId[] points_with_lane_ids -string message -int64[] unconnected_lane_ids diff --git a/planning/autoware_static_centerline_generator/srv/PlanRoute.srv b/planning/autoware_static_centerline_generator/srv/PlanRoute.srv deleted file mode 100644 index fb50c04b0ff26..0000000000000 --- a/planning/autoware_static_centerline_generator/srv/PlanRoute.srv +++ /dev/null @@ -1,5 +0,0 @@ -int64 start_lane_id -int64 end_lane_id ---- -int64[] lane_ids -string message diff --git a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 deleted file mode 100644 index ed3448772b794..0000000000000 Binary files a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 and /dev/null differ diff --git a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-right.db3 b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-right.db3 deleted file mode 100644 index 0883307acbae0..0000000000000 Binary files a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-right.db3 and /dev/null differ diff --git a/planning/autoware_static_centerline_generator/test/data/lanelet2_map.osm b/planning/autoware_static_centerline_generator/test/data/lanelet2_map.osm deleted file mode 100644 index 406cd85c151ea..0000000000000 --- a/planning/autoware_static_centerline_generator/test/data/lanelet2_map.osm +++ /dev/null @@ -1,146 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py deleted file mode 100644 index ca2621212ef83..0000000000000 --- a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py +++ /dev/null @@ -1,112 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2022 TIER IV, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os -import unittest - -from ament_index_python import get_package_share_directory -import launch -from launch import LaunchDescription -from launch_ros.actions import Node -import launch_testing -import pytest - - -@pytest.mark.launch_test -def generate_test_description(): - lanelet2_map_path = os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), - "test/data/lanelet2_map.osm", - ) - - static_centerline_generator_node = Node( - package="autoware_static_centerline_generator", - executable="main", - output="screen", - parameters=[ - {"lanelet2_map_path": lanelet2_map_path}, - {"mode": "AUTO"}, - {"rviz": False}, - {"centerline_source": "optimization_trajectory_base"}, - {"lanelet2_input_file_path": lanelet2_map_path}, - {"lanelet2_output_file_path": "/tmp/lanelet2_map.osm"}, - {"start_lanelet_id": 215}, - {"end_lanelet_id": 216}, - os.path.join( - get_package_share_directory("autoware_mission_planner"), - "config", - "mission_planner.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), - "config/static_centerline_generator.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_behavior_path_planner"), - "config/behavior_path_planner.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_behavior_velocity_planner"), - "config/behavior_velocity_planner.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_path_smoother"), - "config/elastic_band_smoother.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_path_optimizer"), - "config/path_optimizer.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_map_loader"), - "config/lanelet2_map_loader.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), - "config/common.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), - "config/nearest_search.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), - "config/vehicle_info.param.yaml", - ), - ], - ) - - context = {} - - return ( - LaunchDescription( - [ - static_centerline_generator_node, - # Start test after 1s - gives time for the autoware_static_centerline_generator to finish initialization - launch.actions.TimerAction( - period=1.0, actions=[launch_testing.actions.ReadyToTest()] - ), - ] - ), - context, - ) - - -@launch_testing.post_shutdown_test() -class TestProcessOutput(unittest.TestCase): - def test_exit_code(self, proc_info): - # Check that process exits with code 0: no error - launch_testing.asserts.assertExitCodes(proc_info) diff --git a/planning/autoware_surround_obstacle_checker/package.xml b/planning/autoware_surround_obstacle_checker/package.xml index 4a6f946b8eb90..8bbc0b6bf4e02 100644 --- a/planning/autoware_surround_obstacle_checker/package.xml +++ b/planning/autoware_surround_obstacle_checker/package.xml @@ -14,7 +14,6 @@ autoware_cmake eigen3_cmake_module - autoware_adapi_v1_msgs autoware_motion_utils autoware_perception_msgs autoware_planning_msgs diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp index bbd1cac04ed89..2acf6ba1c92f5 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp @@ -67,7 +67,9 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( const double & surround_check_back_distance, const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node) -: vehicle_info_(vehicle_info), +: planning_factor_interface_{std::make_unique( + &node, "surround_obstacle_checker")}, + vehicle_info_(vehicle_info), object_label_(object_label), surround_check_front_distance_(surround_check_front_distance), surround_check_side_distance_(surround_check_side_distance), @@ -77,8 +79,6 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( clock_(clock) { debug_viz_pub_ = node.create_publisher("~/debug/marker", 1); - velocity_factor_pub_ = - node.create_publisher("/planning/velocity_factors/surround_obstacle", 1); vehicle_footprint_pub_ = node.create_publisher("~/debug/footprint", 1); vehicle_footprint_offset_pub_ = node.create_publisher("~/debug/footprint_offset", 1); @@ -143,8 +143,12 @@ void SurroundObstacleCheckerDebugNode::publish() debug_viz_pub_->publish(visualization_msg); /* publish stop reason for autoware api */ - const auto velocity_factor_msg = makeVelocityFactorArray(); - velocity_factor_pub_->publish(velocity_factor_msg); + if (stop_pose_ptr_ != nullptr) { + planning_factor_interface_->add( + 0.0, *stop_pose_ptr_, tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}); + } + planning_factor_interface_->publish(); /* reset variables */ stop_pose_ptr_ = nullptr; @@ -170,25 +174,6 @@ MarkerArray SurroundObstacleCheckerDebugNode::makeVisualizationMarker() return msg; } -VelocityFactorArray SurroundObstacleCheckerDebugNode::makeVelocityFactorArray() -{ - VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); - - if (stop_pose_ptr_) { - using distance_type = VelocityFactor::_distance_type; - VelocityFactor velocity_factor; - velocity_factor.behavior = PlanningBehavior::SURROUNDING_OBSTACLE; - velocity_factor.pose = *stop_pose_ptr_; - velocity_factor.distance = std::numeric_limits::quiet_NaN(); - velocity_factor.status = VelocityFactor::UNKNOWN; - velocity_factor.detail = std::string(); - velocity_factor_array.factors.push_back(velocity_factor); - } - return velocity_factor_array; -} - PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped( const Polygon2d & boost_polygon, const double & z) { diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp index b2c350c1b4698..c49e277f2dc6c 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp @@ -15,13 +15,13 @@ #ifndef DEBUG_MARKER_HPP_ #define DEBUG_MARKER_HPP_ +#include #include #include -#include -#include #include #include +#include #include #include @@ -34,10 +34,10 @@ namespace autoware::surround_obstacle_checker { using autoware::vehicle_info_utils::VehicleInfo; -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::VelocityFactor; -using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using geometry_msgs::msg::PolygonStamped; +using tier4_planning_msgs::msg::ControlPoint; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::PlanningFactorArray; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -65,12 +65,13 @@ class SurroundObstacleCheckerDebugNode private: rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Publisher::SharedPtr velocity_factor_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_offset_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_recover_offset_pub_; + std::unique_ptr planning_factor_interface_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; std::string object_label_; double surround_check_front_distance_; @@ -80,7 +81,6 @@ class SurroundObstacleCheckerDebugNode geometry_msgs::msg::Pose self_pose_; MarkerArray makeVisualizationMarker(); - VelocityFactorArray makeVelocityFactorArray(); PolygonStamped boostPolygonToPolygonStamped(const Polygon2d & boost_polygon, const double & z); diff --git a/planning/autoware_surround_obstacle_checker/src/node.cpp b/planning/autoware_surround_obstacle_checker/src/node.cpp index 0adca312ca733..b3881a8f4fbd9 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -81,8 +81,8 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio "~/output/velocity_limit_clear_command", rclcpp::QoS{1}.transient_local()); pub_velocity_limit_ = this->create_publisher( "~/output/max_velocity", rclcpp::QoS{1}.transient_local()); - pub_processing_time_ = - this->create_publisher("~/debug/processing_time_ms", 1); + pub_processing_time_ = this->create_publisher( + "~/debug/processing_time_ms", 1); using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( @@ -231,7 +231,7 @@ void SurroundObstacleCheckerNode::onTimer() debug_ptr_->pushPose(odometry_ptr_->pose.pose, PoseType::NoStart); } - 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 = stop_watch.toc(); pub_processing_time_->publish(processing_time_msg); diff --git a/planning/autoware_surround_obstacle_checker/src/node.hpp b/planning/autoware_surround_obstacle_checker/src/node.hpp index f84ad3a8f5987..b6d6bffefed1a 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.hpp +++ b/planning/autoware_surround_obstacle_checker/src/node.hpp @@ -24,12 +24,12 @@ #include #include +#include #include #include #include #include #include -#include #include #include #include @@ -106,7 +106,8 @@ class SurroundObstacleCheckerNode : public rclcpp::Node this, "~/input/objects"}; rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; rclcpp::Publisher::SharedPtr pub_velocity_limit_; - rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr + pub_processing_time_; // parameter callback result OnSetParametersCallbackHandle::SharedPtr set_param_res_; 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 fc0066b1a84f3..d519356c7aa18 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -20,6 +20,7 @@ #include "autoware/osqp_interface/osqp_interface.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "autoware/universe_utils/ros/self_pose_listener.hpp" @@ -45,8 +46,7 @@ #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_planning_msgs/msg/stop_speed_exceeded.hpp" // temporary -#include "tier4_planning_msgs/msg/velocity_limit.hpp" // temporary +#include "tier4_planning_msgs/msg/velocity_limit.hpp" // temporary #include "visualization_msgs/msg/marker_array.hpp" #include @@ -61,6 +61,7 @@ namespace autoware::velocity_smoother using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; +using autoware::universe_utils::DiagnosticsInterface; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_internal_debug_msgs::msg::Float32Stamped; using autoware_internal_debug_msgs::msg::Float64Stamped; @@ -68,8 +69,7 @@ using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using nav_msgs::msg::Odometry; -using tier4_planning_msgs::msg::StopSpeedExceeded; // temporary -using tier4_planning_msgs::msg::VelocityLimit; // temporary +using tier4_planning_msgs::msg::VelocityLimit; // temporary using visualization_msgs::msg::MarkerArray; struct Motion @@ -89,7 +89,6 @@ class VelocitySmootherNode : public rclcpp::Node private: rclcpp::Publisher::SharedPtr pub_trajectory_; rclcpp::Publisher::SharedPtr pub_virtual_wall_; - rclcpp::Publisher::SharedPtr pub_over_stop_velocity_; rclcpp::Subscription::SharedPtr sub_current_trajectory_; autoware::universe_utils::InterProcessPollingSubscriber sub_current_odometry_{ this, "/localization/kinematic_state"}; @@ -290,6 +289,8 @@ class VelocitySmootherNode : public rclcpp::Node std::unique_ptr published_time_publisher_; mutable std::shared_ptr time_keeper_{nullptr}; + + std::unique_ptr diagnostics_interface_{nullptr}; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 1e0f6119ac21e..1f2b1cad6e42a 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -36,7 +36,8 @@ namespace autoware::velocity_smoother { VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_options) -: Node("velocity_smoother", node_options) +: Node("velocity_smoother", node_options), + diagnostics_interface_(std::make_unique(this, "velocity_smoother")) { using std::placeholders::_1; @@ -63,7 +64,6 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti pub_velocity_limit_ = create_publisher( "~/output/current_velocity_limit_mps", rclcpp::QoS{1}.transient_local()); pub_dist_to_stopline_ = create_publisher("~/distance_to_stopline", 1); - pub_over_stop_velocity_ = create_publisher("~/stop_speed_exceeded", 1); sub_current_trajectory_ = create_subscription( "~/input/trajectory", 1, std::bind(&VelocitySmootherNode::onCurrentTrajectory, this, _1)); @@ -444,6 +444,7 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr RCLCPP_DEBUG(get_logger(), "========================= run start ========================="); stop_watch_.tic(); + diagnostics_interface_->clear(); base_traj_raw_ptr_ = msg; // receive data @@ -524,6 +525,10 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr // Publish Calculation Time publishStopWatchTime(); + + // Publish diagnostics + diagnostics_interface_->publish(now()); + RCLCPP_DEBUG(get_logger(), "========================== run() end ==========================\n\n"); } @@ -906,12 +911,8 @@ void VelocitySmootherNode::overwriteStopPoint( input_stop_vel, output_stop_vel, over_stop_velocity_warn_thr_); } - { - StopSpeedExceeded msg{}; - msg.stamp = this->now(); - msg.stop_speed_exceeded = is_stop_velocity_exceeded; - pub_over_stop_velocity_->publish(msg); - } + diagnostics_interface_->add_key_value( + "The velocity on the stop point is larger than 0.", is_stop_velocity_exceeded); } void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp index 3080ba1045b41..37bee1a178f90 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -32,13 +32,15 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( const std::shared_ptr & avoidance_by_lane_change_parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface) : LaneChangeInterface{ name, node, parameters, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, + planning_factor_interface, std::make_unique(parameters, avoidance_by_lane_change_parameters)} { } diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp index c7fbba34b1adb..6e524f3c91341 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp @@ -40,7 +40,8 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface const std::shared_ptr & avoidance_by_lane_change_parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface); bool isExecutionRequested() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index c54774a575332..2bf8a60676d72 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -190,7 +190,7 @@ SMIPtr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp index f09196b2cc8e1..ea8f09c4f9336 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp @@ -42,7 +42,7 @@ class DynamicObstacleAvoidanceModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp index 44b56f4ea0990..8987e7c0446ee 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp @@ -351,7 +351,8 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); void updateModuleParams(const std::any & parameters) override { diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index dc7efebe88fba..145e0df2675b4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -331,8 +331,9 @@ DynamicObstacleAvoidanceModule::DynamicObstacleAvoidanceModule( std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{std::move(parameters)}, target_objects_manager_{TargetObjectsManager( parameters_->successive_num_to_entry_dynamic_avoidance_condition, diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp index 45c997f364633..0b438d1a1758e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp @@ -28,7 +28,7 @@ ExternalRequestLaneChangeRightModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_, std::make_unique(parameters_, direction_)); } @@ -37,7 +37,7 @@ ExternalRequestLaneChangeLeftModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_, std::make_unique(parameters_, direction_)); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp index 56e207c160873..6172fb75978cd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp @@ -288,7 +288,7 @@ bool hasEnoughDistance( std::vector selectPullOverPaths( const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates, const std::shared_ptr planner_data, - const GoalPlannerParameters & parameters, const BehaviorModuleOutput & previous_module_output) + const GoalPlannerParameters & parameters, const BehaviorModuleOutput & upstream_module_output) { using autoware::behavior_path_planner::utils::getExtendedCurrentLanesFromPath; using autoware::motion_utils::calcSignedArcLength; @@ -313,15 +313,15 @@ std::vector selectPullOverPaths( } const double prev_path_front_to_goal_dist = calcSignedArcLength( - previous_module_output.path.points, - previous_module_output.path.points.front().point.pose.position, goal_pose.position); + upstream_module_output.path.points, + upstream_module_output.path.points.front().point.pose.position, goal_pose.position); const auto & long_tail_reference_path = [&]() { if (prev_path_front_to_goal_dist > backward_length) { - return previous_module_output.path; + return upstream_module_output.path; } // get road lanes which is at least backward_length[m] behind the goal const auto road_lanes = getExtendedCurrentLanesFromPath( - previous_module_output.path, planner_data, backward_length, 0.0, false); + upstream_module_output.path, planner_data, backward_length, 0.0, false); const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; return planner_data->route_handler->getCenterLinePath( road_lanes, std::max(0.0, goal_pose_length - backward_length), @@ -564,13 +564,12 @@ int main(int argc, char ** argv) node.get(), "goal_planner."); goal_planner_parameter.bus_stop_area.use_bus_stop_area = true; goal_planner_parameter.lane_departure_check_expansion_margin = 0.2; - const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo(); - autoware::lane_departure_checker::LaneDepartureChecker lane_departure_checker{}; - lane_departure_checker.setVehicleInfo(vehicle_info); autoware::lane_departure_checker::Param lane_departure_checker_params; lane_departure_checker_params.footprint_extra_margin = goal_planner_parameter.lane_departure_check_expansion_margin; - lane_departure_checker.setParam(lane_departure_checker_params); + autoware::lane_departure_checker::LaneDepartureChecker lane_departure_checker( + lane_departure_checker_params, vehicle_info); + const auto footprint = vehicle_info.createFootprint(); autoware::behavior_path_planner::GoalSearcher goal_searcher(goal_planner_parameter, footprint); auto goal_candidates = goal_searcher.search(planner_data); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp index 102e5ef53b164..f7fefa8ba9dc0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp @@ -287,7 +287,7 @@ bool hasEnoughDistance( std::vector selectPullOverPaths( const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates, const std::shared_ptr planner_data, - const GoalPlannerParameters & parameters, const BehaviorModuleOutput & previous_module_output) + const GoalPlannerParameters & parameters, const BehaviorModuleOutput & upstream_module_output) { using autoware::behavior_path_planner::utils::getExtendedCurrentLanesFromPath; using autoware::motion_utils::calcSignedArcLength; @@ -312,15 +312,15 @@ std::vector selectPullOverPaths( } const double prev_path_front_to_goal_dist = calcSignedArcLength( - previous_module_output.path.points, - previous_module_output.path.points.front().point.pose.position, goal_pose.position); + upstream_module_output.path.points, + upstream_module_output.path.points.front().point.pose.position, goal_pose.position); const auto & long_tail_reference_path = [&]() { if (prev_path_front_to_goal_dist > backward_length) { - return previous_module_output.path; + return upstream_module_output.path; } // get road lanes which is at least backward_length[m] behind the goal const auto road_lanes = getExtendedCurrentLanesFromPath( - previous_module_output.path, planner_data, backward_length, 0.0, false); + upstream_module_output.path, planner_data, backward_length, 0.0, false); const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; return planner_data->route_handler->getCenterLinePath( road_lanes, std::max(0.0, goal_pose_length - backward_length), @@ -573,12 +573,11 @@ int main(int argc, char ** argv) autoware::behavior_path_planner::GoalPlannerModuleManager::initGoalPlannerParameters( node.get(), "goal_planner."); const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo(); - autoware::lane_departure_checker::LaneDepartureChecker lane_departure_checker{}; - lane_departure_checker.setVehicleInfo(vehicle_info); autoware::lane_departure_checker::Param lane_departure_checker_params; lane_departure_checker_params.footprint_extra_margin = goal_planner_parameter.lane_departure_check_expansion_margin; - lane_departure_checker.setParam(lane_departure_checker_params); + autoware::lane_departure_checker::LaneDepartureChecker lane_departure_checker( + lane_departure_checker_params, vehicle_info); const auto footprint = vehicle_info.createFootprint(); autoware::behavior_path_planner::GoalSearcher goal_searcher(goal_planner_parameter, footprint); auto goal_candidates = goal_searcher.search(planner_data); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp index f670e3b05fa77..201b7c2d33bf6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp @@ -38,15 +38,15 @@ class FixedGoalPlannerBase virtual BehaviorModuleOutput plan( const std::shared_ptr & planner_data) const = 0; - void setPreviousModuleOutput(const BehaviorModuleOutput & previous_module_output) + void setPreviousModuleOutput(const BehaviorModuleOutput & upstream_module_output) { - previous_module_output_ = previous_module_output; + upstream_module_output_ = upstream_module_output; } - BehaviorModuleOutput getPreviousModuleOutput() const { return previous_module_output_; } + BehaviorModuleOutput getPreviousModuleOutput() const { return upstream_module_output_; } protected: - BehaviorModuleOutput previous_module_output_; + BehaviorModuleOutput upstream_module_output_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 3a1773324fe24..84367fdd91b80 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -23,8 +23,6 @@ #include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include - #include #include @@ -40,7 +38,6 @@ namespace autoware::behavior_path_planner { -using autoware::lane_departure_checker::LaneDepartureChecker; using autoware_vehicle_msgs::msg::HazardLightsCommand; using geometry_msgs::msg::PoseArray; using nav_msgs::msg::OccupancyGrid; @@ -133,12 +130,10 @@ bool isOnModifiedGoal( const GoalPlannerParameters & parameters); bool hasPreviousModulePathShapeChanged( - const BehaviorModuleOutput & previous_module_output, - const BehaviorModuleOutput & last_previous_module_output); -bool hasDeviatedFromLastPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & last_previous_module_output); -bool hasDeviatedFromCurrentPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & previous_module_output); + const BehaviorModuleOutput & upstream_module_output, + const BehaviorModuleOutput & last_upstream_module_output); +bool hasDeviatedFromPath( + const Point & ego_position, const BehaviorModuleOutput & upstream_module_output); bool needPathUpdate( const Pose & current_pose, const double path_update_duration, const rclcpp::Time & now, @@ -189,6 +184,7 @@ class LaneParkingPlanner void onTimer(); private: + const GoalPlannerParameters parameters_; std::mutex & mutex_; const std::optional & request_; LaneParkingResponse & response_; @@ -196,6 +192,10 @@ class LaneParkingPlanner rclcpp::Logger logger_; std::vector> pull_over_planners_; + BehaviorModuleOutput + original_upstream_module_output_; // & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); ~GoalPlannerModule() { @@ -455,7 +456,7 @@ class GoalPlannerModule : public SceneModuleInterface // steering factor void updateSteeringFactor( const PullOverContextData & context_data, const std::array & pose, - const std::array distance, const uint16_t type); + const std::array distance); // rtc std::pair calcDistanceToPathChange( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp index e1e91a1bd8af6..9b97f87c933a8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp @@ -29,18 +29,16 @@ using autoware::lane_departure_checker::LaneDepartureChecker; class BezierPullOver : public PullOverPlannerBase { public: - BezierPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker); + BezierPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::BEZIER; } std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) override; + const BehaviorModuleOutput & upstream_module_output) override; std::vector plans( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output); + const BehaviorModuleOutput & upstream_module_output); private: const LaneDepartureChecker lane_departure_checker_; @@ -50,7 +48,7 @@ class BezierPullOver : public PullOverPlannerBase std::vector generateBezierPath( const GoalCandidate & goal_candidate, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, + const BehaviorModuleOutput & upstream_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const; PathWithLaneId generateReferencePath( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp index e32488965f69a..06ff47224dd44 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp @@ -30,9 +30,7 @@ using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; class FreespacePullOver : public PullOverPlannerBase { public: - FreespacePullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); + FreespacePullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } @@ -41,7 +39,7 @@ class FreespacePullOver : public PullOverPlannerBase std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) override; + const BehaviorModuleOutput & upstream_module_output) override; protected: const double velocity_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp index 89181b258fbea..e12a759eb2137 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp @@ -32,8 +32,7 @@ class GeometricPullOver : public PullOverPlannerBase { public: GeometricPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker, const bool is_forward); + rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward); PullOverPlannerType getPlannerType() const override { @@ -45,7 +44,7 @@ class GeometricPullOver : public PullOverPlannerBase std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) override; + const BehaviorModuleOutput & upstream_module_output) override; std::vector generatePullOverPaths( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp index 5b336a7de6acc..5af69894c0a2e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp @@ -130,7 +130,7 @@ class PullOverPlannerBase virtual std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) = 0; + const BehaviorModuleOutput & upstream_module_output) = 0; protected: const autoware::vehicle_info_utils::VehicleInfo vehicle_info_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp index 08e0b8508c991..994d8283fe36c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp @@ -31,14 +31,12 @@ using autoware::lane_departure_checker::LaneDepartureChecker; class ShiftPullOver : public PullOverPlannerBase { public: - ShiftPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker); + ShiftPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::SHIFT; }; std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) override; + const BehaviorModuleOutput & upstream_module_output) override; protected: PathWithLaneId generateReferencePath( @@ -49,7 +47,7 @@ class ShiftPullOver : public PullOverPlannerBase std::optional generatePullOverPath( const GoalCandidate & goal_candidate, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, + const BehaviorModuleOutput & upstream_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const; static double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double after_shifted_arc_length, const double dr); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp index bfef4226b2661..7173b275e26fb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp @@ -32,35 +32,27 @@ class LaneParkingRequest { public: LaneParkingRequest( - const GoalPlannerParameters & parameters, const autoware::universe_utils::LinearRing2d & vehicle_footprint, - const GoalCandidates & goal_candidates, const BehaviorModuleOutput & previous_module_output) - : parameters_(parameters), - vehicle_footprint_(vehicle_footprint), + const GoalCandidates & goal_candidates, const BehaviorModuleOutput & upstream_module_output) + : vehicle_footprint_(vehicle_footprint), goal_candidates_(goal_candidates), - previous_module_output_(previous_module_output), - last_previous_module_output_(previous_module_output) + upstream_module_output_(upstream_module_output) { } void update( const PlannerData & planner_data, const ModuleStatus & current_status, - const BehaviorModuleOutput & previous_module_output, + const BehaviorModuleOutput & upstream_module_output, const std::optional & pull_over_path, const PathDecisionState & prev_data); - const GoalPlannerParameters parameters_; const autoware::universe_utils::LinearRing2d vehicle_footprint_; const GoalCandidates goal_candidates_; const std::shared_ptr & get_planner_data() const { return planner_data_; } const ModuleStatus & get_current_status() const { return current_status_; } - const BehaviorModuleOutput & get_previous_module_output() const - { - return previous_module_output_; - } - const BehaviorModuleOutput & get_last_previous_module_output() const + const BehaviorModuleOutput & get_upstream_module_output() const { - return last_previous_module_output_; + return upstream_module_output_; } const std::optional & get_pull_over_path() const { return pull_over_path_; } const PathDecisionState & get_prev_data() const { return prev_data_; } @@ -68,8 +60,7 @@ class LaneParkingRequest private: std::shared_ptr planner_data_; ModuleStatus current_status_; - BehaviorModuleOutput previous_module_output_; - BehaviorModuleOutput last_previous_module_output_; + BehaviorModuleOutput upstream_module_output_; std::optional pull_over_path_; //autoware_behavior_path_planner autoware_behavior_path_planner_common autoware_bezier_sampler + autoware_freespace_planning_algorithms autoware_motion_utils autoware_rtc_interface autoware_test_utils diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 60c056280e4db..c7e32a47a59f9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -66,8 +66,9 @@ GoalPlannerModule::GoalPlannerModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{parameters}, vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, is_lane_parking_cb_running_{false}, @@ -101,7 +102,7 @@ GoalPlannerModule::GoalPlannerModule( // freespace parking if (parameters_->enable_freespace_parking) { - auto freespace_planner = std::make_shared(node, *parameters, vehicle_info); + auto freespace_planner = std::make_shared(node, *parameters); const auto freespace_parking_period_ns = rclcpp::Rate(1.0).period(); freespace_parking_timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -120,9 +121,6 @@ GoalPlannerModule::GoalPlannerModule( initializeSafetyCheckParameters(); } - steering_factor_interface_.init(PlanningBehavior::GOAL_PLANNER); - velocity_factor_interface_.init(PlanningBehavior::GOAL_PLANNER); - /** * NOTE: Add `universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);` to functions called * from the main thread only. @@ -150,17 +148,17 @@ bool isOnModifiedGoal( } bool hasPreviousModulePathShapeChanged( - const BehaviorModuleOutput & previous_module_output, - const BehaviorModuleOutput & last_previous_module_output) + const BehaviorModuleOutput & upstream_module_output, + const BehaviorModuleOutput & last_upstream_module_output) { // Calculate the lateral distance between each point of the current path and the nearest point of // the last path - constexpr double LATERAL_DEVIATION_THRESH = 0.3; - for (const auto & p : previous_module_output.path.points) { + constexpr double LATERAL_DEVIATION_THRESH = 0.1; + for (const auto & p : upstream_module_output.path.points) { const size_t nearest_seg_idx = autoware::motion_utils::findNearestSegmentIndex( - last_previous_module_output.path.points, p.point.pose.position); - const auto seg_front = last_previous_module_output.path.points.at(nearest_seg_idx); - const auto seg_back = last_previous_module_output.path.points.at(nearest_seg_idx + 1); + last_upstream_module_output.path.points, p.point.pose.position); + const auto seg_front = last_upstream_module_output.path.points.at(nearest_seg_idx); + const auto seg_back = last_upstream_module_output.path.points.at(nearest_seg_idx + 1); // Check if the target point is within the segment const Eigen::Vector3d segment_vec{ seg_back.point.pose.position.x - seg_front.point.pose.position.x, @@ -176,7 +174,7 @@ bool hasPreviousModulePathShapeChanged( continue; } const double lateral_distance = std::abs(autoware::motion_utils::calcLateralOffset( - last_previous_module_output.path.points, p.point.pose.position, nearest_seg_idx)); + last_upstream_module_output.path.points, p.point.pose.position, nearest_seg_idx)); if (lateral_distance > LATERAL_DEVIATION_THRESH) { return true; } @@ -184,21 +182,12 @@ bool hasPreviousModulePathShapeChanged( return false; } -bool hasDeviatedFromLastPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & last_previous_module_output) -{ - return std::abs(autoware::motion_utils::calcLateralOffset( - last_previous_module_output.path.points, - planner_data.self_odometry->pose.pose.position)) > 0.3; -} - -bool hasDeviatedFromCurrentPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & previous_module_output) +bool hasDeviatedFromPath( + const Point & ego_position, const BehaviorModuleOutput & upstream_module_output) { - constexpr double LATERAL_DEVIATION_THRESH = 0.3; + constexpr double LATERAL_DEVIATION_THRESH = 0.1; return std::abs(autoware::motion_utils::calcLateralOffset( - previous_module_output.path.points, planner_data.self_odometry->pose.pose.position)) > - LATERAL_DEVIATION_THRESH; + upstream_module_output.path.points, ego_position)) > LATERAL_DEVIATION_THRESH; } bool needPathUpdate( @@ -282,30 +271,22 @@ LaneParkingPlanner::LaneParkingPlanner( const std::optional & request, LaneParkingResponse & response, std::atomic & is_lane_parking_cb_running, const rclcpp::Logger & logger, const GoalPlannerParameters & parameters) -: mutex_(lane_parking_mutex), +: parameters_(parameters), + mutex_(lane_parking_mutex), request_(request), response_(response), is_lane_parking_cb_running_(is_lane_parking_cb_running), logger_(logger) { - const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); - LaneDepartureChecker lane_departure_checker{}; - lane_departure_checker.setVehicleInfo(vehicle_info); - lane_departure_checker::Param lane_departure_checker_params; - lane_departure_checker_params.footprint_extra_margin = - parameters.lane_departure_check_expansion_margin; - lane_departure_checker.setParam(lane_departure_checker_params); - for (const std::string & planner_type : parameters.efficient_path_order) { if (planner_type == "SHIFT" && parameters.enable_shift_parking) { - pull_over_planners_.push_back( - std::make_shared(node, parameters, lane_departure_checker)); + pull_over_planners_.push_back(std::make_shared(node, parameters)); } else if (planner_type == "ARC_FORWARD" && parameters.enable_arc_forward_parking) { - pull_over_planners_.push_back(std::make_shared( - node, parameters, lane_departure_checker, /*is_forward*/ true)); + pull_over_planners_.push_back( + std::make_shared(node, parameters, /*is_forward*/ true)); } else if (planner_type == "ARC_BACKWARD" && parameters.enable_arc_backward_parking) { - pull_over_planners_.push_back(std::make_shared( - node, parameters, lane_departure_checker, /*is_forward*/ false)); + pull_over_planners_.push_back( + std::make_shared(node, parameters, /*is_forward*/ false)); } } @@ -335,12 +316,10 @@ void LaneParkingPlanner::onTimer() return; } const auto & local_request = local_request_opt.value(); - const auto & parameters = local_request.parameters_; const auto & goal_candidates = local_request.goal_candidates_; const auto & local_planner_data = local_request.get_planner_data(); const auto & current_status = local_request.get_current_status(); - const auto & previous_module_output = local_request.get_previous_module_output(); - const auto & last_previous_module_output = local_request.get_last_previous_module_output(); + const auto & upstream_module_output = local_request.get_upstream_module_output(); const auto & pull_over_path_opt = local_request.get_pull_over_path(); const auto & prev_data = local_request.get_prev_data(); @@ -371,19 +350,22 @@ void LaneParkingPlanner::onTimer() ? std::make_optional(pull_over_path_opt.value().modified_goal()) : std::nullopt; if (isOnModifiedGoal( - local_planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) { + local_planner_data->self_odometry->pose.pose, modified_goal_opt, parameters_)) { return false; } - if (hasDeviatedFromCurrentPreviousModulePath(*local_planner_data, previous_module_output)) { + if (hasDeviatedFromPath( + local_planner_data->self_odometry->pose.pose.position, upstream_module_output)) { RCLCPP_DEBUG(getLogger(), "has deviated from current previous module path"); return false; } - if (hasPreviousModulePathShapeChanged(previous_module_output, last_previous_module_output)) { + if (hasPreviousModulePathShapeChanged( + upstream_module_output, original_upstream_module_output_)) { RCLCPP_DEBUG(getLogger(), "has previous module path shape changed"); return true; } if ( - hasDeviatedFromLastPreviousModulePath(*local_planner_data, last_previous_module_output) && + hasDeviatedFromPath( + local_planner_data->self_odometry->pose.pose.position, original_upstream_module_output_) && current_state != PathDecisionState::DecisionKind::DECIDED) { RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path"); return true; @@ -400,8 +382,8 @@ void LaneParkingPlanner::onTimer() // generate valid pull over path candidates and calculate closest start pose const auto current_lanes = utils::getExtendedCurrentLanes( - local_planner_data, parameters.backward_goal_search_length, - parameters.forward_goal_search_length, + local_planner_data, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); std::vector path_candidates{}; std::optional closest_start_pose{}; @@ -410,7 +392,7 @@ void LaneParkingPlanner::onTimer() const std::shared_ptr & planner, const GoalCandidate & goal_candidate) { const auto pull_over_path = planner->plan( - goal_candidate, path_candidates.size(), local_planner_data, previous_module_output); + goal_candidate, path_candidates.size(), local_planner_data, upstream_module_output); if (pull_over_path) { // calculate absolute maximum curvature of parking path(start pose to end pose) for path // priority @@ -428,13 +410,13 @@ void LaneParkingPlanner::onTimer() // todo: currently non centerline input path is supported only by shift pull over const bool is_center_line_input_path = goal_planner_utils::isReferencePath( - previous_module_output.reference_path, previous_module_output.path, 0.1); + upstream_module_output.reference_path, upstream_module_output.path, 0.1); RCLCPP_DEBUG( getLogger(), "the input path of pull over planner is center line: %d", is_center_line_input_path); // plan candidate paths and set them to the member variable - if (parameters.path_priority == "efficient_path") { + if (parameters_.path_priority == "efficient_path") { for (const auto & planner : pull_over_planners_) { // todo: temporary skip NON SHIFT planner when input path is not center line if (!is_center_line_input_path && planner->getPlannerType() != PullOverPlannerType::SHIFT) { @@ -444,7 +426,7 @@ void LaneParkingPlanner::onTimer() planCandidatePaths(planner, goal_candidate); } } - } else if (parameters.path_priority == "close_goal") { + } else if (parameters_.path_priority == "close_goal") { for (const auto & goal_candidate : goal_candidates) { for (const auto & planner : pull_over_planners_) { // todo: temporary skip NON SHIFT planner when input path is not center line @@ -457,14 +439,15 @@ void LaneParkingPlanner::onTimer() } else { RCLCPP_ERROR( getLogger(), "path_priority should be efficient_path or close_goal, but %s is given.", - parameters.path_priority.c_str()); + parameters_.path_priority.c_str()); throw std::domain_error("[pull_over] invalid path_priority"); } // set response { + original_upstream_module_output_ = upstream_module_output; std::lock_guard guard(mutex_); - response_.pull_over_path_candidates = path_candidates; + response_.pull_over_path_candidates = std::move(path_candidates); response_.closest_start_pose = closest_start_pose; RCLCPP_INFO( getLogger(), "generated %lu pull over path candidates", @@ -562,7 +545,7 @@ std::pair GoalPlannerModule::sync // In PlannerManager::run(), it calls SceneModuleInterface::setData and // SceneModuleInterface::setPreviousModuleOutput before module_ptr->run(). // Then module_ptr->run() invokes GoalPlannerModule::updateData and then - // planWaitingApproval()/plan(), so we can copy latest current_status/previous_module_output to + // planWaitingApproval()/plan(), so we can copy latest current_status/upstream_module_output to // lane_parking_request/freespace_parking_request std::optional pull_over_path = @@ -578,7 +561,7 @@ std::pair GoalPlannerModule::sync std::lock_guard guard(lane_parking_mutex_); if (!lane_parking_request_) { lane_parking_request_.emplace( - *parameters_, vehicle_footprint_, goal_candidates_, getPreviousModuleOutput()); + vehicle_footprint_, goal_candidates_, getPreviousModuleOutput()); } // NOTE: for the above reasons, PlannerManager/behavior_path_planner_node ensure that // planner_data_ is not nullptr, so it is OK to copy as value @@ -1359,19 +1342,20 @@ void GoalPlannerModule::setTurnSignalInfo( void GoalPlannerModule::updateSteeringFactor( const PullOverContextData & context_data, const std::array & pose, - const std::array distance, const uint16_t type) + const std::array distance) { - const uint16_t steering_factor_direction = std::invoke([&]() { + const uint16_t planning_factor_direction = std::invoke([&]() { const auto turn_signal = calcTurnSignalInfo(context_data); if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - return SteeringFactor::LEFT; + return PlanningFactor::SHIFT_LEFT; } else if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { - return SteeringFactor::RIGHT; + return PlanningFactor::SHIFT_RIGHT; } - return SteeringFactor::STRAIGHT; + return PlanningFactor::NONE; }); - steering_factor_interface_.set(pose, distance, steering_factor_direction, type, ""); + planning_factor_interface_->add( + distance[0], distance[1], pose[0], pose[1], planning_factor_direction, SafetyFactorArray{}); } void GoalPlannerModule::decideVelocity(PullOverPath & pull_over_path) @@ -1583,10 +1567,9 @@ void GoalPlannerModule::postProcess() updateSteeringFactor( context_data, {pull_over_path.start_pose(), pull_over_path.modified_goal_pose()}, - {distance_to_path_change.first, distance_to_path_change.second}, - has_decided_path ? SteeringFactor::TURNING : SteeringFactor::APPROACHING); + {distance_to_path_change.first, distance_to_path_change.second}); - setVelocityFactor(pull_over_path.full_path()); + set_longitudinal_planning_factor(pull_over_path.full_path()); } BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() @@ -1671,7 +1654,10 @@ PathWithLaneId GoalPlannerModule::generateStopPath( const auto reference_path = std::invoke([&]() -> PathWithLaneId { const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); - const double s_end = s_current + common_parameters.forward_path_length; + const double s_end = std::clamp( + lanelet::utils::getArcCoordinates(current_lanes, route_handler->getGoalPose()).length, + s_current + std::numeric_limits::epsilon(), + s_current + common_parameters.forward_path_length); return route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); }); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 6bbc34a495080..a99252c923328 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -31,7 +31,7 @@ std::unique_ptr GoalPlannerModuleManager::createNewSceneMo { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp index 0da5ab5dae38b..ec3984a6d9443 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp @@ -31,19 +31,17 @@ #include namespace autoware::behavior_path_planner { -BezierPullOver::BezierPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker) -: PullOverPlannerBase{node, parameters}, - lane_departure_checker_{lane_departure_checker}, - left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} +BezierPullOver::BezierPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters) +: PullOverPlannerBase(node, parameters), + lane_departure_checker_( + lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_), + left_side_parking_(parameters.parking_policy == ParkingPolicy::LEFT_SIDE) { } - std::optional BezierPullOver::plan( [[maybe_unused]] const GoalCandidate & modified_goal_pose, [[maybe_unused]] const size_t id, [[maybe_unused]] const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) + [[maybe_unused]] const BehaviorModuleOutput & upstream_module_output) { const auto & route_handler = planner_data->route_handler; const double min_jerk = parameters_.minimum_lateral_jerk; @@ -55,7 +53,7 @@ std::optional BezierPullOver::plan( std::abs(max_jerk - min_jerk) / shift_sampling_num; const auto road_lanes = utils::getExtendedCurrentLanesFromPath( - previous_module_output.path, planner_data, backward_search_length, forward_search_length, + upstream_module_output.path, planner_data, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( @@ -67,7 +65,7 @@ std::optional BezierPullOver::plan( // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { const auto pull_over_path = generateBezierPath( - modified_goal_pose, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, + modified_goal_pose, id, planner_data, upstream_module_output, road_lanes, pull_over_lanes, lateral_jerk); if (!pull_over_path.empty()) { return pull_over_path.at(0); @@ -80,7 +78,7 @@ std::optional BezierPullOver::plan( std::vector BezierPullOver::plans( [[maybe_unused]] const GoalCandidate & modified_goal_pose, [[maybe_unused]] const size_t id, [[maybe_unused]] const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) + [[maybe_unused]] const BehaviorModuleOutput & upstream_module_output) { const auto & route_handler = planner_data->route_handler; const double min_jerk = parameters_.minimum_lateral_jerk; @@ -92,7 +90,7 @@ std::vector BezierPullOver::plans( std::abs(max_jerk - min_jerk) / shift_sampling_num; const auto road_lanes = utils::getExtendedCurrentLanesFromPath( - previous_module_output.path, planner_data, backward_search_length, forward_search_length, + upstream_module_output.path, planner_data, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( @@ -105,7 +103,7 @@ std::vector BezierPullOver::plans( std::vector paths; for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { auto pull_over_paths = generateBezierPath( - modified_goal_pose, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, + modified_goal_pose, id, planner_data, upstream_module_output, road_lanes, pull_over_lanes, lateral_jerk); std::copy( std::make_move_iterator(pull_over_paths.begin()), @@ -118,7 +116,7 @@ std::vector BezierPullOver::plans( std::vector BezierPullOver::generateBezierPath( const GoalCandidate & goal_candidate, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, + const BehaviorModuleOutput & upstream_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const { const double pull_over_velocity = parameters_.pull_over_velocity; @@ -133,7 +131,7 @@ std::vector BezierPullOver::generateBezierPath( generateReferencePath(planner_data, road_lanes, shift_end_pose), parameters_.center_line_path_interval); const auto prev_module_path = utils::resamplePathWithSpline( - previous_module_output.path, parameters_.center_line_path_interval); + upstream_module_output.path, parameters_.center_line_path_interval); const auto prev_module_path_terminal_pose = prev_module_path.points.back().point.pose; // process previous module path for path shifter input path @@ -318,7 +316,7 @@ std::vector BezierPullOver::generateBezierPath( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); const auto combined_drivable = utils::combineDrivableLanes( - expanded_lanes, previous_module_output.drivable_area_info.drivable_lanes); + expanded_lanes, upstream_module_output.drivable_area_info.drivable_lanes); return !lane_departure_checker_.checkPathWillLeaveLane( utils::transformToLanelets(combined_drivable), pull_over_path.parking_path()); }); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp index 5b11d1b170ce1..6605d258a8d5e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp @@ -32,9 +32,7 @@ namespace autoware::behavior_path_planner using autoware::freespace_planning_algorithms::AstarSearch; using autoware::freespace_planning_algorithms::RRTStar; -FreespacePullOver::FreespacePullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) +FreespacePullOver::FreespacePullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters) : PullOverPlannerBase{node, parameters}, velocity_{parameters.freespace_parking_velocity}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE}, @@ -45,7 +43,7 @@ FreespacePullOver::FreespacePullOver( } { autoware::freespace_planning_algorithms::VehicleShape vehicle_shape( - vehicle_info, parameters.vehicle_shape_margin); + vehicle_info_, parameters.vehicle_shape_margin); if (parameters.freespace_parking_algorithm == "astar") { planner_ = std::make_unique( parameters.freespace_parking_common_parameters, vehicle_shape, parameters.astar_parameters, @@ -60,7 +58,7 @@ FreespacePullOver::FreespacePullOver( std::optional FreespacePullOver::plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) + [[maybe_unused]] const BehaviorModuleOutput & upstream_module_output) { const Pose & current_pose = planner_data->self_odometry->pose.pose; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp index c0bac5c5ce2a8..b7b4ad83de362 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -26,11 +26,15 @@ namespace autoware::behavior_path_planner { GeometricPullOver::GeometricPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker, const bool is_forward) + rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward) : PullOverPlannerBase{node, parameters}, parallel_parking_parameters_{parameters.parallel_parking_parameters}, - lane_departure_checker_{lane_departure_checker}, + lane_departure_checker_{[&]() { + auto lane_departure_checker_params = lane_departure_checker::Param{}; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; + return LaneDepartureChecker{lane_departure_checker_params, vehicle_info_}; + }()}, is_forward_{is_forward}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { @@ -40,7 +44,7 @@ GeometricPullOver::GeometricPullOver( std::optional GeometricPullOver::plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) + [[maybe_unused]] const BehaviorModuleOutput & upstream_module_output) { const auto & route_handler = planner_data->route_handler; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index edecfd733d3be..c8e0788e39b6f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -30,18 +30,21 @@ namespace autoware::behavior_path_planner { -ShiftPullOver::ShiftPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker) +ShiftPullOver::ShiftPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters) : PullOverPlannerBase{node, parameters}, - lane_departure_checker_{lane_departure_checker}, + lane_departure_checker_{[&]() { + auto lane_departure_checker_params = lane_departure_checker::Param{}; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; + return LaneDepartureChecker{lane_departure_checker_params, vehicle_info_}; + }()}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { } std::optional ShiftPullOver::plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) + const BehaviorModuleOutput & upstream_module_output) { const auto & route_handler = planner_data->route_handler; const double min_jerk = parameters_.minimum_lateral_jerk; @@ -52,7 +55,7 @@ std::optional ShiftPullOver::plan( const double jerk_resolution = std::abs(max_jerk - min_jerk) / shift_sampling_num; const auto road_lanes = utils::getExtendedCurrentLanesFromPath( - previous_module_output.path, planner_data, backward_search_length, forward_search_length, + upstream_module_output.path, planner_data, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( @@ -64,7 +67,7 @@ std::optional ShiftPullOver::plan( // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { const auto pull_over_path = generatePullOverPath( - modified_goal_pose, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, + modified_goal_pose, id, planner_data, upstream_module_output, road_lanes, pull_over_lanes, lateral_jerk); if (!pull_over_path) continue; return pull_over_path; @@ -134,7 +137,7 @@ std::optional ShiftPullOver::cropPrevModulePath( std::optional ShiftPullOver::generatePullOverPath( const GoalCandidate & goal_candidate, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, + const BehaviorModuleOutput & upstream_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const { const double pull_over_velocity = parameters_.pull_over_velocity; @@ -151,7 +154,7 @@ std::optional ShiftPullOver::generatePullOverPath( generateReferencePath(planner_data, road_lanes, shift_end_pose), parameters_.center_line_path_interval); const auto prev_module_path = utils::resamplePathWithSpline( - previous_module_output.path, parameters_.center_line_path_interval); + upstream_module_output.path, parameters_.center_line_path_interval); const auto prev_module_path_terminal_pose = prev_module_path.points.back().point.pose; // process previous module path for path shifter input path diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp index f12d510e23030..89458c199c130 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp @@ -21,14 +21,13 @@ namespace autoware::behavior_path_planner void LaneParkingRequest::update( const PlannerData & planner_data, const ModuleStatus & current_status, - const BehaviorModuleOutput & previous_module_output, + const BehaviorModuleOutput & upstream_module_output, const std::optional & pull_over_path, const PathDecisionState & prev_data) { planner_data_ = std::make_shared(planner_data); planner_data_->route_handler = std::make_shared(*(planner_data.route_handler)); current_status_ = current_status; - last_previous_module_output_ = previous_module_output_; - previous_module_output_ = previous_module_output; + upstream_module_output_ = upstream_module_output; pull_over_path_ = pull_over_path; prev_data_ = prev_data; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 86d8d1c0c1413..601fb893f82c6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -722,47 +722,174 @@ If the ego vehicle gets stuck, to avoid stuck, it enables lane change in crosswa If the ego vehicle stops more than `stuck_detection.stop_time` seconds, it is regarded as a stuck. If the ego vehicle velocity is smaller than `stuck_detection.velocity`, it is regarded as stopping. -### Aborting lane change +## Lane change completion checks + +To determine if the ego vehicle has successfully changed lanes, one of two criteria must be met: either the longitudinal or the lateral criteria. -The abort process may result in three different outcome; Cancel, Abort and Stop/Cruise. +For the longitudinal criteria, the ego vehicle must pass the lane-changing end pose and be within the `finish_judge_buffer` distance from it. The module then checks if the ego vehicle is in the target lane. If true, the module returns success. This check ensures that the planner manager updates the root lanelet correctly based on the ego vehicle's current pose. Without this check, if the ego vehicle is changing lanes while avoiding an obstacle and its current pose is in the original lane, the planner manager might set the root lanelet as the original lane. This would force the ego vehicle to perform the lane change again. With the target lane check, the ego vehicle is confirmed to be in the target lane, and the planner manager can correctly update the root lanelets. -The following depicts the flow of the abort lane change check. +If the longitudinal criteria are not met, the module evaluates the lateral criteria. For the lateral criteria, the ego vehicle must be within `finish_judge_lateral_threshold` distance from the target lane's centerline, and the angle deviation must be within `finish_judge_lateral_angle_deviation` degrees. The angle deviation check ensures there is no sudden steering. If the angle deviation is set too high, the ego vehicle's orientation could deviate significantly from the centerline, causing the trajectory follower to aggressively correct the steering to return to the centerline. Keeping the angle deviation value as small as possible avoids this issue. + +The process of determining lane change completion is shown in the following diagram. ```plantuml @startuml -skinparam monochrome true skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +title Lane change completion judge + +start + +:Calculate distance from current ego pose to lane change end pose; + +if (Is ego velocity < 1.0?) then (YES) + :Set finish_judge_buffer to 0.0; +else (NO) + :Set finish_judge_buffer to lane_change_finish_judge_buffer; +endif + +if (ego has passed the end_pose and ego is finish_judge_buffer meters away from end_pose?) then (YES) + if (Current ego pose is in target lanes' polygon?) then (YES) + :Lane change is completed; + stop + else (NO) +:Lane change is NOT completed; +stop + endif +else (NO) +endif + +if (ego's yaw deviation to centerline exceeds finish_judge_lateral_angle_deviation?) then (YES) + :Lane change is NOT completed; + stop +else (NO) + :Calculate distance to the target lanes' centerline; + if (abs(distance to the target lanes' centerline) is less than finish_judge_lateral_threshold?) then (YES) + :Lane change is completed; + stop + else (NO) + :Lane change is NOT completed; + stop + endif +endif + +@enduml +``` + +## Terminal Lane Change Path + +Depending on the space configuration around the Ego vehicle, it is possible that a valid LC path cannot be generated. If that happens, then Ego will get stuck at `terminal_start` and not be able to proceed. Therefore we introduced the terminal LC path feature; when ego gets near to the terminal point (dist to `terminal_start` is less than the maximum lane change length) a terminal lane changing path will be computed starting from the terminal start point on the current lane and connects to the target lane. The terminal path only needs to be computed once in each execution of LC module. If no valid candidate paths are found in the path generation process, then the terminal path will be used as a fallback candidate path, the safety of the terminal path is not ensured and therefore it can only be force approved. The following images illustrate the expected behavior without and with the terminal path feature respectively: + +![no terminal path](./images/lane_change-no_terminal_path.png) + +![terminal path](./images/lane_change-terminal_path.png) + +Additionally if terminal path feature is enabled and path is computed, stop point placement can be configured to be at the edge of the current lane instead of at the `terminal_start` position, as indicated by the dashed red line in the image above. + +## Generating Path Using Frenet Planner + +!!! warning + + Generating path using Frenet planner applies only when ego is near terminal start + +If the ego vehicle is far from the terminal, the lane change module defaults to using the [path shifter](https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design/). This ensures that the lane change is completed while the target lane remains a neighbor of the current lane. However, this approach may result in high curvature paths near the terminal, potentially causing long vehicles to deviate from the lane. + +To address this, the lane change module provides an option to choose between the path shifter and the [Frenet planner](https://autowarefoundation.github.io/autoware.universe/main/planning/sampling_based_planner/autoware_frenet_planner/). The Frenet planner allows for some flexibility in the lane change endpoint, extending the lane changing end point slightly beyond the current lane's neighbors. + +The following table provides comparisons between the planners + +
+ + + + + + + + + + + + + + + + + +
With Path ShifterWith Frenet Planner
Path shifter result at straight laneletsFrenet planner result at straight lanelets
Path shifter result at branching laneletsFrenet planner result at branching lanelets
Path shifter result at curved laneletsFrenet planner result at curved lanelets
+
+ +!!! note + + The planner can be enabled or disabled using the `frenet.enable` flag. + +!!! note + + Since only a segment of the target lane is used as input to generate the lane change path, the end pose of the lane change segment may not smoothly connect to the target lane centerline. To address this, increase the value of `frenet.th_curvature_smoothing` to improve the smoothness. + +!!! note + + The yaw difference threshold (`frenet.th_yaw_diff`) limits the maximum curvature difference between the end of the prepare segment and the lane change segment. This threshold might prevent the generation of a lane change path when the lane curvature is high. In such cases, you can increase the frenet.th_yaw_diff value. However, note that if the prepare path was initially shifted by other modules, the resultant steering may not be continuous. + +## Aborting a Previously Approved Lane Change + +Once the lane change path is approved, there are several situations where we may need to abort the maneuver. The abort process is triggered when any of the following conditions is met + +1. The ego vehicle is near a traffic light, crosswalk, or intersection, and it is possible to complete the lane change after the ego vehicle passes these areas. +2. The target object list is updated, requiring us to [delay lane change](#delay-lane-change-check) +3. The lane change is forcefully canceled via [RTC](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/features/cooperation/). +4. The path has become unsafe. + +Furthermore, if the path has become unsafe, there are three possible outcomes for the maneuver: + +1. **CANCEL**: The approved lane change path is canceled, and the ego vehicle resumes its previous maneuver. +2. **ABORT**: The lane change module generates a return path to bring the ego vehicle back to its current lane. +3. **CRUISE** or **STOP**: If aborting is not feasible, the ego vehicle continues with the lane change. [Another module](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_obstacle_cruise_planner/) should decide whether the ego vehicle should cruise or stop in this scenario. + +**CANCEL** can be enabled by setting the `cancel.enable_on_prepare_phase` flag to `true`, and **ABORT** can be enabled by setting the `cancel.enable_on_lane_changing_phase` flag to true. + +!!! warning + + Enabling **CANCEL** is a prerequisite for enabling **ABORT**. + +!!! warning -title Abort Lane Change + When **CANCEL** is disabled, all maneuvers will default to either **CRUISE** or **STOP**. + +The chart shows the high level flow of the lane change abort process. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +title High-Level Flow of Lane Change Abort Process while(Lane Following) if (Lane Change required) then (**YES**) - if (Safe to change lane) then (**SAFE**) - while(Lane Changing) - if (Lane Change Completed) then (**YES**) - break - else (**NO**) - if (Is Abort Condition Satisfied) then (**NO**) + if (Safe to change lane) then (SAFE) + :Approve safe path; + while(Lane change maneuver is completed?) is (**NO**) + if (Is cancel/abort Condition satisfied) then (**NO**) else (**YES**) if (Is Enough margin to retry lane change) then (**YES**) - if (Ego is on lane change prepare phase) then (**YES**) - :Cancel lane change; + if (Ego is preparing to change lane) then (**YES**) + #LightPink:CANCEL; break else (**NO**) - if (Will the overhang to target lane be less than threshold?) then (**YES**) - :Perform abort maneuver; + if (Overhang from current lanes is less than threshold?) then (**YES**) + #Cyan:ABORT; break else (NO) - :Stop or Cruise depending on the situation; endif endif else (**NO**) endif + #Yellow:CRUISE/STOP; endif - endif - :Stop and wait; - endwhile - else (**UNSAFE**) + endwhile (**YES**) + else (UNSAFE) endif else (**NO**) endif @@ -772,107 +899,137 @@ detach @enduml ``` -During a lane change, a safety check is made in consideration of the deceleration of the ego vehicle, and a safety check is made for `cancel.deceleration_sampling_num` deceleration patterns, and the lane change will be canceled if the abort condition is satisfied for all deceleration patterns. +### Preventing Oscillating Paths When Unsafe -To preventive measure for lane change path oscillations caused by alternating safe and unsafe conditions, an additional hysteresis count check is implemented before executing an abort or cancel maneuver. If unsafe, the `unsafe_hysteresis_count_` is incremented and compared against `unsafe_hysteresis_threshold`; exceeding it prompts an abort condition check, ensuring decisions are made with consideration to recent safety assessments as shown in flow chart above. This mechanism stabilizes decision-making, preventing abrupt changes due to transient unsafe conditions. +Lane change paths can oscillate when conditions switch between safe and unsafe. To address this, a hysteresis count check is added before executing an abort maneuver. When the path is unsafe, the `unsafe_hysteresis_count_` increases. If it exceeds the `unsafe_hysteresis_threshold`, an abort condition check is triggered. This logic stabilizes the path approval process and prevents abrupt changes caused by temporary unsafe conditions. ```plantuml @startuml skinparam defaultTextAlignment center skinparam backgroundColor #WHITE -title Abort Lane Change +title Hysteresis count flow for oscillation prevention +while (lane changing completed?) is (FALSE) if (Perform collision check?) then (SAFE) :Reset unsafe_hysteresis_count_; else (UNSAFE) :Increase unsafe_hysteresis_count_; - if (unsafe_hysteresis_count_ > unsafe_hysteresis_threshold?) then (FALSE) + if (unsafe_hysteresis_count_ is more than\n unsafe_hysteresis_threshold?) then (FALSE) else (TRUE) #LightPink:Check abort condition; - stop + -[hidden]-> + detach endif endif -:Continue lane changing; +endwhile (TRUE) +stop @enduml ``` -#### Cancel +### Evaluating Ego Vehicle's Position to Prevent Abrupt Maneuvers + +To avoid abrupt maneuvers during **CANCEL** or **ABORT**, the lane change module ensures the ego vehicle can safely return to the original lane. This is done through geometric checks that verify whether the ego vehicle remains within the lane boundaries. + +The edges of the ego vehicle’s footprint are compared against the boundary of the current lane to determine if they exceed the overhang tolerance, `cancel.overhang_tolerance`. If the distance from any edge of the footprint to the boundary exceeds this threshold, the vehicle is considered to be diverging. + +The footprints checked against the lane boundary include: + +1. Current Footprint: Based on the ego vehicle's current position. +2. Future Footprint: Based on the ego vehicle's estimated position after traveling a distance, calculated as $𝑑_{est}=𝑣_{ego} \cdot \Delta_{𝑡}$, where + + - $v_{ego}$ is ego vehicle's current velocity + - $\Delta_{t}$ is parameterized time constant value, `cancel.delta_time`. + + as depicted in the following diagram + + ![can_return](./images/check_able_to_return.png) + +!!! note + + The ego vehicle is considered capable of safely returning to the current lane only if **BOTH** the current and future footprint checks are `true`. + +### Checking Approved Path Safety + +The lane change module samples accelerations along the path and recalculates velocity to perform safety checks. The motivation for this feature is explained in the [Limitation](#limitation) section. + +The computation of sampled accelerations is as follows: -Suppose the lane change trajectory is evaluated as unsafe. In that case, if the ego vehicle has not departed from the current lane yet, the trajectory will be reset, and the ego vehicle will resume the lane following the maneuver. +Let -The function can be enabled by setting `enable_on_prepare_phase` to `true`. +$$ +\text{resolution} = \frac{a_{\text{min}} - a_{\text{LC}}}{N} +$$ -The following image illustrates the cancel process. +The sequence of sampled accelerations is then given as + +$$ +\text{acc} = a_{\text{LC}} + k \cdot \text{resolution}, \quad k = [0, N] +$$ + +where + +- $a_{\text{min}}$, is the minimum of the parameterized [global acceleration constant](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/common/common.param.yaml) `normal.min_acc` or the [parameterized constant](#essential-lane-change-parameters) `trajectory.min_longitudinal_acceleration`. +- $a_{\text{LC}}$ is the acceleration used to generate the approved path. +- $N$ is the parameterized constant `cancel.deceleration_sampling` + +If none of the sampled accelerations pass the safety check, the lane change path will be canceled, subject to the [hysteresis check](#preventing-oscillating-paths-when-unsafe). + +### Cancel + +When lane change is canceled, the approved path is reset. After the reset, the ego vehicle will return to following the original reference path (the last approved path before the lane change started), as illustrated in the following image ![cancel](./images/lane_change-cancel.png) -#### Abort +The following parameters can be configured to tune the behavior of the cancel process: -Assume the ego vehicle has already departed from the current lane. In that case, it is dangerous to cancel the path, and it will cause the ego vehicle to change the heading direction abruptly. In this case, planning a trajectory that allows the ego vehicle to return to the current path while minimizing the heading changes is necessary. In this case, the lane change module will generate an abort path. The following images show an example of the abort path. Do note that the function DOESN'T GUARANTEE a safe abort process, as it didn't check the presence of the surrounding objects and/or their reactions. The function can be enable manually by setting both `enable_on_prepare_phase` and `enable_on_lane_changing_phase` to `true`. The parameter `max_lateral_jerk` need to be set to a high value in order for it to work. +1. [Safety constraints](#safety-constraints-to-cancel-lane-change-path) for cancel. +2. [Safety constraints](#safety-constraints-specifically-for-stopped-or-parked-vehicles) for parked vehicle. -![abort](./images/lane_change-abort.png) +!!! note -#### Stop/Cruise + To ensure feasible behavior, all safety constraint values must be equal to or less than their corresponding parameters in the [execution](#safety-constraints-during-lane-change-path-is-computed) settings. -The last behavior will also occur if the ego vehicle has departed from the current lane. If the abort function is disabled or the abort is no longer possible, the ego vehicle will attempt to stop or transition to the obstacle cruise mode. Do note that the module DOESN'T GUARANTEE safe maneuver due to the unexpected behavior that might've occurred during these critical scenarios. The following images illustrate the situation. + - The closer the values, the more conservative the lane change behavior will be. This means it will be easier to cancel the lane change but harder for the ego vehicle to complete a lane change. + - The larger the difference, the more aggressive the lane change behavior will be. This makes it harder to cancel the lane change but easier for the ego vehicle to change lanes. -![stop](./images/lane_change-cant_cancel_no_abort.png) +### Abort -## Lane change completion checks +During the prepare phase, the ego vehicle follows the previously approved path. However, once the ego vehicle begins the lane change, its heading starts to diverge from this path. Resetting to the previously approved path in this situation would cause abrupt steering, as the controller would attempt to rapidly realign the vehicle with the reference trajectory. -To determine if the ego vehicle has successfully changed lanes, one of two criteria must be met: either the longitudinal or the lateral criteria. +Instead, the lane change module generates an abort path. This return path is specifically designed to guide the ego vehicle back to the current lane, avoiding any sudden maneuvers. The following image provides an illustration of the abort process. -For the longitudinal criteria, the ego vehicle must pass the lane-changing end pose and be within the `finish_judge_buffer` distance from it. The module then checks if the ego vehicle is in the target lane. If true, the module returns success. This check ensures that the planner manager updates the root lanelet correctly based on the ego vehicle's current pose. Without this check, if the ego vehicle is changing lanes while avoiding an obstacle and its current pose is in the original lane, the planner manager might set the root lanelet as the original lane. This would force the ego vehicle to perform the lane change again. With the target lane check, the ego vehicle is confirmed to be in the target lane, and the planner manager can correctly update the root lanelets. +![abort](./images/lane_change-abort.png) -If the longitudinal criteria are not met, the module evaluates the lateral criteria. For the lateral criteria, the ego vehicle must be within `finish_judge_lateral_threshold` distance from the target lane's centerline, and the angle deviation must be within `finish_judge_lateral_angle_deviation` degrees. The angle deviation check ensures there is no sudden steering. If the angle deviation is set too high, the ego vehicle's orientation could deviate significantly from the centerline, causing the trajectory follower to aggressively correct the steering to return to the centerline. Keeping the angle deviation value as small as possible avoids this issue. +The abort path is generated by shifting the approved lane change path using the path shifter. This ensures the continuity in lateral velocity, and prevents abrupt changes in the vehicle’s movement. The abort start shift and abort end shift are computed as follows: -The process of determining lane change completion is shown in the following diagram. +1. Start Shift: $d_{start}^{abort} = v_{ego} \cdot \Delta_{t}$ +2. End Shift: $d_{end}^{abort} = v_{ego} \cdot ( \Delta_{t} + t_{end} )$ -```plantuml -@startuml -skinparam defaultTextAlignment center -skinparam backgroundColor #WHITE +- $v_{ego}$ is ego vehicle's current velocity +- $\Delta_{t}$ is parameterized time constant value, `cancel.delta_time`. +- $t_{end}$ is the parameterized time constant value, `cancel.duration`. -title Lane change completion judge +as depicted in the following diagram -start +![abort_computation](./images/lane_change-abort_computation.png) -:Calculate distance from current ego pose to lane change end pose; +!!! note -if (Is ego velocity < 1.0?) then (YES) - :Set finish_judge_buffer to 0.0; -else (NO) - :Set finish_judge_buffer to lane_change_finish_judge_buffer; -endif + When executing the abort process, comfort is not a primary concern. However, due to safety considerations, limited real-world testing has been conducted to tune or validate this parameter. Currently, the maximum lateral jerk is set to an arbitrary value. To avoid generating a path with excessive lateral jerk, this value can be configured using `cancel.max_lateral_jerk`. -if (ego has passed the end_pose and ego is finish_judge_buffer meters away from end_pose?) then (YES) - if (Current ego pose is in target lanes' polygon?) then (YES) - :Lane change is completed; - stop - else (NO) -:Lane change is NOT completed; -stop - endif -else (NO) -endif +!!! note -if (ego's yaw deviation to centerline exceeds finish_judge_lateral_angle_deviation?) then (YES) - :Lane change is NOT completed; - stop -else (NO) - :Calculate distance to the target lanes' centerline; - if (abs(distance to the target lanes' centerline) is less than finish_judge_lateral_threshold?) then (YES) - :Lane change is completed; - stop - else (NO) - :Lane change is NOT completed; - stop - endif -endif + Lane change module returns `ModuleStatus::FAILURE` once abort is completed. -@enduml -``` +### Stop/Cruise + +Once canceling or aborting the lane change is no longer an option, the ego vehicle will proceed with the lane change. This can happen in the following situations: + +- The ego vehicle is performing a lane change near a terminal or dead-end, making it impossible to return to the original lane. In such cases, completing the lane change is necessary. +- If safety parameters are tuned too aggressively, it becomes harder to cancel or abort the lane change. This reduces tolerance for unexpected behaviors from surrounding vehicles, such as a trailing vehicle in the target lane suddenly accelerating or a leading vehicle suddenly decelerating. Aggressive settings leave less room for error during the maneuver. + +![stop](./images/lane_change-cant_cancel_no_abort.png) ## Parameters @@ -882,10 +1039,12 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | Name | Unit | Type | Description | Default value | | :------------------------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------------------- | ------------------ | +| `time_limit` | [ms] | double | Time limit for lane change candidate path generation | 50.0 | | `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.0 | | `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | | `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | | `backward_length_from_intersection` | [m] | double | Distance threshold from the last intersection to invalidate or cancel the lane change path | 5.0 | +| `enable_stopped_vehicle_buffer` | [-] | bool | If true, will keep enough distance from current lane front stopped object to perform lane change when possible | true | | `trajectory.max_prepare_duration` | [s] | double | The maximum preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | | `trajectory.min_prepare_duration` | [s] | double | The minimum preparation time for the ego vehicle to be ready to perform lane change. | 2.0 | | `trajectory.lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | @@ -895,6 +1054,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `trajectory.max_longitudinal_acc` | [m/s2] | double | maximum longitudinal acceleration for lane change | 1.0 | | `trajectory.min_longitudinal_acc` | [m/s2] | double | maximum longitudinal deceleration for lane change | -1.0 | | `trajectory.lane_changing_decel_factor` | [-] | double | longitudinal deceleration factor during lane changing phase | 0.5 | +| `trajectory.th_prepare_curvature` | [-] | double | If the maximum curvature of the prepare segment exceeds the threshold, the prepare segment is invalid. | 0.03 | | `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | | `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | | `lateral_acceleration.min_values` | [m/s2] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | @@ -934,6 +1094,28 @@ The following parameters are used to judge lane change completion. | `delay_lane_change.min_road_shoulder_width` | [m] | double | Width considered as road shoulder if lane doesn't have road shoulder when checking for parked vehicle | 0.5 | | `delay_lane_change.th_parked_vehicle_shift_ratio` | [-] | double | Stopped vehicles beyond this distance ratio from center line will be considered as parked | 0.6 | +### Terminal Lane Change Path + +The following parameters are used to configure terminal lane change path feature. + +| Name | Unit | Type | Description | Default value | +| :-------------------------------- | ---- | ---- | ------------------------------------------------------------------------- | ------------- | +| `terminal_path.enable` | [-] | bool | Flag to enable/disable terminal path feature | true | +| `terminal_path.disable_near_goal` | [-] | bool | Flag to disable terminal path feature if ego is near goal | true | +| `terminal_path.stop_at_boundary` | [-] | bool | If true, ego will stop at current lane boundary instead of middle of lane | false | + +### Generating Lane Changing Path using Frenet Planner + +!!! warning + + Only applicable when ego is near terminal start + +| Name | Unit | Type | Description | Default value | +| :------------------------------ | ----- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `frenet.enable` | [-] | bool | Flag to enable/disable frenet planner when ego is near terminal start. | true | +| `frenet.th_yaw_diff` | [deg] | double | If the yaw diff between of the prepare segment's end and lane changing segment's start exceed the threshold , the lane changing segment is invalid. | 10.0 | +| `frenet.th_curvature_smoothing` | [-] | double | Filters and appends target path points with curvature below the threshold to candidate path. | 0.1 | + ### Collision checks #### Target Objects @@ -967,7 +1149,8 @@ The following parameters are used to judge lane change completion. | `collision_check.check_other_lanes` | [-] | boolean | If true, the lane change module includes objects in other lanes when performing collision assessment. | false | | `collision_check.use_all_predicted_paths` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | | `collision_check.prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | -| `collision_check.yaw_diff_threshold` | [rad] | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 3.1416 | +| `collision_check.yaw_diff_threshold` | [rad] | double | Maximum yaw difference between predicted ego pose and predicted object pose when executing rss-based collision checking | 3.1416 | +| `collision_check.th_incoming_object_yaw` | [rad] | double | Maximum yaw difference between current ego pose and current object pose. Objects with a yaw difference exceeding this value are excluded from the safety check. | 2.3562 | #### safety constraints during lane change path is computed @@ -980,6 +1163,7 @@ The following parameters are used to judge lane change completion. | `safety_check.execution.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | | `safety_check.execution.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | | `safety_check.execution.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | +| `safety_check.execution.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | #### safety constraints specifically for stopped or parked vehicles @@ -992,6 +1176,7 @@ The following parameters are used to judge lane change completion. | `safety_check.parked.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.0 | | `safety_check.parked.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | | `safety_check.parked.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | +| `safety_check.parked.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | ##### safety constraints to cancel lane change path @@ -1004,6 +1189,7 @@ The following parameters are used to judge lane change completion. | `safety_check.cancel.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.0 | | `safety_check.cancel.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 2.5 | | `safety_check.cancel.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.6 | +| `safety_check.cancel.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | ##### safety constraints used during lane change path is computed when ego is stuck @@ -1016,6 +1202,7 @@ The following parameters are used to judge lane change completion. | `safety_check.stuck.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | | `safety_check.stuck.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | | `safety_check.stuck.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | +| `safety_check.stuck.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | (\*1) the value must be negative. @@ -1074,3 +1261,10 @@ Available information 3. Object is safe or not, shown by the color of the polygon (Green = Safe, Red = unsafe) 4. Valid candidate paths. 5. Position when lane changing start and end. + +## Limitation + +1. When a lane change is canceled, the lane change module returns `ModuleStatus::FAILURE`. As the module is removed from the approved module stack (see [Failure modules](https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design/#failure-modules)), a new instance of the lane change module is initiated. Due to this, any information stored prior to the reset is lost. For example, the `lane_change_prepare_duration` in the `TransientData` is reset to its maximum value. +2. The lane change module has no knowledge of any velocity modifications introduced to the path after it is approved. This is because other modules may add deceleration points after subscribing to the behavior path planner output, and the final velocity is managed by the [velocity smoother](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_velocity_smoother/). Since this limitation affects **CANCEL**, the lane change module mitigates it by [sampling accelerations along the approved lane change path](#checking-approved-path-safety). These sampled accelerations are used during safety checks to estimate the velocity that might occur if the ego vehicle decelerates. +3. Ideally, the abort path should account for whether its execution would affect trailing vehicles in the current lane. However, the lane change module does not evaluate such interactions or assess whether the abort path is safe. As a result, **the abort path is not guaranteed to be safe**. To minimize the risk of unsafe situations, the abort maneuver is only permitted if the ego vehicle has not yet diverged from the current lane. +4. Due to limited resources, the abort path logic is not fully optimized. The generated path may overshoot, causing the return trajectory to slightly shift toward the opposite lane. This can be dangerous, especially if the opposite lane has traffic moving in the opposite direction. Furthermore, the logic does not account for different vehicle types, which can lead to varying effects. For instance, the behavior might differ significantly between a bus and a small passenger car. diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index bf892b3058a16..71fac1246598f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -1,14 +1,22 @@ /**: ros__parameters: lane_change: + time_limit: 50.0 # [ms] backward_lane_length: 200.0 backward_length_buffer_for_end_of_lane: 3.0 # [m] backward_length_buffer_for_blocking_object: 3.0 # [m] backward_length_from_intersection: 5.0 # [m] + enable_stopped_vehicle_buffer: true # turn signal min_length_for_turn_signal_activation: 10.0 # [m] + # terminal path + terminal_path: + enable: true + disable_near_goal: true + stop_at_boundary: false + # trajectory generation trajectory: max_prepare_duration: 4.0 @@ -22,6 +30,7 @@ lon_acc_sampling_num: 5 lat_acc_sampling_num: 3 lane_changing_decel_factor: 0.5 + th_prepare_curvature: 0.03 # [/] # delay lane change delay_lane_change: @@ -30,6 +39,12 @@ min_road_shoulder_width: 0.5 # [m] th_parked_vehicle_shift_ratio: 0.6 + # trajectory generation near terminal using frenet planner + frenet: + enable: true + th_yaw_diff: 10.0 # [deg] + th_curvature_smoothing: 0.1 # [/] + # safety check safety_check: allow_loose_check_for_cancel: true @@ -43,6 +58,7 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" parked: expected_front_deceleration: -1.0 expected_rear_deceleration: -2.0 @@ -51,6 +67,7 @@ lateral_distance_max_threshold: 1.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" cancel: expected_front_deceleration: -1.0 expected_rear_deceleration: -2.0 @@ -59,6 +76,7 @@ lateral_distance_max_threshold: 1.0 longitudinal_distance_min_threshold: 2.5 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" stuck: expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 @@ -67,6 +85,7 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" # lane expansion for object filtering lane_expansion: @@ -108,7 +127,8 @@ intersection: true turns: true prediction_time_resolution: 0.5 - yaw_diff_threshold: 3.1416 + th_incoming_object_yaw: 2.3562 # [rad] + yaw_diff_threshold: 3.1416 # [rad] check_current_lanes: false check_other_lanes: false use_all_predicted_paths: false diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/check_able_to_return.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/check_able_to_return.png new file mode 100644 index 0000000000000..1f01b6df5464a Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/check_able_to_return.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort.png index 0913912b6a332..41e2bddbdafba 100644 Binary files a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort.png and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort_computation.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort_computation.png new file mode 100644 index 0000000000000..47d4ba81b3354 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort_computation.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cancel.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cancel.png index 10b7beb6ad7d0..91e44e609bf25 100644 Binary files a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cancel.png and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cancel.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png index c3780540f8d00..f142b42c8a644 100644 Binary files a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-no_terminal_path.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-no_terminal_path.png new file mode 100644 index 0000000000000..2c7b76e174997 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-no_terminal_path.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-terminal_path.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-terminal_path.png new file mode 100644 index 0000000000000..83e9d377e8f34 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-terminal_path.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_frenet.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_frenet.png new file mode 100644 index 0000000000000..51f3a2fc46500 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_frenet.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_path_shifter.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_path_shifter.png new file mode 100644 index 0000000000000..4af931348d6ca Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_path_shifter.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_frenet.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_frenet.png new file mode 100644 index 0000000000000..f834a64867e6c Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_frenet.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_path_shifter.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_path_shifter.png new file mode 100644 index 0000000000000..22ba8d2d7c47d Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_path_shifter.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_frenet.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_frenet.png new file mode 100644 index 0000000000000..9a36642b2d545 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_frenet.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_path_shifter.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_path_shifter.png new file mode 100644 index 0000000000000..7a4e3a453304b Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_path_shifter.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp index 1061e07e516a2..43a929591dad1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp @@ -104,6 +104,8 @@ 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; @@ -130,7 +132,7 @@ class LaneChangeBase virtual void insert_stop_point( [[maybe_unused]] const lanelet::ConstLanelets & lanelets, - [[maybe_unused]] PathWithLaneId & path) + [[maybe_unused]] PathWithLaneId & path, [[maybe_unused]] const bool is_waiting_approval = false) { } @@ -285,8 +287,7 @@ class LaneChangeBase FilteredLanesObjects filtered_objects_{}; BehaviorModuleOutput prev_module_output_{}; PoseWithDetailOpt lane_change_stop_pose_{std::nullopt}; - - PathWithLaneId prev_approved_path_; + mutable std::optional terminal_lane_change_path_{std::nullopt}; int unsafe_hysteresis_count_{0}; bool is_abort_path_approved_{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index 9d6fbd65acfd8..82b042d1135d2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -35,6 +35,7 @@ #include #include #include +#include namespace autoware::behavior_path_planner { @@ -52,6 +53,7 @@ class LaneChangeInterface : public SceneModuleInterface const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface, std::unique_ptr && module_type); LaneChangeInterface(const LaneChangeInterface &) = delete; @@ -120,6 +122,8 @@ class LaneChangeInterface : public SceneModuleInterface } } + std::pair check_transit_failure(); + void updateDebugMarker() const; void updateSteeringFactorPtr(const BehaviorModuleOutput & output); @@ -129,13 +133,13 @@ class LaneChangeInterface : public SceneModuleInterface mutable MarkerArray virtual_wall_marker_; - std::unique_ptr prev_approved_path_; - void clearAbortApproval() { is_abort_path_approved_ = false; } bool is_abort_path_approved_{false}; bool is_abort_approval_requested_{false}; + + lane_change::InterfaceDebug interface_debug_; }; } // namespace autoware::behavior_path_planner 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 5f1da79bc7ea0..c11839f0356c6 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 @@ -65,13 +65,18 @@ class NormalLaneChange : public LaneChangeBase LaneChangePath getLaneChangePath() const override; + BehaviorModuleOutput getTerminalLaneChangePath() const override; + BehaviorModuleOutput generateOutput() override; void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; - void insert_stop_point(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; + void insert_stop_point( + const lanelet::ConstLanelets & lanelets, PathWithLaneId & path, + const bool is_waiting_approval = false) override; - void insert_stop_point_on_current_lanes(PathWithLaneId & path); + void insert_stop_point_on_current_lanes( + PathWithLaneId & path, const bool is_waiting_approval = false); PathWithLaneId getReferencePath() const override; @@ -130,13 +135,28 @@ class NormalLaneChange : public LaneChangeBase std::vector get_prepare_metrics() const; std::vector get_lane_changing_metrics( const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metrics, - const double shift_length, const double dist_to_reg_element) const; + const double shift_length, const double dist_to_reg_element, + lane_change::MetricsDebug & debug_metrics) const; bool get_lane_change_paths(LaneChangePaths & candidate_paths) const; + bool get_path_using_frenet( + const std::vector & prepare_metrics, + const lane_change::TargetObjects & target_objects, + const std::vector> & sorted_lane_ids, + LaneChangePaths & candidate_paths) const; + + bool get_path_using_path_shifter( + const std::vector & prepare_metrics, + const lane_change::TargetObjects & target_objects, + const std::vector> & sorted_lane_ids, + LaneChangePaths & candidate_paths) const; + bool check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const; + std::optional compute_terminal_lane_change_path() const; + bool isValidPath(const PathWithLaneId & path) const override; PathSafetyStatus isLaneChangePathSafe( 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 5f36be806bba4..c844b6b218d10 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 @@ -48,6 +48,7 @@ enum class States { Cancel, Abort, Stop, + Warning, }; struct PhaseInfo @@ -128,6 +129,22 @@ struct Info terminal_lane_changing_velocity = _lc_metrics.velocity; shift_line = _shift_line; } + + void set_prepare(const PhaseMetrics & _prep_metrics) + { + longitudinal_acceleration.prepare = _prep_metrics.actual_lon_accel; + velocity.prepare = _prep_metrics.velocity; + duration.prepare = _prep_metrics.duration; + length.prepare = _prep_metrics.length; + } + + void set_lane_changing(const PhaseMetrics & _lc_metrics) + { + longitudinal_acceleration.lane_changing = _lc_metrics.actual_lon_accel; + velocity.lane_changing = _lc_metrics.velocity; + duration.lane_changing = _lc_metrics.duration; + length.lane_changing = _lc_metrics.length; + } }; struct TargetLaneLeadingObjects @@ -219,6 +236,8 @@ struct TransientData double target_lane_length{std::numeric_limits::min()}; + double dist_to_target_end{std::numeric_limits::max()}; + lanelet::ArcCoordinates current_lanes_ego_arc; // arc coordinates of ego pose along current lanes lanelet::ArcCoordinates target_lanes_ego_arc; // arc coordinates of ego pose along target lanes @@ -250,6 +269,7 @@ struct CommonData LanesPolygonPtr lanes_polygon_ptr; TransientData transient_data; PathWithLaneId current_lanes_path; + PathWithLaneId target_lanes_path; ModuleType lc_type; Direction direction; 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 8738b412e18b9..eb07d2f0d3704 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp @@ -25,10 +25,34 @@ #include #include +#include namespace autoware::behavior_path_planner::lane_change { using utils::path_safety_checker::CollisionCheckDebugMap; + +struct MetricsDebug +{ + LaneChangePhaseMetrics prep_metric; + std::vector> lc_metrics; + double max_prepare_length; + double max_lane_changing_length; +}; + +struct FrenetStateDebug +{ + LaneChangePhaseMetrics prep_metric; + frenet_planner::SamplingParameter sampling_parameter; + double max_lane_changing_length; + + FrenetStateDebug( + LaneChangePhaseMetrics prep_metric, frenet_planner::SamplingParameter sampling_param, + const double max_len) + : prep_metric(prep_metric), sampling_parameter(sampling_param), max_lane_changing_length(max_len) + { + } +}; + struct Debug { std::string module_type; @@ -41,13 +65,14 @@ struct Debug lanelet::ConstLanelets current_lanes; lanelet::ConstLanelets target_lanes; lanelet::ConstLanelets target_backward_lanes; + std::vector lane_change_metrics; + std::vector frenet_states; double collision_check_object_debug_lifetime{0.0}; double distance_to_end_of_current_lane{std::numeric_limits::max()}; double distance_to_lane_change_finished{std::numeric_limits::max()}; double distance_to_abort_finished{std::numeric_limits::max()}; bool is_able_to_return_to_current_lane{true}; bool is_stuck{false}; - bool is_abort{false}; void reset() { @@ -64,6 +89,7 @@ struct Debug current_lanes.clear(); target_lanes.clear(); target_backward_lanes.clear(); + lane_change_metrics.clear(); collision_check_object_debug_lifetime = 0.0; distance_to_end_of_current_lane = std::numeric_limits::max(); @@ -71,9 +97,14 @@ struct Debug distance_to_abort_finished = std::numeric_limits::max(); is_able_to_return_to_current_lane = true; is_stuck = false; - is_abort = false; } }; + +struct InterfaceDebug +{ + std::string_view failing_reason; + LaneChangeStates lc_state; +}; } // namespace autoware::behavior_path_planner::lane_change #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DEBUG_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp index 719bbbbf3057a..358f60f3193d5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp @@ -94,6 +94,7 @@ struct CollisionCheckParameters bool check_current_lane{true}; bool check_other_lanes{true}; bool use_all_predicted_paths{false}; + double th_incoming_object_yaw{2.3562}; double th_yaw_diff{3.1416}; double prediction_time_resolution{0.5}; }; @@ -113,6 +114,13 @@ struct SafetyParameters CollisionCheckParameters collision_check{}; }; +struct FrenetPlannerParameters +{ + bool enable{true}; + double th_yaw_diff_deg{10.0}; + double th_curvature_smoothing{0.1}; +}; + struct TrajectoryParameters { double max_prepare_duration{4.0}; @@ -124,6 +132,7 @@ struct TrajectoryParameters double th_lane_changing_length_diff{0.5}; double min_lane_changing_velocity{5.6}; double lane_changing_decel_factor{0.5}; + double th_prepare_curvature{0.03}; int lon_acc_sampling_num{10}; int lat_acc_sampling_num{10}; LateralAccelerationMap lat_acc_map{}; @@ -137,18 +146,29 @@ struct DelayParameters double th_parked_vehicle_shift_ratio{0.6}; }; +struct TerminalPathParameters +{ + bool enable{false}; + bool disable_near_goal{false}; + bool stop_at_boundary{false}; +}; + struct Parameters { TrajectoryParameters trajectory{}; SafetyParameters safety{}; CancelParameters cancel{}; DelayParameters delay{}; + TerminalPathParameters terminal_path{}; + FrenetPlannerParameters frenet{}; // lane change parameters + double time_limit{50.0}; double backward_lane_length{200.0}; double backward_length_buffer_for_end_of_lane{0.0}; double backward_length_buffer_for_blocking_object{0.0}; double backward_length_from_intersection{5.0}; + bool enable_stopped_vehicle_buffer{false}; // parked vehicle double object_check_min_road_shoulder_width{0.5}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp index 0fa0a9d977a26..0fa2c6cc8dfbc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp @@ -19,19 +19,53 @@ #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include + #include +#include #include namespace autoware::behavior_path_planner::lane_change { +enum class PathType { ConstantJerk = 0, FrenetPlanner }; + using autoware::behavior_path_planner::TurnSignalInfo; using tier4_planning_msgs::msg::PathWithLaneId; +struct TrajectoryGroup +{ + PathWithLaneId prepare; + PathWithLaneId target_lane_ref_path; + std::vector target_lane_ref_path_dist; + LaneChangePhaseMetrics prepare_metric; + frenet_planner::Trajectory lane_changing; + frenet_planner::FrenetState initial_state; + double max_lane_changing_length{0.0}; + + TrajectoryGroup() = default; + TrajectoryGroup( + PathWithLaneId prepare, PathWithLaneId target_lane_ref_path, + std::vector target_lane_ref_path_dist, LaneChangePhaseMetrics prepare_metric, + frenet_planner::Trajectory lane_changing, frenet_planner::FrenetState initial_state, + const double max_lane_changing_length) + : prepare(std::move(prepare)), + target_lane_ref_path(std::move(target_lane_ref_path)), + target_lane_ref_path_dist(std::move(target_lane_ref_path_dist)), + prepare_metric(prepare_metric), + lane_changing(std::move(lane_changing)), + initial_state(initial_state), + max_lane_changing_length(max_lane_changing_length) + { + } +}; + struct Path { + Info info; PathWithLaneId path; ShiftedPath shifted_path; - Info info; + TrajectoryGroup frenet_path; + PathType type = PathType::ConstantJerk; }; struct Status @@ -39,7 +73,6 @@ struct Status Path lane_change_path; bool is_safe{false}; bool is_valid_path{false}; - double start_distance{0.0}; }; } // namespace autoware::behavior_path_planner::lane_change diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index eced227a5be32..29a9f258545a2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -67,6 +67,9 @@ double calc_dist_to_last_fit_width( const lanelet::ConstLanelets & lanelets, const Pose & src_pose, const BehaviorPathPlannerParameters & bpp_param, const double margin = 0.1); +double calc_dist_to_last_fit_width( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const double margin = 0.1); + /** * @brief Calculates the maximum preparation longitudinal distance for lane change. * diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp index 059db8e38300f..a9ea35cb9b83e 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 @@ -32,6 +32,7 @@ namespace marker_utils::lane_change_markers using autoware::behavior_path_planner::FilteredLanesObjects; using autoware::behavior_path_planner::LaneChangePath; using autoware::behavior_path_planner::lane_change::Debug; +using autoware::behavior_path_planner::lane_change::InterfaceDebug; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using visualization_msgs::msg::MarkerArray; MarkerArray showAllValidLaneChangePath( @@ -42,9 +43,12 @@ MarkerArray createLaneChangingVirtualWallMarker( MarkerArray showFilteredObjects( const FilteredLanesObjects & filtered_objects, const std::string & ns); MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area); -MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose); +MarkerArray showExecutionInfo( + const InterfaceDebug & interface_debug_data, const Debug & scene_debug_data, + const geometry_msgs::msg::Pose & ego_pose); MarkerArray createDebugMarkerArray( - const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose); + const InterfaceDebug & interface_debug_data, const Debug & scene_debug_data, + const geometry_msgs::msg::Pose & ego_pose); } // namespace marker_utils::lane_change_markers #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ 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 index dcc327b4793e1..42586e7b1df92 100644 --- 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 @@ -11,7 +11,6 @@ // 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_ @@ -26,6 +25,7 @@ namespace autoware::behavior_path_planner::utils::lane_change { using behavior_path_planner::LaneChangePath; using behavior_path_planner::lane_change::CommonDataPtr; +using behavior_path_planner::lane_change::TrajectoryGroup; /** * @brief Generates a prepare segment for a lane change maneuver. @@ -48,7 +48,7 @@ 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 double prep_length, PathWithLaneId & prepare_segment); /** * @brief Generates the candidate path for a lane change maneuver. @@ -98,5 +98,65 @@ 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); + +/** + * @brief Generates candidate trajectories in the Frenet frame for a lane change maneuver. + * + * This function computes a set of candidate trajectories for the ego vehicle in the Frenet frame, + * based on the provided metrics, target lane reference path, and preparation segments. It ensures + * that the generated trajectories adhere to specified constraints, such as lane boundaries and + * velocity limits, while optimizing for smoothness and curvature. + * + * @param common_data_ptr Shared pointer to CommonData containing route, lane, and transient + * information. + * @param prev_module_path The previous module's path used as a base for preparation segments. + * @param prep_metric Metrics for the preparation phase, including path length and velocity. + * + * @return std::vector A vector of trajectory groups representing + * valid lane change candidates, each containing the prepare segment, target reference path, and + * Frenet trajectory. + */ +std::vector generate_frenet_candidates( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, + const std::vector & prep_metrics); + +/** + * @brief Constructs a lane change path candidate based on a Frenet trajectory group. + * + * This function generates a candidate lane change path by converting a Frenet trajectory group + * into a shifted path and combining it with a prepare segment. It validates the path's feasibility + * by ensuring yaw differences are within allowable thresholds and calculates lane change + * information, such as acceleration, velocity, and duration. + * + * @param trajectory_group A Frenet trajectory group containing the prepared path and Frenet + * trajectory data. + * @param common_data_ptr Shared pointer to CommonData providing parameters and constraints for lane + * changes. + * @param sorted_lane_ids A vector of sorted lane IDs used to update the lane IDs in the path. + * + * @return std::optional The constructed candidate lane change path if valid, or + * std::nullopt if the path is not feasible. + * + * @throws std::logic_error If the yaw difference exceeds the threshold, or other critical errors + * occur. + */ +std::optional get_candidate_path( + const TrajectoryGroup & trajectory_group, const CommonDataPtr & common_data_ptr, + const std::vector> & sorted_lane_ids); + +/** + * @brief Appends the target lane reference path to the candidate lane change path. + * + * This function extends the lane change candidate path by appending points from the + * target lane reference path beyond the lane change end position. The appending process + * is constrained by a curvature threshold to ensure smooth transitions and avoid sharp turns. + * + * @param frenet_candidate The candidate lane change path to which the target reference path is + * appended. This includes the lane change path and associated Frenet trajectory data. + * @param th_curvature_smoothing A threshold for the allowable curvature during the extension + * process. Points with curvature exceeding this threshold are excluded. + */ +void append_target_ref_to_candidate(LaneChangePath & frenet_candidate, const double th_curvature); } // 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 69362994dbbac..422c392cac462 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 @@ -24,6 +24,8 @@ #include #include +#include +#include #include #include @@ -56,6 +58,7 @@ 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; +using behavior_path_planner::lane_change::TrajectoryGroup; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; @@ -69,9 +72,27 @@ bool is_mandatory_lane_change(const ModuleType lc_type); void set_prepare_velocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity); -std::vector replaceWithSortedIds( - const std::vector & original_lane_ids, - const std::vector> & sorted_lane_ids); +/** + * @brief Replaces the current lane IDs with a sorted set of IDs based on a predefined mapping. + * + * This function checks if the current lane IDs match the previously processed lane IDs. + * If they do, it returns the previously sorted IDs for efficiency. Otherwise, it matches + * the current lane IDs to the appropriate sorted IDs from the provided mapping and updates + * the cached values. + * + * @param current_lane_ids The current lane IDs to be replaced or verified. + * @param sorted_lane_ids A vector of sorted lane ID groups, each representing a predefined + * order of IDs for specific conditions. + * @param prev_lane_ids Reference to the previously processed lane IDs for caching purposes. + * @param prev_sorted_lane_ids Reference to the previously sorted lane IDs for caching purposes. + * + * @return std::vector The sorted lane IDs if a match is found, or the original + * `current_lane_ids` if no match exists. + */ +std::vector replace_with_sorted_ids( + const std::vector & current_lane_ids, + const std::vector> & sorted_lane_ids, std::vector & prev_lane_ids, + std::vector & prev_sorted_lane_ids); std::vector> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr); @@ -432,5 +453,24 @@ std::vector> convert_to_predicted_paths( * @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); + +/** + * @brief Converts a lane change frenet candidate into a predicted path for the ego vehicle. + * + * This function generates a predicted path based on the provided Frenet candidate, + * simulating the vehicle's trajectory during the preparation and lane-changing phases. + * It interpolates poses and velocities over the duration of the prediction, considering + * the ego vehicle's initial conditions and the candidate's trajectory data. + * + * @param common_data_ptr Shared pointer to CommonData containing parameters and ego vehicle state. + * @param frenet_candidate A Frenet trajectory group representing the lane change candidate. + * @param deceleration_sampling_num Unused parameter for deceleration sampling count. + * + * @return std::vector The predicted path as a series of stamped poses + * with associated velocities over the prediction time. + */ +std::vector convert_to_predicted_path( + const CommonDataPtr & common_data_ptr, const lane_change::TrajectoryGroup & frenet_candidate, + [[maybe_unused]] const size_t deceleration_sampling_num); } // 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 fad98ecf8f8e1..cf7600556080e 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 @@ -23,6 +23,7 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_frenet_planner autoware_motion_utils autoware_rtc_interface autoware_universe_utils 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 f80aad721a07c..4519b20d4644d 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 @@ -40,16 +40,14 @@ LaneChangeInterface::LaneChangeInterface( const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface, std::unique_ptr && module_type) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{std::move(parameters)}, - module_type_{std::move(module_type)}, - prev_approved_path_{std::make_unique()} + module_type_{std::move(module_type)} { module_type_->setTimeKeeper(getTimeKeeper()); logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr()); - steering_factor_interface_.init(PlanningBehavior::LANE_CHANGE); - velocity_factor_interface_.init(PlanningBehavior::LANE_CHANGE); } void LaneChangeInterface::processOnExit() @@ -109,7 +107,6 @@ BehaviorModuleOutput LaneChangeInterface::plan() auto output = module_type_->generateOutput(); path_reference_ = std::make_shared(output.reference_path); - *prev_approved_path_ = getPreviousModuleOutput().path; stop_pose_ = module_type_->getStopPose(); @@ -141,25 +138,22 @@ BehaviorModuleOutput LaneChangeInterface::plan() const auto force_activated = std::any_of( rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); }); - const bool safe = force_activated ? false : true; updateRTCStatus( - path.start_distance_to_path_change, path.finish_distance_to_path_change, safe, + path.start_distance_to_path_change, path.finish_distance_to_path_change, !force_activated, State::RUNNING); } } - setVelocityFactor(output.path); + set_longitudinal_planning_factor(output.path); return output; } BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { - BehaviorModuleOutput out = getPreviousModuleOutput(); + BehaviorModuleOutput out = module_type_->getTerminalLaneChangePath(); - *prev_approved_path_ = out.path; - - module_type_->insert_stop_point(module_type_->get_current_lanes(), out.path); + module_type_->insert_stop_point(module_type_->get_current_lanes(), out.path, true); out.turn_signal_info = module_type_->get_current_turn_signal_info(); const auto & lane_change_debug = module_type_->getDebugData(); @@ -184,7 +178,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() isExecutionReady(), State::WAITING_FOR_EXECUTION); is_abort_path_approved_ = false; - setVelocityFactor(out.path); + set_longitudinal_planning_factor(out.path); return out; } @@ -224,9 +218,7 @@ bool LaneChangeInterface::canTransitSuccessState() if (module_type_->specialExpiredCheck() && isWaitingApproval()) { log_debug_throttled("Run specialExpiredCheck."); - if (isWaitingApproval()) { - return true; - } + return true; } if (module_type_->hasFinishedLaneChange()) { @@ -241,13 +233,6 @@ bool LaneChangeInterface::canTransitSuccessState() bool LaneChangeInterface::canTransitFailureState() { - auto log_debug_throttled = [&](std::string_view message) -> void { - RCLCPP_DEBUG(getLogger(), "%s", message.data()); - }; - - updateDebugMarker(); - log_debug_throttled(__func__); - const auto force_activated = std::any_of( rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); }); @@ -257,121 +242,106 @@ bool LaneChangeInterface::canTransitFailureState() return false; } - if (module_type_->isAbortState() && !module_type_->hasFinishedAbort()) { - log_debug_throttled("Abort process has on going."); - return false; - } + const auto [state, reason] = check_transit_failure(); - if (isWaitingApproval()) { - if (module_type_->is_near_regulatory_element()) { - log_debug_throttled("Ego is close to regulatory element. Cancel lane change"); - updateRTCStatus( - std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, - State::FAILED); - return true; - } - log_debug_throttled("Can't transit to failure state. Module is WAITING_FOR_APPROVAL"); - return false; - } + interface_debug_.failing_reason = reason; + interface_debug_.lc_state = state; - if (!module_type_->isValidPath()) { - log_debug_throttled("Transit to failure state due not to find valid path"); + updateDebugMarker(); + + if (state == LaneChangeStates::Cancel) { updateRTCStatus( std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, State::FAILED); + module_type_->toCancelState(); return true; } - if (module_type_->isAbortState() && module_type_->hasFinishedAbort()) { - log_debug_throttled("Abort process has completed."); - updateRTCStatus( - std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, - State::FAILED); - return true; + if (state == LaneChangeStates::Abort) { + module_type_->toAbortState(); + return false; } - if (module_type_->is_near_terminal()) { - log_debug_throttled("Unsafe, but ego is approaching terminal. Continue lane change"); + // Note: Ideally, if it is unsafe, but for some reason, we can't abort or cancel, then we should + // stop. Note: This feature is not working properly for now. + const auto [is_safe, unsafe_trailing_obj] = post_process_safety_status_; + if (!is_safe && module_type_->isRequiredStop(unsafe_trailing_obj)) { + module_type_->toStopState(); + return false; + } + + module_type_->toNormalState(); + return false; +} - if (module_type_->isRequiredStop(post_process_safety_status_.is_trailing_object)) { - log_debug_throttled("Module require stopping"); +std::pair LaneChangeInterface::check_transit_failure() +{ + if (module_type_->isAbortState()) { + if (module_type_->hasFinishedAbort()) { + return {LaneChangeStates::Cancel, "Aborted"}; } - return false; + return {LaneChangeStates::Abort, "Aborting"}; } - if (module_type_->isCancelEnabled() && module_type_->isEgoOnPreparePhase()) { - if (module_type_->isStoppedAtRedTrafficLight()) { - log_debug_throttled("Stopping at traffic light while in prepare phase. Cancel lane change"); - module_type_->toCancelState(); - updateRTCStatus( - std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, - State::FAILED); - return true; + if (isWaitingApproval()) { + if (module_type_->is_near_regulatory_element()) { + return {LaneChangeStates::Cancel, "CloseToRegElement"}; } + return {LaneChangeStates::Normal, "WaitingForApproval"}; + } + if (!module_type_->isValidPath()) { + return {LaneChangeStates::Cancel, "InvalidPath"}; + } + + const auto is_preparing = module_type_->isEgoOnPreparePhase(); + const auto can_return_to_current = module_type_->isAbleToReturnCurrentLane(); + + // regardless of safe and unsafe, we want to cancel lane change. + if (is_preparing) { const auto force_deactivated = std::any_of( rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { return rtc.second->isForceDeactivated(uuid_map_.at(rtc.first)); }); - if (force_deactivated && module_type_->isAbleToReturnCurrentLane()) { - log_debug_throttled("Cancel lane change due to force deactivation"); - module_type_->toCancelState(); - updateRTCStatus( - std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, - State::FAILED); - return true; - } - - if (post_process_safety_status_.is_safe) { - log_debug_throttled("Can't transit to failure state. Ego is on prepare, and it's safe."); - return false; - } - - if (module_type_->isAbleToReturnCurrentLane()) { - log_debug_throttled("It's possible to return to current lane. Cancel lane change."); - updateRTCStatus( - std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, - State::FAILED); - return true; + if (force_deactivated && can_return_to_current) { + return {LaneChangeStates::Cancel, "ForceDeactivation"}; } } if (post_process_safety_status_.is_safe) { - log_debug_throttled("Can't transit to failure state. Ego is lane changing, and it's safe."); - return false; + return {LaneChangeStates::Normal, "SafeToLaneChange"}; } - if (module_type_->isRequiredStop(post_process_safety_status_.is_trailing_object)) { - log_debug_throttled("Module require stopping"); + if (!module_type_->isCancelEnabled()) { + return {LaneChangeStates::Warning, "CancelDisabled"}; } - if (!module_type_->isCancelEnabled()) { - log_debug_throttled( - "Lane change path is unsafe but cancel was not enabled. Continue lane change."); - return false; + // We also check if the ego can return to the current lane, as prepare segment might be out of the + // lane, for example, during an evasive maneuver around a static object. + if (is_preparing && can_return_to_current) { + return {LaneChangeStates::Cancel, "SafeToCancel"}; + } + + if (module_type_->is_near_terminal()) { + return {LaneChangeStates::Warning, "TooNearTerminal"}; } if (!module_type_->isAbortEnabled()) { - log_debug_throttled( - "Lane change path is unsafe but abort was not enabled. Continue lane change."); - return false; + return {LaneChangeStates::Warning, "AbortDisabled"}; } - if (!module_type_->isAbleToReturnCurrentLane()) { - log_debug_throttled("It's is not possible to return to original lane. Continue lane change."); - return false; + // To prevent the lane module from having to check rear objects in the current lane, we limit the + // abort maneuver to cases where the ego vehicle is still in the current lane. + if (!can_return_to_current) { + return {LaneChangeStates::Warning, "TooLateToAbort"}; } const auto found_abort_path = module_type_->calcAbortPath(); if (!found_abort_path) { - log_debug_throttled( - "Lane change path is unsafe but abort path not found. Continue lane change."); - return false; + return {LaneChangeStates::Warning, "AbortPathNotFound"}; } - log_debug_throttled("Lane change path is unsafe. Abort lane change."); - module_type_->toAbortState(); - return false; + return {LaneChangeStates::Abort, "SafeToAbort"}; } void LaneChangeInterface::updateDebugMarker() const @@ -381,7 +351,8 @@ void LaneChangeInterface::updateDebugMarker() const return; } using marker_utils::lane_change_markers::createDebugMarkerArray; - debug_marker_ = createDebugMarkerArray(module_type_->getDebugData(), module_type_->getEgoPose()); + debug_marker_ = createDebugMarkerArray( + interface_debug_, module_type_->getDebugData(), module_type_->getEgoPose()); } MarkerArray LaneChangeInterface::getModuleVirtualWall() @@ -412,15 +383,6 @@ MarkerArray LaneChangeInterface::getModuleVirtualWall() void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & output) { universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); - const auto steering_factor_direction = std::invoke([&]() { - if (module_type_->getDirection() == Direction::LEFT) { - return SteeringFactor::LEFT; - } - if (module_type_->getDirection() == Direction::RIGHT) { - return SteeringFactor::RIGHT; - } - return SteeringFactor::UNKNOWN; - }); const auto current_position = module_type_->getEgoPosition(); const auto status = module_type_->getLaneChangeStatus(); @@ -429,24 +391,34 @@ void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & o const auto finish_distance = autoware::motion_utils::calcSignedArcLength( output.path.points, current_position, status.lane_change_path.info.shift_line.end.position); - steering_factor_interface_.set( - {status.lane_change_path.info.shift_line.start, status.lane_change_path.info.shift_line.end}, - {start_distance, finish_distance}, steering_factor_direction, SteeringFactor::TURNING, ""); + const auto planning_factor_direction = std::invoke([&]() { + if (module_type_->getDirection() == Direction::LEFT) { + return PlanningFactor::SHIFT_LEFT; + } + if (module_type_->getDirection() == Direction::RIGHT) { + return PlanningFactor::SHIFT_RIGHT; + } + return PlanningFactor::UNKNOWN; + }); + + planning_factor_interface_->add( + start_distance, finish_distance, status.lane_change_path.info.shift_line.start, + status.lane_change_path.info.shift_line.end, planning_factor_direction, SafetyFactorArray{}); } void LaneChangeInterface::updateSteeringFactorPtr( const CandidateOutput & output, const LaneChangePath & selected_path) const { - const uint16_t steering_factor_direction = std::invoke([&output]() { + const uint16_t planning_factor_direction = std::invoke([&output]() { if (output.lateral_shift > 0.0) { - return SteeringFactor::LEFT; + return PlanningFactor::SHIFT_LEFT; } - return SteeringFactor::RIGHT; + return PlanningFactor::SHIFT_RIGHT; }); - steering_factor_interface_.set( - {selected_path.info.shift_line.start, selected_path.info.shift_line.end}, - {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - steering_factor_direction, SteeringFactor::APPROACHING, ""); + planning_factor_interface_->add( + output.start_distance_to_path_change, output.finish_distance_to_path_change, + selected_path.info.shift_line.start, selected_path.info.shift_line.end, + planning_factor_direction, SafetyFactorArray{}); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 1bb41069140e7..589338d7b3ace 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -63,6 +63,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("trajectory.min_lane_changing_velocity")); p.trajectory.lane_changing_decel_factor = getOrDeclareParameter(*node, parameter("trajectory.lane_changing_decel_factor")); + p.trajectory.th_prepare_curvature = + getOrDeclareParameter(*node, parameter("trajectory.th_prepare_curvature")); p.trajectory.lon_acc_sampling_num = getOrDeclareParameter(*node, parameter("trajectory.lon_acc_sampling_num")); p.trajectory.lat_acc_sampling_num = @@ -150,6 +152,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("collision_check.prediction_time_resolution")); p.safety.collision_check.th_yaw_diff = getOrDeclareParameter(*node, parameter("collision_check.yaw_diff_threshold")); + p.safety.collision_check.th_incoming_object_yaw = + getOrDeclareParameter(*node, parameter("collision_check.th_incoming_object_yaw")); // rss check auto set_rss_params = [&](auto & params, const std::string & prefix) { @@ -167,6 +171,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s *node, parameter(prefix + ".rear_vehicle_safety_time_margin")); params.lateral_distance_max_threshold = getOrDeclareParameter(*node, parameter(prefix + ".lateral_distance_max_threshold")); + params.extended_polygon_policy = + getOrDeclareParameter(*node, parameter(prefix + ".extended_polygon_policy")); }; set_rss_params(p.safety.rss_params, "safety_check.execution"); set_rss_params(p.safety.rss_params_for_parked, "safety_check.parked"); @@ -188,6 +194,7 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s } // lane change parameters + p.time_limit = getOrDeclareParameter(*node, parameter("time_limit")); p.backward_lane_length = getOrDeclareParameter(*node, parameter("backward_lane_length")); p.backward_length_buffer_for_end_of_lane = getOrDeclareParameter(*node, parameter("backward_length_buffer_for_end_of_lane")); @@ -195,6 +202,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("backward_length_buffer_for_blocking_object")); p.backward_length_from_intersection = getOrDeclareParameter(*node, parameter("backward_length_from_intersection")); + p.enable_stopped_vehicle_buffer = + getOrDeclareParameter(*node, parameter("enable_stopped_vehicle_buffer")); if (p.backward_length_buffer_for_end_of_lane < 1.0) { RCLCPP_WARN_STREAM( @@ -211,6 +220,12 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s p.delay.th_parked_vehicle_shift_ratio = getOrDeclareParameter( *node, parameter("delay_lane_change.th_parked_vehicle_shift_ratio")); + // trajectory generation near terminal using frenet planner + p.frenet.enable = getOrDeclareParameter(*node, parameter("frenet.enable")); + p.frenet.th_yaw_diff_deg = getOrDeclareParameter(*node, parameter("frenet.th_yaw_diff")); + p.frenet.th_curvature_smoothing = + getOrDeclareParameter(*node, parameter("frenet.th_curvature_smoothing")); + // lane change cancel p.cancel.enable_on_prepare_phase = getOrDeclareParameter(*node, parameter("cancel.enable_on_prepare_phase")); @@ -240,6 +255,13 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s // debug marker p.publish_debug_marker = getOrDeclareParameter(*node, parameter("publish_debug_marker")); + // terminal lane change path + p.terminal_path.enable = getOrDeclareParameter(*node, parameter("terminal_path.enable")); + p.terminal_path.disable_near_goal = + getOrDeclareParameter(*node, parameter("terminal_path.disable_near_goal")); + p.terminal_path.stop_at_boundary = + getOrDeclareParameter(*node, parameter("terminal_path.stop_at_boundary")); + // validation of safety check parameters // if loose check is not enabled, lane change module will keep on chattering and canceling, and // false positive situation might occur @@ -284,7 +306,7 @@ std::unique_ptr LaneChangeModuleManager::createNewSceneMod { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_, std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); } @@ -296,6 +318,17 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortime_limit; + updateParam(parameters, ns + "time_limit", time_limit); + if (time_limit >= 10.0) { + p->time_limit = time_limit; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Parameter 'time_limit' is not updated because the value (%.3f ms) is not valid, " + "keep current value (%.3f ms)", + time_limit, p->time_limit); + } updateParam(parameters, ns + "backward_lane_length", p->backward_lane_length); updateParam( parameters, ns + "backward_length_buffer_for_end_of_lane", @@ -305,10 +338,14 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectorbackward_length_buffer_for_blocking_object); updateParam( parameters, ns + "lane_change_finish_judge_buffer", p->lane_change_finish_judge_buffer); - + updateParam( + parameters, ns + "enable_stopped_vehicle_buffer", p->enable_stopped_vehicle_buffer); updateParam( parameters, ns + "finish_judge_lateral_threshold", p->th_finish_judge_lateral_diff); updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); + updateParam( + parameters, ns + "min_length_for_turn_signal_activation", + p->min_length_for_turn_signal_activation); } { @@ -319,24 +356,37 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortrajectory.min_prepare_duration); updateParam(parameters, ns + "lateral_jerk", p->trajectory.lateral_jerk); updateParam( - parameters, ns + ".min_lane_changing_velocity", p->trajectory.min_lane_changing_velocity); - // longitudinal acceleration + parameters, ns + "min_lane_changing_velocity", p->trajectory.min_lane_changing_velocity); updateParam( parameters, ns + "min_longitudinal_acc", p->trajectory.min_longitudinal_acc); updateParam( parameters, ns + "max_longitudinal_acc", p->trajectory.max_longitudinal_acc); updateParam( parameters, ns + "lane_changing_decel_factor", p->trajectory.lane_changing_decel_factor); - int longitudinal_acc_sampling_num = 0; + updateParam( + parameters, ns + "th_prepare_curvature", p->trajectory.th_prepare_curvature); + int longitudinal_acc_sampling_num = p->trajectory.lon_acc_sampling_num; updateParam(parameters, ns + "lon_acc_sampling_num", longitudinal_acc_sampling_num); if (longitudinal_acc_sampling_num > 0) { p->trajectory.lon_acc_sampling_num = longitudinal_acc_sampling_num; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Parameter 'lon_acc_sampling_num' is not updated because the value (%d) is not " + "positive", + longitudinal_acc_sampling_num); } - int lateral_acc_sampling_num = 0; + int lateral_acc_sampling_num = p->trajectory.lat_acc_sampling_num; updateParam(parameters, ns + "lat_acc_sampling_num", lateral_acc_sampling_num); if (lateral_acc_sampling_num > 0) { p->trajectory.lat_acc_sampling_num = lateral_acc_sampling_num; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Parameter 'lat_acc_sampling_num' is not updated because the value (%d) is not " + "positive", + lateral_acc_sampling_num); } updateParam( @@ -345,12 +395,84 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortrajectory.th_lane_changing_length_diff); } + { + const std::string ns = "lane_change.frenet."; + updateParam(parameters, ns + "enable", p->frenet.enable); + updateParam(parameters, ns + "th_yaw_diff", p->frenet.th_yaw_diff_deg); + updateParam( + parameters, ns + "th_curvature_smoothing", p->frenet.th_curvature_smoothing); + } + { const std::string ns = "lane_change.safety_check.lane_expansion."; updateParam(parameters, ns + "left_offset", p->safety.lane_expansion_left_offset); updateParam(parameters, ns + "right_offset", p->safety.lane_expansion_right_offset); } + { + const std::string ns = "lane_change.lateral_acceleration."; + std::vector velocity = p->trajectory.lat_acc_map.base_vel; + std::vector min_values = p->trajectory.lat_acc_map.base_min_acc; + std::vector max_values = p->trajectory.lat_acc_map.base_max_acc; + + updateParam>(parameters, ns + "velocity", velocity); + updateParam>(parameters, ns + "min_values", min_values); + updateParam>(parameters, ns + "max_values", max_values); + if ( + velocity.size() >= 2 && velocity.size() == min_values.size() && + velocity.size() == max_values.size()) { + LateralAccelerationMap lat_acc_map; + for (size_t i = 0; i < velocity.size(); ++i) { + lat_acc_map.add(velocity.at(i), min_values.at(i), max_values.at(i)); + } + p->trajectory.lat_acc_map = lat_acc_map; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "Mismatched size for lateral acceleration. Expected size: %lu, but velocity: %lu, " + "min_values: %lu, max_values: %lu", + std::max(2ul, velocity.size()), velocity.size(), min_values.size(), max_values.size()); + } + } + + { + const std::string ns = "lane_change.collision_check."; + updateParam( + parameters, ns + "enable_for_prepare_phase.general_lanes", + p->safety.collision_check.enable_for_prepare_phase_in_general_lanes); + updateParam( + parameters, ns + "enable_for_prepare_phase.intersection", + p->safety.collision_check.enable_for_prepare_phase_in_intersection); + updateParam( + parameters, ns + "enable_for_prepare_phase.turns", + p->safety.collision_check.enable_for_prepare_phase_in_turns); + updateParam( + parameters, ns + "check_current_lanes", p->safety.collision_check.check_current_lane); + updateParam( + parameters, ns + "check_other_lanes", p->safety.collision_check.check_other_lanes); + updateParam( + parameters, ns + "use_all_predicted_paths", + p->safety.collision_check.use_all_predicted_paths); + updateParam( + parameters, ns + "prediction_time_resolution", + p->safety.collision_check.prediction_time_resolution); + updateParam( + parameters, ns + "yaw_diff_threshold", p->safety.collision_check.th_yaw_diff); + + auto th_incoming_object_yaw = p->safety.collision_check.th_incoming_object_yaw; + updateParam(parameters, ns + "th_incoming_object_yaw", th_incoming_object_yaw); + if (th_incoming_object_yaw >= M_PI_2) { + p->safety.collision_check.th_incoming_object_yaw = th_incoming_object_yaw; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, + "The value of th_incoming_object_yaw (%.3f rad) is less than the minimum possible value " + "(%.3f " + "rad).", + th_incoming_object_yaw, M_PI_2); + } + } + { const std::string ns = "lane_change.target_object."; updateParam(parameters, ns + "car", p->safety.target_object_types.check_car); @@ -378,23 +500,96 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "stop_time", p->th_stop_time); } + auto update_rss_params = [¶meters, this](const std::string & prefix, auto & params) { + using autoware::universe_utils::updateParam; + updateParam( + parameters, prefix + "longitudinal_distance_min_threshold", + params.longitudinal_distance_min_threshold); + updateParam( + parameters, prefix + "longitudinal_velocity_delta_time", + params.longitudinal_velocity_delta_time); + updateParam( + parameters, prefix + "expected_front_deceleration", params.front_vehicle_deceleration); + updateParam( + parameters, prefix + "expected_rear_deceleration", params.rear_vehicle_deceleration); + updateParam( + parameters, prefix + "rear_vehicle_reaction_time", params.rear_vehicle_reaction_time); + updateParam( + parameters, prefix + "rear_vehicle_safety_time_margin", + params.rear_vehicle_safety_time_margin); + updateParam( + parameters, prefix + "lateral_distance_max_threshold", params.lateral_distance_max_threshold); + + auto extended_polygon_policy = params.extended_polygon_policy; + updateParam( + parameters, prefix + "extended_polygon_policy", extended_polygon_policy); + if (extended_polygon_policy == "rectangle" || extended_polygon_policy == "along_path") { + params.extended_polygon_policy = extended_polygon_policy; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "Policy %s not supported or there's typo. Make sure you choose either 'rectangle' or " + "'along_path'", + extended_polygon_policy.c_str()); + } + }; + + update_rss_params("lane_change.safety_check.execution.", p->safety.rss_params); + update_rss_params("lane_change.safety_check.parked.", p->safety.rss_params_for_parked); + update_rss_params("lane_change.safety_check.cancel.", p->safety.rss_params_for_abort); + update_rss_params("lane_change.safety_check.stuck.", p->safety.rss_params_for_stuck); + + { + const std::string ns = "lane_change.delay_lane_change."; + updateParam(parameters, ns + "enable", p->delay.enable); + updateParam( + parameters, ns + "check_only_parked_vehicle", p->delay.check_only_parked_vehicle); + updateParam( + parameters, ns + "min_road_shoulder_width", p->delay.min_road_shoulder_width); + updateParam( + parameters, ns + "th_parked_vehicle_shift_ratio", p->delay.th_parked_vehicle_shift_ratio); + } + + { + const std::string ns = "lane_change.terminal_path."; + updateParam(parameters, ns + "enable", p->terminal_path.enable); + updateParam(parameters, ns + "disable_near_goal", p->terminal_path.disable_near_goal); + updateParam(parameters, ns + "stop_at_boundary", p->terminal_path.stop_at_boundary); + } + { const std::string ns = "lane_change.cancel."; - bool enable_on_prepare_phase = true; + bool enable_on_prepare_phase = p->cancel.enable_on_prepare_phase; updateParam(parameters, ns + "enable_on_prepare_phase", enable_on_prepare_phase); if (!enable_on_prepare_phase) { - RCLCPP_WARN_ONCE(node_->get_logger(), "WARNING! Lane Change cancel function is disabled."); + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Lane Change cancel function is disabled."); p->cancel.enable_on_prepare_phase = enable_on_prepare_phase; } - bool enable_on_lane_changing_phase = true; + bool enable_on_lane_changing_phase = p->cancel.enable_on_lane_changing_phase; updateParam( parameters, ns + "enable_on_lane_changing_phase", enable_on_lane_changing_phase); if (!enable_on_lane_changing_phase) { - RCLCPP_WARN_ONCE(node_->get_logger(), "WARNING! Lane Change abort function is disabled."); + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Lane Change abort function is disabled."); p->cancel.enable_on_lane_changing_phase = enable_on_lane_changing_phase; } + int deceleration_sampling_num = p->cancel.deceleration_sampling_num; + updateParam(parameters, ns + "deceleration_sampling_num", deceleration_sampling_num); + if (deceleration_sampling_num > 0) { + p->cancel.deceleration_sampling_num = deceleration_sampling_num; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "Parameter 'deceleration_sampling_num' is not updated because the value (%d) is not " + "positive", + deceleration_sampling_num); + } + updateParam(parameters, ns + "delta_time", p->cancel.delta_time); updateParam(parameters, ns + "duration", p->cancel.duration); updateParam(parameters, ns + "max_lateral_jerk", p->cancel.max_lateral_jerk); @@ -402,6 +597,7 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector( parameters, ns + "unsafe_hysteresis_threshold", p->cancel.th_unsafe_hysteresis); } + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { if (!observer.expired()) observer.lock()->updateModuleParams(p); }); 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 7f0e6d7f575f6..611a0b60a4366 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 @@ -28,7 +28,10 @@ #include #include #include +#include #include +#include +#include #include #include #include @@ -36,13 +39,17 @@ #include #include +#include + #include #include +#include #include #include #include #include +#include #include #include #include @@ -103,6 +110,9 @@ void NormalLaneChange::update_lanes(const bool is_approved) common_data_ptr_->current_lanes_path = route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + common_data_ptr_->target_lanes_path = + route_handler_ptr->getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); + common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::get_target_neighbor_lanes( *route_handler_ptr, current_lanes, common_data_ptr_->lc_type); @@ -169,6 +179,9 @@ void NormalLaneChange::update_transient_data(const bool is_approved) transient_data.dist_to_terminal_start = transient_data.dist_to_terminal_end - transient_data.current_dist_buffer.min; + transient_data.dist_to_target_end = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->target, common_data_ptr_->get_ego_pose()); + transient_data.max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); transient_data.target_lane_length = @@ -228,8 +241,6 @@ void NormalLaneChange::updateLaneChangeStatus() // Update status status_.is_valid_path = found_valid_path; status_.is_safe = found_safe_path; - - status_.start_distance = common_data_ptr_->transient_data.target_lanes_ego_arc.length; status_.lane_change_path.path.header = getRouteHeader(); } @@ -246,6 +257,10 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) bool found_safe_path = get_lane_change_paths(valid_paths); // if no safe path is found and ego is stuck, try to find a path with a small margin + if (valid_paths.empty() && terminal_lane_change_path_) { + valid_paths.push_back(terminal_lane_change_path_.value()); + } + lane_change_debug_.valid_paths = valid_paths; if (valid_paths.empty()) { @@ -380,6 +395,45 @@ LaneChangePath NormalLaneChange::getLaneChangePath() const return status_.lane_change_path; } +BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const +{ + if ( + !lane_change_parameters_->terminal_path.enable || + !common_data_ptr_->transient_data.is_ego_near_current_terminal_start) { + return prev_module_output_; + } + + const auto is_near_goal = lane_change_parameters_->terminal_path.disable_near_goal && + common_data_ptr_->lanes_ptr->target_lane_in_goal_section && + common_data_ptr_->transient_data.dist_to_target_end < + common_data_ptr_->transient_data.lane_changing_length.max; + if (is_near_goal) { + return prev_module_output_; + } + + const auto & current_lanes = get_current_lanes(); + 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_lc_path = compute_terminal_lane_change_path(); + + if (!terminal_lc_path) { + RCLCPP_DEBUG(logger_, "Terminal path not found. Returning previous module's path as output."); + return prev_module_output_; + } + + auto output = prev_module_output_; + + output.path = *terminal_lc_path; + output.turn_signal_info = get_current_turn_signal_info(); + + extendOutputDrivableArea(output); + + return output; +} + BehaviorModuleOutput NormalLaneChange::generateOutput() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -450,7 +504,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) c } void NormalLaneChange::insert_stop_point( - const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) + const lanelet::ConstLanelets & lanelets, PathWithLaneId & path, const bool is_waiting_approval) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (lanelets.empty()) { @@ -475,56 +529,58 @@ void NormalLaneChange::insert_stop_point( return; } - insert_stop_point_on_current_lanes(path); + insert_stop_point_on_current_lanes(path, is_waiting_approval); } -void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) +void NormalLaneChange::insert_stop_point_on_current_lanes( + PathWithLaneId & path, const bool is_waiting_approval) { const auto & path_front_pose = path.points.front().point.pose; - const auto & center_line = common_data_ptr_->current_lanes_path.points; - const auto get_arc_length_along_lanelet = [&](const geometry_msgs::msg::Pose & target) { - return motion_utils::calcSignedArcLength( - center_line, path_front_pose.position, target.position); - }; + const auto & ego_pose = common_data_ptr_->get_ego_pose(); + const auto dist_from_path_front = + motion_utils::calcSignedArcLength(path.points, path_front_pose.position, ego_pose.position); const auto & transient_data = common_data_ptr_->transient_data; const auto & lanes_ptr = common_data_ptr_->lanes_ptr; const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; - const auto dist_to_terminal = std::invoke([&]() -> double { - const auto target_pose = (lanes_ptr->current_lane_in_goal_section) - ? common_data_ptr_->route_handler_ptr->getGoalPose() - : center_line.back().point.pose; - return get_arc_length_along_lanelet(target_pose); - }); - - const auto & bpp_param_ptr = common_data_ptr_->bpp_param_ptr; - const auto min_dist_buffer = transient_data.current_dist_buffer.min; const auto dist_to_terminal_start = - dist_to_terminal - min_dist_buffer - calculation::calc_stopping_distance(lc_param_ptr); + transient_data.dist_to_terminal_start - calculation::calc_stopping_distance(lc_param_ptr); - const auto distance_to_last_fit_width = std::invoke([&]() -> double { + const auto & bpp_param_ptr = common_data_ptr_->bpp_param_ptr; + const auto dist_to_terminal_stop = std::invoke([&]() -> double { const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current; - if (utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), *bpp_param_ptr)) { - return utils::lane_change::calculation::calc_dist_to_last_fit_width( - lanes_ptr->current, path.points.front().point.pose, *bpp_param_ptr); + if (!utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), *bpp_param_ptr)) { + return dist_from_path_front + dist_to_terminal_start; + } + + if ( + terminal_lane_change_path_ && is_waiting_approval && + lc_param_ptr->terminal_path.stop_at_boundary) { + return calculation::calc_dist_to_last_fit_width(common_data_ptr_, path); } - return std::numeric_limits::max(); - }); - const auto dist_to_terminal_stop = std::min(dist_to_terminal_start, distance_to_last_fit_width); + const auto dist_to_last_fit_width = calculation::calc_dist_to_last_fit_width( + lanes_ptr->current, path.points.front().point.pose, *bpp_param_ptr); + + return std::min(dist_from_path_front + dist_to_terminal_start, dist_to_last_fit_width); + }); const auto terminal_stop_reason = status_.is_valid_path ? "no safe path" : "no valid path"; - if (filtered_objects_.current_lane.empty()) { + if ( + filtered_objects_.current_lane.empty() || + !lane_change_parameters_->enable_stopped_vehicle_buffer) { set_stop_pose(dist_to_terminal_stop, path, terminal_stop_reason); return; } + const auto & center_line = common_data_ptr_->current_lanes_path.points; const auto dist_to_target_lane_start = std::invoke([&]() -> double { const auto & front_lane = lanes_ptr->target_neighbor.front(); const auto target_front = utils::to_geom_msg_pose(front_lane.centerline2d().front(), front_lane); - return get_arc_length_along_lanelet(target_front); + return motion_utils::calcSignedArcLength( + center_line, path_front_pose.position, target_front.position); }); const auto arc_length_to_current_obj = utils::lane_change::get_min_dist_to_current_lanes_obj( @@ -843,7 +899,6 @@ bool NormalLaneChange::isAbleToStopSafely() const bool NormalLaneChange::hasFinishedAbort() const { if (!abort_path_) { - lane_change_debug_.is_abort = true; return true; } @@ -852,7 +907,6 @@ bool NormalLaneChange::hasFinishedAbort() const lane_change_debug_.distance_to_abort_finished = distance_to_finish; const auto has_finished_abort = distance_to_finish < 0.0; - lane_change_debug_.is_abort = has_finished_abort; return has_finished_abort; } @@ -867,12 +921,7 @@ bool NormalLaneChange::isAbortState() const return false; } - if (!abort_path_) { - return false; - } - - lane_change_debug_.is_abort = true; - return true; + return abort_path_ != nullptr; } int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) const @@ -948,8 +997,6 @@ FilteredLanesObjects NormalLaneChange::filter_objects() const filtered_objects.target_lane_trailing.reserve(reserve_size); filtered_objects.others.reserve(reserve_size); - const auto stopped_obj_vel_th = lane_change_parameters_->safety.th_stopped_object_velocity; - for (const auto & object : objects.objects) { auto ext_object = utils::lane_change::transform(object, *lane_change_parameters_); const auto & ext_obj_pose = ext_object.initial_pose; @@ -968,12 +1015,8 @@ FilteredLanesObjects NormalLaneChange::filter_objects() const continue; } - // TODO(Azu): We have to think about how to remove is_within_vel_th without breaking AW behavior - const auto is_moving = velocity_filter( - ext_object.initial_twist, stopped_obj_vel_th, std::numeric_limits::max()); - if ( - ahead_of_ego && is_moving && is_before_terminal && + ahead_of_ego && is_before_terminal && !boost::geometry::disjoint(ext_object.initial_polygon, lanes_polygon.current)) { // check only the objects that are in front of the ego vehicle filtered_objects.current_lane.push_back(ext_object); @@ -1008,7 +1051,9 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const const auto is_same_direction = [&](const PredictedObject & object) { const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - return !utils::path_safety_checker::isTargetObjectOncoming(current_pose, object_pose); + return !utils::path_safety_checker::isTargetObjectOncoming( + current_pose, object_pose, + common_data_ptr_->lc_param_ptr->safety.collision_check.th_incoming_object_yaw); }; // Perception noise could make stationary objects seem opposite the ego vehicle; check the @@ -1047,7 +1092,8 @@ std::vector NormalLaneChange::get_prepare_metrics() cons std::vector NormalLaneChange::get_lane_changing_metrics( const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metric, - const double shift_length, const double dist_to_reg_element) const + const double shift_length, const double dist_to_reg_element, + lane_change::MetricsDebug & debug_metrics) const { const auto & transient_data = common_data_ptr_->transient_data; const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( @@ -1064,6 +1110,7 @@ std::vector NormalLaneChange::get_lane_changing_metrics( return max_length; }); + debug_metrics.max_lane_changing_length = max_lane_changing_length; const auto max_path_velocity = prep_segment.points.back().point.longitudinal_velocity_mps; return calculation::calc_shift_phase_metrics( common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, @@ -1073,6 +1120,7 @@ std::vector NormalLaneChange::get_lane_changing_metrics( bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const { lane_change_debug_.collision_check_objects.clear(); + lane_change_debug_.lane_change_metrics.clear(); if (!common_data_ptr_->is_lanes_available()) { RCLCPP_WARN(logger_, "lanes are not available. Not expected."); @@ -1085,15 +1133,97 @@ 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 sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); const auto target_objects = get_target_objects(filtered_objects_, current_lanes); const auto prepare_phase_metrics = get_prepare_metrics(); + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); + if ( + common_data_ptr_->lc_param_ptr->frenet.enable && + common_data_ptr_->transient_data.is_ego_near_current_terminal_start) { + return get_path_using_frenet( + prepare_phase_metrics, target_objects, sorted_lane_ids, candidate_paths); + } + + return get_path_using_path_shifter( + prepare_phase_metrics, target_objects, sorted_lane_ids, candidate_paths); +} + +bool NormalLaneChange::get_path_using_frenet( + const std::vector & prepare_metrics, + const lane_change::TargetObjects & target_objects, + const std::vector> & sorted_lane_ids, + LaneChangePaths & candidate_paths) const +{ + stop_watch_.tic(__func__); + constexpr auto found_safe_path = true; + const auto frenet_candidates = utils::lane_change::generate_frenet_candidates( + common_data_ptr_, prev_module_output_.path, prepare_metrics); + RCLCPP_DEBUG( + logger_, "Generated %lu candidate paths in %2.2f[us]", frenet_candidates.size(), + stop_watch_.toc(__func__)); + + candidate_paths.reserve(frenet_candidates.size()); + lane_change_debug_.frenet_states.clear(); + lane_change_debug_.frenet_states.reserve(frenet_candidates.size()); + for (const auto & frenet_candidate : frenet_candidates) { + if (stop_watch_.toc(__func__) >= lane_change_parameters_->time_limit) { + break; + } + + lane_change_debug_.frenet_states.emplace_back( + frenet_candidate.prepare_metric, frenet_candidate.lane_changing.sampling_parameter, + frenet_candidate.max_lane_changing_length); + + std::optional candidate_path_opt; + try { + candidate_path_opt = + utils::lane_change::get_candidate_path(frenet_candidate, common_data_ptr_, sorted_lane_ids); + } catch (const std::exception & e) { + RCLCPP_DEBUG(logger_, "%s", e.what()); + } + + if (!candidate_path_opt) { + continue; + } + + try { + if (check_candidate_path_safety(*candidate_path_opt, target_objects)) { + RCLCPP_DEBUG( + logger_, "Found safe path after %lu candidate(s). Total time: %2.2f[us]", + frenet_candidates.size(), stop_watch_.toc("__func__")); + utils::lane_change::append_target_ref_to_candidate( + *candidate_path_opt, common_data_ptr_->lc_param_ptr->frenet.th_curvature_smoothing); + candidate_paths.push_back(*candidate_path_opt); + return found_safe_path; + } + } catch (const std::exception & e) { + RCLCPP_DEBUG(logger_, "%s", e.what()); + } + + // appending all paths affect performance + if (candidate_paths.empty()) { + candidate_paths.push_back(*candidate_path_opt); + } + } + + RCLCPP_DEBUG( + logger_, "No safe path after %lu candidate(s). Total time: %2.2f[us]", frenet_candidates.size(), + stop_watch_.toc(__func__)); + return !found_safe_path; +} + +bool NormalLaneChange::get_path_using_path_shifter( + const std::vector & prepare_metrics, + const lane_change::TargetObjects & target_objects, + const std::vector> & sorted_lane_ids, + LaneChangePaths & candidate_paths) const +{ + stop_watch_.tic(__func__); + const auto & target_lanes = get_target_lanes(); candidate_paths.reserve( - prepare_phase_metrics.size() * lane_change_parameters_->trajectory.lat_acc_sampling_num); + prepare_metrics.size() * lane_change_parameters_->trajectory.lat_acc_sampling_num); const bool only_tl = getStopTime() >= lane_change_parameters_->th_stop_time; const auto dist_to_next_regulatory_element = @@ -1112,7 +1242,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) return lc_diff > lane_change_parameters_->trajectory.th_lane_changing_length_diff; }; - for (const auto & prep_metric : prepare_phase_metrics) { + for (const auto & prep_metric : prepare_metrics) { const auto debug_print = [&](const std::string & s) { RCLCPP_DEBUG( logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s.c_str(), @@ -1127,7 +1257,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) PathWithLaneId prepare_segment; try { if (!utils::lane_change::get_prepare_segment( - common_data_ptr_, prev_module_output_.path, prep_metric, prepare_segment)) { + common_data_ptr_, prev_module_output_.path, prep_metric.length, prepare_segment)) { debug_print("Reject: failed to get valid prepare segment!"); continue; } @@ -1143,8 +1273,12 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); + lane_change_debug_.lane_change_metrics.emplace_back(); + auto & debug_metrics = lane_change_debug_.lane_change_metrics.back(); + debug_metrics.prep_metric = prep_metric; + debug_metrics.max_prepare_length = common_data_ptr_->transient_data.dist_to_terminal_start; const auto lane_changing_metrics = get_lane_changing_metrics( - prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element); + prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element, debug_metrics); // 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. @@ -1152,6 +1286,13 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) prepare_segment, common_data_ptr_->get_ego_speed(), prep_metric.velocity); for (const auto & lc_metric : lane_changing_metrics) { + if (stop_watch_.toc(__func__) >= lane_change_parameters_->time_limit) { + RCLCPP_DEBUG(logger_, "Time limit reached and no safe path was found."); + return false; + } + + debug_metrics.lc_metrics.emplace_back(lc_metric, -1); + const auto debug_print_lat = [&](const std::string & s) { RCLCPP_DEBUG( logger_, "%s | lc_time: %.5f | lon_acc: %.5f | lat_acc: %.5f | lc_len: %.5f", s.c_str(), @@ -1173,6 +1314,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) } candidate_paths.push_back(candidate_path); + debug_metrics.lc_metrics.back().second = static_cast(candidate_paths.size()) - 1; try { if (check_candidate_path_safety(candidate_path, target_objects)) { @@ -1245,6 +1387,91 @@ bool NormalLaneChange::check_candidate_path_safety( return safety_check_with_normal_rss.is_safe; } +std::optional NormalLaneChange::compute_terminal_lane_change_path() const +{ + const auto & transient_data = common_data_ptr_->transient_data; + const auto dist_to_terminal_start = transient_data.dist_to_terminal_start; + const auto min_lc_velocity = lane_change_parameters_->trajectory.min_lane_changing_velocity; + const auto current_velocity = getEgoVelocity(); + + PathWithLaneId prepare_segment; + try { + if (!utils::lane_change::get_prepare_segment( + common_data_ptr_, prev_module_output_.path, dist_to_terminal_start, prepare_segment)) { + RCLCPP_DEBUG(logger_, "failed to get valid prepare segment for terminal LC path"); + return std::nullopt; + } + } catch (const std::exception & e) { + RCLCPP_DEBUG(logger_, "failed to get terminal LC path: %s", e.what()); + return std::nullopt; + } + + // t = 2 * d / (v1 + v2) + const auto duration_to_lc_start = + 2.0 * dist_to_terminal_start / (current_velocity + min_lc_velocity); + const auto lon_accel = std::invoke([&]() -> double { + if (duration_to_lc_start < calculation::eps) { + return 0.0; + } + return std::clamp( + (min_lc_velocity - current_velocity) / duration_to_lc_start, + lane_change_parameters_->trajectory.min_longitudinal_acc, + lane_change_parameters_->trajectory.max_longitudinal_acc); + }); + const auto vel_on_prep = current_velocity + lon_accel * duration_to_lc_start; + const LaneChangePhaseMetrics prep_metric( + duration_to_lc_start, dist_to_terminal_start, vel_on_prep, lon_accel, lon_accel, 0.0); + + if (terminal_lane_change_path_) { + terminal_lane_change_path_->info.set_prepare(prep_metric); + if (prepare_segment.points.empty()) { + terminal_lane_change_path_->path = terminal_lane_change_path_->shifted_path.path; + return terminal_lane_change_path_->path; + } + prepare_segment.points.pop_back(); + terminal_lane_change_path_->path = + utils::combinePath(prepare_segment, terminal_lane_change_path_->shifted_path.path); + return terminal_lane_change_path_->path; + } + + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto shift_length = + lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); + + const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, + prepare_segment.points.back().point.pose); + + const auto max_lane_changing_length = std::invoke([&]() { + double max_length = transient_data.dist_to_terminal_end - prep_metric.length; + return std::min( + max_length, dist_lc_start_to_end_of_lanes - transient_data.next_dist_buffer.min); + }); + + const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; + constexpr double lane_changing_lon_accel{0.0}; + const auto lane_changing_metrics = calculation::calc_shift_phase_metrics( + common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, + lane_changing_lon_accel, max_lane_changing_length); + + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); + + LaneChangePath candidate_path; + for (const auto & lc_metric : lane_changing_metrics) { + try { + candidate_path = utils::lane_change::get_candidate_path( + common_data_ptr_, prep_metric, lc_metric, prepare_segment, sorted_lane_ids, shift_length); + } catch (const std::exception & e) { + continue; + } + terminal_lane_change_path_ = candidate_path; + return candidate_path.path; + } + + return std::nullopt; +} + PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { const auto & path = status_.lane_change_path; @@ -1357,14 +1584,8 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const bool NormalLaneChange::isRequiredStop(const bool is_trailing_object) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if ( - common_data_ptr_->transient_data.is_ego_near_current_terminal_start && isAbleToStopSafely() && - is_trailing_object) { - current_lane_change_state_ = LaneChangeStates::Stop; - return true; - } - current_lane_change_state_ = LaneChangeStates::Normal; - return false; + return common_data_ptr_->transient_data.is_ego_near_current_terminal_start && + isAbleToStopSafely() && is_trailing_object; } bool NormalLaneChange::calcAbortPath() @@ -1573,7 +1794,6 @@ bool NormalLaneChange::is_colliding( constexpr auto is_safe{true}; auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj); - constexpr auto collision_check_yaw_diff_threshold{M_PI}; constexpr auto hysteresis_factor{1.0}; const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->safety.collision_check.use_all_predicted_paths); @@ -1598,7 +1818,8 @@ bool NormalLaneChange::is_colliding( const auto collided_polygons = utils::path_safety_checker::get_collided_polygons( lane_change_path.path, ego_predicted_path, obj, predicted_obj_path, bpp_param.vehicle_info, selected_rss_param, hysteresis_factor, safety_check_max_vel, - collision_check_yaw_diff_threshold, current_debug_data.second); + common_data_ptr_->lc_param_ptr->safety.collision_check.th_yaw_diff, + current_debug_data.second); if (collided_polygons.empty()) { utils::path_safety_checker::updateCollisionCheckDebugMap( 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 550c0fa290a99..a0130fcd27041 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -58,22 +58,10 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr) } double calc_dist_to_last_fit_width( - const lanelet::ConstLanelets & lanelets, const Pose & src_pose, + const lanelet::ConstLanelets & lanelets, const lanelet::BasicPolygon2d & lanelet_polygon, + const universe_utils::LineString2d & line_string, const Pose & src_pose, const BehaviorPathPlannerParameters & bpp_param, const double margin) { - if (lanelets.empty()) return 0.0; - - const auto lane_polygon = lanelets.back().polygon2d().basicPolygon(); - const auto center_line = lanelet::utils::generateFineCenterline(lanelets.back(), 1.0); - - if (center_line.size() <= 1) return 0.0; - - universe_utils::LineString2d line_string; - line_string.reserve(center_line.size() - 1); - std::for_each(center_line.begin() + 1, center_line.end(), [&line_string](const auto & point) { - boost::geometry::append(line_string, universe_utils::Point2d(point.x(), point.y())); - }); - const double buffer_distance = 0.5 * bpp_param.vehicle_width + margin; universe_utils::MultiPolygon2d center_line_polygon; namespace strategy = boost::geometry::strategy::buffer; @@ -85,7 +73,7 @@ double calc_dist_to_last_fit_width( if (center_line_polygon.empty()) return 0.0; std::vector intersection_points; - boost::geometry::intersection(lane_polygon, center_line_polygon, intersection_points); + boost::geometry::intersection(lanelet_polygon, center_line_polygon, intersection_points); if (intersection_points.empty()) { return utils::getDistanceToEndOfLane(src_pose, lanelets); @@ -102,6 +90,46 @@ double calc_dist_to_last_fit_width( return std::max(distance - (bpp_param.base_link2front + margin), 0.0); } +double calc_dist_to_last_fit_width( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const double margin) +{ + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & current_lanes_polygon = common_data_ptr->lanes_polygon_ptr->current; + const auto & bpp_param = *common_data_ptr->bpp_param_ptr; + + universe_utils::LineString2d line_string; + line_string.reserve(path.points.size() - 1); + std::for_each(path.points.begin() + 1, path.points.end(), [&line_string](const auto & point) { + const auto & position = point.point.pose.position; + boost::geometry::append(line_string, universe_utils::Point2d(position.x, position.y)); + }); + + const auto & src_pose = path.points.front().point.pose; + return calc_dist_to_last_fit_width( + current_lanes, current_lanes_polygon, line_string, src_pose, bpp_param, margin); +} + +double calc_dist_to_last_fit_width( + const lanelet::ConstLanelets & lanelets, const Pose & src_pose, + const BehaviorPathPlannerParameters & bpp_param, const double margin) +{ + if (lanelets.empty()) return 0.0; + + const auto lane_polygon = lanelets.back().polygon2d().basicPolygon(); + const auto center_line = lanelet::utils::generateFineCenterline(lanelets.back(), 1.0); + + if (center_line.size() <= 1) return 0.0; + + universe_utils::LineString2d line_string; + line_string.reserve(center_line.size() - 1); + std::for_each(center_line.begin() + 1, center_line.end(), [&line_string](const auto & point) { + boost::geometry::append(line_string, universe_utils::Point2d(point.x(), point.y())); + }); + + return calc_dist_to_last_fit_width( + lanelets, lane_polygon, line_string, src_pose, bpp_param, margin); +} + double calc_maximum_prepare_length(const CommonDataPtr & common_data_ptr) { const auto max_prepare_duration = common_data_ptr->lc_param_ptr->trajectory.max_prepare_duration; 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 30af582175d14..bf0af0d133dc7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp @@ -16,8 +16,10 @@ #include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" #include +#include #include #include +#include #include #include @@ -89,8 +91,10 @@ MarkerArray showAllValidLaneChangePath( 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: " + "type: {type} | 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("type", magic_enum::enum_name(lc_path.type)), 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), @@ -163,7 +167,9 @@ MarkerArray showFilteredObjects( return marker_array; } -MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose) +MarkerArray showExecutionInfo( + const InterfaceDebug & interface_debug_data, const Debug & scene_debug_data, + const geometry_msgs::msg::Pose & ego_pose) { auto default_text_marker = [&]() { return createDefaultMarker( @@ -176,17 +182,101 @@ MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg auto safety_check_info_text = default_text_marker(); safety_check_info_text.pose = ego_pose; safety_check_info_text.pose.position.z += 4.0; + const auto lc_state = interface_debug_data.lc_state; + const auto & failing_reason = interface_debug_data.failing_reason; 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" : "")); + "{stuck} | {return_lane} | {state} : {reason}", + fmt::arg("stuck", scene_debug_data.is_stuck ? "is stuck" : ""), + fmt::arg( + "return_lane", scene_debug_data.is_able_to_return_to_current_lane ? "" : "can't return"), + fmt::arg("state", magic_enum::enum_name(lc_state)), fmt::arg("reason", failing_reason)); marker_array.markers.push_back(safety_check_info_text); return marker_array; } +MarkerArray ShowLaneChangeMetricsInfo( + const Debug & debug_data, const geometry_msgs::msg::Pose & pose) +{ + MarkerArray marker_array; + + auto text_marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "sampling_metrics", 0, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.6, 0.6, 0.6), colors::yellow()); + text_marker.pose = autoware::universe_utils::calcOffsetPose(pose, 10.0, 15.0, 0.0); + + if (!debug_data.lane_change_metrics.empty()) { + text_marker.text = + fmt::format("{:<12}", "") + fmt::format("{:^18}|", "lat_accel[m/s2]") + + fmt::format("{:^18}|", "lon_accel[m/s2]") + fmt::format("{:^17}|", "velocity[m/s]") + + fmt::format("{:^15}|", "duration[s]") + fmt::format("{:^15}|", "length[m]") + + fmt::format("{:^20}|", "max_length_th[m]") + fmt::format("{:^15}\n", "path_index"); + for (const auto & metrics : debug_data.lane_change_metrics) { + text_marker.text += fmt::format("{:-<190}\n", ""); + const auto & p_m = metrics.prep_metric; + text_marker.text += + fmt::format("{:<17}", "prep_metrics:") + fmt::format("{:^10.3f}", p_m.lat_accel) + + fmt::format("{:^21.3f}", p_m.actual_lon_accel) + fmt::format("{:^12.3f}", p_m.velocity) + + fmt::format("{:^15.3f}", p_m.duration) + fmt::format("{:^15.3f}", p_m.length) + + fmt::format("{:^17.3f}", metrics.max_prepare_length) + fmt::format("{:^15}\n", "-"); + text_marker.text += fmt::format("{:<20}\n", "lc_metrics:"); + for (const auto & lc_m : metrics.lc_metrics) { + const auto & metric = lc_m.first; + const auto path_index = lc_m.second < 0 ? "-" : std::to_string(lc_m.second); + text_marker.text += fmt::format("{:<15}", "") + fmt::format("{:^10.3f}", metric.lat_accel) + + fmt::format("{:^21.3f}", metric.actual_lon_accel) + + fmt::format("{:^12.3f}", metric.velocity) + + fmt::format("{:^15.3f}", metric.duration) + + fmt::format("{:^15.3f}", metric.length) + + fmt::format("{:^17.3f}", metrics.max_lane_changing_length) + + fmt::format("{:^15}\n", path_index); + } + } + marker_array.markers.push_back(text_marker); + } + + if (!debug_data.frenet_states.empty()) { + text_marker.text = fmt::format("{:<12}", "") + fmt::format("{:^18}|", "lon_accel[m/s2]") + + fmt::format("{:^17}|", "lon_vel[m/s]") + + fmt::format("{:^15}|", "duration[s]") + fmt::format("{:^15}|", "length[m]") + + fmt::format("{:^17}|", "lat_accel[m/s2]") + + fmt::format("{:^15}|", "lat_vel[m/s2]") + fmt::format("{:^15}|", "s[m]") + + fmt::format("{:^15}|", "d[m]") + fmt::format("{:^20}\n", "max_length_th[m]"); + for (const auto & metrics : debug_data.frenet_states) { + text_marker.text += fmt::format("{:-<250}\n", ""); + const auto & p_m = metrics.prep_metric; + const auto max_len = metrics.max_lane_changing_length; + text_marker.text += + fmt::format("{:<17}", "prep_metrics:") + fmt::format("{:^13.3f}", p_m.actual_lon_accel) + + fmt::format("{:^15.3f}", p_m.velocity) + fmt::format("{:^15.3f}", p_m.duration) + + fmt::format("{:^12.3f}", p_m.length) + + fmt::format("{:^13}", "") + // Empty string for lat_accel + fmt::format("{:^13}", "") + // Empty string for lat_vel + fmt::format("{:^15}", "") + // Empty string for s + fmt::format("{:^15}", "") + // Empty string for d // Empty string for d + fmt::format("{:^20.3f}\n", max_len); // Empty string for max_length_t + const auto & lc_m = metrics.sampling_parameter.target_state; // Assuming lc_metric exists + const auto duration = metrics.sampling_parameter.target_duration; + text_marker.text += + fmt::format("{:<17}", "frenet_state:") + + fmt::format("{:^15.3f}", lc_m.longitudinal_acceleration) + + fmt::format("{:^13.3f}", lc_m.longitudinal_velocity) + fmt::format("{:^17.3f}", duration) + + fmt::format("{:^10.3f}", lc_m.position.s) + + fmt::format("{:^19.3f}", lc_m.lateral_acceleration) + + fmt::format("{:^10.3f}", lc_m.lateral_velocity) + + fmt::format("{:^18.3f}", lc_m.position.s) + fmt::format("{:^15.3f}", lc_m.position.d) + + fmt::format("{:^16.3f}\n", max_len); // Empty string for max_length_t + } + + marker_array.markers.push_back(text_marker); + } + + return marker_array; +} + MarkerArray createDebugMarkerArray( - const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose) + const InterfaceDebug & interface_debug_data, const Debug & scene_debug_data, + const geometry_msgs::msg::Pose & ego_pose) { using lanelet::visualization::laneletsAsTriangleMarkerArray; using marker_utils::showPolygon; @@ -195,30 +285,32 @@ MarkerArray createDebugMarkerArray( using marker_utils::lane_change_markers::showAllValidLaneChangePath; using marker_utils::lane_change_markers::showFilteredObjects; - const auto & debug_collision_check_object = debug_data.collision_check_objects; + const auto & debug_collision_check_object = scene_debug_data.collision_check_objects; const auto & debug_collision_check_object_after_approval = - debug_data.collision_check_objects_after_approval; - const auto & debug_valid_paths = debug_data.valid_paths; - const auto & debug_filtered_objects = debug_data.filtered_objects; + scene_debug_data.collision_check_objects_after_approval; + const auto & debug_valid_paths = scene_debug_data.valid_paths; + const auto & debug_filtered_objects = scene_debug_data.filtered_objects; MarkerArray debug_marker; const auto add = [&debug_marker](const MarkerArray & added) { autoware::universe_utils::appendMarkerArray(added, &debug_marker); }; - if (!debug_data.execution_area.points.empty()) { + if (!scene_debug_data.execution_area.points.empty()) { add(createPolygonMarkerArray( - debug_data.execution_area, "execution_area", 0, 0.16, 1.0, 0.69, 0.1)); + scene_debug_data.execution_area, "execution_area", 0, 0.16, 1.0, 0.69, 0.1)); } - add(showExecutionInfo(debug_data, ego_pose)); + add(showExecutionInfo(interface_debug_data, scene_debug_data, ego_pose)); + add(ShowLaneChangeMetricsInfo(scene_debug_data, ego_pose)); // lanes add(laneletsAsTriangleMarkerArray( - "current_lanes", debug_data.current_lanes, colors::light_yellow(0.2))); - add(laneletsAsTriangleMarkerArray("target_lanes", debug_data.target_lanes, colors::aqua(0.2))); + "current_lanes", scene_debug_data.current_lanes, colors::light_yellow(0.2))); + add(laneletsAsTriangleMarkerArray( + "target_lanes", scene_debug_data.target_lanes, colors::aqua(0.2))); add(laneletsAsTriangleMarkerArray( - "target_backward_lanes", debug_data.target_backward_lanes, colors::blue(0.2))); + "target_backward_lanes", scene_debug_data.target_backward_lanes, colors::blue(0.2))); add(showAllValidLaneChangePath(debug_valid_paths, "lane_change_valid_paths")); add(showFilteredObjects(debug_filtered_objects, "object_filtered")); 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 index d7303d7d1df2d..df45da3f08fa1 100644 --- 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 @@ -15,17 +15,22 @@ #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/calculation.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 + #include #include #include @@ -42,8 +47,22 @@ using autoware::behavior_path_planner::lane_change::LCParamPtr; using autoware::behavior_path_planner::LaneChangePhaseMetrics; using autoware::behavior_path_planner::ShiftLine; +using autoware::behavior_path_planner::lane_change::TrajectoryGroup; +using autoware::frenet_planner::FrenetState; +using autoware::frenet_planner::SamplingParameters; +using autoware::route_handler::Direction; +using autoware::sampler_common::FrenetPoint; +using autoware::sampler_common::transform::Spline2D; +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; using geometry_msgs::msg::Pose; +template +T sign(const Direction direction) +{ + return static_cast(direction == Direction::LEFT ? 1.0 : -1.0); +} + double calc_resample_interval( const double lane_changing_length, const double lane_changing_velocity) { @@ -53,7 +72,7 @@ double calc_resample_interval( lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); } -PathWithLaneId get_reference_path_from_target_Lane( +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) { @@ -146,15 +165,201 @@ std::optional exceed_yaw_threshold( } return std::nullopt; } + +Spline2D init_reference_spline(const std::vector & target_lanes_ref_path) +{ + std::vector xs; + std::vector ys; + xs.reserve(target_lanes_ref_path.size()); + ys.reserve(target_lanes_ref_path.size()); + for (const auto & p : target_lanes_ref_path) { + xs.push_back(p.point.pose.position.x); + ys.push_back(p.point.pose.position.y); + } + + return {xs, ys}; +} + +FrenetState init_frenet_state( + const FrenetPoint & start_position, const LaneChangePhaseMetrics & prepare_metrics) +{ + FrenetState initial_state; + initial_state.position = start_position; + initial_state.longitudinal_velocity = prepare_metrics.velocity; + initial_state.lateral_velocity = + 0.0; // TODO(Maxime): this can be sampled if we want but it would impact the LC duration + initial_state.longitudinal_acceleration = prepare_metrics.sampled_lon_accel; + initial_state.lateral_acceleration = prepare_metrics.lat_accel; + return initial_state; +} + +std::optional init_sampling_parameters( + const CommonDataPtr & common_data_ptr, const LaneChangePhaseMetrics & prepare_metrics, + const FrenetState & initial_state, const Spline2D & ref_spline, const Pose & lc_start_pose) +{ + const auto & trajectory = common_data_ptr->lc_param_ptr->trajectory; + const auto min_lc_vel = trajectory.min_lane_changing_velocity; + const auto [min_lateral_acc, max_lateral_acc] = + trajectory.lat_acc_map.find(std::max(min_lc_vel, prepare_metrics.velocity)); + const auto duration = autoware::motion_utils::calc_shift_time_from_jerk( + std::abs(initial_state.position.d), trajectory.lateral_jerk, max_lateral_acc); + const auto final_velocity = + std::max(min_lc_vel, prepare_metrics.velocity + prepare_metrics.sampled_lon_accel * duration); + const auto lc_length = duration * (prepare_metrics.velocity + final_velocity) * 0.5; + const auto target_s = initial_state.position.s + lc_length; + const auto initial_yaw = tf2::getYaw(lc_start_pose.orientation); + const auto target_lat_vel = + (1.0 - ref_spline.curvature(target_s + 1e-3) * initial_state.position.d) * + std::tan(initial_yaw - ref_spline.yaw(target_s)); + + if (std::isnan(target_lat_vel)) { + return std::nullopt; + } + + SamplingParameters sampling_parameters; + const auto & safety = common_data_ptr->lc_param_ptr->safety; + sampling_parameters.resolution = safety.collision_check.prediction_time_resolution; + sampling_parameters.parameters.emplace_back(); + sampling_parameters.parameters.back().target_duration = duration; + sampling_parameters.parameters.back().target_state.position = {target_s, 0.0}; + // TODO(Maxime): not sure if we should use curvature at initial or target s + sampling_parameters.parameters.back().target_state.lateral_velocity = + sign(common_data_ptr->direction) * target_lat_vel; + sampling_parameters.parameters.back().target_state.lateral_acceleration = 0.0; + sampling_parameters.parameters.back().target_state.longitudinal_velocity = final_velocity; + sampling_parameters.parameters.back().target_state.longitudinal_acceleration = + prepare_metrics.sampled_lon_accel; + return sampling_parameters; +} + +std::vector calc_curvatures( + const std::vector & points, const Pose & current_pose) +{ + const auto nearest_segment_idx = + autoware::motion_utils::findNearestSegmentIndex(points, current_pose.position); + + // Ignore all path points behind ego vehicle. + if (points.size() <= nearest_segment_idx + 2) { + return {}; + } + + std::vector curvatures; + curvatures.reserve(points.size() - nearest_segment_idx + 2); + for (const auto & [p1, p2, p3] : ranges::views::zip( + points | ranges::views::drop(nearest_segment_idx), + points | ranges::views::drop(nearest_segment_idx + 1), + points | ranges::views::drop(nearest_segment_idx + 2))) { + const auto point1 = autoware::universe_utils::getPoint(p1); + const auto point2 = autoware::universe_utils::getPoint(p2); + const auto point3 = autoware::universe_utils::getPoint(p3); + + curvatures.push_back(autoware::universe_utils::calcCurvature(point1, point2, point3)); + } + + return curvatures; +} + +double calc_average_curvature(const std::vector & curvatures) +{ + const auto filter_zeros = [](const auto & k) { return k != 0.0; }; + auto filtered_k = curvatures | ranges::views::filter(filter_zeros); + if (filtered_k.empty()) { + return 0.0; + } + const auto sums_of_curvatures = [](float sum, const double k) { return sum + std::abs(k); }; + const auto sum_of_k = ranges::accumulate(filtered_k, 0.0, sums_of_curvatures); + const auto count_k = static_cast(ranges::distance(filtered_k)); + return sum_of_k / count_k; +} + +LineString2d get_linestring_bound(const lanelet::ConstLanelets & lanes, const Direction direction) +{ + LineString2d line_string; + const auto & get_bound = [&](const lanelet::ConstLanelet & lane) { + return (direction == Direction::LEFT) ? lane.leftBound2d() : lane.rightBound2d(); + }; + + const auto reserve_size = ranges::accumulate( + lanes, 0UL, + [&](auto sum, const lanelet::ConstLanelet & lane) { return sum + get_bound(lane).size(); }); + line_string.reserve(reserve_size); + for (const auto & lane : lanes) { + ranges::for_each(get_bound(lane), [&line_string](const auto & point) { + boost::geometry::append(line_string, Point2d(point.x(), point.y())); + }); + } + return line_string; +} + +Point2d shift_point(const Point2d & p1, const Point2d & p2, const double offset) +{ + // Calculate the perpendicular vector + double dx = p2.x() - p1.x(); + double dy = p2.y() - p1.y(); + double length = std::sqrt(dx * dx + dy * dy); + + // Normalize and find the perpendicular direction + double nx = -dy / length; + double ny = dx / length; + + return {p1.x() + nx * offset, p1.y() + ny * offset}; +} + +bool check_out_of_bound_paths( + const CommonDataPtr & common_data_ptr, const std::vector & lane_changing_poses, + const LineString2d & lane_boundary, const Direction direction) +{ + const auto distance = (0.5 * common_data_ptr->bpp_param_ptr->vehicle_width + 0.1); + const auto offset = sign(direction) * distance; // invert direction + if (lane_changing_poses.size() <= 2) { + return true; // Remove candidates with insufficient poses + } + + LineString2d path_ls; + path_ls.reserve(lane_changing_poses.size()); + + const auto segments = lane_changing_poses | ranges::views::sliding(2); + ranges::for_each(segments | ranges::views::drop(1), [&](const auto & segment) { + const auto & p1 = segment[0].position; + const auto & p2 = segment[1].position; + boost::geometry::append(path_ls, shift_point({p2.x, p2.y}, {p1.x, p1.y}, offset)); + }); + + return boost::geometry::disjoint(path_ls, lane_boundary); // Remove if disjoint +} + +double calc_limit(const CommonDataPtr & common_data_ptr, const Pose & lc_end_pose) +{ + const auto dist_to_target_end = std::invoke([&]() { + if (common_data_ptr->lanes_ptr->target_lane_in_goal_section) { + return autoware::motion_utils::calcSignedArcLength( + common_data_ptr->target_lanes_path.points, lc_end_pose.position, + common_data_ptr->route_handler_ptr->getGoalPose().position); + } + return autoware::motion_utils::calcSignedArcLength( + common_data_ptr->target_lanes_path.points, lc_end_pose.position, + common_data_ptr->target_lanes_path.points.back().point.pose.position); + }); + + // v2 = u2 + 2ad + // u = sqrt(2ad) + return std::clamp( + std::sqrt( + std::abs(2.0 * common_data_ptr->bpp_param_ptr->min_acc * std::max(dist_to_target_end, 0.0))), + common_data_ptr->lc_param_ptr->trajectory.min_lane_changing_velocity, + common_data_ptr->bpp_param_ptr->max_vel); +} + }; // namespace namespace autoware::behavior_path_planner::utils::lane_change { using behavior_path_planner::lane_change::CommonDataPtr; +using behavior_path_planner::lane_change::PathType; bool get_prepare_segment( const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, - const LaneChangePhaseMetrics prep_metric, PathWithLaneId & prepare_segment) + const double prep_length, PathWithLaneId & prepare_segment) { const auto & current_lanes = common_data_ptr->lanes_ptr->current; const auto & target_lanes = common_data_ptr->lanes_ptr->target; @@ -168,7 +373,7 @@ bool get_prepare_segment( 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); + utils::clipPathLength(prepare_segment, current_seg_idx, prep_length, backward_path_length); if (prepare_segment.points.empty()) return false; @@ -187,7 +392,17 @@ bool get_prepare_segment( throw std::logic_error("lane change start is behind target lanelet!"); } - return true; + const auto curvatures = calc_curvatures(prepare_segment.points, common_data_ptr->get_ego_pose()); + + // curvatures may be empty if ego is near the terminal start of the path. + if (curvatures.empty()) { + return true; + } + + const auto average_curvature = calc_average_curvature(curvatures); + + RCLCPP_DEBUG(get_logger(), "average curvature: %.3f", average_curvature); + return average_curvature <= common_data_ptr->lc_param_ptr->trajectory.th_prepare_curvature; } LaneChangePath get_candidate_path( @@ -205,7 +420,7 @@ LaneChangePath get_candidate_path( } const auto & lc_start_pose = prep_segment.points.back().point.pose; - const auto target_lane_reference_path = get_reference_path_from_target_Lane( + 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()) { @@ -259,10 +474,14 @@ LaneChangePath construct_candidate_path( throw std::logic_error("Lane change end idx not found on target path."); } + std::vector prev_ids; + std::vector prev_sorted_ids; 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); + const auto & current_ids = point.lane_ids; + point.lane_ids = + replace_with_sorted_ids(current_ids, sorted_lane_ids, prev_ids, prev_sorted_ids); point.point.longitudinal_velocity_mps = std::min( point.point.longitudinal_velocity_mps, static_cast(terminal_lane_changing_velocity)); continue; @@ -286,7 +505,251 @@ LaneChangePath construct_candidate_path( candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); candidate_path.shifted_path = shifted_path; candidate_path.info = lane_change_info; + candidate_path.type = PathType::ConstantJerk; return candidate_path; } + +std::vector generate_frenet_candidates( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, + const std::vector & prep_metrics) +{ + std::vector trajectory_groups; + universe_utils::StopWatch sw; + + const auto & transient_data = common_data_ptr->transient_data; + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto direction = common_data_ptr->direction; + const auto current_lane_boundary = get_linestring_bound(current_lanes, direction); + + for (const auto & metric : prep_metrics) { + PathWithLaneId prepare_segment; + try { + if (!utils::lane_change::get_prepare_segment( + common_data_ptr, prev_module_path, metric.length, prepare_segment)) { + RCLCPP_DEBUG(get_logger(), "Reject: failed to get valid prepare segment!"); + continue; + } + } catch (const std::exception & e) { + RCLCPP_WARN(get_logger(), "%s", e.what()); + break; + } + const auto lc_start_pose = prepare_segment.points.back().point.pose; + + const auto dist_to_end_from_lc_start = + calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr, target_lanes, lc_start_pose) - + common_data_ptr->lc_param_ptr->lane_change_finish_judge_buffer; + const auto max_lc_len = transient_data.lane_changing_length.max; + const auto max_lane_changing_length = std::min(dist_to_end_from_lc_start, max_lc_len); + + constexpr auto resample_interval = 0.5; + const auto target_lane_reference_path = get_reference_path_from_target_lane( + common_data_ptr, lc_start_pose, max_lane_changing_length, resample_interval); + if (target_lane_reference_path.points.empty()) { + continue; + } + + std::vector target_ref_path_sums{0.0}; + target_ref_path_sums.reserve(target_lane_reference_path.points.size() - 1); + double ref_s = 0.0; + for (auto it = target_lane_reference_path.points.begin(); + std::next(it) != target_lane_reference_path.points.end(); ++it) { + ref_s += universe_utils::calcDistance2d(*it, *std::next(it)); + target_ref_path_sums.push_back(ref_s); + } + + const auto reference_spline = init_reference_spline(target_lane_reference_path.points); + + const auto initial_state = init_frenet_state( + reference_spline.frenet({lc_start_pose.position.x, lc_start_pose.position.y}), metric); + + RCLCPP_DEBUG( + get_logger(), "Initial state [s=%2.2f, d=%2.2f, s'=%2.2f, d'=%2.2f, s''=%2.2f, d''=%2.2f]", + initial_state.position.s, initial_state.position.d, initial_state.longitudinal_velocity, + initial_state.lateral_velocity, initial_state.longitudinal_acceleration, + initial_state.lateral_acceleration); + + const auto sampling_parameters_opt = init_sampling_parameters( + common_data_ptr, metric, initial_state, reference_spline, lc_start_pose); + + if (!sampling_parameters_opt) { + continue; + } + + auto frenet_trajectories = frenet_planner::generateTrajectories( + reference_spline, initial_state, *sampling_parameters_opt); + + trajectory_groups.reserve(trajectory_groups.size() + frenet_trajectories.size()); + for (const auto & traj : frenet_trajectories) { + if (!trajectory_groups.empty()) { + const auto diff = std::abs( + traj.frenet_points.back().s - + trajectory_groups.back().lane_changing.frenet_points.back().s); + if (diff < common_data_ptr->lc_param_ptr->trajectory.th_lane_changing_length_diff) { + continue; + } + } + + const auto out_of_bound = + check_out_of_bound_paths(common_data_ptr, traj.poses, current_lane_boundary, direction); + + if (out_of_bound) { + continue; + } + + trajectory_groups.emplace_back( + prepare_segment, target_lane_reference_path, target_ref_path_sums, metric, traj, + initial_state, max_lane_changing_length); + } + } + + const auto limit_vel = [&](TrajectoryGroup & group) { + const auto max_vel = calc_limit(common_data_ptr, group.lane_changing.poses.back()); + for (auto & vel : group.lane_changing.longitudinal_velocities) { + vel = std::clamp(vel, 0.0, max_vel); + } + }; + + ranges::for_each(trajectory_groups, limit_vel); + + ranges::sort(trajectory_groups, [&](const auto & p1, const auto & p2) { + return calc_average_curvature(p1.lane_changing.curvatures) < + calc_average_curvature(p2.lane_changing.curvatures); + }); + + return trajectory_groups; +} + +std::optional get_candidate_path( + const TrajectoryGroup & trajectory_group, const CommonDataPtr & common_data_ptr, + const std::vector> & sorted_lane_ids) +{ + if (trajectory_group.lane_changing.frenet_points.empty()) { + return std::nullopt; + } + + ShiftedPath shifted_path; + std::vector prev_ids; + std::vector prev_sorted_ids; + const auto & lane_changing_candidate = trajectory_group.lane_changing; + const auto & target_lane_ref_path = trajectory_group.target_lane_ref_path; + const auto & prepare_segment = trajectory_group.prepare; + const auto & prepare_metric = trajectory_group.prepare_metric; + const auto & initial_state = trajectory_group.initial_state; + const auto & target_ref_sums = trajectory_group.target_lane_ref_path_dist; + auto zipped_candidates = ranges::views::zip( + lane_changing_candidate.poses, lane_changing_candidate.frenet_points, + lane_changing_candidate.longitudinal_velocities, lane_changing_candidate.lateral_velocities, + lane_changing_candidate.curvatures); + + shifted_path.path.points.reserve(zipped_candidates.size()); + shifted_path.shift_length.reserve(zipped_candidates.size()); + for (const auto & [pose, frenet_point, longitudinal_velocity, lateral_velocity, curvature] : + zipped_candidates) { + // Find the reference index + const auto & s = frenet_point.s; + auto ref_i_itr = std::find_if( + target_ref_sums.begin(), target_ref_sums.end(), + [s](const double ref_s) { return ref_s > s; }); + auto ref_i = std::distance(target_ref_sums.begin(), ref_i_itr); + + PathPointWithLaneId point; + point.point.pose = pose; + point.point.longitudinal_velocity_mps = static_cast(longitudinal_velocity); + point.point.lateral_velocity_mps = static_cast(lateral_velocity); + point.point.heading_rate_rps = static_cast(curvature); + point.point.pose.position.z = target_lane_ref_path.points[ref_i].point.pose.position.z; + const auto & current_ids = target_lane_ref_path.points[ref_i].lane_ids; + point.lane_ids = + replace_with_sorted_ids(current_ids, sorted_lane_ids, prev_ids, prev_sorted_ids); + + // Add to shifted path + shifted_path.shift_length.push_back(frenet_point.d); + shifted_path.path.points.push_back(point); + } + + if (shifted_path.path.points.empty()) { + return std::nullopt; + } + + for (auto & point : shifted_path.path.points) { + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, + static_cast(shifted_path.path.points.back().point.longitudinal_velocity_mps)); + } + + const auto th_yaw_diff_deg = common_data_ptr->lc_param_ptr->frenet.th_yaw_diff_deg; + if ( + const auto yaw_diff_opt = exceed_yaw_threshold( + prepare_segment, shifted_path.path, autoware::universe_utils::deg2rad(th_yaw_diff_deg))) { + const auto yaw_diff_deg = autoware::universe_utils::rad2deg(yaw_diff_opt.value()); + const auto err_msg = fmt::format( + "Excessive yaw difference {yaw_diff:2.2f}[deg]. The threshold is {th_yaw_diff:2.2f}[deg].", + fmt::arg("yaw_diff", yaw_diff_deg), fmt::arg("th_yaw_diff", th_yaw_diff_deg)); + throw std::logic_error(err_msg); + } + + LaneChangeInfo info; + info.longitudinal_acceleration = { + prepare_metric.actual_lon_accel, lane_changing_candidate.longitudinal_accelerations.front()}; + info.velocity = {prepare_metric.velocity, initial_state.longitudinal_velocity}; + info.duration = { + prepare_metric.duration, lane_changing_candidate.sampling_parameter.target_duration}; + info.length = {prepare_metric.length, lane_changing_candidate.lengths.back()}; + info.lane_changing_start = prepare_segment.points.back().point.pose; + info.lane_changing_end = lane_changing_candidate.poses.back(); + + ShiftLine sl; + + sl.start = lane_changing_candidate.poses.front(); + sl.end = lane_changing_candidate.poses.back(); + sl.start_shift_length = 0.0; + sl.end_shift_length = initial_state.position.d; + sl.start_idx = 0UL; + sl.end_idx = shifted_path.shift_length.size() - 1; + + info.shift_line = sl; + info.terminal_lane_changing_velocity = lane_changing_candidate.longitudinal_velocities.back(); + info.lateral_acceleration = lane_changing_candidate.lateral_accelerations.front(); + + LaneChangePath candidate_path; + candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); + candidate_path.info = info; + candidate_path.shifted_path = shifted_path; + candidate_path.frenet_path = trajectory_group; + candidate_path.type = PathType::FrenetPlanner; + + return candidate_path; +} + +void append_target_ref_to_candidate( + LaneChangePath & frenet_candidate, const double th_curvature_smoothing) +{ + const auto & target_lane_ref_path = frenet_candidate.frenet_path.target_lane_ref_path.points; + const auto & lc_end_pose = frenet_candidate.info.lane_changing_end; + const auto lc_end_idx = + motion_utils::findNearestIndex(target_lane_ref_path, lc_end_pose.position); + auto & candidate_path = frenet_candidate.path.points; + if (target_lane_ref_path.size() <= lc_end_idx + 2) { + return; + } + const auto add_size = target_lane_ref_path.size() - (lc_end_idx + 1); + candidate_path.reserve(candidate_path.size() + add_size); + const auto & points = target_lane_ref_path; + for (const auto & [p2, p3] : ranges::views::zip( + points | ranges::views::drop(lc_end_idx + 1), + points | ranges::views::drop(lc_end_idx + 2))) { + const auto point1 = autoware::universe_utils::getPoint(candidate_path.back().point.pose); + const auto point2 = autoware::universe_utils::getPoint(p2); + const auto point3 = autoware::universe_utils::getPoint(p3); + const auto curvature = + std::abs(autoware::universe_utils::calcCurvature(point1, point2, point3)); + if (curvature < th_curvature_smoothing) { + candidate_path.push_back(p2); + } + } + candidate_path.push_back(target_lane_ref_path.back()); +} } // 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 147ef3856ac4e..9002270185847 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 @@ -24,12 +24,18 @@ #include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" +// for the geometry types +#include +#include +// for the svg mapper #include #include #include #include #include #include +#include +#include #include #include #include @@ -40,9 +46,10 @@ #include #include -#include - +#include #include +#include +#include #include #include @@ -64,9 +71,11 @@ namespace autoware::behavior_path_planner::utils::lane_change { using autoware::route_handler::RouteHandler; using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; +using behavior_path_planner::lane_change::PathType; using geometry_msgs::msg::Pose; using tier4_planning_msgs::msg::PathWithLaneId; @@ -382,18 +391,26 @@ std::vector> get_sorted_lane_ids(const CommonDataPtr & comm return sorted_lane_ids; } -std::vector replaceWithSortedIds( - const std::vector & original_lane_ids, - const std::vector> & sorted_lane_ids) +std::vector replace_with_sorted_ids( + const std::vector & current_lane_ids, + const std::vector> & sorted_lane_ids, std::vector & prev_lane_ids, + std::vector & prev_sorted_lane_ids) { - for (const auto original_id : original_lane_ids) { + if (current_lane_ids == prev_lane_ids) { + return prev_sorted_lane_ids; + } + + for (const auto original_id : current_lane_ids) { for (const auto & sorted_id : sorted_lane_ids) { if (std::find(sorted_id.cbegin(), sorted_id.cend(), original_id) != sorted_id.cend()) { - return sorted_id; + prev_lane_ids = current_lane_ids; + prev_sorted_lane_ids = sorted_id; + return prev_sorted_lane_ids; } } } - return original_lane_ids; + + return current_lane_ids; } CandidateOutput assignToCandidate( @@ -475,6 +492,7 @@ std::vector convert_to_predicted_path( 0.5 * lane_changing_acceleration * delta_t * delta_t + offset; const auto pose = autoware::motion_utils::calcInterpolatedPose( path.points, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); } @@ -920,19 +938,17 @@ double get_min_dist_to_current_lanes_obj( continue; } - // calculate distance from path front to the stationary object polygon on the ego lane. - for (const auto & polygon_p : object.initial_polygon.outer()) { - const auto p_fp = autoware::universe_utils::toMsg(polygon_p.to_3d()); - const auto lateral_fp = motion_utils::calcLateralOffset(path_points, p_fp); - - // ignore if the point is not on ego path - if (std::abs(lateral_fp) > (common_data_ptr->bpp_param_ptr->vehicle_width / 2)) { - continue; - } - - const auto current_distance_to_obj = motion_utils::calcSignedArcLength(path_points, 0, p_fp); - min_dist_to_obj = std::min(min_dist_to_obj, current_distance_to_obj); + // check if object is on ego path + const auto obj_half_width = object.shape.dimensions.y / 2; + const auto obj_lat_dist_to_path = + std::abs(motion_utils::calcLateralOffset(path_points, object.initial_pose.position)) - + obj_half_width; + if (obj_lat_dist_to_path > (common_data_ptr->bpp_param_ptr->vehicle_width / 2)) { + continue; } + + min_dist_to_obj = std::min(min_dist_to_obj, dist_to_obj); + break; } return min_dist_to_obj; } @@ -1129,6 +1145,10 @@ std::vector> convert_to_predicted_paths( const auto acc_resolution = (min_acc - lane_changing_acc) / static_cast(sampling_num); const auto ego_predicted_path = [&](size_t n) { + if (lane_change_path.type == PathType::FrenetPlanner) { + return convert_to_predicted_path( + common_data_ptr, lane_change_path.frenet_path, deceleration_sampling_num); + } 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); }; @@ -1137,6 +1157,48 @@ std::vector> convert_to_predicted_paths( ranges::to(); } +std::vector convert_to_predicted_path( + const CommonDataPtr & common_data_ptr, const lane_change::TrajectoryGroup & frenet_candidate, + [[maybe_unused]] const size_t deceleration_sampling_num) +{ + const auto initial_velocity = common_data_ptr->get_ego_speed(); + const auto prepare_time = frenet_candidate.prepare_metric.duration; + const auto resolution = + common_data_ptr->lc_param_ptr->safety.collision_check.prediction_time_resolution; + const auto prepare_acc = frenet_candidate.prepare_metric.sampled_lon_accel; + std::vector predicted_path; + const auto & path = frenet_candidate.prepare.points; + const auto & vehicle_pose = common_data_ptr->get_ego_pose(); + const auto & bpp_param_ptr = common_data_ptr->bpp_param_ptr; + const auto nearest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path, vehicle_pose, bpp_param_ptr->ego_nearest_dist_threshold, + bpp_param_ptr->ego_nearest_yaw_threshold); + + const auto vehicle_pose_frenet = + convertToFrenetPoint(path, vehicle_pose.position, nearest_seg_idx); + + for (double t = 0.0; t < prepare_time; t += resolution) { + const auto velocity = + std::clamp(initial_velocity + prepare_acc * t, 0.0, frenet_candidate.prepare_metric.velocity); + const auto length = initial_velocity * t + 0.5 * prepare_acc * t * t; + const auto pose = + autoware::motion_utils::calcInterpolatedPose(path, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); + } + + const auto & poses = frenet_candidate.lane_changing.poses; + const auto & velocities = frenet_candidate.lane_changing.longitudinal_velocities; + const auto & times = frenet_candidate.lane_changing.times; + + for (const auto [t, pose, velocity] : + ranges::views::zip(times, poses, velocities) | ranges::views::drop(1)) { + predicted_path.emplace_back(prepare_time + t, pose, velocity); + } + + return predicted_path; +} + 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); 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 114a38a77876c..5fb445788672c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp @@ -242,15 +242,14 @@ TEST_F(TestNormalLaneChange, testFilteredObjects) const auto & filtered_objects = get_filtered_objects(); - // Note: There's 1 stopping object in current lanes, however, it was filtered out. const auto filtered_size = filtered_objects.current_lane.size() + filtered_objects.target_lane_leading.size() + filtered_objects.target_lane_trailing.size() + filtered_objects.others.size(); EXPECT_EQ(filtered_size, planner_data_->dynamic_object->objects.size()); - EXPECT_EQ(filtered_objects.current_lane.size(), 0); + EXPECT_EQ(filtered_objects.current_lane.size(), 1); EXPECT_EQ(filtered_objects.target_lane_leading.size(), 2); EXPECT_EQ(filtered_objects.target_lane_trailing.size(), 0); - EXPECT_EQ(filtered_objects.others.size(), 2); + EXPECT_EQ(filtered_objects.others.size(), 1); } TEST_F(TestNormalLaneChange, testGetPathWhenValid) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/README.md b/planning/behavior_path_planner/autoware_behavior_path_planner/README.md index ab8d02dc83f92..81cb945802595 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/README.md @@ -4,11 +4,11 @@ The Behavior Path Planner's main objective is to significantly enhance the safet The module begins by thoroughly analyzing the ego vehicle's current situation, including its position, speed, and surrounding environment. This analysis leads to essential driving decisions about lane changes or stopping and subsequently generates a path that is both safe and efficient. It considers road geometry, traffic rules, and dynamic conditions while also incorporating obstacle avoidance to respond to static and dynamic obstacles such as other vehicles, pedestrians, or unexpected roadblocks, ensuring safe navigation. -Moreover, the planner actively interacts with other traffic participants, predicting their actions and accordingly adjusting the vehicle's path. This ensures not only the safety of the autonomous vehicle but also contributes to smooth traffic flow. Its adherence to traffic laws, including speed limits and traffic signals, further guarantees lawful and predictable driving behavior. The planner is also designed to minimize sudden or abrupt maneuvers, aiming for a comfortable and natural driving experience. +Moreover, the planner responds to the behavior of other traffic participants, predicting their actions and accordingly adjusting the vehicle's path. This ensures not only the safety of the autonomous vehicle but also contributes to smooth traffic flow. Its adherence to traffic laws, including speed limits and compliance with traffic signals, further guarantees lawful and predictable driving behavior. The planner is also designed to minimize sudden or abrupt maneuvers, aiming for a comfortable and natural driving experience. !!! note - The [Planning Component Design](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/planning/) Document outlines the foundational philosophy guiding the design and future development of the Behavior Path Planner module. We strongly encourage readers to consult this document to understand the rationale behind its current configuration and the direction of its ongoing development. + The [Planning Component Design](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/planning/) documentation outlines the foundational philosophy guiding the design and future development of the Behavior Path Planner module. We strongly encourage readers to consult this document to understand the rationale behind its current configuration and the direction of its ongoing development. ## Purpose / Use Cases @@ -22,23 +22,23 @@ Essentially, the module has three primary responsibilities: ### Supported Scene Modules -Behavior Path Planner has following scene modules +Behavior Path Planner has the following scene modules -| Name | Description | Details | -| :------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :---------------------------------------------------------------------------- | -| Lane Following | this module generates reference path from lanelet centerline. | LINK | -| Static Obstacle Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](../autoware_behavior_path_static_obstacle_avoidance_module/README.md) | -| Dynamic Obstacle Avoidance | WIP | [LINK](../autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md) | -| Avoidance By Lane Change | this module generates lane change path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_by_lane_change_module/README.md) | -| Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](../autoware_behavior_path_lane_change_module/README.md) | -| External Lane Change | WIP | LINK | -| Goal Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](../autoware_behavior_path_goal_planner_module/README.md) | -| Start Planner | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](../autoware_behavior_path_start_planner_module/README.md) | -| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](../autoware_behavior_path_side_shift_module/README.md) | +| Name | Description | Details | +| :------------------------- | :--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :---------------------------------------------------------------------------- | +| Lane Following | This module generates a reference path from lanelet centerline. | LINK | +| Static Obstacle Avoidance | This module generates an avoidance path when there are objects that should be avoided. | [LINK](../autoware_behavior_path_static_obstacle_avoidance_module/README.md) | +| Dynamic Obstacle Avoidance | WIP | [LINK](../autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md) | +| Avoidance By Lane Change | This module generates a lane change path when there are objects that should be avoided. | [LINK](../behavior_path_avoidance_by_lane_change_module/README.md) | +| Lane Change | This module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](../autoware_behavior_path_lane_change_module/README.md) | +| External Lane Change | WIP | LINK | +| Goal Planner | This module is performed when the ego vehicle is in a driving lane and the goal is in the shoulder lane. The ego vehicle will stop at the goal. | [LINK](../autoware_behavior_path_goal_planner_module/README.md) | +| Start Planner | This module is performed when the ego vehicle is stationary and the footprint of the ego vehicle is included in the shoulder lane. This module ends when the ego vehicle merges into the road. | [LINK](../autoware_behavior_path_start_planner_module/README.md) | +| Side Shift | This module shifts the path to the left or right based on external instructions, intended for remote control applications. | [LINK](../autoware_behavior_path_side_shift_module/README.md) | !!! Note - click on the following images to view the video of their execution + Click on the following images to view videos of their execution
@@ -59,13 +59,13 @@ Behavior Path Planner has following scene modules Users can refer to [Planning component design](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/planning/#supported-functions) for some additional behavior. -#### How to add or implement new module? +#### How to add or implement new module -All scene modules are implemented by inheriting base class `scene_module_interface.hpp`. +All scene modules are implemented by inheriting the base class `scene_module_interface.hpp`. !!! Warning - The remainder of this subsection is work in progress (WIP). + The remainder of this subsection is a work in progress (WIP). ### Planner Manager @@ -77,65 +77,65 @@ The Planner Manager's responsibilities include: !!! note - To check the scene module's transition, i.e.: registered, approved and candidate modules, set `verbose: true` in the [behavior path planner configuration file](https://github.com/autowarefoundation/autoware_launch/blob/0cd5d891a36ac34a32a417205905c109f2bafe7b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml#L3). + To check the scene module's transition – i.e., registered, approved and candidate modules – set `verbose: true` in the [Behavior Path Planner configuration file](https://github.com/autowarefoundation/autoware_launch/blob/0cd5d891a36ac34a32a417205905c109f2bafe7b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml#L3). ![Scene module's transition table](./image/checking_module_transition.png) !!! note - For more in-depth information, refer to [Manager design](./docs/behavior_path_planner_manager_design.md) document. + For more in-depth information, refer to the [Manager design](./docs/behavior_path_planner_manager_design.md) document. ## Inputs / Outputs / API ### Input -| Name | Required? | Type | Description | -| :---------------------------- | :-------: | :------------------------------------------------------ | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| ~/input/odometry | ○ | `nav_msgs::msg::Odometry` | for ego velocity. | -| ~/input/accel | ○ | `geometry_msgs::msg::AccelWithCovarianceStamped` | for ego acceleration. | -| ~/input/objects | ○ | `autoware_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | -| ~/input/occupancy_grid_map | ○ | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Goal Planner module. | -| ~/input/traffic_signals | ○ | `autoware_perception_msgs::msg::TrafficLightGroupArray` | traffic signals information from the perception module | -| ~/input/vector_map | ○ | `autoware_map_msgs::msg::LaneletMapBin` | vector map information. | -| ~/input/route | ○ | `autoware_planning_msgs::msg::LaneletRoute` | current route from start to goal. | -| ~/input/scenario | ○ | `tier4_planning_msgs::msg::Scenario` | Launches behavior path planner if current scenario == `Scenario:LaneDriving`. | -| ~/input/lateral_offset | △ | `tier4_planning_msgs::msg::LateralOffset` | lateral offset to trigger side shift | -| ~/system/operation_mode/state | ○ | `autoware_adapi_v1_msgs::msg::OperationModeState` | Allows planning module to know if vehicle is in autonomous mode or can be controlled[ref](https://github.com/autowarefoundation/autoware.universe/blob/main/system/autoware_default_adapi/document/operation-mode.md) | - -- ○ Mandatory: Planning Module would not work if anyone of this is not present. -- △ Optional: Some module would not work, but Planning Module can still be operated. +| Name | Required? | Type | Description | +| :---------------------------- | :-------: | :------------------------------------------------------ | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ~/input/odometry | ○ | `nav_msgs::msg::Odometry` | For ego velocity | +| ~/input/accel | ○ | `geometry_msgs::msg::AccelWithCovarianceStamped` | For ego acceleration | +| ~/input/objects | ○ | `autoware_perception_msgs::msg::PredictedObjects` | Dynamic objects from the perception module | +| ~/input/occupancy_grid_map | ○ | `nav_msgs::msg::OccupancyGrid` | Occupancy grid map from the perception module. This is used for only the Goal Planner module | +| ~/input/traffic_signals | ○ | `autoware_perception_msgs::msg::TrafficLightGroupArray` | Traffic signal information from the perception module | +| ~/input/vector_map | ○ | `autoware_map_msgs::msg::LaneletMapBin` | Vector map information | +| ~/input/route | ○ | `autoware_planning_msgs::msg::LaneletRoute` | Current route from start to goal | +| ~/input/scenario | ○ | `tier4_planning_msgs::msg::Scenario` | Launches Behavior Path Planner if current scenario == `Scenario:LaneDriving` | +| ~/input/lateral_offset | △ | `tier4_planning_msgs::msg::LateralOffset` | Lateral offset to trigger side shift | +| ~/system/operation_mode/state | ○ | `autoware_adapi_v1_msgs::msg::OperationModeState` | Allows the planning module to know if vehicle is in autonomous mode or if it can be controlled[ref](https://github.com/autowarefoundation/autoware.universe/blob/main/system/autoware_default_adapi/document/operation-mode.md) | + +- ○ Mandatory: The planning module would not work if anyone of these were not present. +- △ Optional: Some modules would not work, but the planning module can still be operated. ### Output -| Name | Type | Description | QoS Durability | -| :---------------------------- | :-------------------------------------------------- | :--------------------------------------------------------------------------------------------- | ----------------- | -| ~/output/path | `tier4_planning_msgs::msg::PathWithLaneId` | the path generated by modules. | `volatile` | -| ~/output/turn_indicators_cmd | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command. | `volatile` | -| ~/output/hazard_lights_cmd | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command. | `volatile` | -| ~/output/modified_goal | `autoware_planning_msgs::msg::PoseWithUuidStamped` | output modified goal commands. | `transient_local` | -| ~/output/reroute_availability | `tier4_planning_msgs::msg::RerouteAvailability` | the path the module is about to take. to be executed as soon as external approval is obtained. | `volatile` | +| Name | Type | Description | QoS Durability | +| :---------------------------- | :-------------------------------------------------- | :-------------------------------------------------------------------------------------------- | ----------------- | +| ~/output/path | `tier4_planning_msgs::msg::PathWithLaneId` | The path generated by modules | `volatile` | +| ~/output/turn_indicators_cmd | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | Turn indicators command | `volatile` | +| ~/output/hazard_lights_cmd | `autoware_vehicle_msgs::msg::HazardLightsCommand` | Hazard lights command | `volatile` | +| ~/output/modified_goal | `autoware_planning_msgs::msg::PoseWithUuidStamped` | Output modified goal commands | `transient_local` | +| ~/output/reroute_availability | `tier4_planning_msgs::msg::RerouteAvailability` | The path the module is about to take. To be executed as soon as external approval is obtained | `volatile` | ### Debug -| Name | Type | Description | QoS Durability | -| :-------------------------------------- | :-------------------------------------------------- | :---------------------------------------------------------------------------------------- | -------------- | -| ~/debug/avoidance_debug_message_array | `tier4_planning_msgs::msg::AvoidanceDebugMsgArray` | debug message for avoidance. notify users reasons for avoidance path cannot be generated. | `volatile` | -| ~/debug/lane_change_debug_message_array | `tier4_planning_msgs::msg::LaneChangeDebugMsgArray` | debug message for lane change. notify users unsafe reason during lane changing process | `volatile` | -| ~/debug/maximum_drivable_area | `visualization_msgs::msg::MarkerArray` | shows maximum static drivable area. | `volatile` | -| ~/debug/turn_signal_info | `visualization_msgs::msg::MarkerArray` | TBA | `volatile` | -| ~/debug/bound | `visualization_msgs::msg::MarkerArray` | debug for static drivable area | `volatile` | -| ~/planning/path_candidate/\* | `autoware_planning_msgs::msg::Path` | the path before approval. | `volatile` | -| ~/planning/path_reference/\* | `autoware_planning_msgs::msg::Path` | reference path generated by each modules. | `volatile` | +| Name | Type | Description | QoS Durability | +| :-------------------------------------- | :-------------------------------------------------- | :---------------------------------------------------------------------------------------------- | -------------- | +| ~/debug/avoidance_debug_message_array | `tier4_planning_msgs::msg::AvoidanceDebugMsgArray` | Debug message for avoidance. Notifies users of reasons avoidance path cannot be generated | `volatile` | +| ~/debug/lane_change_debug_message_array | `tier4_planning_msgs::msg::LaneChangeDebugMsgArray` | Debug message for lane change. Notifies users of unsafe conditions during lane-changing process | `volatile` | +| ~/debug/maximum_drivable_area | `visualization_msgs::msg::MarkerArray` | Shows maximum static drivable area | `volatile` | +| ~/debug/turn_signal_info | `visualization_msgs::msg::MarkerArray` | TBA | `volatile` | +| ~/debug/bound | `visualization_msgs::msg::MarkerArray` | Debug for static drivable area | `volatile` | +| ~/planning/path_candidate/\* | `autoware_planning_msgs::msg::Path` | The path before approval | `volatile` | +| ~/planning/path_reference/\* | `autoware_planning_msgs::msg::Path` | Reference path generated by each module | `volatile` | !!! note - For specific information of which topics are being subscribed and published, refer to [behavior_path_planner.xml](https://github.com/autowarefoundation/autoware.universe/blob/9000f430c937764c14e43109539302f1f878ed70/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml#L36-L49). + For specific information about which topics are being subscribed to and published, refer to [behavior_path_planner.xml](https://github.com/autowarefoundation/autoware.universe/blob/9000f430c937764c14e43109539302f1f878ed70/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml#L36-L49). -## How to enable or disable the modules +## How to Enable or Disable Modules -Enabling and disabling the modules in the behavior path planner is primarily managed through two key files: `default_preset.yaml` and `behavior_path_planner.launch.xml`. +Enabling and disabling the modules in the Behavior Path Planner is primarily managed through two key files: `default_preset.yaml` and `behavior_path_planner.launch.xml`. -The `default_preset.yaml` file acts as a configuration file for enabling or disabling specific modules within the planner. It contains a series of arguments which represent the behavior path planner's modules or features. For example: +The `default_preset.yaml` file acts as a configuration file for enabling or disabling specific modules within the planner. It contains a series of arguments which represent the Behavior Path Planner's modules or features. For example: - `launch_static_obstacle_avoidance_module`: Set to `true` to enable the avoidance module, or `false` to disable it. @@ -143,14 +143,12 @@ The `default_preset.yaml` file acts as a configuration file for enabling or disa Click [here](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/preset/default_preset.yaml) to view the `default_preset.yaml`. -The `behavior_path_planner.launch.xml` file references the settings defined in `default_preset.yaml` to apply the configurations when the behavior path planner's node is running. For instance, the parameter `static_obstacle_avoidance.enable_module` in +The `behavior_path_planner.launch.xml` file references the settings defined in `default_preset.yaml` to apply the configurations when the Behavior Path Planner's node is running. For instance, the parameter `static_obstacle_avoidance.enable_module` in the following segment corresponds to launch_static_obstacle_avoidance_module from `default_preset.yaml`: ```xml ``` -corresponds to launch_static_obstacle_avoidance_module from `default_preset.yaml`. - Therefore, to enable or disable a module, simply set the corresponding module in `default_preset.yaml` to `true` or `false`. These changes will be applied upon the next launch of Autoware. ## Generating Path @@ -163,13 +161,13 @@ The `ShiftLine` struct (as seen [here](https://github.com/autowarefoundation/aut Furthermore, the design and its implementation incorporate various equations and mathematical models to calculate essential parameters for the path shift. These include the total distance of the lateral shift, the maximum allowable lateral acceleration and jerk, and the total time required for the shift. Practical considerations are also noted, such as simplifying assumptions in the absence of a specific time interval for most lane change and avoidance cases. -The shifted path generation logic enables the behavior path planner to dynamically generate safe and efficient paths, precisely controlling the vehicle’s lateral movements to ensure the smooth execution of lane changes and avoidance maneuvers. This careful planning and execution adhere to the vehicle's dynamic capabilities and safety constraints, maximizing efficiency and safety in autonomous vehicle navigation. +The shifted path generation logic enables the Behavior Path Planner to dynamically generate safe and efficient paths, precisely controlling the vehicle’s lateral movements to ensure the smooth execution of lane changes and avoidance maneuvers. This careful planning and execution adhere to the vehicle's dynamic capabilities and safety constraints, maximizing efficiency and safety in autonomous vehicle navigation. !!! note If you're a math lover, refer to [Path Generation Design](../autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md) for the nitty-gritty. -## Collision Assessment / Safety check +## Collision Assessment / Safety Check The purpose of the collision assessment function in the Behavior Path Planner is to evaluate the potential for collisions with target objects across all modules. It is utilized in two scenarios: @@ -207,7 +205,7 @@ Static drivable area expansion operates under assumptions about the correct arra !!! note - Further details can is provided in [Drivable Area Design](../autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md). + Further details can be found in [Drivable Area Design](../autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md). ### Dynamic Drivable Area Logic @@ -233,7 +231,7 @@ The `TurnIndicatorsCommand` message structure has a command field that can take !!! warning - Rerouting is a feature that was still under progress. Further information will be included on a later date. + The rerouting feature is under development. Further information will be included at a later date. ## Parameters and Configuration @@ -278,5 +276,5 @@ preset ## Limitations & Future Work -1. Goal Planner module cannot be simultaneously executed together with other modules. -2. Module is not designed as plugin. Integrating custom module is not straightforward and user have to modify some part of the behavior path planner main code. +1. The Goal Planner module cannot be simultaneously executed together with other modules. +2. The module is not designed as a plugin. Integrating a custom module is not straightforward. Users have to modify part of the Behavior Path Planner's main code. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md index 1e8532eba2f3a..3b787303d8d20 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md @@ -533,9 +533,9 @@ If this case happened in the slot, `is_upstream_waiting_approved` is set to true ### Failure modules -The failure modules return the status `ModuleStatus::FAILURE`. The manager removes the module from approved modules stack as well as waiting approval modules, but the failure module is not moved to candidate modules stack. +If a module returns `ModuleStatus::FAILURE`, the manager removes the failed module. Additionally, all modules after the failed module are removed, even if they did not return `ModuleStatus::FAILURE`. These modules are not added back to the candidate modules stack and will instead run again from the beginning. Once these modules are removed, the output of the module prior to the failed module will be used as the planner's output. -As a result, the module A's output is used as approved modules stack. +As shown in the example below, modules B, A, and C are running. When module A returns `ModuleStatus::FAILURE`, both module A and C are removed from the approved modules stack. Module B's output is then used as the final output of the planner. ![failure_modules](../image/manager/failure_modules.drawio.svg) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp index ebe7c47e6fab2..7f7ecd7326b08 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp @@ -20,7 +20,7 @@ #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "planner_manager.hpp" -#include +#include #include #include @@ -51,7 +51,7 @@ namespace autoware::behavior_path_planner { -using autoware::motion_utils::SteeringFactorInterface; +using autoware::motion_utils::PlanningFactorInterface; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_map_msgs::msg::LaneletMapBin; using autoware_perception_msgs::msg::PredictedObject; @@ -121,7 +121,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr bound_publisher_; rclcpp::Publisher::SharedPtr modified_goal_publisher_; rclcpp::Publisher::SharedPtr reroute_availability_publisher_; - rclcpp::Publisher::SharedPtr pub_steering_factors_; rclcpp::TimerBase::SharedPtr timer_; std::map::SharedPtr> path_candidate_publishers_; @@ -136,7 +135,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node std::shared_ptr planner_manager_; - SteeringFactorInterface steering_factor_interface_; + std::unique_ptr planning_factor_interface_; std::mutex mutex_pd_; // mutex for planner_data_ std::mutex mutex_manager_; // mutex for bt_manager_ or planner_manager_ 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 7b7e2438a3fd6..f9c736b9d5dde 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml @@ -21,6 +21,7 @@ Kyoichi Sugahara Takayuki Murooka Go Sakayori + Mamoru Sobue Apache License 2.0 @@ -29,7 +30,6 @@ Fumiya Watanabe Zulfaqar Azmi Kosuke Takeuchi - Yutaka Shimizu Takayuki Murooka Ryohsuke Mitsudome diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index 7758ab530e88c..568d432e1faa3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -32,7 +32,9 @@ using autoware::vehicle_info_utils::VehicleInfoUtils; using tier4_planning_msgs::msg::PathChangeModuleId; BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & node_options) -: Node("behavior_path_planner", node_options) +: Node("behavior_path_planner", node_options), + planning_factor_interface_{ + std::make_unique(this, "behavior_path_planner")} { using std::placeholders::_1; using std::chrono_literals::operator""ms; @@ -51,8 +53,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const auto durable_qos = rclcpp::QoS(1).transient_local(); modified_goal_publisher_ = create_publisher("~/output/modified_goal", durable_qos); - pub_steering_factors_ = - create_publisher("/planning/steering_factor/intersection", 1); reroute_availability_publisher_ = create_publisher("~/output/is_reroute_available", 1); debug_avoidance_msg_array_publisher_ = @@ -115,8 +115,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod turn_signal_search_time, turn_signal_intersection_angle_threshold_deg); } - steering_factor_interface_.init(PlanningBehavior::INTERSECTION); - // Start timer { const auto planning_hz = declare_parameter("planning_hz"); @@ -463,33 +461,22 @@ void BehaviorPathPlannerNode::publish_steering_factor( const auto [intersection_flag, approaching_intersection_flag] = planner_data->turn_signal_decider.getIntersectionTurnSignalFlag(); if (intersection_flag || approaching_intersection_flag) { - const uint16_t steering_factor_direction = std::invoke([&turn_signal]() { - if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - return SteeringFactor::LEFT; - } - return SteeringFactor::RIGHT; - }); - const auto [intersection_pose, intersection_distance] = planner_data->turn_signal_decider.getIntersectionPoseAndDistance(); - steering_factor_interface_.set( - {intersection_pose, intersection_pose}, {intersection_distance, intersection_distance}, - steering_factor_direction, SteeringFactor::TURNING, ""); - } else { - steering_factor_interface_.reset(); - } - - autoware_adapi_v1_msgs::msg::SteeringFactorArray steering_factor_array; - steering_factor_array.header.frame_id = "map"; - steering_factor_array.header.stamp = this->now(); + const uint16_t planning_factor_direction = std::invoke([&turn_signal]() { + if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { + return PlanningFactor::TURN_LEFT; + } + return PlanningFactor::TURN_RIGHT; + }); - const auto steering_factor = steering_factor_interface_.get(); - if (steering_factor.behavior != PlanningBehavior::UNKNOWN) { - steering_factor_array.factors.emplace_back(steering_factor); + planning_factor_interface_->add( + intersection_distance, intersection_distance, intersection_pose, intersection_pose, + planning_factor_direction, SafetyFactorArray{}); } - pub_steering_factors_->publish(steering_factor_array); + planning_factor_interface_->publish(); } void BehaviorPathPlannerNode::publish_reroute_availability() const diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index 041b7ad20a107..322209d46732b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -182,8 +182,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); m->publishRTCStatus(); - m->publishSteeringFactor(); - m->publishVelocityFactor(); + m->publish_planning_factors(); }); generateCombinedDrivableArea(result_output.valid_output, data); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index 391ac92411220..c96bc601b6403 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -21,8 +21,7 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include -#include -#include +#include #include #include #include @@ -36,7 +35,6 @@ #include #include -#include #include #include #include @@ -54,18 +52,16 @@ namespace autoware::behavior_path_planner { -using autoware::motion_utils::SteeringFactorInterface; -using autoware::motion_utils::VelocityFactorInterface; +using autoware::motion_utils::PlanningFactorInterface; using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using autoware::rtc_interface::RTCInterface; using autoware::universe_utils::calcOffsetPose; using autoware::universe_utils::generateUUID; -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::SteeringFactor; -using autoware_adapi_v1_msgs::msg::VelocityFactor; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::SafetyFactorArray; using tier4_rtc_msgs::msg::State; using unique_identifier_msgs::msg::UUID; using visualization_msgs::msg::MarkerArray; @@ -86,13 +82,15 @@ class SceneModuleInterface const std::string & name, rclcpp::Node & node, std::unordered_map> rtc_interface_ptr_map, std::unordered_map> - objects_of_interest_marker_interface_ptr_map) + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) : name_{name}, logger_{node.get_logger().get_child(name)}, clock_{node.get_clock()}, rtc_interface_ptr_map_(std::move(rtc_interface_ptr_map)), objects_of_interest_marker_interface_ptr_map_( std::move(objects_of_interest_marker_interface_ptr_map)), + planning_factor_interface_{planning_factor_interface}, time_keeper_(std::make_shared()) { for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { @@ -184,8 +182,6 @@ class SceneModuleInterface unlockNewModuleLaunch(); unlockOutputPath(); - reset_factor(); - processOnExit(); } @@ -258,16 +254,6 @@ class SceneModuleInterface ModuleStatus getCurrentStatus() const { return current_state_; } - void reset_factor() - { - steering_factor_interface_.reset(); - velocity_factor_interface_.reset(); - } - - auto get_steering_factor() const -> SteeringFactor { return steering_factor_interface_.get(); } - - auto get_velocity_factor() const -> VelocityFactor { return velocity_factor_interface_.get(); } - std::string name() const { return name_; } PoseWithDetailOpt getStopPose() const @@ -558,11 +544,12 @@ class SceneModuleInterface } } - void setVelocityFactor(const PathWithLaneId & path) + void set_longitudinal_planning_factor(const PathWithLaneId & path) { if (stop_pose_.has_value()) { - velocity_factor_interface_.set( - path.points, getEgoPose(), stop_pose_.value().pose, VelocityFactor::APPROACHING, "stop"); + planning_factor_interface_->add( + path.points, getEgoPose(), stop_pose_.value().pose, PlanningFactor::STOP, + SafetyFactorArray{}); return; } @@ -570,8 +557,9 @@ class SceneModuleInterface return; } - velocity_factor_interface_.set( - path.points, getEgoPose(), slow_pose_.value().pose, VelocityFactor::APPROACHING, "slow down"); + planning_factor_interface_->add( + path.points, getEgoPose(), slow_pose_.value().pose, PlanningFactor::SLOW_DOWN, + SafetyFactorArray{}); } void setDrivableLanes(const std::vector & drivable_lanes); @@ -627,9 +615,7 @@ class SceneModuleInterface std::unordered_map> objects_of_interest_marker_interface_ptr_map_; - mutable SteeringFactorInterface steering_factor_interface_; - - mutable VelocityFactorInterface velocity_factor_interface_; + mutable std::shared_ptr planning_factor_interface_; mutable PoseWithDetailOpt stop_pose_{std::nullopt}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index 73b133952935b..446829d1d0f9d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -24,8 +24,6 @@ #include #include -#include -#include #include #include @@ -42,8 +40,6 @@ using autoware::motion_utils::createDeadLineVirtualWallMarker; using autoware::motion_utils::createSlowDownVirtualWallMarker; using autoware::motion_utils::createStopVirtualWallMarker; using autoware::universe_utils::toHexString; -using autoware_adapi_v1_msgs::msg::SteeringFactorArray; -using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using unique_identifier_msgs::msg::UUID; using SceneModulePtr = std::shared_ptr; using SceneModuleObserver = std::weak_ptr; @@ -106,45 +102,7 @@ class SceneModuleManagerInterface } } - void publishSteeringFactor() - { - SteeringFactorArray steering_factor_array; - steering_factor_array.header.frame_id = "map"; - steering_factor_array.header.stamp = node_->now(); - - for (const auto & m : observers_) { - if (m.expired()) { - continue; - } - - const auto steering_factor = m.lock()->get_steering_factor(); - if (steering_factor.behavior != PlanningBehavior::UNKNOWN) { - steering_factor_array.factors.emplace_back(steering_factor); - } - } - - pub_steering_factors_->publish(steering_factor_array); - } - - void publishVelocityFactor() - { - VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = node_->now(); - - for (const auto & m : observers_) { - if (m.expired()) { - continue; - } - - const auto velocity_factor = m.lock()->get_velocity_factor(); - if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } - } - - pub_velocity_factors_->publish(velocity_factor_array); - } + void publish_planning_factors() { planning_factor_interface_->publish(); } void publishVirtualWall() const { @@ -304,10 +262,6 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_drivable_lanes_; - rclcpp::Publisher::SharedPtr pub_steering_factors_; - - rclcpp::Publisher::SharedPtr pub_velocity_factors_; - rclcpp::Publisher::SharedPtr pub_processing_time_; std::string name_; @@ -318,6 +272,8 @@ class SceneModuleManagerInterface std::unique_ptr idle_module_ptr_; + std::shared_ptr planning_factor_interface_; + std::unordered_map> rtc_interface_ptr_map_; std::unordered_map> diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp index b91db3a740cfe..11be3b28a7f94 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp @@ -61,10 +61,11 @@ void SceneModuleManagerInterface::initInterface( pub_drivable_lanes_ = node->create_publisher("~/drivable_lanes/" + name_, 20); pub_processing_time_ = node->create_publisher( "~/processing_time/" + name_, 20); - pub_steering_factors_ = - node->create_publisher("/planning/steering_factor/" + name_, 1); - pub_velocity_factors_ = - node->create_publisher("/planning/velocity_factors/" + name_, 1); + } + + // planning factor + { + planning_factor_interface_ = std::make_shared(node, name_); } // misc diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp index 28c310ae20ec2..f25c073e879da 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp @@ -38,7 +38,7 @@ class SamplingPlannerModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams( diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp index c9b75f7fa96de..f84fe45d08ca4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp @@ -86,7 +86,8 @@ class SamplingPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); bool isExecutionRequested() const override; bool isExecutionReady() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index fac3cc92de1d6..5bb57f17ce813 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -42,8 +42,9 @@ SamplingPlannerModule::SamplingPlannerModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()} { internal_params_ = std::make_shared(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp index b495e6619a1c7..2b4a20ddc8cca 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp @@ -39,7 +39,7 @@ class SideShiftModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp index 15d81ff45abcf..c8a5e47287477 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp @@ -45,7 +45,8 @@ class SideShiftModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); bool isExecutionRequested() const override; bool isExecutionReady() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp index 22ee7ab77e077..9536c1ba17336 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp @@ -42,8 +42,9 @@ SideShiftModule::SideShiftModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{parameters} { } 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 e28339fb51a6c..5961b98bdda08 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 @@ -19,12 +19,22 @@ ament_auto_add_library(${PROJECT_NAME} SHARED if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + # Add test helper library + ament_auto_add_library(start_planner_test_helper SHARED + test/src/start_planner_test_helper.cpp + ) + target_include_directories(start_planner_test_helper PUBLIC + test/include + ) + file(GLOB_RECURSE TEST_SOURCES test/*.cpp) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${TEST_SOURCES} ) - target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} + start_planner_test_helper + ) endif() - ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp index e358e35ed7794..e1a7ae455e374 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp @@ -35,15 +35,16 @@ using autoware::freespace_planning_algorithms::RRTStar; class FreespacePullOut : public PullOutPlannerBase { public: - FreespacePullOut( - rclcpp::Node & node, const StartPlannerParameters & parameters, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, - std::shared_ptr time_keeper); + FreespacePullOut(rclcpp::Node & node, const StartPlannerParameters & parameters); PlannerType getPlannerType() const override { return PlannerType::FREESPACE; } std::optional plan( - const Pose & start_pose, const Pose & end_pose, PlannerDebugData & planner_debug_data) override; + const Pose & start_pose, const Pose & end_pose, + const std::shared_ptr & planner_data, + PlannerDebugData & planner_debug_data) override; + + friend class TestFreespacePullOut; protected: std::unique_ptr planner_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp index 18d1f3c3b9b81..5d36c996cca3a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -33,13 +33,13 @@ class GeometricPullOut : public PullOutPlannerBase public: explicit GeometricPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const std::shared_ptr - lane_departure_checker, - std::shared_ptr time_keeper); + std::shared_ptr time_keeper = + std::make_shared()); PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; }; std::optional plan( const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) override; GeometricParallelParking planner_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp index 5d3d224673124..68b1e67e484c3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp @@ -38,7 +38,7 @@ class StartPlannerModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp index f606679e7f2da..0316d6a14a4f9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -40,37 +40,35 @@ class PullOutPlannerBase public: explicit PullOutPlannerBase( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr time_keeper) - : time_keeper_(time_keeper) + std::shared_ptr time_keeper = + std::make_shared()) + : parameters_{parameters}, + vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, + vehicle_footprint_{vehicle_info_.createFootprint()}, + time_keeper_(time_keeper) { - vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); - vehicle_footprint_ = vehicle_info_.createFootprint(); - parameters_ = parameters; } virtual ~PullOutPlannerBase() = default; - void setPlannerData(const std::shared_ptr & planner_data) - { - planner_data_ = planner_data; - } - void setCollisionCheckMargin(const double collision_check_margin) { collision_check_margin_ = collision_check_margin; }; virtual PlannerType getPlannerType() const = 0; virtual std::optional plan( - const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) = 0; + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, + PlannerDebugData & planner_debug_data) = 0; protected: bool isPullOutPathCollided( autoware::behavior_path_planner::PullOutPath & pull_out_path, + const std::shared_ptr & planner_data, double collision_check_distance_from_end) const; - std::shared_ptr planner_data_; + StartPlannerParameters parameters_; autoware::vehicle_info_utils::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; - StartPlannerParameters parameters_; double collision_check_margin_; mutable std::shared_ptr time_keeper_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp index 5b4f78b252d22..1000437bdea56 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 @@ -35,17 +35,19 @@ class ShiftPullOut : public PullOutPlannerBase public: explicit ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr & lane_departure_checker, - std::shared_ptr time_keeper); + std::shared_ptr time_keeper = + std::make_shared()); PlannerType getPlannerType() const override { return PlannerType::SHIFT; }; std::optional plan( const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) override; std::vector calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, - const Pose & start_pose, const Pose & goal_pose); + const Pose & start_pose, const Pose & goal_pose, + const BehaviorPathPlannerParameters & behavior_path_parameters); double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double target_after_arc_length, const double dr); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index 47eecb54e2b2a..9b1186cb5fc66 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -27,7 +27,6 @@ #include "autoware/behavior_path_start_planner_module/pull_out_path.hpp" #include "autoware/behavior_path_start_planner_module/shift_pull_out.hpp" -#include #include #include @@ -51,7 +50,6 @@ using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilter using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using autoware::behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; -using autoware::lane_departure_checker::LaneDepartureChecker; using geometry_msgs::msg::PoseArray; using PriorityOrder = std::vector>>; @@ -88,7 +86,8 @@ class StartPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); ~StartPlannerModule() override { @@ -200,18 +199,18 @@ class StartPlannerModule : public SceneModuleInterface bool requiresDynamicObjectsCollisionDetection() const; - uint16_t getSteeringFactorDirection( + uint16_t getPlanningFactorDirection( const autoware::behavior_path_planner::BehaviorModuleOutput & output) const { switch (output.turn_signal_info.turn_signal.command) { case TurnIndicatorsCommand::ENABLE_LEFT: - return SteeringFactor::LEFT; + return PlanningFactor::SHIFT_LEFT; case TurnIndicatorsCommand::ENABLE_RIGHT: - return SteeringFactor::RIGHT; + return PlanningFactor::SHIFT_RIGHT; default: - return SteeringFactor::STRAIGHT; + return PlanningFactor::NONE; } }; @@ -310,8 +309,6 @@ ego pose. std::vector searchPullOutStartPoseCandidates( const PathWithLaneId & back_path_from_start_pose) const; - std::shared_ptr lane_departure_checker_; - // turn signal TurnSignalInfo calcTurnSignalInfo(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml index c7cabb403f164..214731f96cebc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml @@ -22,6 +22,7 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_freespace_planning_algorithms autoware_motion_utils autoware_rtc_interface autoware_universe_utils diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp index 42698f799c6b3..6893c7d11d09d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -28,15 +28,11 @@ namespace autoware::behavior_path_planner { -FreespacePullOut::FreespacePullOut( - rclcpp::Node & node, const StartPlannerParameters & parameters, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, - std::shared_ptr time_keeper) -: PullOutPlannerBase{node, parameters, time_keeper}, - velocity_{parameters.freespace_planner_velocity} +FreespacePullOut::FreespacePullOut(rclcpp::Node & node, const StartPlannerParameters & parameters) +: PullOutPlannerBase{node, parameters}, velocity_{parameters.freespace_planner_velocity} { autoware::freespace_planning_algorithms::VehicleShape vehicle_shape( - vehicle_info, parameters.vehicle_shape_margin); + vehicle_info_, parameters.vehicle_shape_margin); if (parameters.freespace_planner_algorithm == "astar") { use_back_ = parameters.astar_parameters.use_back; planner_ = std::make_unique( @@ -51,13 +47,14 @@ FreespacePullOut::FreespacePullOut( } std::optional FreespacePullOut::plan( - const Pose & start_pose, const Pose & end_pose, PlannerDebugData & planner_debug_data) + const Pose & start_pose, const Pose & end_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) { - const auto & route_handler = planner_data_->route_handler; - const double backward_path_length = planner_data_->parameters.backward_path_length; - const double forward_path_length = planner_data_->parameters.forward_path_length; + const auto & route_handler = planner_data->route_handler; + const double backward_path_length = planner_data->parameters.backward_path_length; + const double forward_path_length = planner_data->parameters.forward_path_length; - planner_->setMap(*planner_data_->costmap); + planner_->setMap(*planner_data->costmap); try { if (!planner_->makePlan(start_pose, end_pose)) { @@ -69,11 +66,11 @@ std::optional FreespacePullOut::plan( } const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), + planner_data, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); // find candidate paths const auto pull_out_lanes = - start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); + start_planner_utils::getPullOutLanes(planner_data, backward_path_length); const auto lanes = utils::combineLanelets(road_lanes, pull_out_lanes); const PathWithLaneId path = diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp index bbc6c05725434..ed09e0c0a33d6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -36,35 +36,39 @@ using start_planner_utils::getPullOutLanes; GeometricPullOut::GeometricPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const std::shared_ptr - lane_departure_checker, std::shared_ptr time_keeper) : PullOutPlannerBase{node, parameters, time_keeper}, - parallel_parking_parameters_{parameters.parallel_parking_parameters}, - lane_departure_checker_(lane_departure_checker) + parallel_parking_parameters_{parameters.parallel_parking_parameters} { + auto lane_departure_checker_params = autoware::lane_departure_checker::Param{}; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; + lane_departure_checker_ = + std::make_shared( + lane_departure_checker_params, vehicle_info_); planner_.setParameters(parallel_parking_parameters_); } std::optional GeometricPullOut::plan( - const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) { PullOutPath output; // combine road lane and pull out lane const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_.max_back_distance; + planner_data->parameters.backward_path_length + parameters_.max_back_distance; const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), + planner_data, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); - const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length); + const auto pull_out_lanes = getPullOutLanes(planner_data, backward_path_length); // check if the ego is at left or right side of road lane center const bool left_side_start = 0 < getArcCoordinates(road_lanes, start_pose).distance; planner_.setTurningRadius( - planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle); - planner_.setPlannerData(planner_data_); + planner_data->parameters, parallel_parking_parameters_.pull_out_max_steer_angle); + planner_.setPlannerData(planner_data); const bool found_valid_path = planner_.planPullOut( start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start, lane_departure_checker_); if (!found_valid_path) { @@ -122,7 +126,8 @@ std::optional GeometricPullOut::plan( output.start_pose = planner_.getArcPaths().at(0).points.front().point.pose; output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose; - if (isPullOutPathCollided(output, parameters_.geometric_collision_check_distance_from_end)) { + if (isPullOutPathCollided( + output, planner_data, parameters_.geometric_collision_check_distance_from_end)) { planner_debug_data.conditions_evaluation.emplace_back("collision"); return {}; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp index 42c5bead33604..a475b3ad582cb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp @@ -14,21 +14,24 @@ #include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include + namespace autoware::behavior_path_planner { bool PullOutPlannerBase::isPullOutPathCollided( autoware::behavior_path_planner::PullOutPath & pull_out_path, + const std::shared_ptr & planner_data, double collision_check_distance_from_end) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // check for collisions - const auto & dynamic_objects = planner_data_->dynamic_object; + const auto & dynamic_objects = planner_data->dynamic_object; if (!dynamic_objects) { return false; } const auto pull_out_lanes = start_planner_utils::getPullOutLanes( - planner_data_, planner_data_->parameters.backward_path_length + parameters_.max_back_distance); + planner_data, planner_data->parameters.backward_path_length + parameters_.max_back_distance); // extract stop objects in pull out lane for collision check const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *dynamic_objects, parameters_.th_moving_object_velocity); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index 4759559619870..16a3d4ffd842a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -41,31 +41,38 @@ using start_planner_utils::getPullOutLanes; ShiftPullOut::ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr & lane_departure_checker, std::shared_ptr time_keeper) -: PullOutPlannerBase{node, parameters, time_keeper}, lane_departure_checker_{lane_departure_checker} +: PullOutPlannerBase{node, parameters, time_keeper} { + autoware::lane_departure_checker::Param lane_departure_checker_params; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; + lane_departure_checker_ = + std::make_shared( + lane_departure_checker_params, vehicle_info_, time_keeper_); } std::optional ShiftPullOut::plan( - const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) { - const auto & route_handler = planner_data_->route_handler; - const auto & common_parameters = planner_data_->parameters; + const auto & route_handler = planner_data->route_handler; + const auto & common_parameters = planner_data->parameters; const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_.max_back_distance; + planner_data->parameters.backward_path_length + parameters_.max_back_distance; const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), + planner_data, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); // find candidate paths - auto pull_out_paths = calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose); + auto pull_out_paths = + calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose, common_parameters); if (pull_out_paths.empty()) { planner_debug_data.conditions_evaluation.emplace_back("no path found"); return std::nullopt; } - const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr(); + const auto lanelet_map_ptr = planner_data->route_handler->getLaneletMapPtr(); std::vector fused_id_start_to_end{}; std::optional fused_polygon_start_to_end = std::nullopt; @@ -159,9 +166,10 @@ std::optional ShiftPullOut::plan( continue; } shift_path.points = cropped_path.points; - shift_path.header = planner_data_->route_handler->getRouteHeader(); + shift_path.header = planner_data->route_handler->getRouteHeader(); - if (isPullOutPathCollided(pull_out_path, parameters_.shift_collision_check_distance_from_end)) { + if (isPullOutPathCollided( + pull_out_path, planner_data, parameters_.shift_collision_check_distance_from_end)) { planner_debug_data.conditions_evaluation.emplace_back("collision"); continue; } @@ -227,7 +235,8 @@ bool ShiftPullOut::refineShiftedPathToStartPose( std::vector ShiftPullOut::calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, - const Pose & start_pose, const Pose & goal_pose) + const Pose & start_pose, const Pose & goal_pose, + const BehaviorPathPlannerParameters & behavior_path_parameters) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -238,9 +247,8 @@ std::vector ShiftPullOut::calcPullOutPaths( } // rename parameter - const auto & common_parameters = planner_data_->parameters; - const double forward_path_length = common_parameters.forward_path_length; - const double backward_path_length = common_parameters.backward_path_length; + const double forward_path_length = behavior_path_parameters.forward_path_length; + const double backward_path_length = behavior_path_parameters.backward_path_length; const double lateral_jerk = parameters_.lateral_jerk; const double minimum_lateral_acc = parameters_.minimum_lateral_acc; const double maximum_lateral_acc = parameters_.maximum_lateral_acc; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index c223390e454d1..ae05bcf4c084e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -62,36 +62,26 @@ StartPlannerModule::StartPlannerModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{parameters}, vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, is_freespace_planner_cb_running_{false} { - lane_departure_checker_ = std::make_shared(time_keeper_); - lane_departure_checker_->setVehicleInfo(vehicle_info_); - autoware::lane_departure_checker::Param lane_departure_checker_params{}; - lane_departure_checker_params.footprint_extra_margin = - parameters->lane_departure_check_expansion_margin; - - lane_departure_checker_->setParam(lane_departure_checker_params); - // set enabled planner if (parameters_->enable_shift_pull_out) { - start_planners_.push_back( - std::make_shared(node, *parameters, lane_departure_checker_, time_keeper_)); + start_planners_.push_back(std::make_shared(node, *parameters, time_keeper_)); } if (parameters_->enable_geometric_pull_out) { - start_planners_.push_back( - std::make_shared(node, *parameters, lane_departure_checker_, time_keeper_)); + start_planners_.push_back(std::make_shared(node, *parameters, time_keeper_)); } if (start_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); } if (parameters_->enable_freespace_planner) { - freespace_planner_ = - std::make_unique(node, *parameters, vehicle_info_, time_keeper_); + freespace_planner_ = std::make_unique(node, *parameters); const auto freespace_planner_period_ns = rclcpp::Rate(1.0).period(); freespace_planner_timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -100,9 +90,6 @@ StartPlannerModule::StartPlannerModule( std::bind(&StartPlannerModule::onFreespacePlannerTimer, this), freespace_planner_timer_cb_group_); } - - steering_factor_interface_.init(PlanningBehavior::START_PLANNER); - velocity_factor_interface_.init(PlanningBehavior::START_PLANNER); } void StartPlannerModule::onFreespacePlannerTimer() @@ -743,9 +730,9 @@ BehaviorModuleOutput StartPlannerModule::plan() setDrivableAreaInfo(output); - setVelocityFactor(output.path); + set_longitudinal_planning_factor(output.path); - const auto steering_factor_direction = getSteeringFactorDirection(output); + const auto planning_factor_direction = getPlanningFactorDirection(output); if (status_.driving_forward) { const double start_distance = autoware::motion_utils::calcSignedArcLength( @@ -755,9 +742,9 @@ BehaviorModuleOutput StartPlannerModule::plan() path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, - {start_distance, finish_distance}, steering_factor_direction, SteeringFactor::TURNING, ""); + planning_factor_interface_->add( + start_distance, finish_distance, status_.pull_out_path.start_pose, + status_.pull_out_path.end_pose, planning_factor_direction, SafetyFactorArray{}); setDebugData(); return output; } @@ -765,9 +752,9 @@ BehaviorModuleOutput StartPlannerModule::plan() path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - steering_factor_direction, SteeringFactor::TURNING, ""); + planning_factor_interface_->add( + 0.0, distance, status_.pull_out_path.start_pose, status_.pull_out_path.end_pose, + planning_factor_direction, SafetyFactorArray{}); setDebugData(); @@ -850,7 +837,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() setDrivableAreaInfo(output); - const auto steering_factor_direction = getSteeringFactorDirection(output); + const auto planning_factor_direction = getPlanningFactorDirection(output); if (status_.driving_forward) { const double start_distance = autoware::motion_utils::calcSignedArcLength( @@ -860,10 +847,9 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, - {start_distance, finish_distance}, steering_factor_direction, SteeringFactor::APPROACHING, - ""); + planning_factor_interface_->add( + start_distance, finish_distance, status_.pull_out_path.start_pose, + status_.pull_out_path.end_pose, planning_factor_direction, SafetyFactorArray{}); setDebugData(); return output; @@ -872,9 +858,9 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - steering_factor_direction, SteeringFactor::APPROACHING, ""); + planning_factor_interface_->add( + 0.0, distance, status_.pull_out_path.start_pose, status_.pull_out_path.end_pose, + planning_factor_direction, SafetyFactorArray{}); setDebugData(); @@ -986,17 +972,16 @@ bool StartPlannerModule::findPullOutPath( const bool backward_is_unnecessary = backwards_distance < epsilon; planner->setCollisionCheckMargin(collision_check_margin); - planner->setPlannerData(planner_data_); PlannerDebugData debug_data{ planner->getPlannerType(), {}, collision_check_margin, backwards_distance}; - const auto pull_out_path = planner->plan(start_pose_candidate, goal_pose, debug_data); + const auto pull_out_path = + planner->plan(start_pose_candidate, goal_pose, planner_data_, debug_data); debug_data_vector.push_back(debug_data); // If no path is found, return false if (!pull_out_path) { return false; } - if (backward_is_unnecessary) { updateStatusWithCurrentPath(*pull_out_path, start_pose_candidate, planner->getPlannerType()); return true; @@ -1595,9 +1580,9 @@ std::optional StartPlannerModule::planFreespacePath( for (const auto & p : center_line_path.points) { const Pose end_pose = p.point.pose; - freespace_planner_->setPlannerData(planner_data); PlannerDebugData debug_data{freespace_planner_->getPlannerType(), {}, 0.0, 0.0}; - auto freespace_path = freespace_planner_->plan(current_pose, end_pose, debug_data); + auto freespace_path = + freespace_planner_->plan(current_pose, end_pose, planner_data, debug_data); DEBUG_PRINT( "\nFreespace Pull out path search results\n%s%s", debug_data.header_str().c_str(), debug_data.str().c_str()); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/include/start_planner_test_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/include/start_planner_test_helper.hpp new file mode 100644 index 0000000000000..a1d294b279ff6 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/include/start_planner_test_helper.hpp @@ -0,0 +1,39 @@ +// 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. +#pragma once + +#include +#include + +#include + +namespace autoware::behavior_path_planner::testing +{ + +class StartPlannerTestHelper +{ +public: + static rclcpp::NodeOptions make_node_options(); + + static void set_odometry( + std::shared_ptr & planner_data, const geometry_msgs::msg::Pose & start_pose); + static void set_route( + std::shared_ptr & planner_data, const int route_start_lane_id, + const int route_goal_lane_id); + static void set_costmap( + std::shared_ptr & planner_data, const geometry_msgs::msg::Pose & start_pose, + const double grid_resolution, const double grid_length_x, const double grid_length_y); +}; + +} // namespace autoware::behavior_path_planner::testing diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/src/start_planner_test_helper.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/src/start_planner_test_helper.cpp new file mode 100644 index 0000000000000..0f0720edc4a15 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/src/start_planner_test_helper.cpp @@ -0,0 +1,98 @@ +// 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. +// pull_out_test_utils.cpp +#include "start_planner_test_helper.hpp" + +#include +#include + +#include +#include + +namespace autoware::behavior_path_planner::testing +{ +using autoware::test_utils::get_absolute_path_to_config; +using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; + +rclcpp::NodeOptions StartPlannerTestHelper::make_node_options() +{ + // 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 StartPlannerTestHelper::set_odometry( + std::shared_ptr & planner_data, const geometry_msgs::msg::Pose & start_pose) +{ + auto odometry = std::make_shared(); + odometry->pose.pose = start_pose; + odometry->header.frame_id = "map"; + planner_data->self_odometry = odometry; +} + +void StartPlannerTestHelper::set_route( + std::shared_ptr & planner_data, const int route_start_lane_id, + const int route_goal_lane_id) +{ + 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); + + const auto route = makeBehaviorRouteFromLaneId( + route_start_lane_id, route_goal_lane_id, "autoware_test_utils", + "road_shoulder/lanelet2_map.osm"); + route_handler->setRoute(route); + planner_data->route_handler = route_handler; +} + +void StartPlannerTestHelper::set_costmap( + std::shared_ptr & planner_data, const geometry_msgs::msg::Pose & start_pose, + const double grid_resolution, const double grid_length_x, const double grid_length_y) +{ + nav_msgs::msg::OccupancyGrid costmap; + costmap.header.frame_id = "map"; + costmap.info.width = static_cast(grid_length_x / grid_resolution); + costmap.info.height = static_cast(grid_length_y / grid_resolution); + costmap.info.resolution = grid_resolution; + + costmap.info.origin.position.x = start_pose.position.x - grid_length_x / 2; + costmap.info.origin.position.y = start_pose.position.y - grid_length_y / 2; + costmap.data = std::vector(costmap.info.width * costmap.info.height, 0); + + planner_data->costmap = std::make_shared(costmap); +} + +} // namespace autoware::behavior_path_planner::testing diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp new file mode 100644 index 0000000000000..5078c463d7c36 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp @@ -0,0 +1,113 @@ +// 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 "start_planner_test_helper.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +using autoware::behavior_path_planner::FreespacePullOut; +using autoware::behavior_path_planner::StartPlannerParameters; +using autoware::test_utils::get_absolute_path_to_config; +using autoware_planning_msgs::msg::LaneletRoute; +using RouteSections = std::vector; +using autoware::behavior_path_planner::testing::StartPlannerTestHelper; +using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; + +namespace autoware::behavior_path_planner +{ + +class TestFreespacePullOut : public ::testing::Test +{ +public: + std::optional call_plan( + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) + { + return freespace_pull_out_->plan(start_pose, goal_pose, planner_data, planner_debug_data); + } + +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + node_ = + rclcpp::Node::make_shared("freespace_pull_out", StartPlannerTestHelper::make_node_options()); + + planner_data_ = std::make_shared(); + planner_data_->init_parameters(*node_); + + initialize_freespace_pull_out_planner(); + } + + void TearDown() override { rclcpp::shutdown(); } + + // Member variables + std::shared_ptr node_; + std::shared_ptr freespace_pull_out_; + std::shared_ptr planner_data_; + +private: + void initialize_freespace_pull_out_planner() + { + auto parameters = StartPlannerParameters::init(*node_); + freespace_pull_out_ = std::make_shared(*node_, parameters); + } +}; + +TEST_F(TestFreespacePullOut, DISABLED_GenerateValidFreespacePullOutPath) +{ + const auto start_pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(299.796).y(303.529).z(100.000)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(-0.748629).w( + 0.662990)); + + const auto goal_pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(280.721).y(301.025).z(100.000)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(0.991718).w( + 0.128435)); + + StartPlannerTestHelper::set_odometry(planner_data_, start_pose); + StartPlannerTestHelper::set_route(planner_data_, 508, 720); + StartPlannerTestHelper::set_costmap(planner_data_, start_pose, 0.3, 70.0, 70.0); + + // Plan the pull out path + PlannerDebugData debug_data; + auto result = call_plan(start_pose, goal_pose, planner_data_, debug_data); + + // Assert that a valid Freespace pull out path is generated + ASSERT_TRUE(result.has_value()) << "Freespace pull out path generation failed."; + // EXPECT_EQ(result->partial_paths.size(), 2UL) + // << "Freespace pull out path does not have the expected number of partial paths."; + EXPECT_EQ(debug_data.conditions_evaluation.back(), "success") + << "Freespace pull out path planning did not succeed."; +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp index fb39e186afd4e..4548c058b871d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "start_planner_test_helper.hpp" + #include #include #include #include -#include #include #include #include @@ -24,18 +25,16 @@ #include -#include #include -#include #include #include using autoware::behavior_path_planner::GeometricPullOut; 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::behavior_path_planner::testing::StartPlannerTestHelper; using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; namespace autoware::behavior_path_planner @@ -44,143 +43,35 @@ namespace autoware::behavior_path_planner class TestGeometricPullOut : public ::testing::Test { public: - std::optional plan( - const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) + std::optional call_plan( + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) { - return geometric_pull_out_->plan(start_pose, goal_pose, planner_debug_data); + return geometric_pull_out_->plan(start_pose, goal_pose, planner_data, planner_debug_data); } protected: void SetUp() override { rclcpp::init(0, nullptr); - node_ = rclcpp::Node::make_shared("geometric_pull_out", get_node_options()); + node_ = + rclcpp::Node::make_shared("geometric_pull_out", StartPlannerTestHelper::make_node_options()); - load_parameters(); - initialize_vehicle_info(); - initialize_lane_departure_checker(); - initialize_routeHandler(); initialize_geometric_pull_out_planner(); - initialize_planner_data(); } - void TearDown() override { rclcpp::shutdown(); } + // Member variables std::shared_ptr node_; - std::shared_ptr route_handler_; - autoware::vehicle_info_utils::VehicleInfo vehicle_info_; std::shared_ptr geometric_pull_out_; - std::shared_ptr lane_departure_checker_; - PlannerData planner_data_; private: - rclcpp::NodeOptions get_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 load_parameters() - { - const auto dp_double = [&](const std::string & s) { - return node_->declare_parameter(s); - }; - const auto dp_bool = [&](const std::string & s) { return node_->declare_parameter(s); }; - // Load parameters required for planning - const std::string ns = "start_planner."; - lane_departure_check_expansion_margin_ = - dp_double(ns + "lane_departure_check_expansion_margin"); - pull_out_max_steer_angle_ = dp_double(ns + "pull_out_max_steer_angle"); - pull_out_arc_path_interval_ = dp_double(ns + "arc_path_interval"); - center_line_path_interval_ = dp_double(ns + "center_line_path_interval"); - th_moving_object_velocity_ = dp_double(ns + "th_moving_object_velocity"); - divide_pull_out_path_ = dp_bool(ns + "divide_pull_out_path"); - backward_path_length_ = dp_double("backward_path_length"); - forward_path_length_ = dp_double("forward_path_length"); - } - - void initialize_vehicle_info() - { - vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*node_).getVehicleInfo(); - } - - void initialize_lane_departure_checker() - { - lane_departure_checker_ = std::make_shared(); - lane_departure_checker_->setVehicleInfo(vehicle_info_); - - autoware::lane_departure_checker::Param lane_departure_checker_params{}; - lane_departure_checker_params.footprint_extra_margin = lane_departure_check_expansion_margin_; - lane_departure_checker_->setParam(lane_departure_checker_params); - } - - void initialize_routeHandler() - { - // 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); - - route_handler_ = std::make_shared(map_bin_msg); - } - void initialize_geometric_pull_out_planner() { - auto parameters = std::make_shared(); - parameters->parallel_parking_parameters.pull_out_max_steer_angle = pull_out_max_steer_angle_; - parameters->parallel_parking_parameters.pull_out_arc_path_interval = - pull_out_arc_path_interval_; - parameters->parallel_parking_parameters.center_line_path_interval = center_line_path_interval_; - parameters->th_moving_object_velocity = th_moving_object_velocity_; - parameters->divide_pull_out_path = divide_pull_out_path_; - - auto time_keeper = std::make_shared(); - geometric_pull_out_ = - std::make_shared(*node_, *parameters, lane_departure_checker_, time_keeper); - } + auto parameters = StartPlannerParameters::init(*node_); - void initialize_planner_data() - { - planner_data_.parameters.backward_path_length = backward_path_length_; - planner_data_.parameters.forward_path_length = forward_path_length_; - planner_data_.parameters.wheel_base = vehicle_info_.wheel_base_m; - planner_data_.parameters.wheel_tread = vehicle_info_.wheel_tread_m; - planner_data_.parameters.front_overhang = vehicle_info_.front_overhang_m; - planner_data_.parameters.left_over_hang = vehicle_info_.left_overhang_m; - planner_data_.parameters.right_over_hang = vehicle_info_.right_overhang_m; + geometric_pull_out_ = std::make_shared(*node_, parameters); } - - // Parameter variables - double lane_departure_check_expansion_margin_{0.0}; - double pull_out_max_steer_angle_{0.0}; - double pull_out_arc_path_interval_{0.0}; - double center_line_path_interval_{0.0}; - double th_moving_object_velocity_{0.0}; - double backward_path_length_{0.0}; - double forward_path_length_{0.0}; - bool divide_pull_out_path_{false}; }; TEST_F(TestGeometricPullOut, GenerateValidGeometricPullOutPath) @@ -199,26 +90,16 @@ TEST_F(TestGeometricPullOut, GenerateValidGeometricPullOutPath) geometry_msgs::build().x(0.0).y(0.0).z(0.705897).w( 0.708314)); - // 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( - 4619, 4635, "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_; - geometric_pull_out_->setPlannerData(std::make_shared(planner_data_)); + auto planner_data = std::make_shared(); + planner_data->init_parameters(*node_); + StartPlannerTestHelper::set_odometry(planner_data, start_pose); + StartPlannerTestHelper::set_route(planner_data, 4619, 4635); // Plan the pull out path PlannerDebugData debug_data; - auto result = plan(start_pose, goal_pose, debug_data); + auto result = call_plan(start_pose, goal_pose, planner_data, debug_data); - // Assert that a valid geometric geometric pull out path is generated + // Assert that a valid geometric pull out path is generated ASSERT_TRUE(result.has_value()) << "Geometric pull out path generation failed."; EXPECT_EQ(result->partial_paths.size(), 2UL) << "Generated geometric pull out path does not have the expected number of partial paths."; 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 index 156cf62f9ac4a..d2c7c60e1a4ca 100644 --- 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 @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "start_planner_test_helper.hpp" + #include #include #include #include -#include #include #include #include @@ -25,15 +26,16 @@ #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::behavior_path_planner::testing::StartPlannerTestHelper; using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; namespace autoware::behavior_path_planner @@ -43,105 +45,34 @@ class TestShiftPullOut : public ::testing::Test { public: std::optional call_plan( - const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) { - return shift_pull_out_->plan(start_pose, goal_pose, planner_debug_data); + return shift_pull_out_->plan(start_pose, goal_pose, planner_data, planner_debug_data); } protected: void SetUp() override { rclcpp::init(0, nullptr); - node_ = rclcpp::Node::make_shared("shift_pull_out", make_node_options()); + node_ = + rclcpp::Node::make_shared("shift_pull_out", StartPlannerTestHelper::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); + shift_pull_out_ = std::make_shared(*node_, parameters); } }; @@ -160,13 +91,14 @@ TEST_F(TestShiftPullOut, GenerateValidShiftPullOutPath) .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)); + auto planner_data = std::make_shared(); + planner_data->init_parameters(*node_); + StartPlannerTestHelper::set_odometry(planner_data, start_pose); + StartPlannerTestHelper::set_route(planner_data, 4619, 4635); // Plan the pull out path PlannerDebugData debug_data; - auto result = call_plan(start_pose, goal_pose, debug_data); + auto result = call_plan(start_pose, goal_pose, planner_data, debug_data); // Assert that a valid shift pull out path is generated ASSERT_TRUE(result.has_value()) << "shift pull out path generation failed."; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp index 895390f91cc16..6296acd8e71f7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp @@ -41,7 +41,7 @@ class StaticObstacleAvoidanceModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp index 5848cc75f148e..247e22e1aedad 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -46,7 +46,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface); CandidateOutput planCandidate() const override; BehaviorModuleOutput plan() override; @@ -131,9 +132,9 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface } if (finish_distance > -1.0e-03) { - steering_factor_interface_.set( - {left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance}, - SteeringFactor::LEFT, SteeringFactor::TURNING, ""); + planning_factor_interface_->add( + start_distance, finish_distance, left_shift.start_pose, left_shift.finish_pose, + PlanningFactor::SHIFT_LEFT, SafetyFactorArray{}); } } @@ -151,9 +152,9 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface } if (finish_distance > -1.0e-03) { - steering_factor_interface_.set( - {right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance}, - SteeringFactor::RIGHT, SteeringFactor::TURNING, ""); + planning_factor_interface_->add( + start_distance, finish_distance, right_shift.start_pose, right_shift.finish_pose, + PlanningFactor::SHIFT_RIGHT, SafetyFactorArray{}); } } } 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 48729c9c4fa0c..739b5b21f59dc 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 @@ -79,14 +79,13 @@ StaticObstacleAvoidanceModule::StaticObstacleAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT helper_{std::make_shared(parameters)}, parameters_{parameters}, generator_{parameters} { - steering_factor_interface_.init(PlanningBehavior::AVOIDANCE); - velocity_factor_interface_.init(PlanningBehavior::AVOIDANCE); } bool StaticObstacleAvoidanceModule::isExecutionRequested() const @@ -776,7 +775,7 @@ void StaticObstacleAvoidanceModule::updateEgoBehavior( insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path); - setVelocityFactor(path.path); + set_longitudinal_planning_factor(path.path); } bool StaticObstacleAvoidanceModule::isSafePath( @@ -1209,13 +1208,12 @@ CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const output.start_distance_to_path_change = sl_front.start_longitudinal; output.finish_distance_to_path_change = sl_back.end_longitudinal; - const uint16_t steering_factor_direction = std::invoke([&output]() { - return output.lateral_shift > 0.0 ? SteeringFactor::LEFT : SteeringFactor::RIGHT; + const uint16_t planning_factor_direction = std::invoke([&output]() { + return output.lateral_shift > 0.0 ? PlanningFactor::SHIFT_LEFT : PlanningFactor::SHIFT_RIGHT; }); - steering_factor_interface_.set( - {sl_front.start, sl_back.end}, - {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - steering_factor_direction, SteeringFactor::APPROACHING, ""); + planning_factor_interface_->add( + output.start_distance_to_path_change, output.finish_distance_to_path_change, sl_front.start, + sl_back.end, planning_factor_direction, SafetyFactorArray{}, true, 0.0, output.lateral_shift); output.path_candidate = shifted_path.path; return output; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt index b4a688d711221..220c380eb0fd7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.14) project(autoware_behavior_velocity_blind_spot_module) +option(EXPORT_TEST_PLOT_FIGURE "Export plot figures in test" OFF) + find_package(autoware_cmake REQUIRED) autoware_package() pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) @@ -11,6 +13,20 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene.cpp src/decisions.cpp src/util.cpp + src/parameter.cpp ) -ament_auto_package(INSTALL_TO_SHARE config) +if(BUILD_TESTING) + if(EXPORT_TEST_PLOT_FIGURE) + add_definitions(-DEXPORT_TEST_PLOT_FIGURE "-Wno-attributes") # // cspell: ignore DEXPORT + endif() + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + # NOTE(soblin): pybind11::scoped_interpreter needs to be initialized globally, not in the FixtureClass instantiated for each test suite + ament_add_gtest(test_${PROJECT_NAME}_util + test/test_util.cpp + ) + target_link_libraries(test_${PROJECT_NAME}_util ${PROJECT_NAME}) +endif() + +ament_auto_package(INSTALL_TO_SHARE config test_data) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp index f296ad03cbac6..41330e459a524 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp @@ -15,11 +15,12 @@ #ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__MANAGER_HPP_ #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__MANAGER_HPP_ +#include "autoware/behavior_velocity_blind_spot_module/parameter.hpp" #include "autoware/behavior_velocity_blind_spot_module/scene.hpp" #include #include -#include +#include #include #include @@ -38,12 +39,12 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC const char * getModuleName() override { return "blind_spot"; } private: - BlindSpotModule::PlannerParam planner_param_; + PlannerParam planner_param_; void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class BlindSpotModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/parameter.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/parameter.hpp new file mode 100644 index 0000000000000..59322caecb62f --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/parameter.hpp @@ -0,0 +1,40 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__PARAMETER_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__PARAMETER_HPP_ + +#include + +#include + +namespace autoware::behavior_velocity_planner +{ +struct PlannerParam +{ + static PlannerParam init(rclcpp::Node & node, const std::string & ns); + bool use_pass_judge_line{}; + double stop_line_margin{}; + double backward_detection_length{}; + double ignore_width_from_center_line{}; + double adjacent_extend_width{}; + double opposite_adjacent_extend_width{}; + double max_future_movement_time{}; + double ttc_min{}; + double ttc_max{}; + double ttc_ego_minimal_velocity{}; +}; +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__PARAMETER_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp index 1bfa41d86ffbe..91eff0b01346a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp @@ -15,9 +15,10 @@ #ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_ #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_ +#include #include -#include #include +#include #include #include @@ -59,7 +60,7 @@ struct Safe using BlindSpotDecision = std::variant; -class BlindSpotModule : public SceneModuleInterface +class BlindSpotModule : public SceneModuleInterfaceWithRTC { public: struct DebugData @@ -70,24 +71,12 @@ class BlindSpotModule : public SceneModuleInterface }; public: - struct PlannerParam - { - bool use_pass_judge_line; - double stop_line_margin; - double backward_detection_length; - double ignore_width_from_center_line; - double adjacent_extend_width; - double opposite_adjacent_extend_width; - double max_future_movement_time; - double ttc_min; - double ttc_max; - double ttc_ego_minimal_velocity; - }; - BlindSpotModule( const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, const std::shared_ptr planner_data, const PlannerParam & planner_param, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp index 9d908414b6d95..e18d96709ef92 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__UTIL_HPP_ #include +#include #include @@ -25,6 +26,7 @@ #include #include #include +#include namespace autoware::behavior_velocity_planner { @@ -50,31 +52,49 @@ std::optional generateInterpolatedPathInfo( const lanelet::Id lane_id, const tier4_planning_msgs::msg::PathWithLaneId & input_path, rclcpp::Logger logger); +std::optional getFirstPointIntersectsLineByFootprint( + const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length); + std::optional getSiblingStraightLanelet( const lanelet::Lanelet assigned_lane, const lanelet::routing::RoutingGraphConstPtr routing_graph_ptr); /** - * @brief Create half lanelet - * @param lanelet input lanelet - * @return Half lanelet + * @brief generate a new lanelet object on the `turn_direction` side of `lanelet` which is offset + * from `ignore_width_from_centerline` from the centerline of `lanelet` + * @return new lanelet object */ lanelet::ConstLanelet generateHalfLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection & turn_direction, const double ignore_width_from_centerline); +/** + * @brief generate a new lanelet object from the `turn_direction` side neighboring lanelet of the + * input `lanelet` by the width of `adjacent_extend_width` + * @param new lanelet object + */ lanelet::ConstLanelet generateExtendedAdjacentLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection direction, const double adjacent_extend_width); + +/** + * @brief generate a new lanelet object from the `turn_direction` side neighboring opposite lanelet + * of the input `lanelet` by the width of `opposite_adjacent_extend_width` + * @param new lanelet object + */ lanelet::ConstLanelet generateExtendedOppositeAdjacentLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection direction, const double opposite_adjacent_extend_width); +std::vector find_lane_ids_upto( + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::Id lane_id); + lanelet::ConstLanelets generateBlindSpotLanelets( const std::shared_ptr route_handler, - const TurnDirection turn_direction, const lanelet::Id lane_id, - const tier4_planning_msgs::msg::PathWithLaneId & path, const double ignore_width_from_centerline, - const double adjacent_extend_width, const double opposite_adjacent_extend_width); + const TurnDirection turn_direction, const std::vector & lane_ids_upto_intersection, + const double ignore_width_from_centerline, const double adjacent_extend_width, + const double opposite_adjacent_extend_width); /** * @brief Make blind spot areas. Narrow area is made from closest path point to stop line index. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml index a2ea4a82a884d..dabd3045b31d2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml @@ -17,12 +17,15 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_test_utils autoware_universe_utils geometry_msgs pluginlib @@ -31,6 +34,7 @@ tier4_planning_msgs visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp index 64e1435167a89..a978c074e0018 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp @@ -101,8 +101,11 @@ void BlindSpotModule::reactRTCApprovalByDecision( decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, + "blind_spot(module is judging as UNSAFE)"); } return; } @@ -132,8 +135,11 @@ void BlindSpotModule::reactRTCApprovalByDecision( decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, + "blind_spot(module is judging as SAFE and RTC is not approved)"); } return; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp index cc63b42df68e4..ee6516f025709 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp @@ -34,23 +34,7 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(BlindSpotModuleManager::getModuleName()); - planner_param_.use_pass_judge_line = - getOrDeclareParameter(node, ns + ".use_pass_judge_line"); - planner_param_.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); - planner_param_.backward_detection_length = - getOrDeclareParameter(node, ns + ".backward_detection_length"); - planner_param_.ignore_width_from_center_line = - getOrDeclareParameter(node, ns + ".ignore_width_from_center_line"); - planner_param_.adjacent_extend_width = - getOrDeclareParameter(node, ns + ".adjacent_extend_width"); - planner_param_.opposite_adjacent_extend_width = - getOrDeclareParameter(node, ns + ".opposite_adjacent_extend_width"); - planner_param_.max_future_movement_time = - getOrDeclareParameter(node, ns + ".max_future_movement_time"); - planner_param_.ttc_min = getOrDeclareParameter(node, ns + ".ttc_min"); - planner_param_.ttc_max = getOrDeclareParameter(node, ns + ".ttc_max"); - planner_param_.ttc_ego_minimal_velocity = - getOrDeclareParameter(node, ns + ".ttc_ego_minimal_velocity"); + planner_param_ = PlannerParam::init(node, ns); } void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) @@ -75,7 +59,7 @@ void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa registerModule(std::make_shared( module_id, lane_id, turn_direction, planner_data_, planner_param_, - logger_.get_child("blind_spot_module"), clock_)); + logger_.get_child("blind_spot_module"), clock_, time_keeper_, planning_factor_interface_)); generateUUID(module_id); updateRTCStatus( getUUID(module_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), @@ -83,7 +67,7 @@ void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa } } -std::function &)> +std::function &)> BlindSpotModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp new file mode 100644 index 0000000000000..0984de6f73262 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp @@ -0,0 +1,45 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_velocity_blind_spot_module/parameter.hpp" + +#include + +#include + +namespace autoware::behavior_velocity_planner +{ + +PlannerParam PlannerParam::init(rclcpp::Node & node, const std::string & ns) +{ + using autoware::universe_utils::getOrDeclareParameter; + PlannerParam param; + param.use_pass_judge_line = getOrDeclareParameter(node, ns + ".use_pass_judge_line"); + param.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); + param.backward_detection_length = + getOrDeclareParameter(node, ns + ".backward_detection_length"); + param.ignore_width_from_center_line = + getOrDeclareParameter(node, ns + ".ignore_width_from_center_line"); + param.adjacent_extend_width = getOrDeclareParameter(node, ns + ".adjacent_extend_width"); + param.opposite_adjacent_extend_width = + getOrDeclareParameter(node, ns + ".opposite_adjacent_extend_width"); + param.max_future_movement_time = + getOrDeclareParameter(node, ns + ".max_future_movement_time"); + param.ttc_min = getOrDeclareParameter(node, ns + ".ttc_min"); + param.ttc_max = getOrDeclareParameter(node, ns + ".ttc_max"); + param.ttc_ego_minimal_velocity = + getOrDeclareParameter(node, ns + ".ttc_ego_minimal_velocity"); + return param; +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index 7697786adb19d..4fca5f821f934 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -40,14 +40,15 @@ namespace bg = boost::geometry; BlindSpotModule::BlindSpotModule( const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, const std::shared_ptr planner_data, const PlannerParam & planner_param, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), planner_param_{planner_param}, turn_direction_(turn_direction), is_over_pass_judge_line_(false) { - velocity_factor_.init(PlanningBehavior::REAR_CHECK); sibling_straight_lanelet_ = getSiblingStraightLanelet( planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_), planner_data->route_handler_->getRoutingGraphPtr()); @@ -101,8 +102,9 @@ BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(PathWithLaneId * pat } if (!blind_spot_lanelets_) { + const auto lane_ids_upto_intersection = find_lane_ids_upto(input_path, lane_id_); const auto blind_spot_lanelets = generateBlindSpotLanelets( - planner_data_->route_handler_, turn_direction_, lane_id_, input_path, + planner_data_->route_handler_, turn_direction_, lane_ids_upto_intersection, planner_param_.ignore_width_from_center_line, planner_param_.adjacent_extend_width, planner_param_.opposite_adjacent_extend_width); if (!blind_spot_lanelets.empty()) { @@ -179,27 +181,6 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path) return true; } -static std::optional getFirstPointIntersectsLineByFootprint( - const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, - const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) -{ - const auto & path_ip = interpolated_path_info.path; - const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); - const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); - const size_t start = - static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); - const auto line2d = line.basicLineString(); - for (auto i = start; i <= lane_end; ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = autoware::universe_utils::transformVector( - footprint, autoware::universe_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, line2d)) { - return std::make_optional(i); - } - } - return std::nullopt; -} - static std::optional getDuplicatedPointIdx( const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp index 5c5aa0a26b3b4..8451661b2b71f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp @@ -88,6 +88,27 @@ std::optional generateInterpolatedPathInfo( return interpolated_path_info; } +std::optional getFirstPointIntersectsLineByFootprint( + const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); + const auto line2d = line.basicLineString(); + for (auto i = start; i <= lane_end; ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = autoware::universe_utils::transformVector( + footprint, autoware::universe_utils::pose2transform(base_pose)); + if (boost::geometry::intersects(path_footprint, line2d)) { + return std::make_optional(i); + } + } + return std::nullopt; +} + std::optional getSiblingStraightLanelet( const lanelet::Lanelet assigned_lane, const lanelet::routing::RoutingGraphConstPtr routing_graph_ptr) @@ -200,15 +221,9 @@ static lanelet::LineString3d removeConst(lanelet::ConstLineString3d line) return lanelet::LineString3d(lanelet::InvalId, pts); } -lanelet::ConstLanelets generateBlindSpotLanelets( - const std::shared_ptr route_handler, - const TurnDirection turn_direction, const lanelet::Id lane_id, - const tier4_planning_msgs::msg::PathWithLaneId & path, const double ignore_width_from_centerline, - const double adjacent_extend_width, const double opposite_adjacent_extend_width) +std::vector find_lane_ids_upto( + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::Id lane_id) { - const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); - const auto routing_graph_ptr = route_handler->getRoutingGraphPtr(); - std::vector lane_ids; /* get lane ids until intersection */ for (const auto & point : path.points) { @@ -216,19 +231,29 @@ lanelet::ConstLanelets generateBlindSpotLanelets( for (const auto id : point.lane_ids) { if (id == lane_id) { found_intersection_lane = true; - lane_ids.push_back(lane_id); break; } // make lane_ids unique - if (std::find(lane_ids.begin(), lane_ids.end(), lane_id) == lane_ids.end()) { - lane_ids.push_back(lane_id); + if (std::find(lane_ids.begin(), lane_ids.end(), id) == lane_ids.end()) { + lane_ids.push_back(id); } } if (found_intersection_lane) break; } + return lane_ids; +} + +lanelet::ConstLanelets generateBlindSpotLanelets( + const std::shared_ptr route_handler, + const TurnDirection turn_direction, const std::vector & lane_ids_upto_intersection, + const double ignore_width_from_centerline, const double adjacent_extend_width, + const double opposite_adjacent_extend_width) +{ + const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); + const auto routing_graph_ptr = route_handler->getRoutingGraphPtr(); lanelet::ConstLanelets blind_spot_lanelets; - for (const auto i : lane_ids) { + for (const auto i : lane_ids_upto_intersection) { const auto lane = lanelet_map_ptr->laneletLayer.get(i); const auto ego_half_lanelet = generateHalfLanelet(lane, turn_direction, ignore_width_from_centerline); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..e0ceca4e12951 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "blind_spot", "autoware::behavior_velocity_planner::BlindSpotModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp new file mode 100644 index 0000000000000..5c2a239e4142f --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp @@ -0,0 +1,448 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#ifdef EXPORT_TEST_PLOT_FIGURE +#include +#include +#include +#include + +#include +#include // needed for passing std::string to Args + +#endif + +using autoware::test_utils::parse; + +class TestWithAdjLaneData : public ::testing::Test +{ +protected: + void SetUp() override + { + const auto test_data_file = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/object_on_adj_lane.yaml"; + const auto config = YAML::LoadFile(test_data_file); + const auto route = parse(config["route"]); + const auto map_path = + autoware::test_utils::resolve_pkg_share_uri(config["map_path_uri"].as()); + if (!map_path) { + ASSERT_DEATH({ assert(false); }, "invalid map path"); + } + const auto intersection_map = autoware::test_utils::make_map_bin_msg(map_path.value()); + route_handler = std::make_shared(); + route_handler->setMap(intersection_map); + route_handler->setRoute(route); + self_odometry = autoware::test_utils::create_const_shared_ptr( + parse(config["self_odometry"])); + dynamic_object = autoware::test_utils::create_const_shared_ptr( + parse(config["dynamic_object"])); + path_with_lane_id = + parse(config["path_with_lane_id"]); + + // parameter + auto node_options = rclcpp::NodeOptions{}; + node_options.arguments(std::vector{ + "--ros-args", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/config/blind_spot.param.yaml", + }); + + auto node = rclcpp::Node::make_shared("blind_spot_test", node_options); + param = autoware::behavior_velocity_planner::PlannerParam::init(*node, "blind_spot"); + } + + std::shared_ptr route_handler{}; + std::shared_ptr self_odometry{}; + std::shared_ptr dynamic_object{}; + const lanelet::Id lane_id_{2200}; + tier4_planning_msgs::msg::PathWithLaneId path_with_lane_id; + autoware::behavior_velocity_planner::PlannerParam param; +}; + +TEST_F(TestWithAdjLaneData, getSiblingStraightLanelet) +{ + const auto sibling_straight_lanelet_opt = + autoware::behavior_velocity_planner::getSiblingStraightLanelet( + route_handler->getLaneletMapPtr()->laneletLayer.get(lane_id_), + route_handler->getRoutingGraphPtr()); + ASSERT_NO_FATAL_FAILURE({ ASSERT_TRUE(sibling_straight_lanelet_opt.has_value()); }); + EXPECT_EQ(sibling_straight_lanelet_opt.value().id(), 2100); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(lane_id_), ax, + autoware::test_utils::LaneConfig{"intersection turning lanes", LineConfig{"blue"}}); + + // for illustration + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(3010933), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"green"}}); + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(2201), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"blue"}}); + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(2202), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"blue"}}); + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(2010), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"green"}}); + + const auto [x0, x1] = ax.get_xlim(); + const auto [y0, y1] = ax.get_ylim(); + const double width = x1 - x0; + const double height = y1 - y0; + ax.set_xlim(Args(x0, x0 + width * 1.3)); + ax.set_ylim(Args(y0, y0 + height * 1.3)); + autoware::test_utils::plot_lanelet2_object( + sibling_straight_lanelet_opt.value(), ax, + LaneConfig{"sibling_straight_lanelet", LineConfig{"red"}}); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateHalfLanelet) +{ + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(2010); + + const auto half_lanelet = autoware::behavior_velocity_planner::generateHalfLanelet( + lanelet, autoware::behavior_velocity_planner::TurnDirection::LEFT, + param.ignore_width_from_center_line); + + /* + TODO(soblin): how to check if they overlap only on the left line string + EXPECT_EQ( + boost::geometry::within( + half_lanelet.polygon2d().basicPolygon(), lanelet.polygon2d().basicPolygon()), + true); + */ + EXPECT_EQ( + boost::geometry::area(lanelet.polygon2d().basicPolygon()) / 2.0 > + boost::geometry::area(half_lanelet.polygon2d().basicPolygon()), + true); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, autoware::test_utils::LaneConfig{"original", LineConfig{"blue", 1.5}}); + autoware::test_utils::plot_lanelet2_object( + half_lanelet, ax, LaneConfig{"half lanelet", LineConfig{"red", 0.75}}); + + const auto [x0, x1] = ax.get_xlim(); + const auto [y0, y1] = ax.get_ylim(); + const double width = x1 - x0; + // const double height = y1 - y0; + ax.set_xlim(Args(x0, x0 + width * 1.3)); + ax.set_ylim(Args(y0, 650)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateExtendedAdjacentLanelet) +{ + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(2010); + const auto adj_lanelet = *(route_handler->getRoutingGraphPtr()->adjacentLeft(lanelet)); + + const auto extended_adj_lanelet = + autoware::behavior_velocity_planner::generateExtendedAdjacentLanelet( + adj_lanelet, autoware::behavior_velocity_planner::TurnDirection::LEFT, + param.adjacent_extend_width); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, autoware::test_utils::LaneConfig{"given", LineConfig{"blue", 1.0}}); + autoware::test_utils::plot_lanelet2_object( + adj_lanelet, ax, autoware::test_utils::LaneConfig{"adjacent", LineConfig{"green", 1.0}}); + autoware::test_utils::plot_lanelet2_object( + extended_adj_lanelet, ax, LaneConfig{"generated", LineConfig{"red", 2.0}}); + + const auto [x0, x1] = ax.get_xlim(); + const auto [y0, y1] = ax.get_ylim(); + const double width = x1 - x0; + const double height = y1 - y0; + ax.set_xlim(Args(x0, x0 + width * 1.3)); + ax.set_ylim(Args(y0, y0 + height * 1.3)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateBlindSpotLanelets_left) +{ + const auto blind_spot_lanelets = autoware::behavior_velocity_planner::generateBlindSpotLanelets( + route_handler, autoware::behavior_velocity_planner::TurnDirection::LEFT, {2000, 2010}, + param.ignore_width_from_center_line, param.adjacent_extend_width, + param.opposite_adjacent_extend_width); + EXPECT_EQ(blind_spot_lanelets.size(), 2); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {2010, 3010933, 2200, 3010920, 3010933, 2000}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + for (const auto & blind_spot_lanelet : blind_spot_lanelets) { + autoware::test_utils::plot_lanelet2_object( + blind_spot_lanelet, ax, LaneConfig{"blind_spot_lanelet", LineConfig{"blue", 2.0}}); + } + const auto [y0, y1] = ax.get_ylim(); + const double height = y1 - y0; + ax.set_xlim(Args(300, 365)); + ax.set_ylim(Args(y0, y0 + height * 1.3)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateBlindSpotLanelets_right) +{ + const auto blind_spot_lanelets = autoware::behavior_velocity_planner::generateBlindSpotLanelets( + route_handler, autoware::behavior_velocity_planner::TurnDirection::RIGHT, {3008067}, + param.ignore_width_from_center_line, param.adjacent_extend_width, + param.opposite_adjacent_extend_width); + EXPECT_EQ(blind_spot_lanelets.size(), 1); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {3008057, 3008054, 3008056, 3008061, 3008062, 3008059, 3008067, 3008066}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + for (const auto & blind_spot_lanelet : blind_spot_lanelets) { + autoware::test_utils::plot_lanelet2_object( + blind_spot_lanelet, ax, LaneConfig{"blind_spot_lanelet", LineConfig{"blue", 2.0}}); + } + ax.set_xlim(Args(905, 920)); + ax.set_ylim(Args(650, 950)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateInterpolatedPathInfo) +{ + const auto interpolated_path_info_opt = + autoware::behavior_velocity_planner::generateInterpolatedPathInfo( + lane_id_, path_with_lane_id, rclcpp::get_logger("test")); + EXPECT_EQ(interpolated_path_info_opt.has_value(), true); + const auto & interpolated_path_info = interpolated_path_info_opt.value(); + EXPECT_EQ(interpolated_path_info.lane_id_interval.has_value(), true); + const auto [start, end] = interpolated_path_info.lane_id_interval.value(); + tier4_planning_msgs::msg::PathWithLaneId interpolated_path; + for (auto i = start; i <= end; ++i) { + interpolated_path.points.push_back(interpolated_path_info.path.points.at(i)); + } +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + using autoware::test_utils::PathWithLaneIdConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {2010, 3010933, 2200, 3010920, 3010933, 2000, 3500}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + autoware::test_utils::plot_autoware_object( + path_with_lane_id, ax, PathWithLaneIdConfig{"original path", "red", 0.75}); + autoware::test_utils::plot_autoware_object( + interpolated_path, ax, PathWithLaneIdConfig{"interpolated path on lane 2010", "blue", 1.0}); + const auto [x0, x1] = ax.get_xlim(); + ax.set_xlim(Args(335, x1)); + ax.set_ylim(Args(640, 670)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +class TestWithShoulderData : public ::testing::Test +{ +protected: + void SetUp() override + { + const auto test_data_file = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/object_on_shoulder.yaml"; + const auto config = YAML::LoadFile(test_data_file); + const auto route = parse(config["route"]); + const auto map_path = + autoware::test_utils::resolve_pkg_share_uri(config["map_path_uri"].as()); + if (!map_path) { + ASSERT_DEATH({ assert(false); }, "invalid map path"); + } + const auto intersection_map = autoware::test_utils::make_map_bin_msg(map_path.value()); + route_handler = std::make_shared(); + route_handler->setMap(intersection_map); + route_handler->setRoute(route); + self_odometry = autoware::test_utils::create_const_shared_ptr( + parse(config["self_odometry"])); + dynamic_object = autoware::test_utils::create_const_shared_ptr( + parse(config["dynamic_object"])); + path_with_lane_id = + parse(config["path_with_lane_id"]); + + // parameter + auto node_options = rclcpp::NodeOptions{}; + node_options.arguments(std::vector{ + "--ros-args", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/config/blind_spot.param.yaml", + }); + + auto node = rclcpp::Node::make_shared("blind_spot_test", node_options); + param = autoware::behavior_velocity_planner::PlannerParam::init(*node, "blind_spot"); + } + + std::shared_ptr route_handler{}; + std::shared_ptr self_odometry{}; + std::shared_ptr dynamic_object{}; + const lanelet::Id lane_id_{1010}; + tier4_planning_msgs::msg::PathWithLaneId path_with_lane_id; + autoware::behavior_velocity_planner::PlannerParam param; +}; + +TEST_F(TestWithShoulderData, generateBlindSpotLaneletsShoulder_left) +{ + const auto blind_spot_lanelets = autoware::behavior_velocity_planner::generateBlindSpotLanelets( + route_handler, autoware::behavior_velocity_planner::TurnDirection::LEFT, {1000, 1010}, + param.ignore_width_from_center_line, param.adjacent_extend_width, + param.opposite_adjacent_extend_width); + EXPECT_EQ(blind_spot_lanelets.size(), 2); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {1000, 1010, 3010907, 3010879, 1200, 1100}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + for (const auto & blind_spot_lanelet : blind_spot_lanelets) { + autoware::test_utils::plot_lanelet2_object( + blind_spot_lanelet, ax, LaneConfig{"blind_spot_lanelet", LineConfig{"blue", 2.0}}); + } + ax.set_xlim(Args(330, 365)); + ax.set_ylim(Args(580, 625)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper left"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +int main(int argc, char ** argv) +{ +#ifdef EXPORT_TEST_PLOT_FIGURE + py::scoped_interpreter guard{}; +#endif + rclcpp::init(0, nullptr); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_adj_lane.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_adj_lane.yaml new file mode 100644 index 0000000000000..97068d69cd296 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_adj_lane.yaml @@ -0,0 +1,2698 @@ +# +# AUTO GENERATED by autoware_test_utils::topic_snapshot_saver +# format1: +# +# format_version: +# map_path_uri: package:/// +# fields(this is array) +# - name: +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | PathWithLaneId | TBD} +# topic: +# +format_version: 1 +map_path_uri: package://autoware_test_utils/test_map/intersection/lanelet2_map.osm +path_with_lane_id: + header: + stamp: + sec: 563 + nanosec: 125840339 + frame_id: map + points: + - point: + pose: + position: + x: 330.010 + y: 643.206 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00655991 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 332.010 + y: 643.233 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00656125 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 334.009 + y: 643.259 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00658745 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 336.009 + y: 643.285 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00660718 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 338.009 + y: 643.312 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00658719 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 340.009 + y: 643.338 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00656661 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 342.009 + y: 643.364 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00657433 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 342.809 + y: 643.375 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0162350 + w: 0.999868 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - 2200 + - point: + pose: + position: + x: 344.008 + y: 643.414 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0355542 + w: 0.999368 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 346.003 + y: 643.556 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0600763 + w: 0.998194 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 347.986 + y: 643.796 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.134724 + w: 0.990883 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 349.912 + y: 644.329 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.199426 + w: 0.979913 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 351.776 + y: 645.121 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.281519 + w: 0.959556 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 353.448 + y: 646.194 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.345347 + w: 0.938475 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 354.997 + y: 647.513 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.407454 + w: 0.913226 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 356.322 + y: 648.989 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.480104 + w: 0.877212 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 357.418 + y: 650.701 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.544895 + w: 0.838504 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 358.228 + y: 652.525 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.610262 + w: 0.792199 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 358.741 + y: 654.467 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.646506 + w: 0.762909 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.070 + y: 656.445 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.697457 + w: 0.716627 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.124 + y: 658.438 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.713869 + w: 0.700280 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.085 + y: 660.438 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714886 + w: 0.699241 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.052 + y: 661.935 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - 3500 + - point: + pose: + position: + x: 359.040 + y: 662.438 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.993 + y: 664.437 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.945 + y: 666.436 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715334 + w: 0.698782 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.900 + y: 668.374 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714518 + w: 0.699617 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.858 + y: 670.373 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712894 + w: 0.701272 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.825 + y: 672.373 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710606 + w: 0.703590 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.807 + y: 674.174 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705159 + w: 0.709049 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.812 + y: 675.174 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705159 + w: 0.709049 + longitudinal_velocity_mps: 0.00000 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + left_bound: + - x: 329.488 + y: 644.822 + z: 100.000 + - x: 332.672 + y: 644.864 + z: 100.000 + - x: 334.352 + y: 644.886 + z: 100.000 + - x: 336.351 + y: 644.913 + z: 100.000 + - x: 337.719 + y: 644.931 + z: 100.000 + - x: 340.352 + y: 644.965 + z: 100.000 + - x: 342.813 + y: 645.020 + z: 100.000 + - x: 344.277 + y: 645.102 + z: 100.000 + - x: 346.200 + y: 645.256 + z: 100.000 + - x: 347.374 + y: 645.428 + z: 100.000 + - x: 348.470 + y: 645.697 + z: 100.000 + - x: 349.568 + y: 646.035 + z: 100.000 + - x: 350.592 + y: 646.513 + z: 100.000 + - x: 351.572 + y: 647.043 + z: 100.000 + - x: 352.507 + y: 647.713 + z: 100.000 + - x: 353.327 + y: 648.476 + z: 100.000 + - x: 354.109 + y: 649.196 + z: 100.000 + - x: 354.661 + y: 649.830 + z: 100.000 + - x: 355.275 + y: 650.523 + z: 100.000 + - x: 356.177 + y: 652.020 + z: 100.000 + - x: 356.615 + y: 653.084 + z: 100.000 + - x: 356.929 + y: 654.139 + z: 100.000 + - x: 357.146 + y: 655.241 + z: 100.000 + - x: 357.284 + y: 656.160 + z: 100.000 + - x: 357.312 + y: 657.420 + z: 100.000 + - x: 357.425 + y: 658.703 + z: 100.000 + - x: 357.383 + y: 660.465 + z: 100.000 + - x: 357.349 + y: 661.894 + z: 100.000 + - x: 357.386 + y: 664.700 + z: 100.000 + - x: 357.338 + y: 666.699 + z: 100.000 + - x: 357.291 + y: 668.699 + z: 100.000 + - x: 357.243 + y: 670.698 + z: 100.000 + - x: 357.195 + y: 672.698 + z: 100.000 + - x: 357.148 + y: 674.697 + z: 100.000 + - x: 357.021 + y: 680.023 + z: 100.000 + right_bound: + - x: 329.531 + y: 641.578 + z: 100.000 + - x: 332.717 + y: 641.619 + z: 100.000 + - x: 334.395 + y: 641.641 + z: 100.000 + - x: 336.394 + y: 641.668 + z: 100.000 + - x: 337.761 + y: 641.686 + z: 100.000 + - x: 340.394 + y: 641.721 + z: 100.000 + - x: 342.805 + y: 641.730 + z: 100.000 + - x: 344.462 + y: 641.823 + z: 100.000 + - x: 346.283 + y: 641.878 + z: 100.000 + - x: 347.708 + y: 641.985 + z: 100.000 + - x: 349.158 + y: 642.284 + z: 100.000 + - x: 350.570 + y: 642.681 + z: 100.000 + - x: 351.954 + y: 643.219 + z: 100.000 + - x: 353.269 + y: 643.933 + z: 100.000 + - x: 354.499 + y: 644.727 + z: 100.000 + - x: 355.646 + y: 645.678 + z: 100.000 + - x: 356.516 + y: 646.483 + z: 100.000 + - x: 357.282 + y: 647.228 + z: 100.000 + - x: 357.690 + y: 647.819 + z: 100.000 + - x: 358.527 + y: 649.081 + z: 100.000 + - x: 359.237 + y: 650.418 + z: 100.000 + - x: 359.789 + y: 651.774 + z: 100.000 + - x: 360.225 + y: 653.220 + z: 100.000 + - x: 360.523 + y: 654.391 + z: 100.000 + - x: 360.701 + y: 655.780 + z: 100.000 + - x: 360.976 + y: 657.487 + z: 100.000 + - x: 360.804 + y: 658.842 + z: 100.000 + - x: 360.774 + y: 660.766 + z: 100.000 + - x: 360.755 + y: 661.975 + z: 100.000 + - x: 360.585 + y: 664.776 + z: 100.000 + - x: 360.537 + y: 666.776 + z: 100.000 + - x: 360.490 + y: 668.775 + z: 100.000 + - x: 360.442 + y: 670.774 + z: 100.000 + - x: 360.394 + y: 672.774 + z: 100.000 + - x: 360.347 + y: 674.773 + z: 100.000 + - x: 360.220 + y: 680.099 + z: 100.000 +dynamic_object: + header: + stamp: + sec: 563 + nanosec: 160706650 + frame_id: map + objects: + - object_id: + uuid: + - 6 + - 144 + - 167 + - 221 + - 26 + - 196 + - 145 + - 186 + - 207 + - 95 + - 21 + - 211 + - 96 + - 13 + - 92 + - 5 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 404.688 + y: 647.065 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + covariance: + - 0.415785 + - -0.000621846 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000621846 + - 0.388031 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0108039 + initial_twist_with_covariance: + twist: + linear: + x: 9.96635 + y: 3.24754e-05 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 5.69744e-05 + covariance: + - 1.01780 + - -2.02150e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - -3.54650e-05 + - -2.02150e-05 + - 0.00403854 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00708516 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - -3.54650e-05 + - 0.00708516 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0124301 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 404.688 + y: 647.065 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 409.670 + y: 647.155 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 414.653 + y: 647.245 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 419.635 + y: 647.335 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 424.617 + y: 647.425 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 429.600 + y: 647.515 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 434.582 + y: 647.605 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 439.565 + y: 647.694 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 444.547 + y: 647.784 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 449.529 + y: 647.874 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 454.512 + y: 647.964 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 459.494 + y: 648.054 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 464.476 + y: 648.144 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 469.459 + y: 648.233 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 474.441 + y: 648.323 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 479.423 + y: 648.413 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 484.406 + y: 648.503 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 489.388 + y: 648.593 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 494.371 + y: 648.683 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 499.353 + y: 648.773 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 504.335 + y: 648.862 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 1.00000 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.90000 + y: 0.501209 + z: 0.690476 + - object_id: + uuid: + - 32 + - 119 + - 141 + - 16 + - 72 + - 85 + - 32 + - 153 + - 66 + - 136 + - 88 + - 177 + - 247 + - 166 + - 151 + - 80 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + covariance: + - 0.0657687 + - -0.000366900 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000366900 + - 0.0451891 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00300423 + initial_twist_with_covariance: + twist: + linear: + x: 10.0048 + y: -0.00124906 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: -0.00219133 + covariance: + - 0.522271 + - -6.09993e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000107016 + - -6.09993e-05 + - 0.00263830 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00462859 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - -0.000107016 + - 0.00462859 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00812034 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.785 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00453640 + w: 0.999990 + - position: + x: 333.933 + y: 644.723 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00622365 + w: 0.999981 + - position: + x: 338.935 + y: 644.640 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00828910 + w: 0.999966 + - position: + x: 343.877 + y: 644.600 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00409926 + w: 0.999992 + - position: + x: 348.624 + y: 645.122 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0547342 + w: 0.998501 + - position: + x: 352.898 + y: 646.993 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.204867 + w: 0.978790 + - position: + x: 356.221 + y: 650.176 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.372718 + w: 0.927945 + - position: + x: 358.029 + y: 654.521 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.554937 + w: 0.831892 + - position: + x: 358.484 + y: 659.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.673390 + w: 0.739287 + - position: + x: 358.380 + y: 664.391 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714418 + w: 0.699720 + - position: + x: 358.261 + y: 669.391 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.142 + y: 674.391 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.023 + y: 679.392 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.904 + y: 684.392 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.784 + y: 689.392 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.665 + y: 694.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.546 + y: 699.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.427 + y: 704.394 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.308 + y: 709.394 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.188 + y: 714.395 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.069 + y: 719.395 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.950 + y: 724.396 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.831 + y: 729.397 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.712 + y: 734.398 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.593 + y: 739.399 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.473 + y: 744.400 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.354 + y: 749.400 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.235 + y: 754.401 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.116 + y: 759.402 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.997 + y: 764.403 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.786 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00451016 + w: 0.999990 + - position: + x: 333.933 + y: 644.725 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00607237 + w: 0.999982 + - position: + x: 338.935 + y: 644.645 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00796688 + w: 0.999968 + - position: + x: 343.880 + y: 644.606 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00392303 + w: 0.999992 + - position: + x: 348.786 + y: 644.906 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0304731 + w: 0.999536 + - position: + x: 353.463 + y: 645.966 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.111241 + w: 0.993793 + - position: + x: 357.696 + y: 648.148 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.235723 + w: 0.971820 + - position: + x: 361.186 + y: 651.334 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.361617 + w: 0.932327 + - position: + x: 363.716 + y: 655.456 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.488304 + w: 0.872674 + - position: + x: 364.853 + y: 660.052 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.616332 + w: 0.787487 + - position: + x: 364.887 + y: 665.003 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.704715 + w: 0.709490 + - position: + x: 364.768 + y: 670.003 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.648 + y: 675.003 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.529 + y: 680.004 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.410 + y: 685.004 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.291 + y: 690.005 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.172 + y: 695.005 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.053 + y: 700.006 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.933 + y: 705.006 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.814 + y: 710.007 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.695 + y: 715.007 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.576 + y: 720.008 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.457 + y: 725.009 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.338 + y: 730.010 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.218 + y: 735.011 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.099 + y: 740.011 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.980 + y: 745.012 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.861 + y: 750.013 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.742 + y: 755.014 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.622 + y: 760.015 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.786 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00451016 + w: 0.999990 + - position: + x: 333.933 + y: 644.725 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00607237 + w: 0.999982 + - position: + x: 338.935 + y: 644.645 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00796688 + w: 0.999968 + - position: + x: 343.873 + y: 644.613 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00329511 + w: 0.999995 + - position: + x: 348.698 + y: 645.015 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0415760 + w: 0.999135 + - position: + x: 353.223 + y: 646.395 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.147464 + w: 0.989067 + - position: + x: 357.115 + y: 649.075 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.296991 + w: 0.954880 + - position: + x: 359.997 + y: 652.871 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.444541 + w: 0.895759 + - position: + x: 361.525 + y: 657.337 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.581548 + w: 0.813512 + - position: + x: 361.651 + y: 662.207 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.697878 + w: 0.716217 + - position: + x: 361.533 + y: 667.205 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715386 + w: 0.698729 + - position: + x: 361.414 + y: 672.205 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 361.295 + y: 677.206 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 361.176 + y: 682.206 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 361.057 + y: 687.206 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.938 + y: 692.207 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.818 + y: 697.207 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.699 + y: 702.208 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.580 + y: 707.208 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.461 + y: 712.209 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.342 + y: 717.209 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.222 + y: 722.210 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.103 + y: 727.211 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.984 + y: 732.212 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.865 + y: 737.213 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.746 + y: 742.214 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.626 + y: 747.214 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.507 + y: 752.215 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.388 + y: 757.216 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.269 + y: 762.217 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.785 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00457230 + w: 0.999990 + - position: + x: 333.933 + y: 644.721 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00644958 + w: 0.999979 + - position: + x: 338.935 + y: 644.632 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00883007 + w: 0.999961 + - position: + x: 343.938 + y: 644.526 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.0105871 + w: 0.999944 + - position: + x: 348.941 + y: 644.416 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.0110042 + w: 0.999939 + - position: + x: 353.943 + y: 644.319 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00972075 + w: 0.999953 + - position: + x: 358.946 + y: 644.250 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00691289 + w: 0.999976 + - position: + x: 363.948 + y: 644.219 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00309863 + w: 0.999995 + - position: + x: 368.950 + y: 644.228 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.000886733 + w: 1.00000 + - position: + x: 373.952 + y: 644.264 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00363761 + w: 0.999993 + - position: + x: 378.954 + y: 644.305 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00413557 + w: 0.999991 + - position: + x: 383.955 + y: 644.347 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00420318 + w: 0.999991 + - position: + x: 388.957 + y: 644.388 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00411113 + w: 0.999992 + - position: + x: 393.958 + y: 644.432 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00439488 + w: 0.999990 + - position: + x: 398.956 + y: 644.454 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00215849 + w: 0.999998 + - position: + x: 403.941 + y: 644.630 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0176845 + w: 0.999844 + - position: + x: 408.939 + y: 644.821 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 413.938 + y: 645.012 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 418.936 + y: 645.202 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 423.935 + y: 645.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 428.933 + y: 645.584 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 433.932 + y: 645.774 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 438.931 + y: 645.965 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 443.929 + y: 646.156 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 448.928 + y: 646.346 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 453.927 + y: 646.537 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 458.925 + y: 646.728 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 463.924 + y: 646.918 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 468.923 + y: 647.109 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 473.922 + y: 647.300 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.90000 + y: 0.604998 + z: 0.767451 +traffic_signal: + stamp: + sec: 0 + nanosec: 0 + traffic_light_groups: [] +route: + header: + stamp: + sec: 21 + nanosec: 17351334 + frame_id: map + start_pose: + position: + x: 301.475 + y: 642.905 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00500123 + w: 0.999987 + goal_pose: + position: + x: 358.812 + y: 675.174 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705159 + w: 0.709049 + segments: + - preferred_primitive: + id: 2000 + primitive_type: "" + primitives: + - id: 2000 + primitive_type: lane + - id: 2001 + primitive_type: lane + - id: 2002 + primitive_type: lane + - preferred_primitive: + id: 2010 + primitive_type: "" + primitives: + - id: 2010 + primitive_type: lane + - preferred_primitive: + id: 2200 + primitive_type: "" + primitives: + - id: 2200 + primitive_type: lane + - preferred_primitive: + id: 3500 + primitive_type: "" + primitives: + - id: 3500 + primitive_type: lane + - id: 3501 + primitive_type: lane + - id: 3502 + primitive_type: lane + uuid: + uuid: + - 195 + - 84 + - 241 + - 105 + - 133 + - 218 + - 136 + - 83 + - 104 + - 191 + - 48 + - 109 + - 85 + - 60 + - 177 + - 115 + allow_modification: false +operation_mode: + stamp: + sec: 550 + nanosec: 913829317 + mode: 2 + is_autoware_control_enabled: true + is_in_transition: false + is_stop_mode_available: true + is_autonomous_mode_available: true + is_local_mode_available: true + is_remote_mode_available: true +self_acceleration: + header: + stamp: + sec: 563 + nanosec: 188542779 + frame_id: /base_link + accel: + accel: + linear: + x: -0.770147 + y: 0.0409905 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 +self_odometry: + header: + stamp: + sec: 563 + nanosec: 188542779 + frame_id: map + child_frame_id: base_link + pose: + pose: + position: + x: 337.371 + y: 643.219 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00401753 + w: 0.999992 + covariance: + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + twist: + twist: + linear: + x: 4.13155 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00992133 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_shoulder.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_shoulder.yaml new file mode 100644 index 0000000000000..9a20cbee9c1ff --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_shoulder.yaml @@ -0,0 +1,2512 @@ +# +# AUTO GENERATED by autoware_test_utils::topic_snapshot_saver +# format1: +# +# format_version: +# map_path_uri: package:/// +# fields(this is array) +# - name: +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | PathWithLaneId | TBD} +# topic: +# +format_version: 1 +map_path_uri: package://autoware_test_utils/test_map/intersection/lanelet2_map.osm +path_with_lane_id: + header: + stamp: + sec: 74 + nanosec: 11949715 + frame_id: map + points: + - point: + pose: + position: + x: 360.306 + y: 597.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710800 + w: 0.703394 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.285 + y: 599.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710829 + w: 0.703365 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.264 + y: 601.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710857 + w: 0.703336 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.243 + y: 603.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710881 + w: 0.703312 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.222 + y: 605.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710951 + w: 0.703242 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.200 + y: 607.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711036 + w: 0.703156 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.177 + y: 609.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711601 + w: 0.702584 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.152 + y: 611.245 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711932 + w: 0.702248 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.133 + y: 612.628 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.716985 + w: 0.697089 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - 1200 + - point: + pose: + position: + x: 360.116 + y: 613.245 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.732064 + w: 0.681236 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.972 + y: 615.239 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.740537 + w: 0.672016 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.778 + y: 617.231 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.759018 + w: 0.651069 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.474 + y: 619.204 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.784627 + w: 0.619968 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.010 + y: 621.159 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.835622 + w: 0.549305 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 358.221 + y: 622.986 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.876871 + w: 0.480726 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 357.127 + y: 624.700 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.906015 + w: 0.423245 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 355.847 + y: 626.230 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.934815 + w: 0.355135 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 354.341 + y: 627.567 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.956562 + w: 0.291529 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 352.674 + y: 628.688 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.976933 + w: 0.213544 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 350.854 + y: 629.523 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.989029 + w: 0.147720 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 348.920 + y: 630.114 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.997418 + w: 0.0718077 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 346.951 + y: 630.399 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.999926 + w: 0.0121684 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 344.951 + y: 630.448 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 1.00000 + w: 0.000253960 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 342.951 + y: 630.449 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999959 + w: 0.00906744 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 342.741 + y: 630.445 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - 2500 + - point: + pose: + position: + x: 340.951 + y: 630.409 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999893 + w: 0.0146423 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 339.014 + y: 630.352 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999803 + w: 0.0198557 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 337.016 + y: 630.273 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999732 + w: 0.0231414 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 335.018 + y: 630.180 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999702 + w: 0.0243973 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 333.020 + y: 630.083 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999705 + w: 0.0242858 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 332.731 + y: 630.068 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999615 + w: 0.0277448 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 331.733 + y: 630.013 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999615 + w: 0.0277448 + longitudinal_velocity_mps: 0.00000 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + left_bound: + - x: 358.794 + y: 596.731 + z: 100.000 + - x: 358.788 + y: 597.342 + z: 100.000 + - x: 358.758 + y: 600.198 + z: 100.000 + - x: 358.736 + y: 602.392 + z: 100.000 + - x: 358.718 + y: 604.199 + z: 100.000 + - x: 358.698 + y: 606.199 + z: 100.000 + - x: 358.686 + y: 607.440 + z: 100.000 + - x: 358.610 + y: 610.191 + z: 100.000 + - x: 358.512 + y: 612.597 + z: 100.000 + - x: 358.405 + y: 614.096 + z: 100.000 + - x: 358.218 + y: 616.079 + z: 100.000 + - x: 358.086 + y: 617.465 + z: 100.000 + - x: 357.913 + y: 618.622 + z: 100.000 + - x: 357.666 + y: 619.746 + z: 100.000 + - x: 357.285 + y: 620.810 + z: 100.000 + - x: 356.832 + y: 621.812 + z: 100.000 + - x: 356.284 + y: 622.823 + z: 100.000 + - x: 355.663 + y: 623.752 + z: 100.000 + - x: 354.964 + y: 624.613 + z: 100.000 + - x: 354.158 + y: 625.437 + z: 100.000 + - x: 353.306 + y: 626.152 + z: 100.000 + - x: 352.395 + y: 626.788 + z: 100.000 + - x: 351.400 + y: 627.366 + z: 100.000 + - x: 350.389 + y: 627.820 + z: 100.000 + - x: 349.337 + y: 628.212 + z: 100.000 + - x: 348.219 + y: 628.479 + z: 100.000 + - x: 347.083 + y: 628.681 + z: 100.000 + - x: 346.042 + y: 628.729 + z: 100.000 + - x: 344.050 + y: 628.720 + z: 100.000 + - x: 342.764 + y: 628.777 + z: 100.000 + - x: 340.080 + y: 628.723 + z: 100.000 + - x: 338.078 + y: 628.751 + z: 100.000 + - x: 336.079 + y: 628.710 + z: 100.000 + - x: 334.080 + y: 628.670 + z: 100.000 + - x: 332.082 + y: 628.629 + z: 100.000 + - x: 326.872 + y: 628.524 + z: 100.000 + right_bound: + - x: 361.830 + y: 596.762 + z: 100.000 + - x: 361.823 + y: 597.400 + z: 100.000 + - x: 361.792 + y: 600.231 + z: 100.000 + - x: 361.767 + y: 602.446 + z: 100.000 + - x: 361.748 + y: 604.230 + z: 100.000 + - x: 361.726 + y: 606.230 + z: 100.000 + - x: 361.712 + y: 607.493 + z: 100.000 + - x: 361.718 + y: 610.230 + z: 100.000 + - x: 361.754 + y: 612.657 + z: 100.000 + - x: 361.627 + y: 614.331 + z: 100.000 + - x: 361.519 + y: 616.330 + z: 100.000 + - x: 361.452 + y: 617.634 + z: 100.000 + - x: 361.277 + y: 619.056 + z: 100.000 + - x: 360.998 + y: 620.486 + z: 100.000 + - x: 360.581 + y: 621.905 + z: 100.000 + - x: 359.991 + y: 623.293 + z: 100.000 + - x: 359.319 + y: 624.583 + z: 100.000 + - x: 358.490 + y: 625.825 + z: 100.000 + - x: 357.546 + y: 626.984 + z: 100.000 + - x: 356.525 + y: 628.025 + z: 100.000 + - x: 355.377 + y: 628.981 + z: 100.000 + - x: 354.158 + y: 629.845 + z: 100.000 + - x: 352.881 + y: 630.552 + z: 100.000 + - x: 351.515 + y: 631.167 + z: 100.000 + - x: 350.098 + y: 631.615 + z: 100.000 + - x: 348.672 + y: 631.919 + z: 100.000 + - x: 347.236 + y: 632.124 + z: 100.000 + - x: 346.083 + y: 632.098 + z: 100.000 + - x: 344.082 + y: 632.081 + z: 100.000 + - x: 342.719 + y: 632.113 + z: 100.000 + - x: 340.012 + y: 632.058 + z: 100.000 + - x: 338.014 + y: 631.950 + z: 100.000 + - x: 336.015 + y: 631.909 + z: 100.000 + - x: 334.016 + y: 631.869 + z: 100.000 + - x: 332.017 + y: 631.829 + z: 100.000 + - x: 326.807 + y: 631.724 + z: 100.000 +dynamic_object: + header: + stamp: + sec: 73 + nanosec: 980215670 + frame_id: map + objects: + - object_id: + uuid: + - 251 + - 180 + - 23 + - 85 + - 112 + - 118 + - 142 + - 88 + - 241 + - 29 + - 194 + - 27 + - 222 + - 164 + - 130 + - 251 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 357.408 + y: 667.312 + z: 100.774 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712257 + w: 0.701919 + covariance: + - 0.299126 + - -8.23923e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -8.23923e-05 + - 0.349251 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00879510 + initial_twist_with_covariance: + twist: + linear: + x: 9.98083 + y: 0.00185652 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00325706 + covariance: + - 0.970951 + - 0.000139981 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.000245582 + - 0.000139981 + - 0.00384142 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00673936 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.000245582 + - 0.00673936 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0118235 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 357.408 + y: 667.312 + z: 100.774 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712257 + w: 0.701919 + - position: + x: 357.373 + y: 672.303 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.709614 + w: 0.704590 + - position: + x: 357.520 + y: 677.298 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.696604 + w: 0.717456 + - position: + x: 357.917 + y: 682.299 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.678600 + w: 0.734508 + - position: + x: 358.531 + y: 687.305 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.662668 + w: 0.748913 + - position: + x: 359.266 + y: 692.315 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.653723 + w: 0.756734 + - position: + x: 359.995 + y: 697.324 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.654217 + w: 0.756307 + - position: + x: 360.590 + y: 702.329 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.664107 + w: 0.747638 + - position: + x: 360.955 + y: 707.330 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.680844 + w: 0.732428 + - position: + x: 361.063 + y: 712.324 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.699424 + w: 0.714707 + - position: + x: 360.983 + y: 717.314 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712725 + w: 0.701443 + - position: + x: 360.864 + y: 722.303 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.746 + y: 727.292 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.627 + y: 732.280 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.508 + y: 737.269 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.389 + y: 742.258 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.270 + y: 747.247 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.151 + y: 752.236 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.032 + y: 757.225 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.913 + y: 762.214 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.794 + y: 767.203 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.675 + y: 772.192 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.557 + y: 777.181 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.438 + y: 782.170 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.319 + y: 787.159 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.200 + y: 792.148 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.081 + y: 797.137 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.962 + y: 802.126 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.843 + y: 807.115 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.724 + y: 812.104 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.605 + y: 817.093 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.230769 + - path: + - position: + x: 357.408 + y: 667.312 + z: 100.774 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712257 + w: 0.701919 + - position: + x: 357.340 + y: 672.302 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711946 + w: 0.702234 + - position: + x: 357.296 + y: 677.293 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710176 + w: 0.704024 + - position: + x: 357.286 + y: 682.284 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707820 + w: 0.706393 + - position: + x: 357.304 + y: 687.276 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705868 + w: 0.708343 + - position: + x: 357.334 + y: 692.269 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.704976 + w: 0.709232 + - position: + x: 357.357 + y: 697.261 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705452 + w: 0.708758 + - position: + x: 357.355 + y: 702.252 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707256 + w: 0.706957 + - position: + x: 357.314 + y: 707.243 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.709999 + w: 0.704203 + - position: + x: 357.231 + y: 712.233 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712949 + w: 0.701216 + - position: + x: 357.119 + y: 717.222 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715047 + w: 0.699076 + - position: + x: 357.000 + y: 722.211 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.881 + y: 727.199 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.762 + y: 732.188 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.643 + y: 737.177 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.524 + y: 742.166 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.405 + y: 747.155 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.286 + y: 752.144 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.167 + y: 757.133 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.049 + y: 762.122 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.930 + y: 767.111 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.811 + y: 772.100 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.692 + y: 777.089 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.573 + y: 782.078 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.454 + y: 787.067 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.335 + y: 792.056 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.216 + y: 797.045 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.097 + y: 802.033 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 354.978 + y: 807.022 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 354.859 + y: 812.011 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 354.741 + y: 817.000 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.769231 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.89999 + y: 0.500442 + z: 0.612431 + - object_id: + uuid: + - 218 + - 141 + - 152 + - 171 + - 185 + - 127 + - 84 + - 175 + - 69 + - 40 + - 199 + - 89 + - 96 + - 107 + - 61 + - 243 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 358.218 + y: 603.622 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707443 + w: 0.706770 + covariance: + - 0.0636258 + - -0.000170219 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000170219 + - 0.0826995 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00394708 + initial_twist_with_covariance: + twist: + linear: + x: 10.0303 + y: -0.00247386 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: -0.00434011 + covariance: + - 0.579437 + - -9.81341e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000172165 + - -9.81341e-05 + - 0.00311875 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00547149 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - -0.000172165 + - 0.00547149 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00959910 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 358.218 + y: 603.622 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707443 + w: 0.706770 + - position: + x: 358.224 + y: 608.637 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.706633 + w: 0.707581 + - position: + x: 358.279 + y: 613.653 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.703235 + w: 0.710958 + - position: + x: 358.397 + y: 618.671 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.698737 + w: 0.715379 + - position: + x: 358.567 + y: 623.690 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.695068 + w: 0.718944 + - position: + x: 358.758 + y: 628.710 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.693532 + w: 0.720426 + - position: + x: 358.932 + y: 633.729 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.694735 + w: 0.719266 + - position: + x: 359.054 + y: 638.747 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.698453 + w: 0.715656 + - position: + x: 359.097 + y: 643.763 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.704045 + w: 0.710155 + - position: + x: 359.057 + y: 648.778 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.709938 + w: 0.704265 + - position: + x: 358.958 + y: 653.791 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714096 + w: 0.700048 + - position: + x: 358.849 + y: 658.805 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714730 + w: 0.699401 + - position: + x: 358.740 + y: 663.817 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714768 + w: 0.699361 + - position: + x: 358.620 + y: 668.830 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.501 + y: 673.843 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.381 + y: 678.856 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.262 + y: 683.870 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.142 + y: 688.883 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.023 + y: 693.896 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.903 + y: 698.909 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.784 + y: 703.923 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.664 + y: 708.936 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.545 + y: 713.950 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.425 + y: 718.963 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.306 + y: 723.977 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.186 + y: 728.991 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.067 + y: 734.004 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.947 + y: 739.018 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.828 + y: 744.032 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.708 + y: 749.045 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.589 + y: 754.059 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.500000 + - path: + - position: + x: 358.218 + y: 603.622 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707443 + w: 0.706770 + - position: + x: 358.206 + y: 608.635 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707930 + w: 0.706283 + - position: + x: 358.149 + y: 613.642 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711130 + w: 0.703060 + - position: + x: 358.010 + y: 618.635 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.716853 + w: 0.697224 + - position: + x: 357.024 + y: 623.117 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.779378 + w: 0.626554 + - position: + x: 354.441 + y: 627.006 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.881248 + w: 0.472653 + - position: + x: 350.340 + y: 629.665 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.958935 + w: 0.283625 + - position: + x: 345.469 + y: 630.694 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.994592 + w: 0.103864 + - position: + x: 340.508 + y: 630.814 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.999926 + w: 0.0121264 + - position: + x: 335.554 + y: 630.768 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999989 + w: 0.00460609 + - position: + x: 330.597 + y: 630.677 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999957 + w: 0.00925679 + - position: + x: 325.638 + y: 630.576 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 320.674 + y: 630.476 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 315.708 + y: 630.376 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 310.737 + y: 630.276 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 305.763 + y: 630.175 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 300.785 + y: 630.075 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 295.803 + y: 629.974 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 290.817 + y: 629.874 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 285.828 + y: 629.773 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 280.835 + y: 629.672 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 275.839 + y: 629.571 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 270.840 + y: 629.470 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 265.838 + y: 629.370 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 260.833 + y: 629.268 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 255.825 + y: 629.167 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 250.816 + y: 629.066 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 245.805 + y: 628.965 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 240.792 + y: 628.864 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 235.778 + y: 628.763 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 230.764 + y: 628.662 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.500000 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.90000 + y: 0.644288 + z: 0.991445 +traffic_signal: + stamp: + sec: 0 + nanosec: 0 + traffic_light_groups: [] +route: + header: + stamp: + sec: 19 + nanosec: 549630704 + frame_id: map + start_pose: + position: + x: 360.481 + y: 591.844 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.721675 + w: 0.692232 + goal_pose: + position: + x: 331.733 + y: 630.013 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.999615 + w: -0.0277448 + segments: + - preferred_primitive: + id: 1010 + primitive_type: "" + primitives: + - id: 1010 + primitive_type: lane + - preferred_primitive: + id: 1200 + primitive_type: "" + primitives: + - id: 1200 + primitive_type: lane + - preferred_primitive: + id: 2500 + primitive_type: "" + primitives: + - id: 2500 + primitive_type: lane + - id: 2501 + primitive_type: lane + uuid: + uuid: + - 83 + - 230 + - 40 + - 201 + - 182 + - 232 + - 31 + - 237 + - 204 + - 91 + - 39 + - 90 + - 153 + - 179 + - 6 + - 123 + allow_modification: false +operation_mode: + stamp: + sec: 65 + nanosec: 819714871 + mode: 2 + is_autoware_control_enabled: true + is_in_transition: false + is_stop_mode_available: true + is_autonomous_mode_available: true + is_local_mode_available: true + is_remote_mode_available: true +self_acceleration: + header: + stamp: + sec: 74 + nanosec: 42641063 + frame_id: /base_link + accel: + accel: + linear: + x: -1.01094 + y: 0.00400509 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 +self_odometry: + header: + stamp: + sec: 74 + nanosec: 42641063 + frame_id: map + child_frame_id: base_link + pose: + pose: + position: + x: 360.302 + y: 604.493 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707950 + w: 0.706263 + covariance: + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + twist: + twist: + linear: + x: 2.31221 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00173214 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt index 10b694ce22cbd..3a429008c1b8e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt @@ -15,6 +15,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_crosswalk.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) endif() diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index bc0d4f613f10b..97e00ade8256c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -13,8 +13,6 @@ # For the Lanelet2 map with no explicit stop lines stop_distance_from_crosswalk: 3.5 # [m] make stop line away from crosswalk - # For the case where the crosswalk width is very wide - far_object_threshold: 10.0 # [m] If objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object). # For the case where the stop position is determined according to the object position. stop_distance_from_object_preferred: 3.0 # [m] stop_distance_from_object_limit: 3.0 # [m] 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 08bd8f91c7d71..1f0e9e68a65c6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml @@ -21,7 +21,9 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_grid_map_utils autoware_internal_debug_msgs autoware_interpolation diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp index 8ba05be36ae56..5bfd31b75976a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp @@ -192,8 +192,8 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) // NOTE: module_id is always a lane id so that isModuleRegistered works correctly in the case // where both regulatory element and non-regulatory element crosswalks exist. registerModule(std::make_shared( - node_, road_lanelet_id, crosswalk_lanelet_id, reg_elem_id, lanelet_map_ptr, p, logger, - clock_)); + node_, road_lanelet_id, crosswalk_lanelet_id, reg_elem_id, lanelet_map_ptr, p, logger, clock_, + time_keeper_, planning_factor_interface_)); generateUUID(crosswalk_lanelet_id); updateRTCStatus( getUUID(crosswalk_lanelet_id), true, State::WAITING_FOR_EXECUTION, @@ -216,7 +216,7 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) } } -std::function &)> +std::function &)> CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; @@ -233,7 +233,7 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) crosswalk_id_set.insert(crosswalk.first->crosswalkLanelet().id()); } - return [crosswalk_id_set](const std::shared_ptr & scene_module) { + return [crosswalk_id_set](const std::shared_ptr & scene_module) { return crosswalk_id_set.count(scene_module->getModuleId()) == 0; }; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp index d6f6ddfb7b6ad..091a427e14949 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -48,8 +48,8 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const PathWithLaneId & path) override; }; class CrosswalkModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 07b2f1deff0c7..844647e33f8c0 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 @@ -34,12 +34,12 @@ #include #include #include +#include #include #include #include #include #include - namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -175,13 +175,14 @@ CrosswalkModule::CrosswalkModule( rclcpp::Node & node, const int64_t lane_id, const int64_t module_id, const std::optional & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), module_id_(module_id), planner_param_(planner_param), use_regulatory_element_(reg_elem_id) { - velocity_factor_.init(PlanningBehavior::CROSSWALK); passed_safety_slow_point_ = false; if (use_regulatory_element_) { @@ -898,9 +899,11 @@ void CrosswalkModule::applySlowDown( } } if (slowdown_pose) - velocity_factor_.set( - output.points, planner_data_->current_odometry->pose, *slowdown_pose, - VelocityFactor::APPROACHING); + planning_factor_interface_->add( + output.points, planner_data_->current_odometry->pose, *slowdown_pose, *slowdown_pose, + tier4_planning_msgs::msg::PlanningFactor::SLOW_DOWN, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, + safety_slow_down_speed, 0.0 /*shift distance*/, "crosswalk_safety_slowdown_for_approaching"); } void CrosswalkModule::applySlowDownByLanelet2Map( @@ -1377,9 +1380,11 @@ void CrosswalkModule::planStop( // Plan stop insertDecelPointWithDebugInfo(stop_factor->stop_pose.position, 0.0, ego_path); - velocity_factor_.set( + planning_factor_interface_->add( ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose, - VelocityFactor::UNKNOWN); + stop_factor->stop_pose, tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0 /*velocity*/, + 0.0 /*shift distance*/, "crosswalk_stop"); } bool CrosswalkModule::checkRestartSuppression( 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 b3fbc2f6cfaba..f32ee96623822 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -17,7 +17,7 @@ #include "autoware/behavior_velocity_crosswalk_module/util.hpp" -#include +#include #include #include #include @@ -112,7 +112,7 @@ double InterpolateMap( } } // namespace -class CrosswalkModule : public SceneModuleInterface +class CrosswalkModule : public SceneModuleInterfaceWithRTC { public: struct PlannerParam @@ -334,7 +334,9 @@ class CrosswalkModule : public SceneModuleInterface rclcpp::Node & node, const int64_t lane_id, const int64_t module_id, const std::optional & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..7a875327d4dde --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "crosswalk", "autoware::behavior_velocity_planner::CrosswalkModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt index 3aff4a524ffdd..53eafaffbba6c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt @@ -14,6 +14,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_utils.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) endif() diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml index b49b43794685c..9294795274066 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml @@ -8,5 +8,4 @@ state_clear_time: 2.0 hold_stop_margin_distance: 0.0 distance_to_judge_over_stop_line: 0.5 - enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval suppress_pass_judge_when_stopping: false diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml index ff91cf40a32a6..4ae1d20991078 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml @@ -17,6 +17,7 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp index 3f39696ff6bcc..f231d7adc3326 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp @@ -34,8 +34,7 @@ using autoware::universe_utils::getOrDeclareParameter; using lanelet::autoware::DetectionArea; DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) +: SceneModuleManagerInterface(node, getModuleName()) { const std::string ns(DetectionAreaModuleManager::getModuleName()); planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); @@ -65,11 +64,8 @@ void DetectionAreaModuleManager::launchNewModules( if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, lane_id, *detection_area_with_lane_id.first, planner_param_, - logger_.get_child("detection_area_module"), clock_)); - generateUUID(module_id); - updateRTCStatus( - getUUID(module_id), true, State::WAITING_FOR_EXECUTION, - std::numeric_limits::lowest(), path.header.stamp); + logger_.get_child("detection_area_module"), clock_, time_keeper_, + planning_factor_interface_)); } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp index 7aa60cbbaa18b..4b34ac4a45298 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp @@ -29,7 +29,7 @@ namespace autoware::behavior_velocity_planner { -class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC +class DetectionAreaModuleManager : public SceneModuleManagerInterface<> { public: explicit DetectionAreaModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 61b3b185999d8..7e6a320f3ddad 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -36,15 +36,16 @@ DetectionAreaModule::DetectionAreaModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), detection_area_reg_elem_(detection_area_reg_elem), state_(State::GO), planner_param_(planner_param), debug_data_() { - velocity_factor_.init(PlanningBehavior::USER_DEFINED_DETECTION_AREA); } bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) @@ -105,12 +106,10 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) modified_stop_line_seg_idx = current_seg_idx; } - setDistance(stop_dist); - // Check state - setSafe(detection_area::can_clear_stop_state( - last_obstacle_found_time_, clock_->now(), planner_param_.state_clear_time)); - if (isActivated()) { + const bool is_safe = detection_area::can_clear_stop_state( + last_obstacle_found_time_, clock_->now(), planner_param_.state_clear_time); + if (is_safe) { last_obstacle_found_time_ = {}; if (!planner_param_.suppress_pass_judge_when_stopping || !is_stopped) { state_ = State::GO; @@ -139,7 +138,6 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) dead_line_seg_idx); if (dist_from_ego_to_dead_line < 0.0) { RCLCPP_WARN(logger_, "[detection_area] vehicle is over dead line"); - setSafe(true); return true; } } @@ -152,7 +150,6 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) if ( state_ != State::STOP && dist_from_ego_to_stop < -planner_param_.distance_to_judge_over_stop_line) { - setSafe(true); return true; } @@ -169,7 +166,6 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) RCLCPP_WARN_THROTTLE( logger_, *clock_, std::chrono::milliseconds(1000).count(), "[detection_area] vehicle is over stop border"); - setSafe(true); return true; } } @@ -183,9 +179,10 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) // Create StopReason { - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_point->second, - VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); } return true; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp index 9224cf4624687..1d3e5f3540ff3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp @@ -68,7 +68,9 @@ class DetectionAreaModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..f84d22debea8e --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "detection_area", "autoware::behavior_velocity_planner::DetectionAreaModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml index 7a64d1d6638ff..4f7c3aeea7618 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml @@ -19,7 +19,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_internal_debug_msgs autoware_interpolation autoware_lanelet2_extension diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp index 39ed8e80412a6..d6f7dfbac92d4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -338,7 +338,8 @@ void IntersectionModuleManager::launchNewModules( } const auto new_module = std::make_shared( module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, - has_traffic_light, node_, logger_.get_child("intersection_module"), clock_); + has_traffic_light, node_, logger_.get_child("intersection_module"), clock_, time_keeper_, + planning_factor_interface_); generateUUID(module_id); /* set RTC status as non_occluded status initially */ const UUID uuid = getUUID(new_module->getModuleId()); @@ -353,14 +354,14 @@ void IntersectionModuleManager::launchNewModules( } } -std::function &)> +std::function &)> IntersectionModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [lane_set](const std::shared_ptr & scene_module) { + return [lane_set](const std::shared_ptr & scene_module) { const auto intersection_module = std::dynamic_pointer_cast(scene_module); const auto & associative_ids = intersection_module->getAssociativeIds(); for (const auto & lane : lane_set) { @@ -526,7 +527,8 @@ void MergeFromPrivateModuleManager::launchNewModules( planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); registerModule(std::make_shared( module_id, lane_id, planner_data_, merge_from_private_area_param_, associative_ids, - logger_.get_child("merge_from_private_road_module"), clock_)); + logger_.get_child("merge_from_private_road_module"), clock_, time_keeper_, + planning_factor_interface_)); continue; } } else { @@ -540,7 +542,8 @@ void MergeFromPrivateModuleManager::launchNewModules( planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); registerModule(std::make_shared( module_id, lane_id, planner_data_, merge_from_private_area_param_, associative_ids, - logger_.get_child("merge_from_private_road_module"), clock_)); + logger_.get_child("merge_from_private_road_module"), clock_, time_keeper_, + planning_factor_interface_)); continue; } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp index 80c87e55c6696..c6f76d7c39640 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -47,8 +48,8 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; @@ -63,7 +64,7 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC tl_observation_pub_; }; -class MergeFromPrivateModuleManager : public SceneModuleManagerInterface +class MergeFromPrivateModuleManager : public SceneModuleManagerInterface<> { public: explicit MergeFromPrivateModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 7c492a9dd42bf..5b8aca1c8ec9d 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 @@ -18,7 +18,6 @@ #include // for toGeomPoly #include -#include #include #include // for toPolygon2d #include @@ -45,15 +44,15 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using autoware::motion_utils::VelocityFactorInterface; - IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), planner_param_(planner_param), lane_id_(lane_id), associative_ids_(associative_ids), @@ -61,8 +60,6 @@ IntersectionModule::IntersectionModule( has_traffic_light_(has_traffic_light), occlusion_uuid_(autoware::universe_utils::generateUUID()) { - velocity_factor_.init(PlanningBehavior::INTERSECTION); - { collision_state_machine_.setMarginTime( planner_param_.collision_detection.collision_detection_hold_time); @@ -354,9 +351,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); const bool over_stopline = (dist_stopline < 0.0); const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - state_machine.setState(StateMachine::State::GO); - } else if (is_stopped_duration && approached_dist_stopline) { + if (over_stopline || (is_stopped_duration && approached_dist_stopline)) { state_machine.setState(StateMachine::State::GO); } return state_machine.getState() == StateMachine::State::GO; @@ -367,12 +362,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin); const bool is_stopped = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - return true; - } else if (is_stopped && approached_dist_stopline) { - return true; - } - return false; + return over_stopline || (is_stopped && approached_dist_stopline); }; const auto occlusion_wo_tl_pass_judge_line_idx = @@ -712,7 +702,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] motion_utils::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { static_assert("Unsupported type passed to reactRTCByDecisionResult"); @@ -727,7 +717,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] motion_utils::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { return; @@ -741,7 +731,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] motion_utils::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { return; @@ -753,7 +743,8 @@ void reactRTCApprovalByDecisionResult( const StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -767,9 +758,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(pure StuckStop)"); } } if (!rtc_occlusion_approved && decision_result.occlusion_stopline_idx) { @@ -778,9 +772,13 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(occlusion_stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, - path->points.at(occlusion_stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(occlusion_stopline_idx).point.pose, + path->points.at(occlusion_stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(StuckStop with occlusion)"); } } return; @@ -792,7 +790,8 @@ void reactRTCApprovalByDecisionResult( const YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -806,9 +805,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Yield Stuck)"); } } return; @@ -820,7 +822,8 @@ void reactRTCApprovalByDecisionResult( const NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -832,9 +835,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop)"); } } if (!rtc_occlusion_approved) { @@ -843,9 +849,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); } } return; @@ -856,7 +865,8 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - tier4_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, + tier4_planning_msgs::msg::PathWithLaneId * path, + motion_utils::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -869,9 +879,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_first_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(FirstWaitBeforeOcclusion with collision)"); } } if (!rtc_occlusion_approved) { @@ -888,9 +901,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(FirstWaitBeforeOcclusion with occlusion)"); } } return; @@ -901,7 +917,8 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - tier4_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, + tier4_planning_msgs::msg::PathWithLaneId * path, + motion_utils::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -927,9 +944,13 @@ void reactRTCApprovalByDecisionResult( debug_data->static_occlusion_with_traffic_light_timeout = decision_result.static_occlusion_timeout; { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(occlusion_peeking_stopline).point.pose, VelocityFactor::UNKNOWN); + path->points.at(occlusion_peeking_stopline).point.pose, + path->points.at(occlusion_peeking_stopline).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(PeekingToOcclusion)"); } } if (!rtc_default_approved) { @@ -938,9 +959,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(PeekingToOcclusion while stopping for collision)"); } } return; @@ -952,7 +976,8 @@ void reactRTCApprovalByDecisionResult( const OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -964,9 +989,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); } } if (!rtc_occlusion_approved) { @@ -979,9 +1007,12 @@ void reactRTCApprovalByDecisionResult( debug_data->static_occlusion_with_traffic_light_timeout = decision_result.static_occlusion_timeout; { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); } } return; @@ -993,7 +1024,8 @@ void reactRTCApprovalByDecisionResult( const OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -1005,9 +1037,13 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, + "intersection(Occlusion without traffic light, collision detected)"); } } if (!rtc_occlusion_approved && decision_result.temporal_stop_before_attention_required) { @@ -1016,9 +1052,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Occlusion without traffic light)"); } } if (!rtc_occlusion_approved && !decision_result.temporal_stop_before_attention_required) { @@ -1039,7 +1078,8 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -1050,9 +1090,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Safe, but RTC interrupted for collision)"); } } if (!rtc_occlusion_approved) { @@ -1061,9 +1104,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Safe, but RTC interrupted for occlusion)"); } } return; @@ -1075,7 +1121,8 @@ void reactRTCApprovalByDecisionResult( const FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -1087,9 +1134,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(FullyPrioritized, collision detected)"); } } if (!rtc_occlusion_approved) { @@ -1098,9 +1148,13 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, + "intersection(FullyPrioritized, RTC for occlusion is interrupting)"); } } return; @@ -1114,7 +1168,7 @@ void IntersectionModule::reactRTCApproval( VisitorSwitch{[&](const auto & decision) { reactRTCApprovalByDecisionResult( activated_, occlusion_activated_, decision, planner_param_, baselink2front, path, - &velocity_factor_, &debug_data_); + planning_factor_interface_.get(), &debug_data_); }}, decision_result); return; 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 164df7588bd68..db0cfbe98d53c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -22,8 +22,8 @@ #include "object_manager.hpp" #include "result.hpp" -#include #include +#include #include #include @@ -46,7 +46,7 @@ namespace autoware::behavior_velocity_planner { -class IntersectionModule : public SceneModuleInterface +class IntersectionModule : public SceneModuleInterfaceWithRTC { public: struct PlannerParam @@ -304,7 +304,9 @@ class IntersectionModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** *********************************************************** diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 3dfdcc36c0cff..fc442337198af 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -41,12 +41,13 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), associative_ids_(associative_ids) { - velocity_factor_.init(PlanningBehavior::MERGE); planner_param_ = planner_param; state_machine_.setState(StateMachine::State::STOP); } @@ -153,8 +154,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path) /* get stop point and stop factor */ const auto & stop_pose = path->points.at(stopline_idx).point.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "merge_from_private"); const double signed_arc_dist_to_stop_point = autoware::motion_utils::calcSignedArcLength( path->points, current_pose.position, path->points.at(stopline_idx).point.pose.position); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index dc8bf1a96a7a2..fe446d304d9e0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -61,7 +61,9 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface MergeFromPrivateRoadModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..b515107e0ae8e --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "intersection", "autoware::behavior_velocity_planner::IntersectionModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp index 9cb1153a8edd2..517568c93bd35 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -51,7 +51,8 @@ void NoDrivableLaneModuleManager::launchNewModules( } registerModule(std::make_shared( - module_id, lane_id, planner_param_, logger_.get_child("no_drivable_lane_module"), clock_)); + module_id, lane_id, planner_param_, logger_.get_child("no_drivable_lane_module"), clock_, + time_keeper_, planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp index 545af1576c6a8..bea068f5b9579 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp @@ -29,7 +29,7 @@ namespace autoware::behavior_velocity_planner { -class NoDrivableLaneModuleManager : public SceneModuleManagerInterface +class NoDrivableLaneModuleManager : public SceneModuleManagerInterface<> { public: explicit NoDrivableLaneModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp index 8365251904b18..0e646e92035cf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -21,20 +21,23 @@ #include #include +#include + namespace autoware::behavior_velocity_planner { using autoware::universe_utils::createPoint; NoDrivableLaneModule::NoDrivableLaneModule( const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), planner_param_(planner_param), debug_data_(), state_(State::INIT) { - velocity_factor_.init(PlanningBehavior::NO_DRIVABLE_LANE); } bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path) @@ -162,8 +165,10 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path) // Get stop point and stop factor { const auto & stop_pose = op_stop_pose.value(); - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); @@ -210,8 +215,10 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state(PathWithLaneId * // Get stop point and stop factor { const auto & stop_pose = autoware::universe_utils::getPose(path->points.at(0)); - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); const auto & virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); @@ -248,8 +255,10 @@ void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path) // Get stop point and stop factor { const auto & stop_pose = ego_pos_on_path.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::STOPPED); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp index d8c5e3e0425d1..835e946443e90 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -55,7 +55,9 @@ class NoDrivableLaneModule : public SceneModuleInterface NoDrivableLaneModule( const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt index b710924410549..e6b362271afb3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt @@ -14,6 +14,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_utils.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) endif() diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml index 88fafeb5b90dc..28677bfb80b15 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml @@ -17,7 +17,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_interpolation autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp index 9d66aa6672d36..55b6c0dffd1a4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -60,7 +60,7 @@ void NoStoppingAreaModuleManager::launchNewModules( // assign 1 no stopping area for each module registerModule(std::make_shared( module_id, lane_id, *m.first, planner_param_, logger_.get_child("no_stopping_area_module"), - clock_)); + clock_, time_keeper_, planning_factor_interface_)); generateUUID(module_id); updateRTCStatus( getUUID(module_id), true, State::WAITING_FOR_EXECUTION, @@ -69,16 +69,17 @@ void NoStoppingAreaModuleManager::launchNewModules( } } -std::function &)> +std::function &)> NoStoppingAreaModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto no_stopping_area_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [no_stopping_area_id_set](const std::shared_ptr & scene_module) { - return no_stopping_area_id_set.count(scene_module->getModuleId()) == 0; - }; + return + [no_stopping_area_id_set](const std::shared_ptr & scene_module) { + return no_stopping_area_id_set.count(scene_module->getModuleId()) == 0; + }; } } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp index 7009e94612580..523cbba291632 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -41,8 +41,8 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class NoStoppingAreaModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 3769aed71a1ec..c788a54073ed6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -30,6 +30,7 @@ #include #include +#include #include namespace autoware::behavior_velocity_planner @@ -40,14 +41,15 @@ NoStoppingAreaModule::NoStoppingAreaModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), no_stopping_area_reg_elem_(no_stopping_area_reg_elem), planner_param_(planner_param), debug_data_() { - velocity_factor_.init(PlanningBehavior::NO_STOPPING_AREA); state_machine_.setState(StateMachine::State::GO); state_machine_.setMarginTime(planner_param_.state_clear_time); } @@ -141,9 +143,11 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path) // Create StopReason { - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_point->second, - VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_point->second, stop_point->second, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, ""); } } else if (state_machine_.getState() == StateMachine::State::GO) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 51f3a0d261ebd..0d53441924805 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -17,9 +17,9 @@ #include "utils.hpp" -#include #include #include +#include #include #include @@ -31,7 +31,7 @@ namespace autoware::behavior_velocity_planner { -class NoStoppingAreaModule : public SceneModuleInterface +class NoStoppingAreaModule : public SceneModuleInterfaceWithRTC { public: struct PlannerParam @@ -57,7 +57,9 @@ class NoStoppingAreaModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..875e757cbfcec --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "no_stopping_area", "autoware::behavior_velocity_planner::NoStoppingAreaModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp index c4dace9d49244..d28cff55a7a8d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -123,8 +123,8 @@ void OcclusionSpotModuleManager::launchNewModules( // general if (!isModuleRegistered(module_id_)) { registerModule(std::make_shared( - module_id_, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), - clock_)); + module_id_, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), clock_, + time_keeper_, planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp index 32b1818214952..08c1516dea67a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp @@ -39,7 +39,7 @@ namespace autoware::behavior_velocity_planner { -class OcclusionSpotModuleManager : public SceneModuleManagerInterface +class OcclusionSpotModuleManager : public SceneModuleManagerInterface<> { public: explicit OcclusionSpotModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index 522e83390b0f1..fa259b2bea9aa 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -36,7 +36,7 @@ // turn on only when debugging. #define DEBUG_PRINT(enable, n, x) \ if (enable) { \ - const std::string time_msg = n + std::to_string(x); \ + const std::string time_msg = (n) + std::to_string(x); \ RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, time_msg); \ } @@ -64,11 +64,12 @@ namespace utils = occlusion_spot_utils; OcclusionSpotModule::OcclusionSpotModule( const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), param_(planner_param) + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), + param_(planner_param) { - velocity_factor_.init(PlanningBehavior::UNKNOWN); - if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { debug_data_.detection_type = "occupancy"; //! occupancy grid limitation( 100 * 100 ) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index 35c409a9c459b..22b2a91be66b0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -48,7 +48,9 @@ class OcclusionSpotModule : public SceneModuleInterface OcclusionSpotModule( const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** * @brief plan occlusion spot velocity at unknown area in occupancy grid diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt index 37a02b844dfe9..a520b02a43c05 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt @@ -2,20 +2,13 @@ cmake_minimum_required(VERSION 3.14) project(autoware_behavior_velocity_planner) find_package(autoware_cmake REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -rosidl_generate_interfaces( - ${PROJECT_NAME} - "srv/LoadPlugin.srv" - "srv/UnloadPlugin.srv" - DEPENDENCIES -) autoware_package() ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/node.cpp src/planner_manager.cpp + src/test_utils.cpp ) rclcpp_components_register_node(${PROJECT_NAME}_lib @@ -23,18 +16,9 @@ rclcpp_components_register_node(${PROJECT_NAME}_lib EXECUTABLE ${PROJECT_NAME}_node ) -if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(${PROJECT_NAME}_lib - ${PROJECT_NAME} "rosidl_typesupport_cpp") -else() - rosidl_get_typesupport_target( - cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") - target_link_libraries(${PROJECT_NAME}_lib "${cpp_typesupport_target}") -endif() - if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - test/src/test_node_interface.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} gtest_main diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp similarity index 85% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp index 4efb58e38b74f..472538406c382 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp @@ -12,19 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NODE_HPP_ -#define NODE_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ +#include "autoware/behavior_velocity_planner/planner_manager.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" -#include "planner_manager.hpp" #include #include -#include -#include #include +#include #include #include #include @@ -45,8 +44,6 @@ namespace autoware::behavior_velocity_planner { -using autoware_behavior_velocity_planner::srv::LoadPlugin; -using autoware_behavior_velocity_planner::srv::UnloadPlugin; using autoware_map_msgs::msg::LaneletMapBin; using tier4_planning_msgs::msg::VelocityLimit; @@ -84,10 +81,6 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node autoware_perception_msgs::msg::TrafficLightGroupArray> sub_traffic_signals_{this, "~/input/traffic_signals"}; - autoware::universe_utils::InterProcessPollingSubscriber< - tier4_v2x_msgs::msg::VirtualTrafficLightStateArray> - sub_virtual_traffic_light_states_{this, "~/input/virtual_traffic_light_states"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_occupancy_grid_{this, "~/input/occupancy_grid"}; @@ -125,13 +118,14 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node BehaviorVelocityPlannerManager planner_manager_; bool is_driving_forward_{true}; - rclcpp::Service::SharedPtr srv_load_plugin_; - rclcpp::Service::SharedPtr srv_unload_plugin_; + rclcpp::Service::SharedPtr srv_load_plugin_; + rclcpp::Service::SharedPtr srv_unload_plugin_; void onUnloadPlugin( - const UnloadPlugin::Request::SharedPtr request, - const UnloadPlugin::Response::SharedPtr response); + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response); void onLoadPlugin( - const LoadPlugin::Request::SharedPtr request, const LoadPlugin::Response::SharedPtr response); + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response); // mutex for planner_data_ std::mutex mutex_; @@ -150,4 +144,4 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node }; } // namespace autoware::behavior_velocity_planner -#endif // NODE_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp similarity index 91% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp index fddd658cef06e..9bd423ecfef21 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNER_MANAGER_HPP_ -#define PLANNER_MANAGER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ #include #include @@ -57,4 +57,4 @@ class BehaviorVelocityPlannerManager }; } // namespace autoware::behavior_velocity_planner -#endif // PLANNER_MANAGER_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp new file mode 100644 index 0000000000000..f34182e77e6f1 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp @@ -0,0 +1,47 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_ + +#include "autoware/behavior_velocity_planner/node.hpp" + +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; +using autoware::planning_test_manager::PlanningInterfaceTestManager; + +struct PluginInfo +{ + std::string module_name; // e.g. crosswalk + std::string plugin_name; // e.g. autoware::behavior_velocity_planner::CrosswalkModulePlugin +}; + +std::shared_ptr generateTestManager(); + +std::shared_ptr generateNode( + const std::vector & plugin_info_vec); + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node); +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml index e051374ed3dda..c40d80c2bf998 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml @@ -32,9 +32,8 @@ autoware_cmake eigen3_cmake_module - rosidl_default_generators - autoware_behavior_velocity_planner_common + autoware_internal_debug_msgs autoware_lanelet2_extension autoware_map_msgs autoware_motion_utils @@ -57,31 +56,13 @@ tf2_geometry_msgs tf2_ros tier4_planning_msgs - tier4_v2x_msgs visualization_msgs - rosidl_default_runtime - ament_cmake_ros ament_lint_auto - autoware_behavior_velocity_blind_spot_module - autoware_behavior_velocity_crosswalk_module - autoware_behavior_velocity_detection_area_module - autoware_behavior_velocity_intersection_module - autoware_behavior_velocity_no_drivable_lane_module - autoware_behavior_velocity_no_stopping_area_module - autoware_behavior_velocity_occlusion_spot_module - autoware_behavior_velocity_run_out_module - autoware_behavior_velocity_speed_bump_module - autoware_behavior_velocity_stop_line_module - autoware_behavior_velocity_traffic_light_module - autoware_behavior_velocity_virtual_traffic_light_module - autoware_behavior_velocity_walkway_module autoware_lint_common - rosidl_interface_packages - ament_cmake diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp index d78bc883e6b35..443bfebe2a3eb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "node.hpp" +#include "autoware/behavior_velocity_planner/node.hpp" #include #include @@ -70,9 +70,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio this->create_subscription( "~/input/path_with_lane_id", 1, std::bind(&BehaviorVelocityPlannerNode::onTrigger, this, _1)); - srv_load_plugin_ = create_service( + srv_load_plugin_ = create_service( "~/service/load_plugin", std::bind(&BehaviorVelocityPlannerNode::onLoadPlugin, this, _1, _2)); - srv_unload_plugin_ = create_service( + srv_unload_plugin_ = create_service( "~/service/unload_plugin", std::bind(&BehaviorVelocityPlannerNode::onUnloadPlugin, this, _1, _2)); @@ -112,19 +112,19 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio } void BehaviorVelocityPlannerNode::onLoadPlugin( - const LoadPlugin::Request::SharedPtr request, - [[maybe_unused]] const LoadPlugin::Response::SharedPtr response) + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + [[maybe_unused]] const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response) { std::unique_lock lk(mutex_); - planner_manager_.launchScenePlugin(*this, request->plugin_name); + planner_manager_.launchScenePlugin(*this, request->data); } void BehaviorVelocityPlannerNode::onUnloadPlugin( - const UnloadPlugin::Request::SharedPtr request, - [[maybe_unused]] const UnloadPlugin::Response::SharedPtr response) + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + [[maybe_unused]] const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response) { std::unique_lock lk(mutex_); - planner_manager_.removeScenePlugin(*this, request->plugin_name); + planner_manager_.removeScenePlugin(*this, request->data); } void BehaviorVelocityPlannerNode::onParam() @@ -271,9 +271,6 @@ bool BehaviorVelocityPlannerNode::processData(rclcpp::Clock clock) planner_data_.route_handler_ = std::make_shared(*map_data); } - // optional data - getData(planner_data_.virtual_traffic_light_states, sub_virtual_traffic_light_states_); - // planner_data_.external_velocity_limit is std::optional type variable. const auto external_velocity_limit = sub_external_velocity_limit_.takeData(); if (external_velocity_limit) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp index 4820c340058ff..45ee83260d53a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planner_manager.hpp" +#include "autoware/behavior_velocity_planner/planner_manager.hpp" #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp new file mode 100644 index 0000000000000..8b9f39e97d22b --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp @@ -0,0 +1,119 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_velocity_planner/test_utils.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_velocity_planner → test_node_ + test_manager->setPathSubscriber("behavior_velocity_planner_node/output/path"); + + // set behavior_velocity_planner node's input topic name(this topic is changed to test node) + test_manager->setPathWithLaneIdTopicName( + "behavior_velocity_planner_node/input/path_with_lane_id"); + + test_manager->setInitialPoseTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); + test_manager->setOdometryTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); + + return test_manager; +} + +std::shared_ptr generateNode( + const std::vector & plugin_info_vec) +{ + auto node_options = rclcpp::NodeOptions{}; + + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); + const auto behavior_velocity_planner_common_dir = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner_common"); + const auto behavior_velocity_planner_dir = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner"); + const auto velocity_smoother_dir = + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); + + const auto get_behavior_velocity_module_config = [](const std::string & module) { + const auto package_name = "autoware_behavior_velocity_" + module + "_module"; + const auto package_path = ament_index_cpp::get_package_share_directory(package_name); + return package_path + "/config/" + module + ".param.yaml"; + }; + + std::vector plugin_names; + for (const auto & plugin_info : plugin_info_vec) { + plugin_names.emplace_back(plugin_info.plugin_name); + } + + std::vector params; + params.emplace_back("launch_modules", plugin_names); + params.emplace_back("is_simulation", false); + node_options.parameter_overrides(params); + + auto yaml_files = std::vector{ + autoware_test_utils_dir + "/config/test_common.param.yaml", + autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", + autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", + velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", + velocity_smoother_dir + "/config/Analytical.param.yaml", + behavior_velocity_planner_common_dir + "/config/behavior_velocity_planner_common.param.yaml", + behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml"}; + for (const auto & plugin_info : plugin_info_vec) { + yaml_files.push_back(get_behavior_velocity_module_config(plugin_info.module_name)); + } + + autoware::test_utils::updateNodeOptions(node_options, yaml_files); + + // TODO(Takagi, Isamu): set launch_modules + // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light + // TODO(Kyoichi Sugahara) set to true launch_occlusion_spot + // TODO(Kyoichi Sugahara) set to true launch_run_out + // TODO(Kyoichi Sugahara) set to true launch_speed_bump + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishTF(test_target_node, "/tf"); + test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); + test_manager->publishPredictedObjects( + test_target_node, "behavior_velocity_planner_node/input/dynamic_objects"); + test_manager->publishPointCloud( + test_target_node, "behavior_velocity_planner_node/input/no_ground_pointcloud"); + test_manager->publishOdometry( + test_target_node, "behavior_velocity_planner_node/input/vehicle_odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); + test_manager->publishMap(test_target_node, "behavior_velocity_planner_node/input/vector_map"); + test_manager->publishTrafficSignals( + test_target_node, "behavior_velocity_planner_node/input/traffic_signals"); + test_manager->publishMaxVelocity( + test_target_node, "behavior_velocity_planner_node/input/external_velocity_limit_mps"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_velocity_planner_node/input/occupancy_grid"); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/LoadPlugin.srv b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/LoadPlugin.srv deleted file mode 100644 index 7b9f08ab0ded8..0000000000000 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/LoadPlugin.srv +++ /dev/null @@ -1,3 +0,0 @@ -string plugin_name ---- -bool success diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv deleted file mode 100644 index 7b9f08ab0ded8..0000000000000 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv +++ /dev/null @@ -1,3 +0,0 @@ -string plugin_name ---- -bool success diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp deleted file mode 100644 index fe79450d0def8..0000000000000 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ /dev/null @@ -1,179 +0,0 @@ -// Copyright 2023 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "node.hpp" - -#include -#include -#include - -#include - -#include -#include -#include -#include - -using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; -using autoware::planning_test_manager::PlanningInterfaceTestManager; - -std::shared_ptr generateTestManager() -{ - auto test_manager = std::make_shared(); - - // set subscriber with topic name: behavior_velocity_planner → test_node_ - test_manager->setPathSubscriber("behavior_velocity_planner_node/output/path"); - - // set behavior_velocity_planner node's input topic name(this topic is changed to test node) - test_manager->setPathWithLaneIdTopicName( - "behavior_velocity_planner_node/input/path_with_lane_id"); - - test_manager->setInitialPoseTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); - test_manager->setOdometryTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); - - return test_manager; -} - -std::shared_ptr generateNode() -{ - auto node_options = rclcpp::NodeOptions{}; - - const auto autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_test_utils"); - const auto behavior_velocity_planner_common_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner_common"); - const auto behavior_velocity_planner_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner"); - const auto velocity_smoother_dir = - ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); - - const auto get_behavior_velocity_module_config = [](const std::string & module) { - const auto package_name = "autoware_behavior_velocity_" + module + "_module"; - const auto package_path = ament_index_cpp::get_package_share_directory(package_name); - return package_path + "/config/" + module + ".param.yaml"; - }; - - std::vector module_names; - module_names.emplace_back("autoware::behavior_velocity_planner::CrosswalkModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::WalkwayModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::TrafficLightModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::IntersectionModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::MergeFromPrivateModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::BlindSpotModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::DetectionAreaModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::NoStoppingAreaModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::StopLineModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::OcclusionSpotModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::RunOutModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::SpeedBumpModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::NoDrivableLaneModulePlugin"); - - std::vector params; - params.emplace_back("launch_modules", module_names); - params.emplace_back("is_simulation", false); - node_options.parameter_overrides(params); - - autoware::test_utils::updateNodeOptions( - node_options, - {autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", - velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", - velocity_smoother_dir + "/config/Analytical.param.yaml", - behavior_velocity_planner_common_dir + "/config/behavior_velocity_planner_common.param.yaml", - behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", - get_behavior_velocity_module_config("blind_spot"), - get_behavior_velocity_module_config("crosswalk"), - get_behavior_velocity_module_config("walkway"), - get_behavior_velocity_module_config("detection_area"), - get_behavior_velocity_module_config("intersection"), - get_behavior_velocity_module_config("no_stopping_area"), - get_behavior_velocity_module_config("occlusion_spot"), - get_behavior_velocity_module_config("run_out"), - get_behavior_velocity_module_config("speed_bump"), - get_behavior_velocity_module_config("stop_line"), - get_behavior_velocity_module_config("traffic_light"), - get_behavior_velocity_module_config("virtual_traffic_light"), - get_behavior_velocity_module_config("no_drivable_lane")}); - - // TODO(Takagi, Isamu): set launch_modules - // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light - // TODO(Kyoichi Sugahara) set to true launch_occlusion_spot - // TODO(Kyoichi Sugahara) set to true launch_run_out - // TODO(Kyoichi Sugahara) set to true launch_speed_bump - - return std::make_shared(node_options); -} - -void publishMandatoryTopics( - std::shared_ptr test_manager, - std::shared_ptr test_target_node) -{ - // publish necessary topics from test_manager - test_manager->publishTF(test_target_node, "/tf"); - test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); - test_manager->publishPredictedObjects( - test_target_node, "behavior_velocity_planner_node/input/dynamic_objects"); - test_manager->publishPointCloud( - test_target_node, "behavior_velocity_planner_node/input/no_ground_pointcloud"); - test_manager->publishOdometry( - test_target_node, "behavior_velocity_planner_node/input/vehicle_odometry"); - test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); - test_manager->publishMap(test_target_node, "behavior_velocity_planner_node/input/vector_map"); - test_manager->publishTrafficSignals( - test_target_node, "behavior_velocity_planner_node/input/traffic_signals"); - test_manager->publishMaxVelocity( - test_target_node, "behavior_velocity_planner_node/input/external_velocity_limit_mps"); - test_manager->publishVirtualTrafficLightState( - test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states"); - test_manager->publishOccupancyGrid( - test_target_node, "behavior_velocity_planner_node/input/occupancy_grid"); -} - -TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) -{ - rclcpp::init(0, nullptr); - auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); - - publishMandatoryTopics(test_manager, test_target_node); - - // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); - EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - - // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); - rclcpp::shutdown(); -} - -TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) -{ - rclcpp::init(0, nullptr); - - auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); - publishMandatoryTopics(test_manager, test_target_node); - - // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); - - // make sure behavior_path_planner is running - EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); - - rclcpp::shutdown(); -} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp new file mode 100644 index 0000000000000..6e232318c1711 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp @@ -0,0 +1,61 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_velocity_planner/test_utils.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md index 2abbb83575af5..c4c8d97b4b390 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md @@ -1,3 +1,3 @@ # Behavior Velocity Planner Common -This package provides common functions as a library, which are used in the `behavior_velocity_planner` node and modules. +This package provides a behavior velocity interface without RTC, and common functions as a library, which are used in the `behavior_velocity_planner` node and modules. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp index 983aaec2acf4f..f8b37999cb6bf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp @@ -30,7 +30,6 @@ #include #include #include -#include #include #include @@ -64,7 +63,6 @@ struct PlannerData std::map traffic_light_id_map_raw_; std::map traffic_light_id_map_last_observed_; std::optional external_velocity_limit; - tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; bool is_simulation = false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index 4e898d9d28715..5bff3ca343e72 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -16,22 +16,20 @@ #define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ #include -#include +#include +#include #include +#include #include -#include #include #include +#include #include #include -#include -#include #include #include #include -#include -#include #include #include @@ -50,18 +48,14 @@ namespace autoware::behavior_velocity_planner { -using autoware::motion_utils::PlanningBehavior; -using autoware::motion_utils::VelocityFactor; using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; -using autoware::rtc_interface::RTCInterface; using autoware::universe_utils::DebugPublisher; using autoware::universe_utils::getOrDeclareParameter; +using autoware::universe_utils::StopWatch; using autoware_internal_debug_msgs::msg::Float64Stamped; using builtin_interfaces::msg::Time; using tier4_planning_msgs::msg::PathWithLaneId; -using tier4_rtc_msgs::msg::Module; -using tier4_rtc_msgs::msg::State; using unique_identifier_msgs::msg::UUID; struct ObjectOfInterest @@ -81,7 +75,9 @@ class SceneModuleInterface { public: explicit SceneModuleInterface( - const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); virtual ~SceneModuleInterface() = default; virtual bool modifyPathVelocity(PathWithLaneId * path) = 0; @@ -96,58 +92,17 @@ class SceneModuleInterface planner_data_ = planner_data; } - std::optional getInfrastructureCommand() - { - return infrastructure_command_; - } - - void setInfrastructureCommand( - const std::optional & command) - { - infrastructure_command_ = command; - } - - void setTimeKeeper(const std::shared_ptr & time_keeper) - { - time_keeper_ = time_keeper; - } - - std::shared_ptr getTimeKeeper() { return time_keeper_; } - - void setActivation(const bool activated) { activated_ = activated; } - void setRTCEnabled(const bool enable_rtc) { rtc_enabled_ = enable_rtc; } - bool isActivated() const { return activated_; } - bool isSafe() const { return safe_; } - double getDistance() const { return distance_; } - - void resetVelocityFactor() { velocity_factor_.reset(); } - VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); } std::vector getObjectsOfInterestData() const { return objects_of_interest_; } void clearObjectsOfInterestData() { objects_of_interest_.clear(); } protected: const int64_t module_id_; - bool activated_; - bool safe_; - bool rtc_enabled_; - double distance_; rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; - std::optional infrastructure_command_; - autoware::motion_utils::VelocityFactorInterface velocity_factor_; std::vector objects_of_interest_; mutable std::shared_ptr time_keeper_; - - void setSafe(const bool safe) - { - safe_ = safe; - if (!rtc_enabled_) { - syncActivation(); - } - } - void setDistance(const double distance) { distance_ = distance; } - void syncActivation() { setActivation(isSafe()); } + std::shared_ptr planning_factor_interface_; void setObjectsOfInterestData( const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, @@ -160,10 +115,36 @@ class SceneModuleInterface const std::vector & points) const; }; +template class SceneModuleManagerInterface { public: - SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name); + SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name) + : node_(node), clock_(node.get_clock()), logger_(node.get_logger()) + { + const auto ns = std::string("~/debug/") + module_name; + pub_debug_ = node.create_publisher(ns, 1); + if (!node.has_parameter("is_publish_debug_path")) { + is_publish_debug_path_ = node.declare_parameter("is_publish_debug_path"); + } else { + is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool(); + } + if (is_publish_debug_path_) { + pub_debug_path_ = node.create_publisher( + std::string("~/debug/path_with_lane_id/") + module_name, 1); + } + pub_virtual_wall_ = node.create_publisher( + std::string("~/virtual_wall/") + module_name, 5); + planning_factor_interface_ = + std::make_shared(&node, module_name); + + processing_time_publisher_ = std::make_shared(&node, "~/debug"); + + pub_processing_time_detail_ = node.create_publisher( + "~/debug/processing_time_detail_ms/" + std::string(module_name), 1); + + time_keeper_ = std::make_shared(pub_processing_time_detail_); + } virtual ~SceneModuleManagerInterface() = default; @@ -171,31 +152,95 @@ class SceneModuleManagerInterface void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & path); + const tier4_planning_msgs::msg::PathWithLaneId & path) + { + planner_data_ = planner_data; + + launchNewModules(path); + deleteExpiredModules(path); + } virtual void plan(tier4_planning_msgs::msg::PathWithLaneId * path) { modifyPathVelocity(path); } protected: - virtual void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path); + virtual void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) + { + universe_utils::ScopedTimeTrack st( + "SceneModuleManagerInterface::modifyPathVelocity", *time_keeper_); + StopWatch stop_watch; + stop_watch.tic("Total"); + visualization_msgs::msg::MarkerArray debug_marker_array; + + for (const auto & scene_module : scene_modules_) { + scene_module->setPlannerData(planner_data_); + scene_module->modifyPathVelocity(path); + + // The velocity factor must be called after modifyPathVelocity. + + for (const auto & marker : scene_module->createDebugMarkerArray().markers) { + debug_marker_array.markers.push_back(marker); + } + + virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); + } + + planning_factor_interface_->publish(); + pub_debug_->publish(debug_marker_array); + if (is_publish_debug_path_) { + tier4_planning_msgs::msg::PathWithLaneId debug_path; + debug_path.header = path->header; + debug_path.points = path->points; + pub_debug_path_->publish(debug_path); + } + pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); + processing_time_publisher_->publish( + std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total")); + } virtual void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; - virtual std::function &)> - getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; + virtual std::function &)> getModuleExpiredFunction( + const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; - virtual void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path); + virtual void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) + { + const auto isModuleExpired = getModuleExpiredFunction(path); + + auto itr = scene_modules_.begin(); + while (itr != scene_modules_.end()) { + if (isModuleExpired(*itr)) { + registered_module_id_set_.erase((*itr)->getModuleId()); + itr = scene_modules_.erase(itr); + } else { + itr++; + } + } + } bool isModuleRegistered(const int64_t module_id) { return registered_module_id_set_.count(module_id) != 0; } - void registerModule(const std::shared_ptr & scene_module); + void registerModule(const std::shared_ptr & scene_module) + { + RCLCPP_DEBUG( + logger_, "register task: module = %s, id = %lu", getModuleName(), + scene_module->getModuleId()); + registered_module_id_set_.emplace(scene_module->getModuleId()); + scene_modules_.insert(scene_module); + } size_t findEgoSegmentIndex( - const std::vector & points) const; + const std::vector & points) const + { + const auto & p = planner_data_; + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, p->current_odometry->pose, p->ego_nearest_dist_threshold, + p->ego_nearest_yaw_threshold); + } - std::set> scene_modules_; + std::set> scene_modules_; std::set registered_module_id_set_; std::shared_ptr planner_data_; @@ -209,77 +254,28 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_debug_; rclcpp::Publisher::SharedPtr pub_debug_path_; - rclcpp::Publisher::SharedPtr - pub_velocity_factor_; - rclcpp::Publisher::SharedPtr - pub_infrastructure_commands_; std::shared_ptr processing_time_publisher_; rclcpp::Publisher::SharedPtr pub_processing_time_detail_; std::shared_ptr time_keeper_; -}; - -class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface -{ -public: - SceneModuleManagerInterfaceWithRTC( - rclcpp::Node & node, const char * module_name, const bool enable_rtc = true); - - void plan(tier4_planning_msgs::msg::PathWithLaneId * path) override; - -protected: - RTCInterface rtc_interface_; - std::unordered_map map_uuid_; - - ObjectsOfInterestMarkerInterface objects_of_interest_marker_interface_; - - virtual void sendRTC(const Time & stamp); - - virtual void setActivation(); - void updateRTCStatus( - const UUID & uuid, const bool safe, const uint8_t state, const double distance, - const Time & stamp) - { - rtc_interface_.updateCooperateStatus(uuid, safe, state, distance, distance, stamp); - } - - void removeRTCStatus(const UUID & uuid) { rtc_interface_.removeCooperateStatus(uuid); } - - void publishRTCStatus(const Time & stamp) - { - rtc_interface_.removeExpiredCooperateStatus(); - rtc_interface_.publishCooperateStatus(stamp); - } - - UUID getUUID(const int64_t & module_id) const; - - void generateUUID(const int64_t & module_id); - - void removeUUID(const int64_t & module_id); - - void publishObjectsOfInterestMarker(); - - void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - - static bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) - { - bool enable_rtc = true; - - try { - enable_rtc = getOrDeclareParameter(node, "enable_all_modules_auto_mode") - ? false - : getOrDeclareParameter(node, param_name); - } catch (const std::exception & e) { - enable_rtc = getOrDeclareParameter(node, param_name); - } - - return enable_rtc; - } + std::shared_ptr planning_factor_interface_; }; - +extern template SceneModuleManagerInterface::SceneModuleManagerInterface( + rclcpp::Node & node, [[maybe_unused]] const char * module_name); +extern template size_t SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +extern template void SceneModuleManagerInterface::updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const tier4_planning_msgs::msg::PathWithLaneId & path); +extern template void SceneModuleManagerInterface::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path); +extern template void SceneModuleManagerInterface::deleteExpiredModules( + const tier4_planning_msgs::msg::PathWithLaneId & path); +extern template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); } // namespace autoware::behavior_velocity_planner #endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index a0b54cb879cab..9f920dff8f166 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -29,7 +29,6 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler - autoware_rtc_interface autoware_universe_utils autoware_vehicle_info_utils autoware_velocity_smoother @@ -46,7 +45,6 @@ tf2_geometry_msgs tf2_ros tier4_planning_msgs - tier4_v2x_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index cdfde1ce51205..db2a274272f9f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -13,32 +13,26 @@ // limitations under the License. #include -#include #include -#include -#include #include #include #include #include -#include #include namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::StopWatch; - SceneModuleInterface::SceneModuleInterface( - const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) : module_id_(module_id), - activated_(false), - safe_(false), - distance_(std::numeric_limits::lowest()), logger_(logger), clock_(clock), - time_keeper_(std::shared_ptr()) + time_keeper_(time_keeper), + planning_factor_interface_(planning_factor_interface) { } @@ -50,222 +44,17 @@ size_t SceneModuleInterface::findEgoSegmentIndex( points, p->current_odometry->pose, p->ego_nearest_dist_threshold); } -SceneModuleManagerInterface::SceneModuleManagerInterface( - rclcpp::Node & node, [[maybe_unused]] const char * module_name) -: node_(node), clock_(node.get_clock()), logger_(node.get_logger()) -{ - const auto ns = std::string("~/debug/") + module_name; - pub_debug_ = node.create_publisher(ns, 1); - if (!node.has_parameter("is_publish_debug_path")) { - is_publish_debug_path_ = node.declare_parameter("is_publish_debug_path"); - } else { - is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool(); - } - if (is_publish_debug_path_) { - pub_debug_path_ = node.create_publisher( - std::string("~/debug/path_with_lane_id/") + module_name, 1); - } - pub_virtual_wall_ = node.create_publisher( - std::string("~/virtual_wall/") + module_name, 5); - pub_velocity_factor_ = node.create_publisher( - std::string("/planning/velocity_factors/") + module_name, 1); - pub_infrastructure_commands_ = - node.create_publisher( - "~/output/infrastructure_commands", 1); - - processing_time_publisher_ = std::make_shared(&node, "~/debug"); - - pub_processing_time_detail_ = node.create_publisher( - "~/debug/processing_time_detail_ms/" + std::string(module_name), 1); - - time_keeper_ = std::make_shared(pub_processing_time_detail_); -} - -size_t SceneModuleManagerInterface::findEgoSegmentIndex( - const std::vector & points) const -{ - const auto & p = planner_data_; - return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - points, p->current_odometry->pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); -} - -void SceneModuleManagerInterface::updateSceneModuleInstances( +template SceneModuleManagerInterface::SceneModuleManagerInterface( + rclcpp::Node & node, [[maybe_unused]] const char * module_name); +template size_t SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +template void SceneModuleManagerInterface::updateSceneModuleInstances( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - planner_data_ = planner_data; - - launchNewModules(path); - deleteExpiredModules(path); -} - -void SceneModuleManagerInterface::modifyPathVelocity( - tier4_planning_msgs::msg::PathWithLaneId * path) -{ - universe_utils::ScopedTimeTrack st( - "SceneModuleManagerInterface::modifyPathVelocity", *time_keeper_); - StopWatch stop_watch; - stop_watch.tic("Total"); - visualization_msgs::msg::MarkerArray debug_marker_array; - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); - - tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; - infrastructure_command_array.stamp = clock_->now(); - - for (const auto & scene_module : scene_modules_) { - scene_module->resetVelocityFactor(); - scene_module->setPlannerData(planner_data_); - scene_module->modifyPathVelocity(path); - - // The velocity factor must be called after modifyPathVelocity. - const auto velocity_factor = scene_module->getVelocityFactor(); - if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } - - if (const auto command = scene_module->getInfrastructureCommand()) { - infrastructure_command_array.commands.push_back(*command); - } - - for (const auto & marker : scene_module->createDebugMarkerArray().markers) { - debug_marker_array.markers.push_back(marker); - } - - virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); - } - - pub_velocity_factor_->publish(velocity_factor_array); - pub_infrastructure_commands_->publish(infrastructure_command_array); - pub_debug_->publish(debug_marker_array); - if (is_publish_debug_path_) { - tier4_planning_msgs::msg::PathWithLaneId debug_path; - debug_path.header = path->header; - debug_path.points = path->points; - pub_debug_path_->publish(debug_path); - } - pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); - processing_time_publisher_->publish( - std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total")); -} - -void SceneModuleManagerInterface::deleteExpiredModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - const auto isModuleExpired = getModuleExpiredFunction(path); - - auto itr = scene_modules_.begin(); - while (itr != scene_modules_.end()) { - if (isModuleExpired(*itr)) { - itr = scene_modules_.erase(itr); - } else { - itr++; - } - } -} - -void SceneModuleManagerInterface::registerModule( - const std::shared_ptr & scene_module) -{ - RCLCPP_DEBUG( - logger_, "register task: module = %s, id = %lu", getModuleName(), scene_module->getModuleId()); - registered_module_id_set_.emplace(scene_module->getModuleId()); - scene_module->setTimeKeeper(time_keeper_); - scene_modules_.insert(scene_module); -} - -SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( - rclcpp::Node & node, const char * module_name, const bool enable_rtc) -: SceneModuleManagerInterface(node, module_name), - rtc_interface_(&node, module_name, enable_rtc), - objects_of_interest_marker_interface_(&node, module_name) -{ -} - -void SceneModuleManagerInterfaceWithRTC::plan(tier4_planning_msgs::msg::PathWithLaneId * path) -{ - setActivation(); - modifyPathVelocity(path); - sendRTC(path->header.stamp); - publishObjectsOfInterestMarker(); -} - -void SceneModuleManagerInterfaceWithRTC::sendRTC(const Time & stamp) -{ - for (const auto & scene_module : scene_modules_) { - const UUID uuid = getUUID(scene_module->getModuleId()); - const auto state = !scene_module->isActivated() && scene_module->isSafe() - ? State::WAITING_FOR_EXECUTION - : State::RUNNING; - updateRTCStatus(uuid, scene_module->isSafe(), state, scene_module->getDistance(), stamp); - } - publishRTCStatus(stamp); -} - -void SceneModuleManagerInterfaceWithRTC::setActivation() -{ - for (const auto & scene_module : scene_modules_) { - const UUID uuid = getUUID(scene_module->getModuleId()); - scene_module->setActivation(rtc_interface_.isActivated(uuid)); - scene_module->setRTCEnabled(rtc_interface_.isRTCEnabled(uuid)); - } -} - -UUID SceneModuleManagerInterfaceWithRTC::getUUID(const int64_t & module_id) const -{ - if (map_uuid_.count(module_id) == 0) { - const UUID uuid; - return uuid; - } - return map_uuid_.at(module_id); -} - -void SceneModuleManagerInterfaceWithRTC::generateUUID(const int64_t & module_id) -{ - map_uuid_.insert({module_id, autoware::universe_utils::generateUUID()}); -} - -void SceneModuleManagerInterfaceWithRTC::removeUUID(const int64_t & module_id) -{ - const auto result = map_uuid_.erase(module_id); - if (result == 0) { - RCLCPP_WARN_STREAM(logger_, "[removeUUID] module_id = " << module_id << " is not registered."); - } -} - -void SceneModuleManagerInterfaceWithRTC::publishObjectsOfInterestMarker() -{ - for (const auto & scene_module : scene_modules_) { - const auto objects = scene_module->getObjectsOfInterestData(); - for (const auto & obj : objects) { - objects_of_interest_marker_interface_.insertObjectData(obj.pose, obj.shape, obj.color); - } - scene_module->clearObjectsOfInterestData(); - } - - objects_of_interest_marker_interface_.publishMarkerArray(); -} - -void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - const auto isModuleExpired = getModuleExpiredFunction(path); - - auto itr = scene_modules_.begin(); - while (itr != scene_modules_.end()) { - if (isModuleExpired(*itr)) { - const UUID uuid = getUUID((*itr)->getModuleId()); - updateRTCStatus( - uuid, (*itr)->isSafe(), State::SUCCEEDED, std::numeric_limits::lowest(), - clock_->now()); - removeUUID((*itr)->getModuleId()); - registered_module_id_set_.erase((*itr)->getModuleId()); - itr = scene_modules_.erase(itr); - } else { - itr++; - } - } -} - + const tier4_planning_msgs::msg::PathWithLaneId & path); +template void SceneModuleManagerInterface::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path); +template void SceneModuleManagerInterface::deleteExpiredModules( + const tier4_planning_msgs::msg::PathWithLaneId & path); +template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/CMakeLists.txt new file mode 100644 index 0000000000000..2bd9b9b8d20f0 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_velocity_rtc_interface) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene_module_interface_with_rtc.cpp +) + +ament_auto_package(INSTALL_TO_SHARE) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/README.md new file mode 100644 index 0000000000000..79b5a4e3d7b95 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/README.md @@ -0,0 +1,3 @@ +# Behavior Velocity RTC Interface + +This package provides a behavior velocity interface with RTC, which are used in the `behavior_velocity_planner` node and modules. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp new file mode 100644 index 0000000000000..a955538f4f9fe --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp @@ -0,0 +1,156 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_RTC_INTERFACE__SCENE_MODULE_INTERFACE_WITH_RTC_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_RTC_INTERFACE__SCENE_MODULE_INTERFACE_WITH_RTC_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +// Debug +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +using autoware::rtc_interface::RTCInterface; +using autoware::universe_utils::getOrDeclareParameter; +using builtin_interfaces::msg::Time; +using tier4_planning_msgs::msg::PathWithLaneId; +using tier4_rtc_msgs::msg::Module; +using tier4_rtc_msgs::msg::State; +using unique_identifier_msgs::msg::UUID; + +class SceneModuleInterfaceWithRTC : public SceneModuleInterface +{ +public: + explicit SceneModuleInterfaceWithRTC( + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); + virtual ~SceneModuleInterfaceWithRTC() = default; + + void setActivation(const bool activated) { activated_ = activated; } + void setRTCEnabled(const bool enable_rtc) { rtc_enabled_ = enable_rtc; } + bool isActivated() const { return activated_; } + bool isSafe() const { return safe_; } + double getDistance() const { return distance_; } + +protected: + bool activated_; + bool safe_; + bool rtc_enabled_; + double distance_; + + void setSafe(const bool safe) + { + safe_ = safe; + if (!rtc_enabled_) { + syncActivation(); + } + } + void setDistance(const double distance) { distance_ = distance; } + void syncActivation() { setActivation(isSafe()); } +}; + +class SceneModuleManagerInterfaceWithRTC +: public SceneModuleManagerInterface +{ +public: + SceneModuleManagerInterfaceWithRTC( + rclcpp::Node & node, const char * module_name, const bool enable_rtc = true); + + void plan(tier4_planning_msgs::msg::PathWithLaneId * path) override; + +protected: + RTCInterface rtc_interface_; + std::unordered_map map_uuid_; + + ObjectsOfInterestMarkerInterface objects_of_interest_marker_interface_; + + virtual void sendRTC(const Time & stamp); + + virtual void setActivation(); + + void updateRTCStatus( + const UUID & uuid, const bool safe, const uint8_t state, const double distance, + const Time & stamp) + { + rtc_interface_.updateCooperateStatus(uuid, safe, state, distance, distance, stamp); + } + + void removeRTCStatus(const UUID & uuid) { rtc_interface_.removeCooperateStatus(uuid); } + + void publishRTCStatus(const Time & stamp) + { + rtc_interface_.removeExpiredCooperateStatus(); + rtc_interface_.publishCooperateStatus(stamp); + } + + UUID getUUID(const int64_t & module_id) const; + + void generateUUID(const int64_t & module_id); + + void removeUUID(const int64_t & module_id); + + void publishObjectsOfInterestMarker(); + + void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + + static bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) + { + bool enable_rtc = true; + + try { + enable_rtc = getOrDeclareParameter(node, "enable_all_modules_auto_mode") + ? false + : getOrDeclareParameter(node, param_name); + } catch (const std::exception & e) { + enable_rtc = getOrDeclareParameter(node, param_name); + } + + return enable_rtc; + } +}; + +extern template size_t +SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +extern template void +SceneModuleManagerInterface::updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const tier4_planning_msgs::msg::PathWithLaneId & path); +extern template void SceneModuleManagerInterface::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path); +extern template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_RTC_INTERFACE__SCENE_MODULE_INTERFACE_WITH_RTC_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml new file mode 100644 index 0000000000000..88b252106a90a --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml @@ -0,0 +1,38 @@ + + + + autoware_behavior_velocity_rtc_interface + 0.40.0 + The autoware_behavior_velocity_rtc_interface package + + Mamoru Sobue + Takayuki Murooka + Tomoya Kimura + Shumpei Wakabayashi + Takagi, Isamu + Fumiya Watanabe + + Apache License 2.0 + + Takayuki Murooka + + ament_cmake_auto + autoware_cmake + + autoware_behavior_velocity_planner_common + autoware_motion_utils + autoware_planning_msgs + autoware_rtc_interface + autoware_universe_utils + rclcpp + rclcpp_components + tier4_planning_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp new file mode 100644 index 0000000000000..2e73b35e20ed9 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp @@ -0,0 +1,142 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +SceneModuleInterfaceWithRTC::SceneModuleInterfaceWithRTC( + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), + activated_(false), + safe_(false), + distance_(std::numeric_limits::lowest()) +{ +} + +SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( + rclcpp::Node & node, const char * module_name, const bool enable_rtc) +: SceneModuleManagerInterface(node, module_name), + rtc_interface_(&node, module_name, enable_rtc), + objects_of_interest_marker_interface_(&node, module_name) +{ +} + +void SceneModuleManagerInterfaceWithRTC::plan(tier4_planning_msgs::msg::PathWithLaneId * path) +{ + setActivation(); + modifyPathVelocity(path); + sendRTC(path->header.stamp); + publishObjectsOfInterestMarker(); +} + +void SceneModuleManagerInterfaceWithRTC::sendRTC(const Time & stamp) +{ + for (const auto & scene_module : scene_modules_) { + const UUID uuid = getUUID(scene_module->getModuleId()); + const auto state = !scene_module->isActivated() && scene_module->isSafe() + ? State::WAITING_FOR_EXECUTION + : State::RUNNING; + updateRTCStatus(uuid, scene_module->isSafe(), state, scene_module->getDistance(), stamp); + } + publishRTCStatus(stamp); +} + +void SceneModuleManagerInterfaceWithRTC::setActivation() +{ + for (const auto & scene_module : scene_modules_) { + const UUID uuid = getUUID(scene_module->getModuleId()); + scene_module->setActivation(rtc_interface_.isActivated(uuid)); + scene_module->setRTCEnabled(rtc_interface_.isRTCEnabled(uuid)); + } +} + +UUID SceneModuleManagerInterfaceWithRTC::getUUID(const int64_t & module_id) const +{ + if (map_uuid_.count(module_id) == 0) { + const UUID uuid; + return uuid; + } + return map_uuid_.at(module_id); +} + +void SceneModuleManagerInterfaceWithRTC::generateUUID(const int64_t & module_id) +{ + map_uuid_.insert({module_id, autoware::universe_utils::generateUUID()}); +} + +void SceneModuleManagerInterfaceWithRTC::removeUUID(const int64_t & module_id) +{ + const auto result = map_uuid_.erase(module_id); + if (result == 0) { + RCLCPP_WARN_STREAM(logger_, "[removeUUID] module_id = " << module_id << " is not registered."); + } +} + +void SceneModuleManagerInterfaceWithRTC::publishObjectsOfInterestMarker() +{ + for (const auto & scene_module : scene_modules_) { + const auto objects = scene_module->getObjectsOfInterestData(); + for (const auto & obj : objects) { + objects_of_interest_marker_interface_.insertObjectData(obj.pose, obj.shape, obj.color); + } + scene_module->clearObjectsOfInterestData(); + } + + objects_of_interest_marker_interface_.publishMarkerArray(); +} + +void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto isModuleExpired = getModuleExpiredFunction(path); + + auto itr = scene_modules_.begin(); + while (itr != scene_modules_.end()) { + if (isModuleExpired(*itr)) { + const UUID uuid = getUUID((*itr)->getModuleId()); + updateRTCStatus( + uuid, (*itr)->isSafe(), State::SUCCEEDED, std::numeric_limits::lowest(), + clock_->now()); + removeUUID((*itr)->getModuleId()); + registered_module_id_set_.erase((*itr)->getModuleId()); + itr = scene_modules_.erase(itr); + } else { + itr++; + } + } +} + +template size_t SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +template void SceneModuleManagerInterface::updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const tier4_planning_msgs::msg::PathWithLaneId & path); +template void SceneModuleManagerInterface::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path); +template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CMakeLists.txt index da2f4ce33ff60..a2f35ada11b5e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CMakeLists.txt @@ -17,10 +17,11 @@ ament_auto_add_library(${PROJECT_NAME} SHARED if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - tests/test_dynamic_obstacle.cpp - tests/test_path_utils.cpp - tests/test_utils.cpp - tests/test_state_machine.cpp + test/test_dynamic_obstacle.cpp + test/test_path_utils.cpp + test/test_utils.cpp + test/test_state_machine.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} autoware_behavior_velocity_run_out_module diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml index b3ced8b2e9b9f..18db8281356f8 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml @@ -20,6 +20,7 @@ eigen3_cmake_module autoware_behavior_velocity_crosswalk_module + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_internal_debug_msgs autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp index 4a16f263d0a54..4af7882461643 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp @@ -156,7 +156,8 @@ void RunOutModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathW if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, planner_data_, planner_param_, logger_.get_child("run_out_module"), - std::move(dynamic_obstacle_creator_), debug_ptr_, clock_)); + std::move(dynamic_obstacle_creator_), debug_ptr_, clock_, time_keeper_, + planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp index 131ba32f32100..068ed81015fb1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp @@ -25,7 +25,7 @@ namespace autoware::behavior_velocity_planner { -class RunOutModuleManager : public SceneModuleManagerInterface +class RunOutModuleManager : public SceneModuleManagerInterface<> { public: explicit RunOutModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp index efc2d458ae2a7..193ba5eab0553 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -45,15 +45,15 @@ RunOutModule::RunOutModule( const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, std::unique_ptr dynamic_obstacle_creator, - const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), planner_param_(planner_param), dynamic_obstacle_creator_(std::move(dynamic_obstacle_creator)), debug_ptr_(debug_ptr), state_machine_(std::make_unique(planner_param.approaching.state)) { - velocity_factor_.init(PlanningBehavior::RUN_OUT); - if (planner_param.run_out.use_partition_lanelet) { const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr(); planning_utils::getAllPartitionLanelets(ll, partition_lanelets_); @@ -770,9 +770,10 @@ bool RunOutModule::insertStopPoint( stop_point_with_lane_id.point.pose = *stop_point; planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); - velocity_factor_.set( - path.points, planner_data_->current_odometry->pose, stop_point.value(), VelocityFactor::UNKNOWN, - "run_out"); + planning_factor_interface_->add( + path.points, planner_data_->current_odometry->pose, stop_point.value(), stop_point.value(), + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0 /*velocity*/, 0.0 /*shift_distance*/, "run_out_stop"); return true; } @@ -876,8 +877,11 @@ void RunOutModule::insertApproachingVelocity( return; } - velocity_factor_.set( - output_path.points, current_pose, stop_point.value(), VelocityFactor::UNKNOWN, "run_out"); + planning_factor_interface_->add( + output_path.points, planner_data_->current_odometry->pose, stop_point.value(), + stop_point.value(), tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0 /*velocity*/, + 0.0 /*shift_distance*/, "run_out_approaching_velocity"); // debug debug_ptr_->pushStopPose(autoware::universe_utils::calcOffsetPose( 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 83123c71f461e..49de16753a869 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 @@ -47,7 +47,9 @@ class RunOutModule : public SceneModuleInterface const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, std::unique_ptr dynamic_obstacle_creator, - const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock); + const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_dynamic_obstacle.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_dynamic_obstacle.cpp similarity index 100% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_dynamic_obstacle.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_dynamic_obstacle.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..75bf59751ed44 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "run_out", "autoware::behavior_velocity_planner::RunOutModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_path_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_path_utils.cpp similarity index 100% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_path_utils.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_path_utils.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_state_machine.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_state_machine.cpp similarity index 100% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_state_machine.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_state_machine.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_utils.cpp similarity index 100% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_utils.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_utils.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp index 4677d49b78f4b..a69e33e9e0391 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp @@ -63,7 +63,7 @@ void SpeedBumpModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, lane_id, *speed_bump_with_lane_id.first, planner_param_, - logger_.get_child("speed_bump_module"), clock_)); + logger_.get_child("speed_bump_module"), clock_, time_keeper_, planning_factor_interface_)); } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp index 950bb8471cc22..f98db8b88b7a9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp @@ -29,7 +29,7 @@ namespace autoware::behavior_velocity_planner { -class SpeedBumpModuleManager : public SceneModuleManagerInterface +class SpeedBumpModuleManager : public SceneModuleManagerInterface<> { public: explicit SpeedBumpModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp index 54ea807f3268b..4af7801e50ba3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp @@ -21,6 +21,7 @@ #include #include +#include #include namespace autoware::behavior_velocity_planner @@ -33,8 +34,10 @@ using geometry_msgs::msg::Point32; SpeedBumpModule::SpeedBumpModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::SpeedBump & speed_bump_reg_elem, const PlannerParam & planner_param, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), module_id_(module_id), lane_id_(lane_id), speed_bump_reg_elem_(std::move(speed_bump_reg_elem)), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp index 176a01d5112c5..e3ee2c0566381 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -54,7 +55,9 @@ class SpeedBumpModule : public SceneModuleInterface SpeedBumpModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::SpeedBump & speed_bump_reg_elem, const PlannerParam & planner_param, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt index f4528f0d13cf4..a187b4cda9459 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt @@ -14,6 +14,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_scene.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} gtest_main diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md index f373afc2351bf..963d75e5a8492 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md @@ -1,33 +1,32 @@ -## Stop Line +# Stop Line -### Role +## Role -This module plans velocity so that the vehicle can stop right before stop lines and restart driving after stopped. +This module plans the vehicle's velocity to ensure it stops just before stop lines and can resume movement after stopping. ![stop line](docs/stop_line.svg) -### Activation Timing +## Activation Timing This module is activated when there is a stop line in a target lane. -### Module Parameters +## Module Parameters -| Parameter | Type | Description | -| -------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `stop_margin` | double | a margin that the vehicle tries to stop before stop_line | -| `stop_duration_sec` | double | [s] time parameter for the ego vehicle to stop in front of a stop line | -| `hold_stop_margin_distance` | double | [m] parameter for restart prevention (See Algorithm section). Also, when the ego vehicle is within this distance from a stop line, the ego state becomes STOPPED from APPROACHING | -| `use_initialization_stop_state` | bool | A flag to determine whether to return to the approaching state when the vehicle moves away from a stop line. | -| `show_stop_line_collision_check` | bool | A flag to determine whether to show the debug information of collision check with a stop line | +| Parameter | Type | Description | +| ------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `stop_margin` | double | Margin that the vehicle tries to stop in before stop_line | +| `stop_duration_sec` | double | [s] Time parameter for the ego vehicle to stop before stop line | +| `hold_stop_margin_distance` | double | [m] Parameter for restart prevention (See Algorithm section). Also, when the ego vehicle is within this distance from a stop line, the ego state becomes STOPPED from APPROACHING | +| `use_initialization_stop_state` | bool | Flag to determine whether to return to the approaching state when the vehicle moves away from a stop line. | -### Inner-workings / Algorithms +## Inner-workings / Algorithms - Gets a stop line from map information. -- insert a stop point on the path from the stop line defined in the map and the ego vehicle length. +- Inserts a stop point on the path from the stop line defined in the map and the ego vehicle length. - Sets velocities of the path after the stop point to 0[m/s]. -- Release the inserted stop velocity when the vehicle stops at the stop point for `stop_duration_sec` seconds. +- Releases the inserted stop velocity when the vehicle halts at the stop point for `stop_duration_sec` seconds. -#### Flowchart +### Flowchart ```plantuml @startuml @@ -85,15 +84,15 @@ Then, we can get `offset segment` and `offset from segment start`. ![find_offset_segment](docs/./find_offset_segment.drawio.svg) -After that, we can calculate a offset point from `offset segment` and `offset`. This will be `stop_pose`. +After that, we can calculate an offset point from `offset segment` and `offset`. This will be `stop_pose`. ![calculate_stop_pose](docs/./calculate_stop_pose.drawio.svg) -#### Restart prevention +### Restart Prevention -If it needs X meters (e.g. 0.5 meters) to stop once the vehicle starts moving due to the poor vehicle control performance, the vehicle goes over the stopping position that should be strictly observed when the vehicle starts to moving in order to approach the near stop point (e.g. 0.3 meters away). +If the vehicle requires X meters (e.g. 0.5 meters) to stop once it starts moving due to poor vehicle control performance, it may overshoot the stopping position, which must be strictly observed. This happens when the vehicle begins to move in order to approach a nearby stop point (e.g. 0.3 meters away). -This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle is stopped within `hold_stop_margin_distance` meters from stop point of the module (\_front_to_stop_line < hold_stop_margin_distance), the module judges that the vehicle has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is stopped due to other factors. +This module includes the parameter `hold_stop_margin_distance` to prevent redundant restarts in such a situation. If the vehicle is stopped within `hold_stop_margin_distance` meters of the stop point (\_front_to_stop_line < hold_stop_margin_distance), the module determines that the vehicle has already stopped at the stop point and will maintain the current stopping position, even if the vehicle has come to a stop due to other factors.
![example](docs/restart_prevention.svg){width=1000} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg index 182c941907f57..6cd47ffc14a56 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg @@ -233,7 +233,7 @@
= node(i) + node(i+1)
- = all segment has two points + = all segments have two points diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml index fd4b9f83ae998..849ab3915c649 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml @@ -20,6 +20,7 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp index b305a1d2aa404..e289779df34b6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp @@ -81,7 +81,7 @@ void StopLineModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pat if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, stop_line_with_lane_id.first, planner_param_, - logger_.get_child("stop_line_module"), clock_)); + logger_.get_child("stop_line_module"), clock_, time_keeper_, planning_factor_interface_)); } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp index bef8a5eef4ac0..c746e2bf6a314 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp @@ -33,7 +33,7 @@ namespace autoware::behavior_velocity_planner { using StopLineWithLaneId = std::pair; -class StopLineModuleManager : public SceneModuleManagerInterface +class StopLineModuleManager : public SceneModuleManagerInterface<> { public: explicit StopLineModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 2a9c5dab1ebd9..e2ddbc75c7b57 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -20,6 +20,7 @@ #include +#include #include #include @@ -28,14 +29,15 @@ namespace autoware::behavior_velocity_planner StopLineModule::StopLineModule( const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), stop_line_(std::move(stop_line)), planner_param_(planner_param), state_(State::APPROACH), debug_data_() { - velocity_factor_.init(PlanningBehavior::STOP_SIGN); } bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) @@ -59,7 +61,12 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) path->points = trajectory->restore(); - updateVelocityFactor(&velocity_factor_, state_, *stop_point - ego_s); + // TODO(soblin): PlanningFactorInterface use trajectory class + planning_factor_interface_->add( + path->points, trajectory->compute(*stop_point).point.pose, + planner_data_->current_odometry->pose, planner_data_->current_odometry->pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "stopline"); updateStateAndStoppedTime( &state_, &stopped_time_, clock_->now(), *stop_point - ego_s, planner_data_->isVehicleStopped()); @@ -149,24 +156,6 @@ void StopLineModule::updateStateAndStoppedTime( } } -void StopLineModule::updateVelocityFactor( - autoware::motion_utils::VelocityFactorInterface * velocity_factor, const State & state, - const double & distance_to_stop_point) -{ - switch (state) { - case State::APPROACH: { - velocity_factor->set(distance_to_stop_point, VelocityFactor::APPROACHING); - break; - } - case State::STOPPED: { - velocity_factor->set(distance_to_stop_point, VelocityFactor::STOPPED); - break; - } - case State::START: - break; - } -} - void StopLineModule::updateDebugData( DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp index 9ac1463da8618..7d430463c18b9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -18,7 +18,6 @@ #include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp" #include "autoware/behavior_velocity_planner_common/utilization/util.hpp" -#include "autoware/motion_utils/factor/velocity_factor_interface.hpp" #include "autoware/trajectory/path_point_with_lane_id.hpp" #include @@ -30,6 +29,7 @@ #include +#include #include #include @@ -68,7 +68,9 @@ class StopLineModule : public SceneModuleInterface StopLineModule( const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; @@ -95,10 +97,6 @@ class StopLineModule : public SceneModuleInterface State * state, std::optional * stopped_time, const rclcpp::Time & now, const double & distance_to_stop_point, const bool & is_vehicle_stopped) const; - static void updateVelocityFactor( - autoware::motion_utils::VelocityFactorInterface * velocity_factor, const State & state, - const double & distance_to_stop_point); - void updateDebugData( DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..c6d6ff638cfbb --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "stop_line", "autoware::behavior_velocity_planner::StopLineModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp index 1eafb71eb403c..e304b479a005d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp @@ -74,7 +74,10 @@ class StopLineModuleTest : public ::testing::Test clock_ = std::make_shared(); module_ = std::make_shared( - 1, stop_line_, planner_param_, rclcpp::get_logger("test_logger"), clock_); + 1, stop_line_, planner_param_, rclcpp::get_logger("test_logger"), clock_, + std::make_shared(), + std::make_shared( + node_.get(), "test_stopline")); module_->setPlannerData(planner_data_); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp index 4325cc1d5aaf9..a004f5b823138 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp @@ -42,8 +42,9 @@ void TemplateModuleManager::launchNewModules( { int64_t module_id = 0; if (!isModuleRegistered(module_id)) { - registerModule( - std::make_shared(module_id, logger_.get_child(getModuleName()), clock_)); + registerModule(std::make_shared( + module_id, logger_.get_child(getModuleName()), clock_, time_keeper_, + planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp index a7a0d83be6368..c5b9293fcdcc5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp @@ -38,7 +38,7 @@ namespace autoware::behavior_velocity_planner * @param node A reference to the ROS node. */ class TemplateModuleManager -: public autoware::behavior_velocity_planner::SceneModuleManagerInterface +: public autoware::behavior_velocity_planner::SceneModuleManagerInterface<> { public: explicit TemplateModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp index 3ce8502baaf63..d8eea337e7186 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp @@ -19,14 +19,17 @@ #include +#include #include namespace autoware::behavior_velocity_planner { TemplateModule::TemplateModule( - const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock) + const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface) { } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp index 70cd5cb1235e7..ba078f3fd6421 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp @@ -18,9 +18,9 @@ #include #include +#include #include #include - namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathWithLaneId; @@ -29,7 +29,9 @@ class TemplateModule : public SceneModuleInterface { public: TemplateModule( - const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** * @brief Modify the velocity of path points. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CMakeLists.txt index 6370dd5e6c21d..02ed192459970 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CMakeLists.txt @@ -15,6 +15,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_utils.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml index 082973d9431e5..4bc7a15cddc48 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml @@ -19,7 +19,9 @@ eigen3_cmake_module autoware_adapi_v1_msgs + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index b6747724ba6f7..1d1ee7fc996b2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp @@ -53,24 +53,13 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat autoware_perception_msgs::msg::TrafficLightGroup tl_state; - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); - nearest_ref_stop_path_point_index_ = static_cast(path->points.size() - 1); for (const auto & scene_module : scene_modules_) { std::shared_ptr traffic_light_scene_module( std::dynamic_pointer_cast(scene_module)); - traffic_light_scene_module->resetVelocityFactor(); traffic_light_scene_module->setPlannerData(planner_data_); traffic_light_scene_module->modifyPathVelocity(path); - // The velocity factor must be called after modifyPathVelocity. - const auto velocity_factor = traffic_light_scene_module->getVelocityFactor(); - if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } - if ( traffic_light_scene_module->getFirstRefStopPathPointIndex() < nearest_ref_stop_path_point_index_) { @@ -88,7 +77,6 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat virtual_wall_marker_creator_.add_virtual_walls( traffic_light_scene_module->createVirtualWalls()); } - pub_velocity_factor_->publish(velocity_factor_array); pub_debug_->publish(debug_marker_array); pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); pub_tl_state_->publish(tl_state); @@ -114,7 +102,8 @@ void TrafficLightModuleManager::launchNewModules( if (!isModuleRegisteredFromExistingAssociatedModule(lane_id)) { registerModule(std::make_shared( lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_, - logger_.get_child("traffic_light_module"), clock_)); + logger_.get_child("traffic_light_module"), clock_, time_keeper_, + planning_factor_interface_)); generateUUID(lane_id); updateRTCStatus( getUUID(lane_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), @@ -123,7 +112,7 @@ void TrafficLightModuleManager::launchNewModules( } } -std::function &)> +std::function &)> TrafficLightModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { @@ -131,7 +120,7 @@ TrafficLightModuleManager::getModuleExpiredFunction( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); return [this, lanelet_id_set]( - [[maybe_unused]] const std::shared_ptr & scene_module) { + [[maybe_unused]] const std::shared_ptr & scene_module) { for (const auto & id : lanelet_id_set) { if (isModuleRegisteredFromExistingAssociatedModule(id)) { return false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp index eb8ef53ddb604..5ac32d1107880 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -42,8 +42,8 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index 5eb0d5aa5f267..becbaf5ee05f2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -43,8 +43,10 @@ namespace autoware::behavior_velocity_planner TrafficLightModule::TrafficLightModule( const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(lane_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(lane_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), traffic_light_reg_elem_(traffic_light_reg_elem), lane_(lane), @@ -52,7 +54,6 @@ TrafficLightModule::TrafficLightModule( debug_data_(), is_prev_state_stop_(false) { - velocity_factor_.init(PlanningBehavior::TRAFFIC_SIGNAL); planner_param_ = planner_param; } @@ -291,9 +292,11 @@ tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( size_t insert_index = insert_target_point_idx; planning_utils::insertVelocity(modified_path, target_point_with_lane_id, 0.0, insert_index); - velocity_factor_.set( + planning_factor_interface_->add( modified_path.points, planner_data_->current_odometry->pose, - target_point_with_lane_id.point.pose, VelocityFactor::UNKNOWN); + target_point_with_lane_id.point.pose, target_point_with_lane_id.point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); return modified_path; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp index 8221bb3740273..c30cbbdfc1477 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp @@ -23,8 +23,8 @@ #define EIGEN_MPL2_ONLY #include #include -#include #include +#include #include #include @@ -33,7 +33,7 @@ namespace autoware::behavior_velocity_planner { -class TrafficLightModule : public SceneModuleInterface +class TrafficLightModule : public SceneModuleInterfaceWithRTC { public: using TrafficSignal = autoware_perception_msgs::msg::TrafficLightGroup; @@ -69,7 +69,9 @@ class TrafficLightModule : public SceneModuleInterface TrafficLightModule( const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..e24c2dab5dab9 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "traffic_light", "autoware::behavior_velocity_planner::TrafficLightModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml index 6d3bc68242d7c..056f84a970c58 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp index a51cf9211c21f..dade98dfc1133 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -21,6 +21,8 @@ #include #include +#include + #include #include @@ -53,6 +55,14 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node p.check_timeout_after_stop_line = getOrDeclareParameter(node, ns + ".check_timeout_after_stop_line"); } + + sub_virtual_traffic_light_states_ = autoware::universe_utils::InterProcessPollingSubscriber< + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>:: + create_subscription(&node, "~/input/virtual_traffic_light_states"); + + pub_infrastructure_commands_ = + node.create_publisher( + "~/output/infrastructure_commands", 1); } void VirtualTrafficLightModuleManager::launchNewModules( @@ -84,22 +94,49 @@ void VirtualTrafficLightModuleManager::launchNewModules( ego_path_linestring, lanelet::utils::to2D(stop_line_opt.value()).basicLineString())) { registerModule(std::make_shared( module_id, lane_id, *m.first, m.second, planner_param_, - logger_.get_child("virtual_traffic_light_module"), clock_)); + logger_.get_child("virtual_traffic_light_module"), clock_, time_keeper_, + planning_factor_interface_)); } } } -std::function &)> +std::function &)> VirtualTrafficLightModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [id_set](const std::shared_ptr & scene_module) { + return [id_set](const std::shared_ptr & scene_module) { return id_set.count(scene_module->getModuleId()) == 0; }; } + +void VirtualTrafficLightModuleManager::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path) +{ + // NOTE: virtual traffic light specific implementation + // Since the argument of modifyPathVelocity cannot be changed, the specific information + // of virtual traffic light states is set here. + const auto virtual_traffic_light_states = sub_virtual_traffic_light_states_->takeData(); + for (const auto & scene_module : scene_modules_) { + scene_module->setVirtualTrafficLightStates(virtual_traffic_light_states); + } + + SceneModuleManagerInterface::modifyPathVelocity(path); + + // NOTE: virtual traffic light specific implementation + // publish infrastructure_command_array + tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; + infrastructure_command_array.stamp = clock_->now(); + + for (const auto & scene_module : scene_modules_) { + if (const auto command = scene_module->getInfrastructureCommand()) { + infrastructure_command_array.commands.push_back(*command); + } + } + pub_infrastructure_commands_->publish(infrastructure_command_array); +} } // namespace autoware::behavior_velocity_planner #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp index ba5a4b23a6fe0..aecef0851d8ab 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp @@ -20,16 +20,20 @@ #include #include #include +#include #include #include +#include +#include #include #include namespace autoware::behavior_velocity_planner { -class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface +class VirtualTrafficLightModuleManager +: public SceneModuleManagerInterface { public: explicit VirtualTrafficLightModuleManager(rclcpp::Node & node); @@ -38,10 +42,20 @@ class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface private: VirtualTrafficLightModule::PlannerParam planner_param_; + + void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( + std::function &)> getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) override; + + autoware::universe_utils::InterProcessPollingSubscriber< + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>::SharedPtr + sub_virtual_traffic_light_states_; + + rclcpp::Publisher::SharedPtr + pub_infrastructure_commands_; }; class VirtualTrafficLightModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 206fb14b6d41c..a5779cfc0712a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -38,15 +39,15 @@ VirtualTrafficLightModule::VirtualTrafficLightModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), reg_elem_(reg_elem), lane_(lane), planner_param_(planner_param) { - velocity_factor_.init(PlanningBehavior::VIRTUAL_TRAFFIC_LIGHT); - // Get map data const auto instrument = reg_elem_.getVirtualTrafficLight(); const auto instrument_bottom_line = toAutowarePoints(instrument); @@ -333,12 +334,12 @@ bool VirtualTrafficLightModule::isNearAnyEndLine(const size_t end_line_idx) std::optional VirtualTrafficLightModule::findCorrespondingState() { - // No message - if (!planner_data_->virtual_traffic_light_states) { + // Note: This variable is set by virtual traffic light's manager. + if (!virtual_traffic_light_states_) { return {}; } - for (const auto & state : planner_data_->virtual_traffic_light_states->states) { + for (const auto & state : virtual_traffic_light_states_->states) { if (state.id == map_data_.instrument_id) { return state; } @@ -417,9 +418,10 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( } // Set StopReason - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN, - command_.type); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); // Set data for visualization module_data_.stop_head_pose_at_stop_line = @@ -450,11 +452,32 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( } // Set StopReason - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); // Set data for visualization module_data_.stop_head_pose_at_end_line = calcHeadPose(stop_pose, planner_data_->vehicle_info_.max_longitudinal_offset_m); } + +std::optional +VirtualTrafficLightModule::getInfrastructureCommand() const +{ + return infrastructure_command_; +} + +void VirtualTrafficLightModule::setInfrastructureCommand( + const std::optional & command) +{ + infrastructure_command_ = command; +} + +void VirtualTrafficLightModule::setVirtualTrafficLightStates( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr + virtual_traffic_light_states) +{ + virtual_traffic_light_states_ = virtual_traffic_light_states; +} } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index b031ba5b2bb2b..0661267b3d361 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -24,9 +25,13 @@ #include #include +#include +#include + #include #include +#include #include #include @@ -77,29 +82,37 @@ class VirtualTrafficLightModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; + std::optional getInfrastructureCommand() const; + void setInfrastructureCommand( + const std::optional & command); + + void setVirtualTrafficLightStates( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr + virtual_traffic_light_states); + private: const int64_t lane_id_; const lanelet::autoware::VirtualTrafficLight & reg_elem_; const lanelet::ConstLanelet lane_; const PlannerParam planner_param_; + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states_; State state_{State::NONE}; tier4_v2x_msgs::msg::InfrastructureCommand command_; + std::optional infrastructure_command_; MapData map_data_; ModuleData module_data_; void updateInfrastructureCommand(); - void setVelocityFactor( - const geometry_msgs::msg::Pose & stop_pose, - autoware_adapi_v1_msgs::msg::VelocityFactor * velocity_factor); - std::optional getPathIndexOfFirstEndLine(); bool isBeforeStartLine(const size_t end_line_idx); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..ab2fd5847c5be --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp @@ -0,0 +1,74 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "virtual_traffic_light", + "autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + test_manager->publishVirtualTrafficLightState( + test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states"); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "virtual_traffic_light", + "autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + test_manager->publishVirtualTrafficLightState( + test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states"); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/CMakeLists.txt index 11504d9c8999c..8b11fdce7283e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/CMakeLists.txt @@ -11,4 +11,14 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene_walkway.cpp ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + file(GLOB_RECURSE TEST_SOURCES test/*.cpp) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + ${TEST_SOURCES} + ) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) +endif() + ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml index 5f1aea22855a4..857d9b524beb0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_behavior_velocity_crosswalk_module + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_lanelet2_extension autoware_motion_utils @@ -31,6 +32,7 @@ rclcpp visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp index 0f1a7509043b5..bfa8a96531090 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp @@ -61,7 +61,8 @@ void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); registerModule(std::make_shared( - lanelet.id(), lanelet_map_ptr, p, use_regulatory_element, logger, clock_)); + lanelet.id(), lanelet_map_ptr, p, use_regulatory_element, logger, clock_, time_keeper_, + planning_factor_interface_)); }; const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp index 85d4495914823..b323d6d201795 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp @@ -35,7 +35,7 @@ namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathWithLaneId; -class WalkwayModuleManager : public SceneModuleManagerInterface +class WalkwayModuleManager : public SceneModuleManagerInterface<> { public: explicit WalkwayModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp index 6a7505930a334..94421ae27e077 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -18,6 +18,7 @@ #include #include +#include #include namespace autoware::behavior_velocity_planner @@ -32,15 +33,15 @@ using autoware::universe_utils::getPose; WalkwayModule::WalkwayModule( const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const bool use_regulatory_element, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), module_id_(module_id), state_(State::APPROACH), planner_param_(planner_param), use_regulatory_element_(use_regulatory_element) { - velocity_factor_.init(PlanningBehavior::SIDEWALK); - if (use_regulatory_element_) { const auto reg_elem_ptr = std::dynamic_pointer_cast( lanelet_map_ptr->regulatoryElementLayer.get(module_id)); @@ -119,9 +120,10 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path) } /* get stop point and stop factor */ - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose.value(), - VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose.value(), stop_pose.value(), + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0 /*velocity*/, 0.0 /*shift distance*/, "walkway_stop"); // use arc length to identify if ego vehicle is in front of walkway stop or not. const double signed_arc_dist_to_stop_point = diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp index df54eafd11cc2..1bd5003498d34 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp @@ -44,7 +44,9 @@ class WalkwayModule : public SceneModuleInterface WalkwayModule( const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const bool use_regulatory_element, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..e0b7fa5c31965 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "walkway", "autoware::behavior_velocity_planner::WalkwayModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/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 40aa985ee42e2..2000615b51e2c 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 @@ -43,7 +43,9 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo module_name_ = module_name; logger_ = node.get_logger().get_child(ns_); clock_ = node.get_clock(); - velocity_factor_interface_.init(autoware::motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + + planning_factor_interface_ = std::make_unique( + &node, "dynamic_obstacle_stop"); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); @@ -131,7 +133,7 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( ? 0.0 : params_.hysteresis; const auto dynamic_obstacles = dynamic_obstacle_stop::filter_predicted_objects( - planner_data->predicted_objects, ego_data, params_, hysteresis); + planner_data->objects, ego_data, params_, hysteresis); const auto preprocessing_duration_us = stopwatch.toc("preprocessing"); @@ -160,15 +162,9 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( debug_data_.stop_pose = stop_pose; if (stop_pose) { result.stop_points.push_back(stop_pose->position); - const auto stop_pose_reached = - planner_data->current_odometry.twist.twist.linear.x < 1e-3 && - autoware::universe_utils::calcDistance2d(ego_data.pose, *stop_pose) < 1e-3; - velocity_factor_interface_.set( - ego_trajectory_points, ego_data.pose, *stop_pose, - stop_pose_reached ? autoware::motion_utils::VelocityFactor::STOPPED - : autoware::motion_utils::VelocityFactor::APPROACHING, - "dynamic_obstacle_stop"); - result.velocity_factor = velocity_factor_interface_.get(); + planning_factor_interface_->add( + ego_trajectory_points, ego_data.pose, *stop_pose, PlanningFactor::STOP, + SafetyFactorArray{}); create_virtual_walls(); } } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp index 1843d26a22a29..82a6d790bebe7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp @@ -33,6 +33,7 @@ class DynamicObstacleStopModule : public PluginModuleInterface { public: void init(rclcpp::Node & node, const std::string & module_name) override; + void publish_planning_factor() override { planning_factor_interface_->publish(); }; void update_parameters(const std::vector & parameters) override; VelocityPlanningResult plan( const std::vector & ego_trajectory_points, diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp index 2ab3c09e64edd..0726fdba02de0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -96,20 +96,22 @@ bool is_unavoidable( }; std::vector filter_predicted_objects( - const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const std::vector & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis) { std::vector filtered_objects; - for (const auto & object : objects.objects) { - const auto is_not_too_slow = object.kinematics.initial_twist_with_covariance.twist.linear.x >= - params.minimum_object_velocity; + for (const auto & object : objects) { + const auto & predicted_object = object.predicted_object; + const auto is_not_too_slow = + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x >= + params.minimum_object_velocity; if ( - is_vehicle(object) && is_not_too_slow && - is_in_range(object, ego_data.trajectory, params, hysteresis) && - is_not_too_close(object, ego_data, params.ego_longitudinal_offset) && + is_vehicle(predicted_object) && is_not_too_slow && + is_in_range(predicted_object, ego_data.trajectory, params, hysteresis) && + is_not_too_close(predicted_object, ego_data, params.ego_longitudinal_offset) && (!params.ignore_unavoidable_collisions || - !is_unavoidable(object, ego_data.pose, ego_data.earliest_stop_pose, params))) - filtered_objects.push_back(object); + !is_unavoidable(predicted_object, ego_data.pose, ego_data.earliest_stop_pose, params))) + filtered_objects.push_back(predicted_object); } return filtered_objects; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp index d20d6354066c6..6024cde019489 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp @@ -17,6 +17,8 @@ #include "types.hpp" +#include + #include #include @@ -74,7 +76,7 @@ bool is_unavoidable( /// @param hysteresis [m] extra distance threshold used for filtering /// @return filtered predicted objects std::vector filter_predicted_objects( - const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const std::vector & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis); } // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp index 3fa179903c23f..5993f81c7a8f1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp @@ -14,6 +14,7 @@ #include "../src/object_filtering.hpp" +#include #include #include @@ -26,6 +27,8 @@ #include #include +#include + TEST(TestObjectFiltering, isVehicle) { using autoware::motion_velocity_planner::dynamic_obstacle_stop::is_vehicle; @@ -202,8 +205,7 @@ TEST(TestObjectFiltering, isUnavoidable) TEST(TestObjectFiltering, filterPredictedObjects) { using autoware::motion_velocity_planner::dynamic_obstacle_stop::filter_predicted_objects; - autoware_perception_msgs::msg::PredictedObjects objects; - autoware_perception_msgs::msg::PredictedObject object; + std::vector objects; autoware::motion_velocity_planner::dynamic_obstacle_stop::EgoData ego_data; autoware::motion_velocity_planner::dynamic_obstacle_stop::PlannerParam params; double hysteresis{}; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp index 1d980e220ccef..c8aa9895a0451 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp @@ -76,12 +76,10 @@ int main() 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 = diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp index e501b756af6a1..9be7f52bca99a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp @@ -29,7 +29,7 @@ namespace autoware::motion_velocity_planner::obstacle_velocity_limiter { multi_polygon_t createPolygonMasks( - const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const double buffer, + const std::vector & dynamic_obstacles, const double buffer, const double min_vel) { return createObjectPolygons(dynamic_obstacles, buffer, min_vel); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp index d7d2de63b33f8..b0cdb1f802e0f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp @@ -56,7 +56,7 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b /// @param[in] min_vel minimum velocity for an object to be masked /// @return polygon masks around dynamic objects multi_polygon_t createPolygonMasks( - const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const double buffer, + const std::vector & dynamic_obstacles, const double buffer, const double min_vel); /// @brief create footprint polygons from projection lines diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index 532cc834c19ac..eb89c027a3b48 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 @@ -48,7 +48,6 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string projection_params_ = obstacle_velocity_limiter::ProjectionParameters(node); obstacle_params_ = obstacle_velocity_limiter::ObstacleParameters(node); velocity_params_ = obstacle_velocity_limiter::VelocityParameters(node); - velocity_factor_interface_.init(autoware::motion_utils::PlanningBehavior::ROUTE_OBSTACLE); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); @@ -170,7 +169,7 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( const auto preprocessing_us = stopwatch.toc("preprocessing"); stopwatch.tic("obstacles"); obstacle_masks.negative_masks = obstacle_velocity_limiter::createPolygonMasks( - planner_data->predicted_objects, obstacle_params_.dynamic_obstacles_buffer, + planner_data->objects, obstacle_params_.dynamic_obstacles_buffer, obstacle_params_.dynamic_obstacles_min_vel); if (obstacle_params_.ignore_on_path) obstacle_masks.negative_masks.push_back(obstacle_velocity_limiter::createTrajectoryFootprint( @@ -189,8 +188,8 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( obstacle_masks.positive_mask = obstacle_velocity_limiter::createEnvelopePolygon(footprint_polygons); obstacle_velocity_limiter::addSensorObstacles( - obstacles, planner_data->occupancy_grid, planner_data->no_ground_pointcloud, obstacle_masks, - obstacle_params_); + obstacles, planner_data->occupancy_grid, planner_data->no_ground_pointcloud.pointcloud, + obstacle_masks, obstacle_params_); } const auto obstacles_us = stopwatch.toc("obstacles"); autoware::motion_utils::VirtualWalls virtual_walls; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp index 47215ee5cb0c4..187f028dc5969 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp @@ -25,6 +25,7 @@ #include #include +#include namespace autoware::motion_velocity_planner::obstacle_velocity_limiter { @@ -58,17 +59,18 @@ polygon_t createObjectPolygon( } multi_polygon_t createObjectPolygons( - const autoware_perception_msgs::msg::PredictedObjects & objects, const double buffer, - const double min_velocity) + const std::vector & objects, const double buffer, const double min_velocity) { multi_polygon_t polygons; - for (const auto & object : objects.objects) { + for (const auto & object : objects) { + const auto & predicted_object = object.predicted_object; const double obj_vel_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); if (min_velocity <= obj_vel_norm) { polygons.push_back(createObjectPolygon( - object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions, buffer)); + predicted_object.kinematics.initial_pose_with_covariance.pose, + predicted_object.shape.dimensions, buffer)); } } return polygons; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp index 7e4704ba6818c..a98d92994f497 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp @@ -18,6 +18,7 @@ #include "parameters.hpp" #include "types.hpp" +#include #include #include @@ -163,8 +164,7 @@ polygon_t createObjectPolygon( /// @param [in] min_velocity objects with velocity lower will be ignored /// @return polygons of the objects multi_polygon_t createObjectPolygons( - const autoware_perception_msgs::msg::PredictedObjects & objects, const double buffer, - const double min_velocity); + const std::vector & objects, const double buffer, const double min_velocity); /// @brief add obstacles obtained from sensors to the given Obstacles object /// @param[out] obstacles Obstacles object in which to add the sensor obstacles diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp index ac07c62f62cf7..023ae83774917 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp @@ -15,6 +15,7 @@ #include "../src/distance.hpp" #include "../src/obstacles.hpp" #include "../src/types.hpp" +#include "autoware/motion_velocity_planner_common/planner_data.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -27,6 +28,7 @@ #include #include +#include const auto point_in_polygon = [](const auto x, const auto y, const auto & polygon) { return std::find_if(polygon.outer().begin(), polygon.outer().end(), [=](const auto & pt) { @@ -387,11 +389,12 @@ TEST(TestCollisionDistance, arcDistance) TEST(TestCollisionDistance, createObjPolygons) { + using autoware::motion_velocity_planner::PlannerData; using autoware::motion_velocity_planner::obstacle_velocity_limiter::createObjectPolygons; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; - PredictedObjects objects; + std::vector objects; auto polygons = createObjectPolygons(objects, 0.0, 0.0); EXPECT_TRUE(polygons.empty()); @@ -404,7 +407,7 @@ TEST(TestCollisionDistance, createObjPolygons) object1.kinematics.initial_twist_with_covariance.twist.linear.x = 0.0; object1.shape.dimensions.x = 1.0; object1.shape.dimensions.y = 1.0; - objects.objects.push_back(object1); + objects.push_back(PlannerData::Object(object1)); polygons = createObjectPolygons(objects, 0.0, 1.0); EXPECT_TRUE(polygons.empty()); @@ -431,7 +434,7 @@ TEST(TestCollisionDistance, createObjPolygons) object2.kinematics.initial_twist_with_covariance.twist.linear.x = 2.0; object2.shape.dimensions.x = 2.0; object2.shape.dimensions.y = 1.0; - objects.objects.push_back(object2); + objects.push_back(PlannerData::Object(object2)); polygons = createObjectPolygons(objects, 0.0, 2.0); ASSERT_EQ(polygons.size(), 1ul); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index f9ba7f4af9877..a6c8368c20571 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -106,23 +106,26 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params) { autoware_perception_msgs::msg::PredictedObjects filtered_objects; - filtered_objects.header = planner_data.predicted_objects.header; - for (const auto & object : planner_data.predicted_objects.objects) { + filtered_objects.header = planner_data.predicted_objects_header; + for (const auto & object : planner_data.objects) { + const auto & predicted_object = object.predicted_object; const auto is_pedestrian = - std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { - return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; - }) != object.classification.end(); + std::find_if( + predicted_object.classification.begin(), predicted_object.classification.end(), + [](const auto & c) { + return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + }) != predicted_object.classification.end(); if (is_pedestrian) continue; const auto is_coming_from_behind = motion_utils::calcSignedArcLength( ego_data.trajectory_points, 0UL, - object.kinematics.initial_pose_with_covariance.pose.position) < 0.0; + predicted_object.kinematics.initial_pose_with_covariance.pose.position) < 0.0; if (params.objects_ignore_behind_ego && is_coming_from_behind) { continue; } - auto filtered_object = object; + auto filtered_object = predicted_object; const auto is_invalid_predicted_path = [&](const auto & predicted_path) { const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); @@ -130,10 +133,10 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const auto lat_offset_to_current_ego = std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); const auto is_crossing_ego = - lat_offset_to_current_ego <= - object.shape.dimensions.y / 2.0 + std::max( - params.left_offset + params.extra_left_offset, - params.right_offset + params.extra_right_offset); + lat_offset_to_current_ego <= predicted_object.shape.dimensions.y / 2.0 + + std::max( + params.left_offset + params.extra_left_offset, + params.right_offset + params.extra_right_offset); return is_low_confidence || is_crossing_ego; }; auto & predicted_paths = filtered_object.kinematics.predicted_paths; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index bd85546cb6c56..532cc1e8c7d51 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 @@ -58,7 +58,9 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) logger_ = node.get_logger(); clock_ = node.get_clock(); init_parameters(node); - velocity_factor_interface_.init(motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + + planning_factor_interface_ = + std::make_unique(&node, "out_of_lane"); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); @@ -310,15 +312,9 @@ VelocityPlanningResult OutOfLaneModule::plan( slowdown_pose->position, slowdown_pose->position, slowdown_velocity); } - const auto is_approaching = - motion_utils::calcSignedArcLength( - ego_trajectory_points, ego_data.pose.position, slowdown_pose->position) > 0.1 && - planner_data->current_odometry.twist.twist.linear.x > 0.1; - const auto status = is_approaching ? motion_utils::VelocityFactor::APPROACHING - : motion_utils::VelocityFactor::STOPPED; - velocity_factor_interface_.set( - ego_trajectory_points, ego_data.pose, *slowdown_pose, status, "out_of_lane"); - result.velocity_factor = velocity_factor_interface_.get(); + planning_factor_interface_->add( + ego_trajectory_points, ego_data.pose, *slowdown_pose, PlanningFactor::SLOW_DOWN, + SafetyFactorArray{}); virtual_wall_marker_creator.add_virtual_walls( out_of_lane::debug::create_virtual_walls(*slowdown_pose, slowdown_velocity == 0.0, params_)); virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now())); 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 72f20f1c63d96..0e73994dd73a7 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 @@ -38,6 +38,7 @@ class OutOfLaneModule : public PluginModuleInterface { public: void init(rclcpp::Node & node, const std::string & module_name) override; + void publish_planning_factor() override { planning_factor_interface_->publish(); }; void update_parameters(const std::vector & parameters) override; VelocityPlanningResult plan( const std::vector & ego_trajectory_points, 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 9ef3bf5c7f756..07c8e1580e4f0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp @@ -39,6 +39,7 @@ #include #include #include +#include namespace autoware::motion_velocity_planner { @@ -54,11 +55,72 @@ struct PlannerData { } + struct Object + { + public: + Object() = default; + explicit Object(const autoware_perception_msgs::msg::PredictedObject & arg_predicted_object) + : predicted_object(arg_predicted_object) + { + } + + autoware_perception_msgs::msg::PredictedObject predicted_object; + // double get_lon_vel_relative_to_traj() + // { + // if (!lon_vel_relative_to_traj) { + // lon_vel_relative_to_traj = 0.0; + // } + // return *lon_vel_relative_to_traj; + // } + // double get_lat_vel_relative_to_traj() + // { + // if (!lat_vel_relative_to_traj) { + // lat_vel_relative_to_traj = 0.0; + // } + // return *lat_vel_relative_to_traj; + // } + + private: + // TODO(murooka) implement the following variables and their functions. + // std::optional dist_to_traj_poly{std::nullopt}; + // std::optional dist_to_traj_lateral{std::nullopt}; + // std::optional dist_from_ego_longitudinal{std::nullopt}; + // std::optional lon_vel_relative_to_traj{std::nullopt}; + // std::optional lat_vel_relative_to_traj{std::nullopt}; + }; + + struct Pointcloud + { + public: + Pointcloud() = default; + explicit Pointcloud(const pcl::PointCloud & arg_pointcloud) + : pointcloud(arg_pointcloud) + { + } + + pcl::PointCloud pointcloud; + + private: + // NOTE: clustered result will be added. + }; + + void process_predicted_objects( + const autoware_perception_msgs::msg::PredictedObjects & predicted_objects) + { + predicted_objects_header = predicted_objects.header; + + objects.clear(); + for (const auto & predicted_object : predicted_objects.objects) { + objects.push_back(Object(predicted_object)); + } + } + // msgs from callbacks that are used for data-ready nav_msgs::msg::Odometry current_odometry; geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration; - autoware_perception_msgs::msg::PredictedObjects predicted_objects; - pcl::PointCloud no_ground_pointcloud; + std_msgs::msg::Header predicted_objects_header; + std::vector objects; + Pointcloud no_ground_pointcloud; nav_msgs::msg::OccupancyGrid occupancy_grid; std::shared_ptr route_handler; 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 53860c390b4db..6d683acff667f 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 @@ -18,7 +18,7 @@ #include "planner_data.hpp" #include "velocity_planning_result.hpp" -#include +#include #include #include @@ -32,6 +32,9 @@ namespace autoware::motion_velocity_planner { +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::SafetyFactorArray; + class PluginModuleInterface { public: @@ -42,7 +45,7 @@ class PluginModuleInterface const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) = 0; virtual std::string get_module_name() const = 0; - autoware::motion_utils::VelocityFactorInterface velocity_factor_interface_; + virtual void publish_planning_factor() {} rclcpp::Logger logger_ = rclcpp::get_logger(""); rclcpp::Publisher::SharedPtr debug_publisher_; rclcpp::Publisher::SharedPtr virtual_wall_publisher_; @@ -50,6 +53,9 @@ class PluginModuleInterface rclcpp::Publisher::SharedPtr processing_time_publisher_; autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{}; + +protected: + std::unique_ptr planning_factor_interface_; }; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp index 1288884a37979..7b41bf0ee9e3e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp @@ -17,7 +17,6 @@ #include -#include #include #include @@ -43,7 +42,6 @@ struct VelocityPlanningResult { std::vector stop_points{}; std::vector slowdown_intervals{}; - std::optional velocity_factor{}; }; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md index 01062f81e77a2..c65593497b8d5 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md @@ -30,10 +30,10 @@ This means that to stop before a wall, a stop point is inserted in the trajector ## Output topics -| Name | Type | Description | -| --------------------------- | ------------------------------------------------- | -------------------------------------------------- | -| `~/output/trajectory` | autoware_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile | -| `~/output/velocity_factors` | autoware_adapi_v1_msgs::msg::VelocityFactorsArray | factors causing change in the ego velocity profile | +| Name | Type | Description | +| ----------------------------------------- | ---------------------------------------------- | -------------------------------------------------- | +| `~/output/trajectory` | autoware_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile | +| `~/output/planning_factors/` | tier4_planning_msgs::msg::PlanningFactorsArray | factors causing change in the ego velocity profile | ## Services 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 cefc84836beda..8f5aa761573b7 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 @@ -82,9 +82,6 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & // Publishers trajectory_pub_ = this->create_publisher("~/output/trajectory", 1); - velocity_factor_publisher_ = - this->create_publisher( - "~/output/velocity_factors", 1); processing_time_publisher_ = this->create_publisher( "~/debug/processing_time_ms", 1); @@ -161,13 +158,14 @@ bool MotionVelocityPlannerNode::update_planner_data( const auto predicted_objects_ptr = sub_predicted_objects_.takeData(); if (check_with_log(predicted_objects_ptr, "Waiting for predicted objects")) - planner_data_.predicted_objects = *predicted_objects_ptr; + planner_data_.process_predicted_objects(*predicted_objects_ptr); processing_times["update_planner_data.pred_obj"] = sw.toc(true); const auto no_ground_pointcloud_ptr = sub_no_ground_pointcloud_.takeData(); if (check_with_log(no_ground_pointcloud_ptr, "Waiting for pointcloud")) { const auto no_ground_pointcloud = process_no_ground_pointcloud(no_ground_pointcloud_ptr); - if (no_ground_pointcloud) planner_data_.no_ground_pointcloud = *no_ground_pointcloud; + if (no_ground_pointcloud) + planner_data_.no_ground_pointcloud = PlannerData::Pointcloud(*no_ground_pointcloud); } processing_times["update_planner_data.pcd"] = sw.toc(true); @@ -421,20 +419,13 @@ autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_traj resampled_trajectory, std::make_shared(planner_data_)); processing_times["plan_velocities"] = stop_watch.toc("plan_velocities"); - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factors; - velocity_factors.header.frame_id = "map"; - velocity_factors.header.stamp = get_clock()->now(); - for (const auto & planning_result : planning_results) { for (const auto & stop_point : planning_result.stop_points) insert_stop(output_trajectory_msg, stop_point); for (const auto & slowdown_interval : planning_result.slowdown_intervals) insert_slowdown(output_trajectory_msg, slowdown_interval); - if (planning_result.velocity_factor) - velocity_factors.factors.push_back(*planning_result.velocity_factor); } - velocity_factor_publisher_->publish(velocity_factors); return output_trajectory_msg; } 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 18d41f5d7fe5d..4235971e76fca 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 @@ -93,8 +93,6 @@ class MotionVelocityPlannerNode : public rclcpp::Node // publishers rclcpp::Publisher::SharedPtr trajectory_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Publisher::SharedPtr - velocity_factor_publisher_; autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{ this, "~/debug/processing_time_ms_diag"}; rclcpp::Publisher::SharedPtr diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp index b08798cbef772..5bdf386479283 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp @@ -102,6 +102,8 @@ std::vector MotionVelocityPlannerManager::plan_velocitie VelocityPlanningResult res = plugin->plan(ego_trajectory_points, planner_data); results.push_back(res); + plugin->publish_planning_factor(); + if (res.stop_points.size() > 0) { const auto stop_decision_metric = make_decision_metric(plugin->get_module_name(), "stop"); metrics_.push_back(stop_decision_metric); diff --git a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp index cab4acfbb8556..54d4bc2e90714 100644 --- a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -197,6 +197,15 @@ void calculateCartesian( trajectory.longitudinal_accelerations.push_back(0.0); trajectory.lateral_accelerations.push_back(0.0); } + for (auto i = 0UL; i < trajectory.points.size(); ++i) { + geometry_msgs::msg::Pose pose; + pose.position.x = trajectory.points[i].x(); + pose.position.y = trajectory.points[i].y(); + pose.position.z = 0.0; + pose.orientation = + autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, trajectory.yaws[i]); + trajectory.poses.push_back(pose); + } } } diff --git a/sensing/autoware_image_diagnostics/README.md b/sensing/autoware_image_diagnostics/README.md index 03858088564b3..42fcbdc8dc612 100644 --- a/sensing/autoware_image_diagnostics/README.md +++ b/sensing/autoware_image_diagnostics/README.md @@ -28,13 +28,13 @@ After all image's blocks state are evaluated, the whole image status is summariz ### Output -| Name | Type | Description | -| ----------------------------------- | --------------------------------------- | ------------------------------------- | -| `image_diag/debug/gray_image` | `sensor_msgs::msg::Image` | gray image | -| `image_diag/debug/dft_image` | `sensor_msgs::msg::Image` | discrete Fourier transformation image | -| `image_diag/debug/diag_block_image` | `sensor_msgs::msg::Image` | each block state colorization | -| `image_diag/image_state_diag` | `tier4_debug_msgs::msg::Int32Stamped` | image diagnostics status value | -| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | +| Name | Type | Description | +| ----------------------------------- | ------------------------------------------------- | ------------------------------------- | +| `image_diag/debug/gray_image` | `sensor_msgs::msg::Image` | gray image | +| `image_diag/debug/dft_image` | `sensor_msgs::msg::Image` | discrete Fourier transformation image | +| `image_diag/debug/diag_block_image` | `sensor_msgs::msg::Image` | each block state colorization | +| `image_diag/image_state_diag` | `autoware_internal_debug_msgs::msg::Int32Stamped` | image diagnostics status value | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | ## Parameters diff --git a/sensing/autoware_image_diagnostics/package.xml b/sensing/autoware_image_diagnostics/package.xml index 2e4556de2b92b..3140b87e1f1b9 100644 --- a/sensing/autoware_image_diagnostics/package.xml +++ b/sensing/autoware_image_diagnostics/package.xml @@ -12,6 +12,7 @@ ament_cmake_auto + autoware_internal_debug_msgs autoware_universe_utils cv_bridge diagnostic_updater @@ -19,7 +20,6 @@ rclcpp rclcpp_components sensor_msgs - tier4_debug_msgs ament_lint_auto autoware_lint_common diff --git a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp index 1625cba5b72aa..57e53aee46328 100644 --- a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp +++ b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp @@ -33,7 +33,7 @@ ImageDiagNode::ImageDiagNode(const rclcpp::NodeOptions & node_options) dft_image_pub_ = image_transport::create_publisher(this, "image_diag/debug/dft_image"); gray_image_pub_ = image_transport::create_publisher(this, "image_diag/debug/gray_image"); - image_state_pub_ = create_publisher( + image_state_pub_ = create_publisher( "image_diag/image_state_diag", rclcpp::SensorDataQoS()); updater_.setHardwareID("Image_Diagnostics"); @@ -225,7 +225,7 @@ void ImageDiagNode::ImageChecker(const sensor_msgs::msg::Image::ConstSharedPtr i } else { params_.diagnostic_status = 0; } - tier4_debug_msgs::msg::Int32Stamped image_state_out; + autoware_internal_debug_msgs::msg::Int32Stamped image_state_out; image_state_out.data = params_.diagnostic_status; image_state_pub_->publish(image_state_out); diff --git a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.hpp b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.hpp index 325624062b90b..9383da33160f9 100644 --- a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.hpp +++ b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.hpp @@ -21,10 +21,10 @@ #include #include +#include +#include +#include #include -#include -#include -#include #if __has_include() #include @@ -95,8 +95,9 @@ class ImageDiagNode : public rclcpp::Node image_transport::Publisher block_diag_image_pub_; image_transport::Publisher dft_image_pub_; image_transport::Publisher gray_image_pub_; - rclcpp::Publisher::SharedPtr average_pub_; - rclcpp::Publisher::SharedPtr image_state_pub_; + rclcpp::Publisher::SharedPtr + average_pub_; + rclcpp::Publisher::SharedPtr image_state_pub_; }; } // namespace autoware::image_diagnostics diff --git a/sensing/autoware_pointcloud_preprocessor/docs/blockage-diag.md b/sensing/autoware_pointcloud_preprocessor/docs/blockage-diag.md index 911cda3823021..a9dcb8437ec67 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/blockage-diag.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/blockage-diag.md @@ -38,16 +38,16 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Output -| Name | Type | Description | -| :-------------------------------------------------------- | :-------------------------------------- | ------------------------------------------------------------------------------------------------ | -| `~/output/blockage_diag/debug/blockage_mask_image` | `sensor_msgs::msg::Image` | The mask image of detected blockage | -| `~/output/blockage_diag/debug/ground_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in ground region | -| `~/output/blockage_diag/debug/sky_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in sky region | -| `~/output/blockage_diag/debug/lidar_depth_map` | `sensor_msgs::msg::Image` | The depth map image of input point cloud | -| `~/output/blockage_diag/debug/single_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of detected dusty area in latest single frame | -| `~/output/blockage_diag/debug/multi_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of continuous detected dusty area | -| `~/output/blockage_diag/debug/blockage_dust_merged_image` | `sensor_msgs::msg::Image` | The merged image of blockage detection(red) and multi frame dusty area detection(yellow) results | -| `~/output/blockage_diag/debug/ground_dust_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The ratio of dusty area divided by area where ray usually returns from the ground. | +| Name | Type | Description | +| :-------------------------------------------------------- | :-------------------------------------------------- | ------------------------------------------------------------------------------------------------ | +| `~/output/blockage_diag/debug/blockage_mask_image` | `sensor_msgs::msg::Image` | The mask image of detected blockage | +| `~/output/blockage_diag/debug/ground_blockage_ratio` | `autoware_internal_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in ground region | +| `~/output/blockage_diag/debug/sky_blockage_ratio` | `autoware_internal_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in sky region | +| `~/output/blockage_diag/debug/lidar_depth_map` | `sensor_msgs::msg::Image` | The depth map image of input point cloud | +| `~/output/blockage_diag/debug/single_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of detected dusty area in latest single frame | +| `~/output/blockage_diag/debug/multi_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of continuous detected dusty area | +| `~/output/blockage_diag/debug/blockage_dust_merged_image` | `sensor_msgs::msg::Image` | The merged image of blockage detection(red) and multi frame dusty area detection(yellow) results | +| `~/output/blockage_diag/debug/ground_dust_ratio` | `autoware_internal_debug_msgs::msg::Float32Stamped` | The ratio of dusty area divided by area where ray usually returns from the ground. | ## Parameters diff --git a/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md index 8f4da273861a3..0d0423d40ab83 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md @@ -36,11 +36,11 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Output -| Name | Type | Description | -| ---------------------------------------------- | --------------------------------------- | ------------------------------------------------------- | -| `/dual_return_outlier_filter/frequency_image` | `sensor_msgs::msg::Image` | The histogram image that represent visibility | -| `/dual_return_outlier_filter/visibility` | `tier4_debug_msgs::msg::Float32Stamped` | A representation of visibility with a value from 0 to 1 | -| `/dual_return_outlier_filter/pointcloud_noise` | `sensor_msgs::msg::Pointcloud2` | The pointcloud removed as noise | +| Name | Type | Description | +| ---------------------------------------------- | --------------------------------------------------- | ------------------------------------------------------- | +| `/dual_return_outlier_filter/frequency_image` | `sensor_msgs::msg::Image` | The histogram image that represent visibility | +| `/dual_return_outlier_filter/visibility` | `autoware_internal_debug_msgs::msg::Float32Stamped` | A representation of visibility with a value from 0 to 1 | +| `/dual_return_outlier_filter/pointcloud_noise` | `sensor_msgs::msg::Pointcloud2` | The pointcloud removed as noise | ## Parameters diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp index 3601b492c4fe3..2dbda770f6cec 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp @@ -22,10 +22,10 @@ #include #include +#include #include #include #include -#include #if __has_include() #include @@ -58,10 +58,13 @@ class BlockageDiagComponent : public autoware::pointcloud_preprocessor::Filter image_transport::Publisher single_frame_dust_mask_pub; image_transport::Publisher multi_frame_dust_mask_pub; image_transport::Publisher blockage_dust_merged_pub; - rclcpp::Publisher::SharedPtr ground_blockage_ratio_pub_; - rclcpp::Publisher::SharedPtr sky_blockage_ratio_pub_; - rclcpp::Publisher::SharedPtr ground_dust_ratio_pub_; - rclcpp::Publisher::SharedPtr blockage_type_pub_; + rclcpp::Publisher::SharedPtr + ground_blockage_ratio_pub_; + rclcpp::Publisher::SharedPtr + sky_blockage_ratio_pub_; + rclcpp::Publisher::SharedPtr + ground_dust_ratio_pub_; + rclcpp::Publisher::SharedPtr blockage_type_pub_; private: void onBlockageChecker(DiagnosticStatusWrapper & stat); diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index 40adad3521f8d..6164e85c35717 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp @@ -69,14 +69,14 @@ #include #include +#include +#include #include #include #include #include #include #include -#include -#include #include #include diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp index 0e959eedae1b6..d57e44431a8d2 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp @@ -68,13 +68,13 @@ #include #include +#include +#include #include #include #include #include #include -#include -#include #include #include diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp index ef33e88ef5c98..fa55222fb6530 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp @@ -22,9 +22,9 @@ #include #include +#include #include #include -#include #if __has_include() #include @@ -54,7 +54,7 @@ class DualReturnOutlierFilterComponent : public autoware::pointcloud_preprocesso /** \brief Parameter service callback */ rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); image_transport::Publisher image_pub_; - rclcpp::Publisher::SharedPtr visibility_pub_; + rclcpp::Publisher::SharedPtr visibility_pub_; rclcpp::Publisher::SharedPtr noise_cloud_pub_; private: diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp index c898cbacf33ea..96bd2fb2411a0 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp @@ -53,7 +53,7 @@ class RingOutlierFilterComponent : public autoware::pointcloud_preprocessor::Fil const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, const TransformInfo & transform_info) override; - rclcpp::Publisher::SharedPtr visibility_pub_; + rclcpp::Publisher::SharedPtr visibility_pub_; private: /** \brief publisher of excluded pointcloud for debug reason. **/ diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp index d36fcb36a7be1..f4921be66a7e2 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp @@ -68,14 +68,14 @@ #include #include +#include +#include #include #include #include #include #include #include -#include -#include #include #include diff --git a/sensing/autoware_pointcloud_preprocessor/package.xml b/sensing/autoware_pointcloud_preprocessor/package.xml index 4511c5497a3a0..525b1f3eb0699 100644 --- a/sensing/autoware_pointcloud_preprocessor/package.xml +++ b/sensing/autoware_pointcloud_preprocessor/package.xml @@ -26,6 +26,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_lanelet2_extension autoware_pcl_extensions autoware_point_types @@ -51,7 +52,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_debug_msgs ament_lint_auto autoware_lint_common diff --git a/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp index 5021a3551c939..e05d5f789f680 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp @@ -68,7 +68,7 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options std::string(this->get_namespace()) + ": dust_validation", this, &BlockageDiagComponent::dustChecker); - ground_dust_ratio_pub_ = create_publisher( + ground_dust_ratio_pub_ = create_publisher( "blockage_diag/debug/ground_dust_ratio", rclcpp::SensorDataQoS()); if (publish_debug_image_) { single_frame_dust_mask_pub = @@ -86,9 +86,9 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options blockage_mask_pub_ = image_transport::create_publisher(this, "blockage_diag/debug/blockage_mask_image"); } - ground_blockage_ratio_pub_ = create_publisher( + ground_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/ground_blockage_ratio", rclcpp::SensorDataQoS()); - sky_blockage_ratio_pub_ = create_publisher( + sky_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/sky_blockage_ratio", rclcpp::SensorDataQoS()); using std::placeholders::_1; set_param_res_ = this->add_on_set_parameters_callback( @@ -315,7 +315,7 @@ void BlockageDiagComponent::filter( cv::Mat ground_mask(cv::Size(ideal_horizontal_bins, horizontal_ring_id_), CV_8UC1); cv::vconcat(sky_blank, single_dust_ground_img, single_dust_img); - tier4_debug_msgs::msg::Float32Stamped ground_dust_ratio_msg; + autoware_internal_debug_msgs::msg::Float32Stamped ground_dust_ratio_msg; ground_dust_ratio_ = static_cast(cv::countNonZero(single_dust_ground_img)) / (single_dust_ground_img.cols * single_dust_ground_img.rows); ground_dust_ratio_msg.data = ground_dust_ratio_; @@ -386,12 +386,12 @@ void BlockageDiagComponent::filter( } } - tier4_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg; + autoware_internal_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg; ground_blockage_ratio_msg.data = ground_blockage_ratio_; ground_blockage_ratio_msg.stamp = now(); ground_blockage_ratio_pub_->publish(ground_blockage_ratio_msg); - tier4_debug_msgs::msg::Float32Stamped sky_blockage_ratio_msg; + autoware_internal_debug_msgs::msg::Float32Stamped sky_blockage_ratio_msg; sky_blockage_ratio_msg.data = sky_blockage_ratio_; sky_blockage_ratio_msg.stamp = now(); sky_blockage_ratio_pub_->publish(sky_blockage_ratio_msg); diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index f8baae3405873..b49e21b8f30be 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -455,9 +455,9 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } for (const auto & e : cloud_stdmap_) { @@ -468,7 +468,7 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() std::chrono::nanoseconds( (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp index ba29389b88bf7..ba5e2eb51997a 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp @@ -271,7 +271,7 @@ void PointCloudConcatenationComponent::publish() std::chrono::nanoseconds( (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); } } @@ -297,9 +297,9 @@ void PointCloudConcatenationComponent::publish() if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp index ecbc5b8fd13ef..54c940d414db8 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp @@ -194,9 +194,9 @@ void CropBoxFilterComponent::faster_filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); auto pipeline_latency_ms = @@ -204,7 +204,7 @@ void CropBoxFilterComponent::faster_filter( std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp index 9eaafeae9dbc7..cc05a6cc2765c 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp @@ -123,7 +123,7 @@ void DistortionCorrectorComponent::pointcloud_callback(PointCloud2::UniquePtr po std::chrono::nanoseconds( (this->get_clock()->now() - pointcloud_msg->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } @@ -133,9 +133,9 @@ void DistortionCorrectorComponent::pointcloud_callback(PointCloud2::UniquePtr po if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp index 3d2bc0a206c94..0c467234d6591 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp @@ -142,9 +142,9 @@ void PickupBasedVoxelGridDownsampleFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); auto pipeline_latency_ms = @@ -152,7 +152,7 @@ void PickupBasedVoxelGridDownsampleFilterComponent::filter( std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp index a8024e02de2c1..bac3a59e09528 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp @@ -65,7 +65,7 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent( image_pub_ = image_transport::create_publisher(this, "dual_return_outlier_filter/debug/frequency_image"); - visibility_pub_ = create_publisher( + visibility_pub_ = create_publisher( "dual_return_outlier_filter/debug/visibility", rclcpp::SensorDataQoS()); { rclcpp::PublisherOptions pub_options; @@ -322,7 +322,7 @@ void DualReturnOutlierFilterComponent::filter( frequency_image_msg->header = input->header; // Publish histogram image image_pub_.publish(frequency_image_msg); - tier4_debug_msgs::msg::Float32Stamped visibility_msg; + autoware_internal_debug_msgs::msg::Float32Stamped visibility_msg; visibility_msg.data = (1.0f - filled); visibility_msg.stamp = now(); visibility_pub_->publish(visibility_msg); diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp index fae9043143ba4..fcea299ba5aff 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp @@ -41,7 +41,7 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions outlier_pointcloud_publisher_ = this->create_publisher("debug/ring_outlier_filter", 1, pub_options); } - visibility_pub_ = create_publisher( + visibility_pub_ = create_publisher( "ring_outlier_filter/debug/visibility", rclcpp::SensorDataQoS()); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); @@ -262,7 +262,7 @@ void RingOutlierFilterComponent::faster_filter( outlier.header = input->header; outlier_pointcloud_publisher_->publish(outlier); - tier4_debug_msgs::msg::Float32Stamped visibility_msg; + autoware_internal_debug_msgs::msg::Float32Stamped visibility_msg; visibility_msg.data = calculateVisibilityScore(outlier); visibility_msg.stamp = input->header.stamp; visibility_pub_->publish(visibility_msg); @@ -272,9 +272,9 @@ void RingOutlierFilterComponent::faster_filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); auto pipeline_latency_ms = @@ -282,7 +282,7 @@ void RingOutlierFilterComponent::faster_filter( std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp index 42170fd88ee36..e5bd4a955590f 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp @@ -378,7 +378,7 @@ void PointCloudDataSynchronizerComponent::publish() std::chrono::nanoseconds( (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); } auto output = std::make_unique(*e.second); @@ -400,9 +400,9 @@ void PointCloudDataSynchronizerComponent::publish() if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py b/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py index a4f8dee7af1a0..9b978b66feefb 100644 --- a/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py +++ b/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py @@ -147,7 +147,7 @@ def __init__(self): elif sensor["type"] == "sensor.lidar.ray_cast": if sensor["id"] in self.sensor_frequencies: self.pub_lidar[sensor["id"]] = self.ros2_node.create_publisher( - PointCloud2, f'/sensing/lidar/{sensor["id"]}/pointcloud', 10 + PointCloud2, f'/sensing/lidar/{sensor["id"]}/pointcloud_before_sync', 10 ) else: self.ros2_node.get_logger().info( diff --git a/simulator/learning_based_vehicle_model/src/interconnected_model.cpp b/simulator/learning_based_vehicle_model/src/interconnected_model.cpp index d2fef15aa4e88..e495aceb8da9c 100644 --- a/simulator/learning_based_vehicle_model/src/interconnected_model.cpp +++ b/simulator/learning_based_vehicle_model/src/interconnected_model.cpp @@ -79,8 +79,7 @@ void InterconnectedModel::addSubmodel( std::tuple submodel_desc) { const auto [lib_path, param_path, class_name] = submodel_desc; - auto new_model = new SimplePyModel(lib_path, param_path, class_name); - submodels.push_back(std::unique_ptr(new_model)); + submodels.emplace_back(std::make_unique(lib_path, param_path, class_name)); } void InterconnectedModel::initState(std::vector new_state) diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index fdc782182f9a5..5bd7a2f74eacc 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -10,6 +10,7 @@ Mamoru Sobue Zulfaqar Azmi Temkei Kem + Kotaro Yoshimoto Apache License 2.0 ament_cmake_auto diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index d61654c32af02..8c95e144b3f8d 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -585,9 +585,7 @@ void SimplePlanningSimulator::set_input(const InputCommand & cmd, const double a std::visit( [this, acc_by_slope](auto && arg) { using T = std::decay_t; - if constexpr (std::is_same_v) { - set_input(arg, acc_by_slope); - } else if constexpr (std::is_same_v) { + if constexpr (std::is_same_v || std::is_same_v) { set_input(arg, acc_by_slope); } else { throw std::invalid_argument("Invalid input command type"); @@ -640,7 +638,7 @@ void SimplePlanningSimulator::set_input(const Control & cmd, const double acc_by input << vel, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC || - vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { // NOLINT input << combined_acc, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || diff --git a/system/autoware_default_adapi/package.xml b/system/autoware_default_adapi/package.xml index a24d0c8a30b62..e0d3fc0f9e21b 100644 --- a/system/autoware_default_adapi/package.xml +++ b/system/autoware_default_adapi/package.xml @@ -32,6 +32,7 @@ std_srvs tier4_api_msgs tier4_control_msgs + tier4_planning_msgs tier4_system_msgs python3-flask diff --git a/system/autoware_default_adapi/src/planning.cpp b/system/autoware_default_adapi/src/planning.cpp index d89ef6c221666..98f1c846d202e 100644 --- a/system/autoware_default_adapi/src/planning.cpp +++ b/system/autoware_default_adapi/src/planning.cpp @@ -18,6 +18,7 @@ #include +#include #include #include #include @@ -25,6 +26,35 @@ namespace autoware::default_adapi { +const std::map direction_map = { + {PlanningFactor::SHIFT_RIGHT, SteeringFactor::RIGHT}, + {PlanningFactor::SHIFT_LEFT, SteeringFactor::LEFT}, + {PlanningFactor::TURN_RIGHT, SteeringFactor::RIGHT}, + {PlanningFactor::TURN_LEFT, SteeringFactor::LEFT}}; + +const std::map conversion_map = { + {"behavior_path_planner", PlanningBehavior::INTERSECTION}, + {"static_obstacle_avoidance", PlanningBehavior::AVOIDANCE}, + {"crosswalk", PlanningBehavior::CROSSWALK}, + {"goal_planner", PlanningBehavior::GOAL_PLANNER}, + {"intersection", PlanningBehavior::INTERSECTION}, + {"lane_change_left", PlanningBehavior::LANE_CHANGE}, + {"lane_change_right", PlanningBehavior::LANE_CHANGE}, + {"merge_from_private", PlanningBehavior::MERGE}, + {"no_stopping_area", PlanningBehavior::NO_STOPPING_AREA}, + {"blind_spot", PlanningBehavior::REAR_CHECK}, + {"obstacle_cruise_planner", PlanningBehavior::ROUTE_OBSTACLE}, + {"obstacle_stop_planner", PlanningBehavior::ROUTE_OBSTACLE}, + {"motion_velocity_planner", PlanningBehavior::ROUTE_OBSTACLE}, + {"walkway", PlanningBehavior::SIDEWALK}, + {"start_planner", PlanningBehavior::START_PLANNER}, + {"stop_line", PlanningBehavior::STOP_SIGN}, + {"surround_obstacle_checker", PlanningBehavior::SURROUNDING_OBSTACLE}, + {"traffic_light", PlanningBehavior::TRAFFIC_SIGNAL}, + {"detection_area", PlanningBehavior::USER_DEFINED_DETECTION_AREA}, + {"virtual_traffic_light", PlanningBehavior::VIRTUAL_TRAFFIC_LIGHT}, + {"run_out", PlanningBehavior::RUN_OUT}}; + template void concat(std::vector & v1, const std::vector & v2) { @@ -49,16 +79,122 @@ std::vector::SharedPtr> init_factors( } template -T merge_factors(const rclcpp::Time stamp, const std::vector & factors) +std::vector convert([[maybe_unused]] const std::vector & factors) +{ + static_assert(sizeof(T) == 0, "Only specializations of convert can be used."); + throw std::logic_error("Only specializations of convert can be used."); +} + +template <> +std::vector convert(const std::vector & factors) +{ + std::vector velocity_factors; + + for (const auto & factor : factors) { + if (factor.behavior != PlanningFactor::SLOW_DOWN && factor.behavior != PlanningFactor::STOP) { + continue; + } + + if (factor.control_points.empty()) { + continue; + } + + if (conversion_map.count(factor.module) == 0) { + continue; + } + + VelocityFactor velocity_factor; + velocity_factor.behavior = conversion_map.at(factor.module); + velocity_factor.pose = factor.control_points.front().pose; + velocity_factor.distance = factor.control_points.front().distance; + + velocity_factors.push_back(velocity_factor); + } + + return velocity_factors; +} + +template <> +std::vector convert(const std::vector & factors) { - T message; + std::vector steering_factors; + + for (const auto & factor : factors) { + if ( + factor.behavior != PlanningFactor::SHIFT_RIGHT && + factor.behavior != PlanningFactor::SHIFT_LEFT && + factor.behavior != PlanningFactor::TURN_RIGHT && + factor.behavior != PlanningFactor::TURN_LEFT) { + continue; + } + + if (factor.control_points.size() < 2) { + continue; + } + + if (conversion_map.count(factor.module) == 0) { + continue; + } + + if (direction_map.count(factor.behavior) == 0) { + continue; + } + + SteeringFactor steering_factor; + steering_factor.behavior = conversion_map.at(factor.module); + steering_factor.direction = direction_map.at(factor.behavior); + steering_factor.pose = std::array{ + factor.control_points.front().pose, factor.control_points.back().pose}; + steering_factor.distance = std::array{ + factor.control_points.front().distance, factor.control_points.back().distance}; + + steering_factors.push_back(steering_factor); + } + + return steering_factors; +} + +template +T merge_factors( + [[maybe_unused]] const rclcpp::Time stamp, + [[maybe_unused]] const std::vector & factors) +{ + static_assert(sizeof(T) == 0, "Only specializations of merge_factors can be used."); + throw std::logic_error("Only specializations of merge_factors can be used."); +} + +template <> +VelocityFactorArray merge_factors( + const rclcpp::Time stamp, const std::vector & factors) +{ + VelocityFactorArray message; message.header.stamp = stamp; message.header.frame_id = "map"; for (const auto & factor : factors) { - if (factor) { - concat(message.factors, factor->factors); + if (!factor) { + continue; } + + concat(message.factors, convert(factor->factors)); + } + return message; +} + +template <> +SteeringFactorArray merge_factors( + const rclcpp::Time stamp, const std::vector & factors) +{ + SteeringFactorArray message; + message.header.stamp = stamp; + message.header.frame_id = "map"; + + for (const auto & factor : factors) { + if (!factor) { + continue; + } + + concat(message.factors, convert(factor->factors)); } return message; } @@ -70,46 +206,34 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning stop_duration_ = declare_parameter("stop_duration", 1.0); stop_checker_ = std::make_unique(this, stop_duration_ + 1.0); - std::vector velocity_factor_topics = { - "/planning/velocity_factors/blind_spot", - "/planning/velocity_factors/crosswalk", - "/planning/velocity_factors/detection_area", - "/planning/velocity_factors/dynamic_obstacle_stop", - "/planning/velocity_factors/intersection", - "/planning/velocity_factors/merge_from_private", - "/planning/velocity_factors/no_stopping_area", - "/planning/velocity_factors/obstacle_stop", - "/planning/velocity_factors/obstacle_cruise", - "/planning/velocity_factors/occlusion_spot", - "/planning/velocity_factors/run_out", - "/planning/velocity_factors/stop_line", - "/planning/velocity_factors/surround_obstacle", - "/planning/velocity_factors/traffic_light", - "/planning/velocity_factors/virtual_traffic_light", - "/planning/velocity_factors/walkway", - "/planning/velocity_factors/motion_velocity_planner", - "/planning/velocity_factors/static_obstacle_avoidance", - "/planning/velocity_factors/dynamic_obstacle_avoidance", - "/planning/velocity_factors/avoidance_by_lane_change", - "/planning/velocity_factors/lane_change_left", - "/planning/velocity_factors/lane_change_right", - "/planning/velocity_factors/start_planner", - "/planning/velocity_factors/goal_planner"}; - - std::vector steering_factor_topics = { - "/planning/steering_factor/static_obstacle_avoidance", - "/planning/steering_factor/dynamic_obstacle_avoidance", - "/planning/steering_factor/avoidance_by_lane_change", - "/planning/steering_factor/intersection", - "/planning/steering_factor/lane_change_left", - "/planning/steering_factor/lane_change_right", - "/planning/steering_factor/start_planner", - "/planning/steering_factor/goal_planner"}; - - sub_velocity_factors_ = - init_factors(this, velocity_factors_, velocity_factor_topics); - sub_steering_factors_ = - init_factors(this, steering_factors_, steering_factor_topics); + std::vector factor_topics = { + "/planning/planning_factors/behavior_path_planner", + "/planning/planning_factors/blind_spot", + "/planning/planning_factors/crosswalk", + "/planning/planning_factors/detection_area", + "/planning/planning_factors/dynamic_obstacle_stop", + "/planning/planning_factors/intersection", + "/planning/planning_factors/merge_from_private", + "/planning/planning_factors/no_stopping_area", + "/planning/planning_factors/obstacle_cruise_planner", + "/planning/planning_factors/obstacle_stop_planner", + "/planning/planning_factors/occlusion_spot", + "/planning/planning_factors/run_out", + "/planning/planning_factors/stop_line", + "/planning/planning_factors/surround_obstacle_checker", + "/planning/planning_factors/traffic_light", + "/planning/planning_factors/virtual_traffic_light", + "/planning/planning_factors/walkway", + "/planning/planning_factors/motion_velocity_planner", + "/planning/planning_factors/static_obstacle_avoidance", + "/planning/planning_factors/dynamic_obstacle_avoidance", + "/planning/planning_factors/avoidance_by_lane_change", + "/planning/planning_factors/lane_change_left", + "/planning/planning_factors/lane_change_right", + "/planning/planning_factors/start_planner", + "/planning/planning_factors/goal_planner"}; + + sub_factors_ = init_factors(this, factors_, factor_topics); const auto adaptor = autoware::component_interface_utils::NodeAdaptor(this); adaptor.init_pub(pub_velocity_factors_); @@ -139,8 +263,8 @@ void PlanningNode::on_kinematic_state(const KinematicState::ConstSharedPtr msg) void PlanningNode::on_timer() { using autoware_adapi_v1_msgs::msg::VelocityFactor; - auto velocity = merge_factors(now(), velocity_factors_); - auto steering = merge_factors(now(), steering_factors_); + auto velocity = merge_factors(now(), factors_); + auto steering = merge_factors(now(), factors_); // Set the distance if it is nan. if (trajectory_ && kinematic_state_) { @@ -164,6 +288,13 @@ void PlanningNode::on_timer() } } + for (auto & factor : steering.factors) { + if ((factor.status == SteeringFactor::UNKNOWN) && (!std::isnan(factor.distance.front()))) { + const auto is_turning = factor.distance.front() < 0.0; + factor.status = is_turning ? SteeringFactor::TURNING : SteeringFactor::APPROACHING; + } + } + pub_velocity_factors_->publish(velocity); pub_steering_factors_->publish(steering); } diff --git a/system/autoware_default_adapi/src/planning.hpp b/system/autoware_default_adapi/src/planning.hpp index 3a9b99e33a997..49584475734f6 100644 --- a/system/autoware_default_adapi/src/planning.hpp +++ b/system/autoware_default_adapi/src/planning.hpp @@ -21,7 +21,11 @@ #include #include +#include +#include + #include +#include #include // This file should be included after messages. @@ -30,22 +34,26 @@ namespace autoware::default_adapi { +using autoware_adapi_v1_msgs::msg::PlanningBehavior; +using autoware_adapi_v1_msgs::msg::SteeringFactor; +using autoware_adapi_v1_msgs::msg::SteeringFactorArray; +using autoware_adapi_v1_msgs::msg::VelocityFactor; +using autoware_adapi_v1_msgs::msg::VelocityFactorArray; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::PlanningFactorArray; + class PlanningNode : public rclcpp::Node { public: explicit PlanningNode(const rclcpp::NodeOptions & options); private: - using VelocityFactorArray = autoware_adapi_v1_msgs::msg::VelocityFactorArray; - using SteeringFactorArray = autoware_adapi_v1_msgs::msg::SteeringFactorArray; Pub pub_velocity_factors_; Pub pub_steering_factors_; Sub sub_trajectory_; Sub sub_kinematic_state_; - std::vector::SharedPtr> sub_velocity_factors_; - std::vector::SharedPtr> sub_steering_factors_; - std::vector velocity_factors_; - std::vector steering_factors_; + std::vector::SharedPtr> sub_factors_; + std::vector factors_; rclcpp::TimerBase::SharedPtr timer_; using VehicleStopChecker = autoware::motion_utils::VehicleStopCheckerBase; diff --git a/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp b/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp index 485d85c926268..4760c9366da46 100644 --- a/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp +++ b/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp @@ -24,7 +24,6 @@ using namespace std::literals; using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::nanoseconds; -using std::placeholders::_1; namespace dummy_infrastructure { @@ -84,6 +83,7 @@ boost::optional findCommand( DummyInfrastructureNode::DummyInfrastructureNode(const rclcpp::NodeOptions & node_options) : Node("dummy_infrastructure", node_options) { + using std::placeholders::_1; // Parameter Server set_param_res_ = this->add_on_set_parameters_callback(std::bind(&DummyInfrastructureNode::onSetParam, this, _1)); diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 4022bdaadebef..b5f4bbaeefc46 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -115,12 +115,7 @@ void MrmHandler::publishHazardCmd() HazardLightsCommand msg; msg.stamp = this->now(); - if (is_emergency_holding_) { - // turn hazard on during emergency holding - msg.command = HazardLightsCommand::ENABLE; - } else if (isEmergency() && param_.turning_hazard_on.emergency) { - // turn hazard on if vehicle is in emergency state and - // turning hazard on if emergency flag is true + if (param_.turning_hazard_on.emergency && isEmergency()) { msg.command = HazardLightsCommand::ENABLE; } else { msg.command = HazardLightsCommand::NO_COMMAND; diff --git a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp index 740f841382f47..098f4240782ae 100644 --- a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp +++ b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -655,48 +656,56 @@ void run(int port) int main(int argc, char ** argv) { - static struct option long_options[] = { - {"help", no_argument, nullptr, 'h'}, - {"port", required_argument, nullptr, 'p'}, - {nullptr, 0, nullptr, 0}}; - - // Parse command-line options - int c = 0; - int option_index = 0; - int port = PORT; - while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { - switch (c) { - case 'h': - usage(); - return EXIT_SUCCESS; - - case 'p': - try { - port = boost::lexical_cast(optarg); - } catch (const boost::bad_lexical_cast & e) { - printf("Error: %s\n", e.what()); - return EXIT_FAILURE; - } - break; - - default: - break; + try { + static struct option long_options[] = { + {"help", no_argument, nullptr, 'h'}, + {"port", required_argument, nullptr, 'p'}, + {nullptr, 0, nullptr, 0}}; + + // Parse command-line options + int c = 0; + int option_index = 0; + int port = PORT; + while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { + switch (c) { + case 'h': + usage(); + return EXIT_SUCCESS; + + case 'p': + try { + port = boost::lexical_cast(optarg); + } catch (const boost::bad_lexical_cast & e) { + printf("Error: %s\n", e.what()); + return EXIT_FAILURE; + } + break; + + default: + break; + } } - } - // Put the program in the background - if (daemon(0, 0) < 0) { - printf("Failed to put the program in the background. %s\n", strerror(errno)); - return errno; - } + // Put the program in the background + if (daemon(0, 0) < 0) { + printf("Failed to put the program in the background. %s\n", strerror(errno)); + return errno; + } - // Open connection to system logger - openlog(nullptr, LOG_PID, LOG_DAEMON); + // Open connection to system logger + openlog(nullptr, LOG_PID, LOG_DAEMON); - run(port); + run(port); - // Close descriptor used to write to system logger - closelog(); + // Close descriptor used to write to system logger + closelog(); + } catch (const std::exception & e) { + std::cerr << "Exception in main(): " << e.what() << std::endl; + return EXIT_FAILURE; + } catch (...) { + std::cerr << "Unknown exception in main()" << std::endl; + return EXIT_FAILURE; + } return EXIT_SUCCESS; } diff --git a/system/system_monitor/reader/msr_reader/msr_reader.cpp b/system/system_monitor/reader/msr_reader/msr_reader.cpp index fc7bcab795be0..89f95d05c282e 100644 --- a/system/system_monitor/reader/msr_reader/msr_reader.cpp +++ b/system/system_monitor/reader/msr_reader/msr_reader.cpp @@ -212,90 +212,98 @@ void run(int port, const std::vector & list) int main(int argc, char ** argv) { - static struct option long_options[] = { - {"help", no_argument, 0, 'h'}, {"port", required_argument, 0, 'p'}, {0, 0, 0, 0}}; - - // Parse command-line options - int c = 0; - int option_index = 0; - int port = PORT; - while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { - switch (c) { - case 'h': - usage(); - return EXIT_SUCCESS; - - case 'p': - try { - port = boost::lexical_cast(optarg); - } catch (const boost::bad_lexical_cast & e) { - printf("Error: %s\n", e.what()); - return EXIT_FAILURE; - } - break; + try { + static struct option long_options[] = { + {"help", no_argument, 0, 'h'}, {"port", required_argument, 0, 'p'}, {0, 0, 0, 0}}; + + // Parse command-line options + int c = 0; + int option_index = 0; + int port = PORT; + while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { + switch (c) { + case 'h': + usage(); + return EXIT_SUCCESS; + + case 'p': + try { + port = boost::lexical_cast(optarg); + } catch (const boost::bad_lexical_cast & e) { + printf("Error: %s\n", e.what()); + return EXIT_FAILURE; + } + break; + + default: + break; + } + } - default: - break; + if (!fs::exists("/dev/cpu")) { + printf("Failed to access /dev/cpu.\n"); + return EXIT_FAILURE; } - } - if (!fs::exists("/dev/cpu")) { - printf("Failed to access /dev/cpu.\n"); - return EXIT_FAILURE; - } + std::vector list; + const fs::path root("/dev/cpu"); - std::vector list; - const fs::path root("/dev/cpu"); + for (const fs::path & path : boost::make_iterator_range( + fs::recursive_directory_iterator(root), fs::recursive_directory_iterator())) { + if (fs::is_directory(path)) { + continue; + } - for (const fs::path & path : boost::make_iterator_range( - fs::recursive_directory_iterator(root), fs::recursive_directory_iterator())) { - if (fs::is_directory(path)) { - continue; - } + std::cmatch match; + const char * msr = path.generic_string().c_str(); - std::cmatch match; - const char * msr = path.generic_string().c_str(); + // /dev/cpu/[0-9]/msr ? + if (!std::regex_match(msr, match, std::regex(".*msr"))) { + continue; + } - // /dev/cpu/[0-9]/msr ? - if (!std::regex_match(msr, match, std::regex(".*msr"))) { - continue; + list.push_back(path.generic_string()); } - list.push_back(path.generic_string()); - } + std::sort(list.begin(), list.end(), [](const std::string & c1, const std::string & c2) { + std::cmatch match; + const std::regex filter(".*/(\\d+)/msr"); + int n1 = 0; + int n2 = 0; + if (std::regex_match(c1.c_str(), match, filter)) { + n1 = std::stoi(match[1].str()); + } + if (std::regex_match(c2.c_str(), match, filter)) { + n2 = std::stoi(match[1].str()); + } + return n1 < n2; + }); // NOLINT - std::sort(list.begin(), list.end(), [](const std::string & c1, const std::string & c2) { - std::cmatch match; - const std::regex filter(".*/(\\d+)/msr"); - int n1 = 0; - int n2 = 0; - if (std::regex_match(c1.c_str(), match, filter)) { - n1 = std::stoi(match[1].str()); - } - if (std::regex_match(c2.c_str(), match, filter)) { - n2 = std::stoi(match[1].str()); + if (list.empty()) { + printf("No msr found in /dev/cpu.\n"); + return EXIT_FAILURE; } - return n1 < n2; - }); // NOLINT - - if (list.empty()) { - printf("No msr found in /dev/cpu.\n"); - return EXIT_FAILURE; - } - // Put the program in the background - if (daemon(0, 0) < 0) { - printf("Failed to put the program in the background. %s\n", strerror(errno)); - return errno; - } + // Put the program in the background + if (daemon(0, 0) < 0) { + printf("Failed to put the program in the background. %s\n", strerror(errno)); + return errno; + } - // Open connection to system logger - openlog(nullptr, LOG_PID, LOG_DAEMON); + // Open connection to system logger + openlog(nullptr, LOG_PID, LOG_DAEMON); - run(port, list); + run(port, list); - // Close descriptor used to write to system logger - closelog(); + // Close descriptor used to write to system logger + closelog(); + } catch (const std::exception & e) { + std::cerr << "Exception in main(): " << e.what() << std::endl; + return EXIT_FAILURE; + } catch (...) { + std::cerr << "Unknown exception in main()" << std::endl; + return EXIT_FAILURE; + } return EXIT_SUCCESS; } diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CMakeLists.txt b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..7ff4e19c69419 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_string_stamped_rviz_plugin) + +# find dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugins_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + plugins +) diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/README.md b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/README.md new file mode 100644 index 0000000000000..e5add261325f4 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/README.md @@ -0,0 +1,7 @@ +# autoware_string_stamped_rviz_plugin + +Plugin for displaying 2D overlays over the RViz2 3D scene. + +## Purpose + +This plugin displays the ROS message whose topic type is `autoware_internal_debug_msgs::msg::StringStamped` in rviz. diff --git a/common/autoware_osqp_interface/package.xml b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/package.xml similarity index 56% rename from common/autoware_osqp_interface/package.xml rename to visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/package.xml index 92a2afeccc4e0..faef2f674832e 100644 --- a/common/autoware_osqp_interface/package.xml +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/package.xml @@ -1,27 +1,28 @@ - autoware_osqp_interface + autoware_string_stamped_rviz_plugin 0.40.0 - Interface for the OSQP solver - Maxime CLEMENT + + RViz2 plugin for 2D overlays in the 3D view. Mainly a port of the JSK overlay plugin + (https://github.com/jsk-ros-pkg/jsk_visualization). + Takayuki Murooka - Fumiya Watanabe Satoshi Ota - Apache 2.0 + + Apache License 2.0 ament_cmake_auto autoware_cmake - eigen - osqp_vendor - rclcpp - rclcpp_components + ament_index_cpp + autoware_internal_debug_msgs + rviz_common + rviz_ogre_vendor + rviz_rendering - ament_cmake_ros ament_lint_auto autoware_lint_common - eigen ament_cmake diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/plugins/plugins_description.xml b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/plugins/plugins_description.xml new file mode 100644 index 0000000000000..302bcc629b892 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/plugins/plugins_description.xml @@ -0,0 +1,5 @@ + + + String stamped overlay plugin for the 3D view. + + diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.cpp b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.cpp new file mode 100644 index 0000000000000..03fd8bca5aee8 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.cpp @@ -0,0 +1,225 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "jsk_overlay_utils.hpp" + +#include + +namespace jsk_rviz_plugins +{ +ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer) +: pixel_buffer_(pixel_buffer) +{ + pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL); +} + +ScopedPixelBuffer::~ScopedPixelBuffer() +{ + pixel_buffer_->unlock(); +} + +Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() +{ + return pixel_buffer_; +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height) +{ + const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock(); + Ogre::uint8 * pDest = static_cast(pixelBox.data); + memset(pDest, 0, width * height); + return QImage(pDest, width, height, QImage::Format_ARGB32); +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height, QColor & bg_color) +{ + QImage Hud = getQImage(width, height); + for (unsigned int i = 0; i < width; i++) { + for (unsigned int j = 0; j < height; j++) { + Hud.setPixel(i, j, bg_color.rgba()); + } + } + return Hud; +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight()); +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay, QColor & bg_color) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight(), bg_color); +} + +OverlayObject::OverlayObject( + Ogre::SceneManager * manager, rclcpp::Logger logger, const std::string & name) +: name_(name), logger_(logger) +{ + rviz_rendering::RenderSystem::get()->prepareOverlays(manager); + std::string material_name = name_ + "Material"; + Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + overlay_ = mOverlayMgr->create(name_); + panel_ = static_cast( + mOverlayMgr->createOverlayElement("Panel", name_ + "Panel")); + panel_->setMetricsMode(Ogre::GMM_PIXELS); + + panel_material_ = Ogre::MaterialManager::getSingleton().create( + material_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + panel_->setMaterialName(panel_material_->getName()); + overlay_->add2D(panel_); +} + +OverlayObject::~OverlayObject() +{ + hide(); + panel_material_->unload(); + Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); + // Ogre::OverlayManager* mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + // mOverlayMgr->destroyOverlayElement(panel_); + // delete panel_; + // delete overlay_; +} + +std::string OverlayObject::getName() +{ + return name_; +} + +void OverlayObject::hide() +{ + if (overlay_->isVisible()) { + overlay_->hide(); + } +} + +void OverlayObject::show() +{ + if (!overlay_->isVisible()) { + overlay_->show(); + } +} + +bool OverlayObject::isTextureReady() +{ + return static_cast(texture_); +} + +void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) +{ + const std::string texture_name = name_ + "Texture"; + if (width == 0) { + RCLCPP_WARN(logger_, "width=0 is specified as texture size"); + width = 1; + } + if (height == 0) { + RCLCPP_WARN(logger_, "height=0 is specified as texture size"); + height = 1; + } + if (!isTextureReady() || ((width != texture_->getWidth()) || (height != texture_->getHeight()))) { + if (isTextureReady()) { + Ogre::TextureManager::getSingleton().remove(texture_name); + panel_material_->getTechnique(0)->getPass(0)->removeAllTextureUnitStates(); + } + texture_ = Ogre::TextureManager::getSingleton().createManual( + texture_name, // name + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, // type + width, height, // width & height of the render window + 0, // number of mipmaps + Ogre::PF_A8R8G8B8, // pixel format chosen to match a format Qt can use + Ogre::TU_DEFAULT // usage + ); + panel_material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name); + + panel_material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + } +} + +ScopedPixelBuffer OverlayObject::getBuffer() +{ + if (isTextureReady()) { + return ScopedPixelBuffer(texture_->getBuffer()); + } else { + return ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr()); + } +} + +void OverlayObject::setPosition(double left, double top) +{ + panel_->setPosition(left, top); +} + +void OverlayObject::setDimensions(double width, double height) +{ + panel_->setDimensions(width, height); +} + +bool OverlayObject::isVisible() +{ + return overlay_->isVisible(); +} + +unsigned int OverlayObject::getTextureWidth() +{ + if (isTextureReady()) { + return texture_->getWidth(); + } else { + return 0; + } +} + +unsigned int OverlayObject::getTextureHeight() +{ + if (isTextureReady()) { + return texture_->getHeight(); + } else { + return 0; + } +} + +} // namespace jsk_rviz_plugins diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.hpp b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.hpp new file mode 100644 index 0000000000000..e69abed49f371 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.hpp @@ -0,0 +1,143 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef JSK_OVERLAY_UTILS_HPP_ +#define JSK_OVERLAY_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +// see OGRE/OgrePrerequisites.h +// #define OGRE_VERSION +// ((OGRE_VERSION_MAJOR << 16) | (OGRE_VERSION_MINOR << 8) | OGRE_VERSION_PATCH) +#if OGRE_VERSION < ((1 << 16) | (9 << 8) | 0) +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#endif + +#include +#include +#include +#include +#include +#include + +namespace jsk_rviz_plugins +{ +class OverlayObject; + +class ScopedPixelBuffer +{ +public: + explicit ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer); + virtual ~ScopedPixelBuffer(); + virtual Ogre::HardwarePixelBufferSharedPtr getPixelBuffer(); + virtual QImage getQImage(unsigned int width, unsigned int height); + virtual QImage getQImage(OverlayObject & overlay); + virtual QImage getQImage(unsigned int width, unsigned int height, QColor & bg_color); + virtual QImage getQImage(OverlayObject & overlay, QColor & bg_color); + +protected: + Ogre::HardwarePixelBufferSharedPtr pixel_buffer_; + +private: +}; + +// this is a class for put overlay object on rviz 3D panel. +// This class suppose to be instantiated in onInitialize method +// of rviz::Display class. +class OverlayObject +{ +public: + typedef std::shared_ptr Ptr; + + OverlayObject(Ogre::SceneManager * manager, rclcpp::Logger logger, const std::string & name); + virtual ~OverlayObject(); + + virtual std::string getName(); + /*virtual*/ void hide(); // remove "virtual" for cppcheck + virtual void show(); + virtual bool isTextureReady(); + virtual void updateTextureSize(unsigned int width, unsigned int height); + virtual ScopedPixelBuffer getBuffer(); + virtual void setPosition(double left, double top); + virtual void setDimensions(double width, double height); + virtual bool isVisible(); + virtual unsigned int getTextureWidth(); + virtual unsigned int getTextureHeight(); + +protected: + const std::string name_; + rclcpp::Logger logger_; + Ogre::Overlay * overlay_; + Ogre::PanelOverlayElement * panel_; + Ogre::MaterialPtr panel_material_; + Ogre::TexturePtr texture_; + +private: +}; + +// Ogre::Overlay* createOverlay(std::string name); +// Ogre::PanelOverlayElement* createOverlayPanel(Ogre::Overlay* overlay); +// Ogre::MaterialPtr createOverlayMaterial(Ogre::Overlay* overlay); +} // namespace jsk_rviz_plugins + +#endif // JSK_OVERLAY_UTILS_HPP_ diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.cpp b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.cpp new file mode 100644 index 0000000000000..d5746e99ff084 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.cpp @@ -0,0 +1,237 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "string_stamped_overlay_display.hpp" + +#include "jsk_overlay_utils.hpp" + +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace autoware::string_stamped_rviz_plugin +{ +StringStampedOverlayDisplay::StringStampedOverlayDisplay() +{ + const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL)); + + constexpr float hight_4k = 2160.0; + const float scale = static_cast(screen_info->height) / hight_4k; + const auto left = static_cast(std::round(1024 * scale)); + const auto top = static_cast(std::round(128 * scale)); + + property_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this); + property_left_ = new rviz_common::properties::IntProperty( + "Left", left, "Left of the plotter window", this, SLOT(updateVisualization()), this); + property_left_->setMin(0); + property_top_ = new rviz_common::properties::IntProperty( + "Top", top, "Top of the plotter window", this, SLOT(updateVisualization())); + property_top_->setMin(0); + + property_value_height_offset_ = new rviz_common::properties::IntProperty( + "Value height offset", 0, "Height offset of the plotter window", this, + SLOT(updateVisualization())); + property_font_size_ = new rviz_common::properties::IntProperty( + "Font Size", 15, "Font Size", this, SLOT(updateVisualization()), this); + property_font_size_->setMin(1); + property_max_letter_num_ = new rviz_common::properties::IntProperty( + "Max Letter Num", 100, "Max Letter Num", this, SLOT(updateVisualization()), this); + property_max_letter_num_->setMin(10); + + property_last_diag_keep_time_ = new rviz_common::properties::FloatProperty( + "Time To Keep Last Diag", 1.0, "Time To Keep Last Diag", this, SLOT(updateVisualization()), + this); + property_last_diag_keep_time_->setMin(0); + + property_last_diag_erase_time_ = new rviz_common::properties::FloatProperty( + "Time To Erase Last Diag", 2.0, "Time To Erase Last Diag", this, SLOT(updateVisualization()), + this); + property_last_diag_erase_time_->setMin(0.001); +} + +StringStampedOverlayDisplay::~StringStampedOverlayDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void StringStampedOverlayDisplay::onInitialize() +{ + RTDClass::onInitialize(); + + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "StringOverlayDisplayObject" << count++; + auto logger = context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger(); + overlay_.reset(new jsk_rviz_plugins::OverlayObject(scene_manager_, logger, ss.str())); + + overlay_->show(); + + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +void StringStampedOverlayDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void StringStampedOverlayDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void StringStampedOverlayDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + { + std::lock_guard message_lock(mutex_); + if (!last_non_empty_msg_ptr_) { + return; + } + } + + // calculate text and alpha + const auto text_with_alpha = [&]() { + std::lock_guard message_lock(mutex_); + if (last_msg_text_.empty()) { + const auto current_time = context_->getRosNodeAbstraction().lock()->get_raw_node()->now(); + const auto duration = (current_time - last_non_empty_msg_ptr_->stamp).seconds(); + if ( + duration < + property_last_diag_keep_time_->getFloat() + property_last_diag_erase_time_->getFloat()) { + const int dynamic_alpha = static_cast(std::max( + (1.0 - std::max(duration - property_last_diag_keep_time_->getFloat(), 0.0) / + property_last_diag_erase_time_->getFloat()) * + 255, + 0.0)); + return std::make_pair(last_non_empty_msg_ptr_->data, dynamic_alpha); + } + } + return std::make_pair(last_msg_text_, 255); + }(); + + // Display + QColor background_color; + background_color.setAlpha(0); + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(background_color); + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + const int w = overlay_->getTextureWidth() - line_width_; + const int h = overlay_->getTextureHeight() - line_width_; + + // text + QColor text_color(property_text_color_->getColor()); + text_color.setAlpha(text_with_alpha.second); + painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); + QFont font = painter.font(); + font.setPixelSize(property_font_size_->getInt()); + font.setBold(true); + painter.setFont(font); + + // same as above, but align on right side + painter.drawText( + 0, std::min(property_value_height_offset_->getInt(), h - 1), w, + std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignLeft | Qt::AlignTop, + text_with_alpha.first.c_str()); + painter.end(); + updateVisualization(); +} + +void StringStampedOverlayDisplay::processMessage( + const autoware_internal_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + { + std::lock_guard message_lock(mutex_); + last_msg_text_ = msg_ptr->data; + + // keep the non empty last message for visualization + if (!msg_ptr->data.empty()) { + last_non_empty_msg_ptr_ = msg_ptr; + } + } + + queueRender(); +} + +void StringStampedOverlayDisplay::updateVisualization() +{ + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +} // namespace autoware::string_stamped_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::string_stamped_rviz_plugin::StringStampedOverlayDisplay, rviz_common::Display) diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.hpp b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.hpp new file mode 100644 index 0000000000000..6f7cd84b91aaa --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.hpp @@ -0,0 +1,110 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef STRING_STAMPED_OVERLAY_DISPLAY_HPP_ +#define STRING_STAMPED_OVERLAY_DISPLAY_HPP_ + +#include +#include +#include + +#ifndef Q_MOC_RUN +#include "jsk_overlay_utils.hpp" + +#include +#include +#include +#include + +#endif + +#include + +namespace autoware::string_stamped_rviz_plugin +{ +class StringStampedOverlayDisplay +: public rviz_common::RosTopicDisplay + +{ + Q_OBJECT + +public: + StringStampedOverlayDisplay(); + ~StringStampedOverlayDisplay() override; + + void onInitialize() override; + void onEnable() override; + void onDisable() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void update(float wall_dt, float ros_dt) override; + void processMessage( + const autoware_internal_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) override; + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::ColorProperty * property_text_color_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_value_height_offset_; + rviz_common::properties::IntProperty * property_font_size_; + rviz_common::properties::IntProperty * property_max_letter_num_; + rviz_common::properties::FloatProperty * property_last_diag_keep_time_; + rviz_common::properties::FloatProperty * property_last_diag_erase_time_; + +private: + static constexpr int line_width_ = 2; + static constexpr int hand_width_ = 4; + + std::mutex mutex_; + std::string last_msg_text_; + autoware_internal_debug_msgs::msg::StringStamped::ConstSharedPtr last_non_empty_msg_ptr_; +}; +} // namespace autoware::string_stamped_rviz_plugin + +#endif // STRING_STAMPED_OVERLAY_DISPLAY_HPP_ diff --git a/visualization/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp b/visualization/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp index b7d754b02d4bd..386c8cdfb0b13 100644 --- a/visualization/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp +++ b/visualization/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp @@ -184,7 +184,7 @@ void ThirdPersonViewController::handleMouseEvent(rviz_common::ViewportMouseEvent void ThirdPersonViewController::mimic(rviz_common::ViewController * source_view) { - FramePositionTrackingViewController::mimic(source_view); + FramePositionTrackingViewController::mimic(source_view); // NOLINT target_frame_property_->setValue(TARGET_FRAME_START); getNewTransform();