diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 205f6c7fb4438..e03ca0b8ad30f 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -3,8 +3,10 @@ common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.j common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_geography_utils/** koji.minoda@tier4.jp common/autoware_grid_map_utils/** maxime.clement@tier4.jp +common/autoware_interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp common/autoware_kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp common/autoware_motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +common/autoware_object_recognition_utils/** shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai common/autoware_path_distance_calculator/** isamu.takagi@tier4.jp @@ -23,12 +25,10 @@ common/fake_test_node/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabay common/global_parameter_loader/** ryohsuke.mitsudome@tier4.jp common/glog_component/** takamasa.horibe@tier4.jp common/goal_distance_calculator/** taiki.tanaka@tier4.jp -common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp -common/object_recognition_utils/** shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp -common/tensorrt_common/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp +common/tensorrt_common/** amadeusz.szymko.2@tier4.jp dan.umeda@tier4.jp kenzo.lobos@tier4.jp manato.hirabayashi@tier4.jp common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp common/tier4_api_utils/** isamu.takagi@tier4.jp common/tier4_camera_view_rviz_plugin/** makoto.ybauta@tier4.jp uken.ryu@tier4.jp @@ -58,7 +58,7 @@ control/autoware_trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.m control/autoware_trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/autoware_vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -control/obstacle_collision_checker/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +control/autoware_obstacle_collision_checker/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/predicted_path_checker/** berkay@leodrive.ai evaluator/autoware_control_evaluator/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp temkei.kem@tier4.jp evaluator/autoware_evaluator_utils/** daniel.sanchez@tier4.jp takayuki.murooka@tier4.jp @@ -84,6 +84,8 @@ localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/* localization/autoware_landmark_based_localizer/autoware_landmark_manager/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/** shintaro.sakoda@tier4.jp yamato.ando@tier4.jp localization/autoware_localization_error_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/autoware_localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/autoware_ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_pose2twist/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_pose_covariance_modifier/** melike@leodrive.ai localization/autoware_pose_estimator_arbiter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @@ -91,8 +93,6 @@ localization/autoware_pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tie localization/autoware_pose_instability_detector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/autoware_localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/autoware_ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_common/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @@ -106,7 +106,7 @@ perception/autoware_bytetrack/** manato.hirabayashi@tier4.jp yoshi.ri@tier4.jp perception/autoware_cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/autoware_compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp perception/autoware_crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp -perception/autoware_detected_object_feature_remover/** tomoya.kimura@tier4.jp yoshi.ri@tier4.jp kotaro.uetake@tier4.jp +perception/autoware_detected_object_feature_remover/** kotaro.uetake@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp perception/autoware_detected_object_validation/** dai.nguyen@tier4.jp shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/autoware_detection_by_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/autoware_elevation_map_loader/** kosuke.takeuchi@tier4.jp shintaro.tomie@tier4.jp taichi.higashide@tier4.jp @@ -162,9 +162,9 @@ planning/autoware_scenario_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp planning/autoware_surround_obstacle_checker/** go.sakayori@tier4.jp satoshi.ota@tier4.jp planning/autoware_velocity_smoother/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp -planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/** alqudah.mohammad@tier4.jp fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp -planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/** alqudah.mohammad@tier4.jp fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_path_planner/autoware_behavior_path_lane_change_module/** alqudah.mohammad@tier4.jp fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_planner/autoware_behavior_path_planner/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp @@ -191,9 +191,9 @@ planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_li planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** alqudah.mohammad@tier4.jp maxime.clement@tier4.jp -planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** maxime.clement@tier4.jp -planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** maxime.clement@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** alqudah.mohammad@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** alqudah.mohammad@tier4.jp maxime.clement@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** alqudah.mohammad@tier4.jp maxime.clement@tier4.jp planning/sampling_based_planner/autoware_bezier_sampler/** maxime.clement@tier4.jp planning/sampling_based_planner/autoware_frenet_planner/** maxime.clement@tier4.jp planning/sampling_based_planner/autoware_path_sampler/** maxime.clement@tier4.jp @@ -208,7 +208,7 @@ sensing/autoware_radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.m sensing/autoware_radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/autoware_radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/autoware_radar_tracks_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp -sensing/cuda_utils/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp +sensing/cuda_utils/** amadeusz.szymko.2@tier4.jp kenzo.lobos@tier4.jp manato.hirabayashi@tier4.jp sensing/livox/autoware_livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp simulator/autoware_carla_interface/** maxime.clement@tier4.jp mradityagio@gmail.com diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 5e1f3e2c093a5..cda1666603926 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -75,7 +75,7 @@ jobs: clang-tidy-differential: needs: build-and-test-differential - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 container: ghcr.io/autowarefoundation/autoware:latest-autoware-universe-cuda steps: - name: Set PR fetch depth diff --git a/.github/workflows/cancel-previous-workflows.yaml b/.github/workflows/cancel-previous-workflows.yaml index 44983f7deadcb..bd2463d5a8eea 100644 --- a/.github/workflows/cancel-previous-workflows.yaml +++ b/.github/workflows/cancel-previous-workflows.yaml @@ -5,7 +5,7 @@ on: jobs: cancel-previous-workflows: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Cancel previous runs uses: styfle/cancel-workflow-action@0.12.1 diff --git a/.github/workflows/check-build-depends.yaml b/.github/workflows/check-build-depends.yaml index 81618a1db0eea..74c97e3bf4be4 100644 --- a/.github/workflows/check-build-depends.yaml +++ b/.github/workflows/check-build-depends.yaml @@ -7,7 +7,7 @@ on: jobs: check-build-depends: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 container: ${{ matrix.container }} strategy: fail-fast: false diff --git a/.github/workflows/clang-tidy-pr-comments-manually.yaml b/.github/workflows/clang-tidy-pr-comments-manually.yaml index 9bccd972becde..a4df9f9dec3ed 100644 --- a/.github/workflows/clang-tidy-pr-comments-manually.yaml +++ b/.github/workflows/clang-tidy-pr-comments-manually.yaml @@ -8,7 +8,7 @@ on: required: true jobs: clang-tidy-pr-comments-manually: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Check out repository uses: actions/checkout@v3 diff --git a/.github/workflows/clang-tidy-pr-comments.yaml b/.github/workflows/clang-tidy-pr-comments.yaml index baaa0fb8e7744..bf2ed81d7ae48 100644 --- a/.github/workflows/clang-tidy-pr-comments.yaml +++ b/.github/workflows/clang-tidy-pr-comments.yaml @@ -10,7 +10,7 @@ on: jobs: clang-tidy-pr-comments: if: ${{ github.event.workflow_run.event == 'pull_request' && contains(fromJson('["success", "failure"]'), github.event.workflow_run.conclusion) }} - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Check out repository uses: actions/checkout@v3 diff --git a/.github/workflows/comment-on-pr.yaml b/.github/workflows/comment-on-pr.yaml index 3cfec4bf14f05..e10d40706daac 100644 --- a/.github/workflows/comment-on-pr.yaml +++ b/.github/workflows/comment-on-pr.yaml @@ -8,8 +8,10 @@ on: - beta/v0.[0-9]+.[1-9]+ jobs: - comment: - runs-on: ubuntu-latest + comment-on-pr: + runs-on: ubuntu-22.04 + permissions: + pull-requests: write steps: - name: Create comments run: | diff --git a/.github/workflows/cppcheck-differential.yaml b/.github/workflows/cppcheck-differential.yaml index 8c2edd3c76724..09b9b6e983f06 100644 --- a/.github/workflows/cppcheck-differential.yaml +++ b/.github/workflows/cppcheck-differential.yaml @@ -5,7 +5,7 @@ on: jobs: cppcheck-differential: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Set PR fetch depth diff --git a/.github/workflows/cppcheck-weekly.yaml b/.github/workflows/cppcheck-weekly.yaml index 573dcc54aa8c0..28be3897ae4f8 100644 --- a/.github/workflows/cppcheck-weekly.yaml +++ b/.github/workflows/cppcheck-weekly.yaml @@ -7,7 +7,7 @@ on: jobs: cppcheck-weekly: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Checkout code diff --git a/.github/workflows/dco.yaml b/.github/workflows/dco.yaml index db7ace467c658..e95b394ac63d0 100644 --- a/.github/workflows/dco.yaml +++ b/.github/workflows/dco.yaml @@ -4,7 +4,7 @@ on: pull_request: jobs: dco: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v4 diff --git a/.github/workflows/delete-closed-pr-docs.yaml b/.github/workflows/delete-closed-pr-docs.yaml index b7b009fb00263..192e138a83c22 100644 --- a/.github/workflows/delete-closed-pr-docs.yaml +++ b/.github/workflows/delete-closed-pr-docs.yaml @@ -7,7 +7,7 @@ on: jobs: delete-closed-pr-docs: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Check out repository uses: actions/checkout@v3 diff --git a/.github/workflows/deploy-docs.yaml b/.github/workflows/deploy-docs.yaml index b48d70dbacb0c..771b4bd36ca9d 100644 --- a/.github/workflows/deploy-docs.yaml +++ b/.github/workflows/deploy-docs.yaml @@ -27,7 +27,7 @@ jobs: deploy-docs: needs: prevent-no-label-execution if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Check out repository uses: actions/checkout@v3 diff --git a/.github/workflows/github-release.yaml b/.github/workflows/github-release.yaml index b426d0cba6614..4b1d7f47c6c0c 100644 --- a/.github/workflows/github-release.yaml +++ b/.github/workflows/github-release.yaml @@ -15,7 +15,7 @@ on: jobs: github-release: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Set tag name id: set-tag-name diff --git a/.github/workflows/json-schema-check.yaml b/.github/workflows/json-schema-check.yaml index 5c223edf1eec2..be9b8e3ce20fe 100644 --- a/.github/workflows/json-schema-check.yaml +++ b/.github/workflows/json-schema-check.yaml @@ -6,7 +6,7 @@ on: jobs: check-if-relevant-files-changed: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 outputs: run-check: ${{ steps.paths_filter.outputs.json_or_yaml }} steps: @@ -22,7 +22,7 @@ jobs: json-schema-check: needs: check-if-relevant-files-changed if: needs.check-if-relevant-files-changed.outputs.run-check == 'true' - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Check out repository uses: actions/checkout@v4 @@ -33,7 +33,7 @@ jobs: no-relevant-changes: needs: check-if-relevant-files-changed if: needs.check-if-relevant-files-changed.outputs.run-check == 'false' - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Dummy step run: echo "No relevant changes, passing check" diff --git a/.github/workflows/pr-agent.yaml b/.github/workflows/pr-agent.yaml index 98646d8161da2..5cd0845b3d9b4 100644 --- a/.github/workflows/pr-agent.yaml +++ b/.github/workflows/pr-agent.yaml @@ -13,7 +13,7 @@ jobs: pr_agent_job: needs: prevent-no-label-execution-pr-agent if: ${{ needs.prevent-no-label-execution-pr-agent.outputs.run == 'true' }} - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 permissions: issues: write pull-requests: write diff --git a/.github/workflows/pr-labeler.yaml b/.github/workflows/pr-labeler.yaml index d45067bee59ed..cc2b26fd3e422 100644 --- a/.github/workflows/pr-labeler.yaml +++ b/.github/workflows/pr-labeler.yaml @@ -8,7 +8,7 @@ on: jobs: label: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - uses: actions/labeler@v4 with: diff --git a/.github/workflows/pre-commit-autoupdate.yaml b/.github/workflows/pre-commit-autoupdate.yaml index 23b403f2a52af..8d57a53b5ccab 100644 --- a/.github/workflows/pre-commit-autoupdate.yaml +++ b/.github/workflows/pre-commit-autoupdate.yaml @@ -14,7 +14,7 @@ jobs: pre-commit-autoupdate: needs: check-secret if: ${{ needs.check-secret.outputs.set == 'true' }} - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Generate token id: generate-token diff --git a/.github/workflows/pre-commit-optional.yaml b/.github/workflows/pre-commit-optional.yaml index 38738196a0bd3..12f536c551646 100644 --- a/.github/workflows/pre-commit-optional.yaml +++ b/.github/workflows/pre-commit-optional.yaml @@ -5,7 +5,7 @@ on: jobs: pre-commit-optional: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Check out repository uses: actions/checkout@v4 diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index c724885fcb3e4..4d005e849b5ec 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -6,7 +6,7 @@ on: jobs: pre-commit: if: ${{ github.event.repository.private }} # Use pre-commit.ci for public repositories - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Generate token id: generate-token diff --git a/.github/workflows/spell-check-daily.yaml b/.github/workflows/spell-check-daily.yaml index dcee449abc728..8c0373594a90e 100644 --- a/.github/workflows/spell-check-daily.yaml +++ b/.github/workflows/spell-check-daily.yaml @@ -7,7 +7,7 @@ on: jobs: spell-check-daily: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Check out repository uses: actions/checkout@v4 diff --git a/.github/workflows/spell-check-differential.yaml b/.github/workflows/spell-check-differential.yaml index ee9d451ba9b9e..3542659332532 100644 --- a/.github/workflows/spell-check-differential.yaml +++ b/.github/workflows/spell-check-differential.yaml @@ -5,7 +5,7 @@ on: jobs: spell-check-differential: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Check out repository uses: actions/checkout@v4 diff --git a/.github/workflows/sync-files.yaml b/.github/workflows/sync-files.yaml index 51e523b8031bf..0cffbcd2a269f 100644 --- a/.github/workflows/sync-files.yaml +++ b/.github/workflows/sync-files.yaml @@ -14,7 +14,7 @@ jobs: sync-files: needs: check-secret if: ${{ needs.check-secret.outputs.set == 'true' }} - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Generate token id: generate-token diff --git a/.github/workflows/update-codeowners-from-packages.yaml b/.github/workflows/update-codeowners-from-packages.yaml index 8b3d2407fbc75..760a647ffbf56 100644 --- a/.github/workflows/update-codeowners-from-packages.yaml +++ b/.github/workflows/update-codeowners-from-packages.yaml @@ -14,7 +14,7 @@ jobs: update-codeowners-from-packages: needs: check-secret if: ${{ needs.check-secret.outputs.set == 'true' }} - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Generate token id: generate-token diff --git a/README.md b/README.md index 35818990cee95..9636c801a4444 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,5 @@ # Autoware Universe -[![codecov](https://codecov.io/github/autowarefoundation/autoware.universe/graph/badge.svg?token=KQP68YQ65D)](https://codecov.io/github/autowarefoundation/autoware.universe) - ## Welcome to Autoware Universe Autoware Universe serves as a foundational pillar within the Autoware ecosystem, playing a critical role in enhancing the core functionalities of autonomous driving technologies. @@ -16,3 +14,37 @@ To dive into the vast world of Autoware and understand how Autoware Universe fit ### Explore Autoware Universe documentation For those looking to explore the specifics of Autoware Universe components, the [Autoware Universe Documentation](https://autowarefoundation.github.io/autoware.universe/), deployed with MKDocs, offers detailed insights. + +## Code Coverage Metrics + +Below table shows the coverage rate of entire Autoware Universe and sub-components respectively. + +### Entire Project Coverage + +[![codecov](https://codecov.io/github/autowarefoundation/autoware.universe/graph/badge.svg?token=KQP68YQ65D)](https://codecov.io/github/autowarefoundation/autoware.universe) + +### Component-wise Coverage + +You can check more details by clicking the badge and navigating the codecov website. + +| Component | Coverage | +| ------------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| Common | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Common%20Packages&query=$.[0].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Common%20Packages) | +| Control | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Control%20Packages&query=$.[1].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Control%20Packages) | +| Evaluator | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Evaluator%20Packages&query=$.[2].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Evaluator%20Packages) | +| Launch | TBD | +| Localization | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Localization%20Packages&query=$.[4].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Localization%20Packages) | +| Map | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Map%20Packages&query=$.[5].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Map%20Packages) | +| Perception | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Perception%20Packages&query=$.[6].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Perception%20Packages) | +| Planning | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Planning%20Packages&query=$.[7].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Planning%20Packages) | +| Sensing | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Sensing%20Packages&query=$.[8].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Sensing%20Packages) | +| Simulator | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Simulator%20Packages&query=$.[9].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Simulator%20Packages) | +| System | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=System%20Packages&query=$.[10].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=System%20Packages) | +| Vehicle | [![codecov](https://img.shields.io/badge/dynamic/json?url=https://codecov.io/api/v2/github/autowarefoundation/repos/autoware.universe/components&label=Vehicle%20Packages&query=$.[11].coverage)](https://app.codecov.io/gh/autowarefoundation/autoware.universe?components%5B0%5D=Vehicle%20Packages) | + + + diff --git a/codecov.yaml b/codecov.yaml index 75fc6f8bacadd..4c989445a9f54 100644 --- a/codecov.yaml +++ b/codecov.yaml @@ -31,9 +31,162 @@ ignore: - "**/test/*" - "**/test/**/*" - "**/debug.*" + - tools/** component_management: individual_components: + # each entire component + - component_id: common-packages + name: Common Packages + paths: + - common/**/** + + - component_id: control-packages + name: Control Packages + paths: + - control/**/** + + - component_id: evaluator-packages + name: Evaluator Packages + paths: + - evaluator/**/** + + - component_id: launch-packages + name: Launch Packages + paths: + - launch/**/** + + - component_id: localization-packages + name: Localization Packages + paths: + - localization/**/** + + - component_id: map-packages + name: map Packages + paths: + - map/**/** + + - component_id: perception-packages + name: Perception Packages + paths: + - perception/**/** + + - component_id: planning-packages + name: Planning Packages + paths: + - planning/**/** + + - component_id: sensing-packages + name: Sensing Packages + paths: + - sensing/**/** + + - component_id: simulator-packages + name: Simulator Packages + paths: + - simulator/**/** + + - component_id: system-packages + name: System Packages + paths: + - system/**/** + + - component_id: vehicle-packages + name: Vehicle Packages + paths: + - vehicle/**/** + + # TIER IV maintained packages + - component_id: control-tier-iv-maintained-packages + name: Control TIER IV Maintained Packages + paths: + - control/autoware_autonomous_emergency_braking/** + - control/autoware_control_validator/** + - control/autoware_external_cmd_selector/** + # - control/autoware_joy_controller/** + - control/autoware_lane_departure_checker/** + - control/autoware_mpc_lateral_controller/** + - control/autoware_operation_mode_transition_manager/** + - control/autoware_pid_longitudinal_controller/** + # - control/autoware_pure_pursuit/** + - control/autoware_shift_decider/** + # - control/autoware_smart_mpc_trajectory_follower/** + - control/autoware_trajectory_follower_base/** + - control/autoware_trajectory_follower_node/** + - control/autoware_vehicle_cmd_gate/** + # - control/control_performance_analysis/** + - control/obstacle_collision_checker/** + # - control/predicted_path_checker/** + + - component_id: localization-tier-iv-maintained-packages + name: Localization TIER IV Maintained Packages + paths: + - localization/autoware_ekf_localizer/** + - localization/autoware_gyro_odometer/** + - localization/autoware_localization_error_monitor/** + - localization/autoware_localization_util/** + - localization/autoware_ndt_scan_matcher/** + - localization/autoware_pose_initializer/** + - localization/autoware_pose_instability_detector/** + - localization/autoware_stop_filter/** + - localization/autoware_twist2accel/** + + - component_id: map-tier-iv-maintained-packages + name: Map TIER IV Maintained Packages + paths: + - map/**/** + + - component_id: perception-tier-iv-maintained-packages + name: Perception TIER IV Maintained Packages + paths: + - perception/autoware_bytetrack/** + - perception/autoware_cluster_merger/** + - perception/autoware_compare_map_segmentation/** + - perception/autoware_crosswalk_traffic_light_estimator/** + - perception/autoware_detected_object_feature_remover/** + - perception/autoware_detected_object_validation/** + - perception/autoware_detection_by_tracker/** + - perception/autoware_elevation_map_loader/** + - perception/autoware_euclidean_cluster/** + - perception/autoware_ground_segmentation/** + - perception/autoware_image_projection_based_fusion/** + - perception/autoware_lidar_centerpoint/** + - perception/autoware_lidar_transfusion/** + - perception/autoware_map_based_prediction/** + - perception/autoware_multi_object_tracker/** + - perception/autoware_object_merger/** + - perception/autoware_object_range_splitter/** + - perception/autoware_object_velocity_splitter/** + - perception/autoware_occupancy_grid_map_outlier_filter/** + - perception/autoware_probabilistic_occupancy_grid_map/** + - perception/autoware_radar_crossing_objects_noise_filter/** + - perception/autoware_radar_fusion_to_detected_object/** + - perception/autoware_radar_object_clustering/** + - perception/autoware_radar_object_tracker/** + - perception/autoware_radar_tracks_msgs_converter/** + - perception/autoware_raindrop_cluster_filter/** + - perception/autoware_shape_estimation/** + - perception/autoware_simple_object_merger/** + - perception/autoware_tensorrt_classifier/** + - perception/autoware_tensorrt_yolox/** + - perception/autoware_tracking_object_merger/** + - perception/autoware_traffic_light_arbiter/** + - perception/autoware_traffic_light_classifier/** + - perception/autoware_traffic_light_fine_detector/** + - perception/autoware_traffic_light_map_based_detector/** + - perception/autoware_traffic_light_multi_camera_fusion/** + - perception/autoware_traffic_light_occlusion_predictor/** + - perception/autoware_traffic_light_visualization/** + - perception/image_projection_based_fusion/** + # - perception/lidar_apollo_instance_segmentation/** + - perception/lidar_centerpoint/** + - perception/perception_utils/** + - perception/tensorrt_yolo/** + - perception/tensorrt_yolox/** + - perception/traffic_light_classifier/** + - perception/traffic_light_fine_detector/** + - perception/traffic_light_ssd_fine_detector/** + - component_id: planning-tier-iv-maintained-packages name: Planning TIER IV Maintained Packages paths: @@ -72,13 +225,13 @@ component_management: ##### behavior_velocity_planner ##### - planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/** - planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/** - # - planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/** - planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/** # - planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/** - planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/** # - planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/** - planning/behavior_velocity_planner/autoware_behavior_velocity_planner/** - # - planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/** - planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/** # - planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/** - planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/** @@ -92,24 +245,3 @@ component_management: - planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** - planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** - planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** - - - component_id: control-tier-iv-maintained-packages - name: Control TIER IV Maintained Packages - paths: - - control/autoware_autonomous_emergency_braking/** - - control/autoware_control_validator/** - - control/autoware_external_cmd_selector/** - # - control/autoware_joy_controller/** - - control/autoware_lane_departure_checker/** - - control/autoware_mpc_lateral_controller/** - - control/autoware_operation_mode_transition_manager/** - - control/autoware_pid_longitudinal_controller/** - # - control/autoware_pure_pursuit/** - - control/autoware_shift_decider/** - # - control/autoware_smart_mpc_trajectory_follower/** - - control/autoware_trajectory_follower_base/** - - control/autoware_trajectory_follower_node/** - - control/autoware_vehicle_cmd_gate/** - # - control/control_performance_analysis/** - - control/obstacle_collision_checker/** - # - control/predicted_path_checker/** diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp new file mode 100644 index 0000000000000..f4602ffff83e4 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp @@ -0,0 +1,71 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_ + +namespace autoware::motion_utils +{ +/** + * @brief Calculates the velocity required for shifting + * @param lateral lateral distance + * @param jerk lateral jerk + * @param longitudinal_distance longitudinal distance + * @return velocity + */ +double calc_feasible_velocity_from_jerk( + const double lateral, const double jerk, const double longitudinal_distance); + +/** + * @brief Calculates the lateral distance required for shifting + * @param longitudinal longitudinal distance + * @param jerk lateral jerk + * @param velocity velocity + * @return lateral distance + */ +double calc_lateral_dist_from_jerk( + const double longitudinal, const double jerk, const double velocity); + +/** + * @brief Calculates the lateral distance required for shifting + * @param lateral lateral distance + * @param jerk lateral jerk + * @param velocity velocity + * @return longitudinal distance + */ +double calc_longitudinal_dist_from_jerk( + const double lateral, const double jerk, const double velocity); + +/** + * @brief Calculates the total time required for shifting + * @param lateral lateral distance + * @param jerk lateral jerk + * @param acc lateral acceleration + * @return time + */ +double calc_shift_time_from_jerk(const double lateral, const double jerk, const double acc); + +/** + * @brief Calculates the required jerk from lateral/longitudinal distance + * @param lateral lateral distance + * @param longitudinal longitudinal distance + * @param velocity velocity + * @return jerk + */ +double calc_jerk_from_lat_lon_distance( + const double lateral, const double longitudinal, const double velocity); + +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_ diff --git a/common/autoware_motion_utils/src/trajectory/path_shift.cpp b/common/autoware_motion_utils/src/trajectory/path_shift.cpp new file mode 100644 index 0000000000000..6f57f34a56190 --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory/path_shift.cpp @@ -0,0 +1,105 @@ +// 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/motion_utils/trajectory/path_shift.hpp" + +#include "autoware/motion_utils/trajectory/trajectory.hpp" + +namespace autoware::motion_utils +{ +double calc_feasible_velocity_from_jerk( + const double lateral, const double jerk, const double longitudinal_distance) +{ + const double j = std::abs(jerk); + const double l = std::abs(lateral); + const double d = std::abs(longitudinal_distance); + if (j < 1.0e-8 || l < 1.0e-8) { + const std::string error_message( + std::string(__func__) + ": Failed to calculate velocity due to invalid arg"); + RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); + return 1.0e10; + } + return d / (4.0 * std::pow(0.5 * l / j, 1.0 / 3.0)); +} + +double calc_lateral_dist_from_jerk( + const double longitudinal, const double jerk, const double velocity) +{ + const double j = std::abs(jerk); + const double d = std::abs(longitudinal); + const double v = std::abs(velocity); + + if (j < 1.0e-8 || v < 1.0e-8) { + const std::string error_message( + std::string(__func__) + ": Failed to calculate lateral distance due to invalid arg"); + RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); + return 1.0e10; + } + return 2.0 * std::pow(d / (4.0 * v), 3.0) * j; +} + +double calc_longitudinal_dist_from_jerk( + const double lateral, const double jerk, const double velocity) +{ + const double j = std::abs(jerk); + const double l = std::abs(lateral); + const double v = std::abs(velocity); + if (j < 1.0e-8) { + const std::string error_message( + std::string(__func__) + ": Failed to calculate longitudinal distance due to invalid arg"); + RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); + return 1.0e10; + } + return 4.0 * std::pow(0.5 * l / j, 1.0 / 3.0) * v; +} + +double calc_shift_time_from_jerk(const double lateral, const double jerk, const double acc) +{ + const double j = std::abs(jerk); + const double a = std::abs(acc); + const double l = std::abs(lateral); + if (j < 1.0e-8 || a < 1.0e-8) { + const std::string error_message( + std::string(__func__) + ": Failed to calculate shift time due to invalid arg"); + RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); + return 1.0e10; // TODO(Horibe) maybe invalid arg? + } + + // time with constant jerk + double tj = a / j; + + // time with constant acceleration (zero jerk) + double ta = (std::sqrt(a * a + 4.0 * j * j * l / a) - 3.0 * a) / (2.0 * j); + + if (ta < 0.0) { + // it will not hit the acceleration limit this time + tj = std::pow(l / (2.0 * j), 1.0 / 3.0); + ta = 0.0; + } + + const double t_total = 4.0 * tj + 2.0 * ta; + return t_total; +} + +double calc_jerk_from_lat_lon_distance( + const double lateral, const double longitudinal, const double velocity) +{ + constexpr double ep = 1.0e-3; + const double lat = std::abs(lateral); + const double lon = std::max(std::abs(longitudinal), ep); + const double v = std::abs(velocity); + return 0.5 * lat * std::pow(4.0 * v / lon, 3); +} + +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp b/common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp new file mode 100644 index 0000000000000..fbdcc7a8f6f81 --- /dev/null +++ b/common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp @@ -0,0 +1,163 @@ +// 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/motion_utils/trajectory/path_shift.hpp" + +#include + +TEST(path_shift_test, calc_feasible_velocity_from_jerk) +{ + using autoware::motion_utils::calc_feasible_velocity_from_jerk; + + double longitudinal_distance = 0.0; + double lateral_distance = 0.0; + double lateral_jerk = 0.0; + + // Condition: zero lateral jerk + EXPECT_DOUBLE_EQ( + calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), + 1.0e10); + + // Condition: zero lateral distance + lateral_jerk = 1.0; + EXPECT_DOUBLE_EQ( + calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), + 1.0e10); + + // Condition: zero longitudinal distance + lateral_distance = 2.0; + EXPECT_DOUBLE_EQ( + calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), 0.0); + + // Condition: random condition + longitudinal_distance = 50.0; + EXPECT_DOUBLE_EQ( + calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), 12.5); +} + +TEST(path_shift_test, calc_lateral_dist_from_jerk) +{ + using autoware::motion_utils::calc_lateral_dist_from_jerk; + + double longitudinal_distance = 0.0; + double lateral_jerk = 0.0; + double velocity = 0.0; + + // Condition: zero lateral jerk + EXPECT_DOUBLE_EQ( + calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 1.0e10); + + // Condition: zero velocity + lateral_jerk = 2.0; + EXPECT_DOUBLE_EQ( + calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 1.0e10); + + // Condition: zero longitudinal distance + velocity = 10.0; + EXPECT_DOUBLE_EQ(calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 0.0); + + // Condition: random condition + longitudinal_distance = 100.0; + EXPECT_DOUBLE_EQ( + calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 62.5); +} + +TEST(path_shift_test, calc_longitudinal_dist_from_jerk) +{ + using autoware::motion_utils::calc_longitudinal_dist_from_jerk; + + double lateral_distance = 0.0; + double lateral_jerk = 0.0; + double velocity = 0.0; + + // Condition: zero lateral jerk + EXPECT_DOUBLE_EQ( + calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 1.0e10); + + // Condition: zero lateral distance + lateral_jerk = -1.0; + velocity = 10.0; + EXPECT_DOUBLE_EQ(calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 0.0); + + // Condition: zero velocity + velocity = 0.0; + lateral_distance = 54.0; + EXPECT_DOUBLE_EQ(calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 0.0); + + // Condition: random + velocity = 8.0; + EXPECT_DOUBLE_EQ( + calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 96.0); +} + +TEST(path_shift_test, calc_shift_time_from_jerk) +{ + constexpr double epsilon = 1e-6; + + using autoware::motion_utils::calc_shift_time_from_jerk; + + double lateral_distance = 0.0; + double lateral_jerk = 0.0; + double lateral_acceleration = 0.0; + + // Condition: zero lateral jerk + EXPECT_DOUBLE_EQ( + calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 1.0e10); + + // Condition: zero lateral acceleration + lateral_jerk = -2.0; + EXPECT_DOUBLE_EQ( + calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 1.0e10); + + // Condition: zero lateral distance + lateral_acceleration = -4.0; + EXPECT_DOUBLE_EQ( + calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 0.0); + + // Condition: random (TODO: use DOUBLE_EQ in future. currently not precise enough) + lateral_distance = 80.0; + EXPECT_NEAR( + calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 11.16515139, + epsilon); +} + +TEST(path_shift_test, calc_jerk_from_lat_lon_distance) +{ + using autoware::motion_utils::calc_jerk_from_lat_lon_distance; + + double lateral_distance = 0.0; + double longitudinal_distance = 100.0; + double velocity = 10.0; + + // Condition: zero lateral distance + EXPECT_DOUBLE_EQ( + calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.0); + + // Condition: zero velocity + lateral_distance = 5.0; + velocity = 0.0; + EXPECT_DOUBLE_EQ( + calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.0); + + // Condition: zero longitudinal distance + longitudinal_distance = 0.0; + velocity = 10.0; + EXPECT_DOUBLE_EQ( + calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 1.6 * 1e14); + + // Condition: random + longitudinal_distance = 100.0; + EXPECT_DOUBLE_EQ( + calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.16); +} diff --git a/common/object_recognition_utils/CMakeLists.txt b/common/autoware_object_recognition_utils/CMakeLists.txt similarity index 79% rename from common/object_recognition_utils/CMakeLists.txt rename to common/autoware_object_recognition_utils/CMakeLists.txt index 4f05298573c18..9e1276db8d635 100644 --- a/common/object_recognition_utils/CMakeLists.txt +++ b/common/autoware_object_recognition_utils/CMakeLists.txt @@ -1,12 +1,12 @@ cmake_minimum_required(VERSION 3.14) -project(object_recognition_utils) +project(autoware_object_recognition_utils) find_package(autoware_cmake REQUIRED) autoware_package() find_package(Boost REQUIRED) -ament_auto_add_library(object_recognition_utils SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/predicted_path_utils.cpp src/conversion.cpp ) @@ -19,7 +19,7 @@ if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_object_recognition_utils ${test_files}) target_link_libraries(test_object_recognition_utils - object_recognition_utils + ${PROJECT_NAME} ) endif() diff --git a/common/object_recognition_utils/README.md b/common/autoware_object_recognition_utils/README.md similarity index 87% rename from common/object_recognition_utils/README.md rename to common/autoware_object_recognition_utils/README.md index 74a585e2dbd44..8d4eabd19c76d 100644 --- a/common/object_recognition_utils/README.md +++ b/common/autoware_object_recognition_utils/README.md @@ -1,4 +1,4 @@ -# object_recognition_utils +# autoware_object_recognition_utils ## Purpose diff --git a/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/conversion.hpp similarity index 80% rename from common/object_recognition_utils/include/object_recognition_utils/conversion.hpp rename to common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/conversion.hpp index 207f76a1c31a8..5b3be48cc2598 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/conversion.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ -#define OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ +#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ #include #include -namespace object_recognition_utils +namespace autoware::object_recognition_utils { using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; @@ -28,6 +28,6 @@ using autoware_perception_msgs::msg::TrackedObjects; DetectedObject toDetectedObject(const TrackedObject & tracked_object); DetectedObjects toDetectedObjects(const TrackedObjects & tracked_objects); TrackedObject toTrackedObject(const DetectedObject & detected_object); -} // namespace object_recognition_utils +} // namespace autoware::object_recognition_utils -#endif // OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ diff --git a/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/geometry.hpp similarity index 85% rename from common/object_recognition_utils/include/object_recognition_utils/geometry.hpp rename to common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/geometry.hpp index 28cc9a786ecac..a5acc210d92be 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/geometry.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ -#define OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ +#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ #include #include #include #include -namespace object_recognition_utils +namespace autoware::object_recognition_utils { template geometry_msgs::msg::Pose getPose([[maybe_unused]] const T & p) @@ -52,6 +52,6 @@ inline geometry_msgs::msg::Pose getPose(const autoware_perception_msgs::msg::Pre { return obj.kinematics.initial_pose_with_covariance.pose; } -} // namespace object_recognition_utils +} // namespace autoware::object_recognition_utils -#endif // OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ diff --git a/common/object_recognition_utils/include/object_recognition_utils/matching.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/matching.hpp similarity index 93% rename from common/object_recognition_utils/include/object_recognition_utils/matching.hpp rename to common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/matching.hpp index ae50227df1166..59c01c6d873e7 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/matching.hpp +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/matching.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ -#define OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ +#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ +#include "autoware/object_recognition_utils/geometry.hpp" #include "autoware/universe_utils/geometry/boost_geometry.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "object_recognition_utils/geometry.hpp" #include @@ -26,7 +26,7 @@ #include #include -namespace object_recognition_utils +namespace autoware::object_recognition_utils { using autoware::universe_utils::Polygon2d; // minimum area to avoid division by zero @@ -126,6 +126,6 @@ double get2dRecall(const T1 source_object, const T2 target_object) return std::min(1.0, intersection_area / target_area); } -} // namespace object_recognition_utils +} // namespace autoware::object_recognition_utils -#endif // OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ diff --git a/common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_classification.hpp similarity index 94% rename from common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp rename to common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_classification.hpp index 4ffafc22339d6..432abcbe10c55 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_classification.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ -#define OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ +#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ #include "autoware_perception_msgs/msg/object_classification.hpp" #include #include -namespace object_recognition_utils +namespace autoware::object_recognition_utils { using autoware_perception_msgs::msg::ObjectClassification; @@ -169,6 +169,6 @@ inline std::string convertLabelToString( return convertLabelToString(highest_prob_label); } -} // namespace object_recognition_utils +} // namespace autoware::object_recognition_utils -#endif // OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ diff --git a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_recognition_utils.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_recognition_utils.hpp new file mode 100644 index 0000000000000..d73e2e4dc67db --- /dev/null +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_recognition_utils.hpp @@ -0,0 +1,25 @@ +// 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 AUTOWARE__OBJECT_RECOGNITION_UTILS__OBJECT_RECOGNITION_UTILS_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__OBJECT_RECOGNITION_UTILS_HPP_ + +#include "autoware/object_recognition_utils/conversion.hpp" +#include "autoware/object_recognition_utils/geometry.hpp" +#include "autoware/object_recognition_utils/matching.hpp" +#include "autoware/object_recognition_utils/object_classification.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" +#include "autoware/object_recognition_utils/transform.hpp" + +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__OBJECT_RECOGNITION_UTILS_HPP_ diff --git a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/predicted_path_utils.hpp similarity index 88% rename from common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp rename to common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/predicted_path_utils.hpp index 72e70185d553b..836b5296497e8 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/predicted_path_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ -#define OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ +#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ #include "autoware/universe_utils/geometry/geometry.hpp" @@ -24,7 +24,7 @@ #include -namespace object_recognition_utils +namespace autoware::object_recognition_utils { /** * @brief Calculate Interpolated Pose from predicted path by time @@ -61,6 +61,6 @@ autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( const autoware_perception_msgs::msg::PredictedPath & path, const double sampling_time_interval, const double sampling_horizon, const bool use_spline_for_xy = true, const bool use_spline_for_z = false); -} // namespace object_recognition_utils +} // namespace autoware::object_recognition_utils -#endif // OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ diff --git a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/transform.hpp similarity index 95% rename from common/object_recognition_utils/include/object_recognition_utils/transform.hpp rename to common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/transform.hpp index b03270133a52c..ced5956e079e6 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/transform.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ -#define OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ +#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ #include @@ -71,7 +71,7 @@ namespace detail } } // namespace detail -namespace object_recognition_utils +namespace autoware::object_recognition_utils { template bool transformObjects( @@ -147,6 +147,6 @@ bool transformObjectsWithFeature( } return true; } -} // namespace object_recognition_utils +} // namespace autoware::object_recognition_utils -#endif // OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ diff --git a/common/object_recognition_utils/package.xml b/common/autoware_object_recognition_utils/package.xml similarity index 90% rename from common/object_recognition_utils/package.xml rename to common/autoware_object_recognition_utils/package.xml index a3f4a99300716..8fcc19ec7a53c 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/autoware_object_recognition_utils/package.xml @@ -1,9 +1,9 @@ - object_recognition_utils + autoware_object_recognition_utils 0.1.0 - The object_recognition_utils package + The autoware_object_recognition_utils package Takayuki Murooka Shunsuke Miura Yoshi Ri diff --git a/common/object_recognition_utils/src/conversion.cpp b/common/autoware_object_recognition_utils/src/conversion.cpp similarity index 94% rename from common/object_recognition_utils/src/conversion.cpp rename to common/autoware_object_recognition_utils/src/conversion.cpp index c9c057ad93644..6f8e6aa27cfb4 100644 --- a/common/object_recognition_utils/src/conversion.cpp +++ b/common/autoware_object_recognition_utils/src/conversion.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_recognition_utils/conversion.hpp" +#include "autoware/object_recognition_utils/conversion.hpp" -namespace object_recognition_utils +namespace autoware::object_recognition_utils { using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; @@ -68,4 +68,4 @@ TrackedObject toTrackedObject(const DetectedObject & detected_object) tracked_object.shape = detected_object.shape; return tracked_object; } -} // namespace object_recognition_utils +} // namespace autoware::object_recognition_utils diff --git a/common/object_recognition_utils/src/predicted_path_utils.cpp b/common/autoware_object_recognition_utils/src/predicted_path_utils.cpp similarity index 96% rename from common/object_recognition_utils/src/predicted_path_utils.cpp rename to common/autoware_object_recognition_utils/src/predicted_path_utils.cpp index 96c62d9f50323..196f82b69161a 100644 --- a/common/object_recognition_utils/src/predicted_path_utils.cpp +++ b/common/autoware_object_recognition_utils/src/predicted_path_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_recognition_utils/predicted_path_utils.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/interpolation/spherical_linear_interpolation.hpp" @@ -20,7 +20,7 @@ #include -namespace object_recognition_utils +namespace autoware::object_recognition_utils { boost::optional calcInterpolatedPose( const autoware_perception_msgs::msg::PredictedPath & path, const double relative_time) @@ -129,4 +129,4 @@ autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( resampled_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval); return resampled_path; } -} // namespace object_recognition_utils +} // namespace autoware::object_recognition_utils diff --git a/common/object_recognition_utils/test/src/test_conversion.cpp b/common/autoware_object_recognition_utils/test/src/test_conversion.cpp similarity index 98% rename from common/object_recognition_utils/test/src/test_conversion.cpp rename to common/autoware_object_recognition_utils/test/src/test_conversion.cpp index a9b462f9ec4c4..6a05771149ad2 100644 --- a/common/object_recognition_utils/test/src/test_conversion.cpp +++ b/common/autoware_object_recognition_utils/test/src/test_conversion.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/object_recognition_utils/conversion.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "object_recognition_utils/conversion.hpp" #include @@ -42,8 +42,8 @@ autoware_perception_msgs::msg::ObjectClassification createObjectClassification( // NOTE: covariance is not checked TEST(conversion, test_toDetectedObject) { + using autoware::object_recognition_utils::toDetectedObject; using autoware_perception_msgs::msg::ObjectClassification; - using object_recognition_utils::toDetectedObject; autoware_perception_msgs::msg::TrackedObject tracked_obj; // existence probability @@ -160,8 +160,8 @@ TEST(conversion, test_toDetectedObject) // NOTE: covariance is not checked TEST(conversion, test_toTrackedObject) { + using autoware::object_recognition_utils::toTrackedObject; using autoware_perception_msgs::msg::ObjectClassification; - using object_recognition_utils::toTrackedObject; autoware_perception_msgs::msg::DetectedObject detected_obj; // existence probability diff --git a/common/object_recognition_utils/test/src/test_geometry.cpp b/common/autoware_object_recognition_utils/test/src/test_geometry.cpp similarity index 97% rename from common/object_recognition_utils/test/src/test_geometry.cpp rename to common/autoware_object_recognition_utils/test/src/test_geometry.cpp index ab3ba8fcec7d4..f5165482a23bf 100644 --- a/common/object_recognition_utils/test/src/test_geometry.cpp +++ b/common/autoware_object_recognition_utils/test/src/test_geometry.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_recognition_utils/geometry.hpp" +#include "autoware/object_recognition_utils/geometry.hpp" #include @@ -20,7 +20,7 @@ TEST(geometry, getPose) { - using object_recognition_utils::getPose; + using autoware::object_recognition_utils::getPose; const double x_ans = 1.0; const double y_ans = 2.0; diff --git a/common/object_recognition_utils/test/src/test_matching.cpp b/common/autoware_object_recognition_utils/test/src/test_matching.cpp similarity index 97% rename from common/object_recognition_utils/test/src/test_matching.cpp rename to common/autoware_object_recognition_utils/test/src/test_matching.cpp index 6005ac8d1efbc..aefc6251c6276 100644 --- a/common/object_recognition_utils/test/src/test_matching.cpp +++ b/common/autoware_object_recognition_utils/test/src/test_matching.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/object_recognition_utils/matching.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "object_recognition_utils/matching.hpp" #include @@ -37,8 +37,8 @@ geometry_msgs::msg::Pose createPose(const double x, const double y, const double TEST(matching, test_get2dIoU) { + using autoware::object_recognition_utils::get2dIoU; using autoware_perception_msgs::msg::DetectedObject; - using object_recognition_utils::get2dIoU; const double quart_circle = 0.16237976320958225; @@ -93,7 +93,7 @@ TEST(matching, test_get2dIoU) TEST(object_recognition_utils, test_get2dGeneralizedIoU) { - using object_recognition_utils::get2dGeneralizedIoU; + using autoware::object_recognition_utils::get2dGeneralizedIoU; // TODO(Shin-kyoto): // get2dGeneralizedIoU uses outer points of each polygon. // But these points contain an sampling error of outer line, @@ -153,8 +153,8 @@ TEST(object_recognition_utils, test_get2dGeneralizedIoU) TEST(matching, test_get2dPrecision) { + using autoware::object_recognition_utils::get2dPrecision; using autoware_perception_msgs::msg::DetectedObject; - using object_recognition_utils::get2dPrecision; const double quart_circle = 0.16237976320958225; { // non overlapped @@ -220,8 +220,8 @@ TEST(matching, test_get2dPrecision) TEST(matching, test_get2dRecall) { + using autoware::object_recognition_utils::get2dRecall; using autoware_perception_msgs::msg::DetectedObject; - using object_recognition_utils::get2dRecall; const double quart_circle = 0.16237976320958225; { // non overlapped diff --git a/common/object_recognition_utils/test/src/test_object_classification.cpp b/common/autoware_object_recognition_utils/test/src/test_object_classification.cpp similarity index 95% rename from common/object_recognition_utils/test/src/test_object_classification.cpp rename to common/autoware_object_recognition_utils/test/src/test_object_classification.cpp index 697a7e8447732..1482158581a8e 100644 --- a/common/object_recognition_utils/test/src/test_object_classification.cpp +++ b/common/autoware_object_recognition_utils/test/src/test_object_classification.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_recognition_utils/object_classification.hpp" +#include "autoware/object_recognition_utils/object_classification.hpp" #include @@ -34,8 +34,8 @@ autoware_perception_msgs::msg::ObjectClassification createObjectClassification( TEST(object_classification, test_getHighestProbLabel) { + using autoware::object_recognition_utils::getHighestProbLabel; using autoware_perception_msgs::msg::ObjectClassification; - using object_recognition_utils::getHighestProbLabel; { // empty std::vector classifications; @@ -67,8 +67,8 @@ TEST(object_classification, test_getHighestProbLabel) // Test isVehicle TEST(object_classification, test_isVehicle) { + using autoware::object_recognition_utils::isVehicle; using autoware_perception_msgs::msg::ObjectClassification; - using object_recognition_utils::isVehicle; { // True Case with uint8_t EXPECT_TRUE(isVehicle(ObjectClassification::BICYCLE)); @@ -106,8 +106,8 @@ TEST(object_classification, test_isVehicle) // TEST isCarLikeVehicle TEST(object_classification, test_isCarLikeVehicle) { + using autoware::object_recognition_utils::isCarLikeVehicle; using autoware_perception_msgs::msg::ObjectClassification; - using object_recognition_utils::isCarLikeVehicle; { // True Case with uint8_t EXPECT_TRUE(isCarLikeVehicle(ObjectClassification::BUS)); @@ -157,8 +157,8 @@ TEST(object_classification, test_isCarLikeVehicle) // TEST isLargeVehicle TEST(object_classification, test_isLargeVehicle) { + using autoware::object_recognition_utils::isLargeVehicle; using autoware_perception_msgs::msg::ObjectClassification; - using object_recognition_utils::isLargeVehicle; { // True Case with uint8_t EXPECT_TRUE(isLargeVehicle(ObjectClassification::BUS)); @@ -197,8 +197,8 @@ TEST(object_classification, test_isLargeVehicle) TEST(object_classification, test_getHighestProbClassification) { + using autoware::object_recognition_utils::getHighestProbClassification; using autoware_perception_msgs::msg::ObjectClassification; - using object_recognition_utils::getHighestProbClassification; { // empty std::vector classifications; @@ -232,10 +232,10 @@ TEST(object_classification, test_getHighestProbClassification) TEST(object_classification, test_fromString) { + using autoware::object_recognition_utils::toLabel; + using autoware::object_recognition_utils::toObjectClassification; + using autoware::object_recognition_utils::toObjectClassifications; using autoware_perception_msgs::msg::ObjectClassification; - using object_recognition_utils::toLabel; - using object_recognition_utils::toObjectClassification; - using object_recognition_utils::toObjectClassifications; // toLabel { @@ -266,8 +266,8 @@ TEST(object_classification, test_fromString) TEST(object_classification, test_convertLabelToString) { + using autoware::object_recognition_utils::convertLabelToString; using autoware_perception_msgs::msg::ObjectClassification; - using object_recognition_utils::convertLabelToString; // from label { diff --git a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp b/common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp similarity index 96% rename from common/object_recognition_utils/test/src/test_predicted_path_utils.cpp rename to common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp index 305a1173acf12..a93d253bfd052 100644 --- a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp +++ b/common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" + +#include #include @@ -62,10 +64,10 @@ PredictedPath createTestPredictedPath( TEST(predicted_path_utils, testCalcInterpolatedPose) { + using autoware::object_recognition_utils::calcInterpolatedPose; using autoware::universe_utils::createQuaternionFromRPY; using autoware::universe_utils::createQuaternionFromYaw; using autoware::universe_utils::deg2rad; - using object_recognition_utils::calcInterpolatedPose; const auto path = createTestPredictedPath(100, 0.1, 1.0); @@ -75,7 +77,7 @@ TEST(predicted_path_utils, testCalcInterpolatedPose) for (double t = 0.0; t < 9.0 + 1e-6; t += 1.0) { const auto p = calcInterpolatedPose(path, t); - EXPECT_NE(p, boost::none); + EXPECT_TRUE(p); EXPECT_NEAR(p->position.x, t * 1.0, epsilon); EXPECT_NEAR(p->position.y, 0.0, epsilon); EXPECT_NEAR(p->position.z, 0.0, epsilon); @@ -92,7 +94,7 @@ TEST(predicted_path_utils, testCalcInterpolatedPose) for (double t = 0.0; t < 9.0; t += 0.3) { const auto p = calcInterpolatedPose(path, t); - EXPECT_NE(p, boost::none); + EXPECT_TRUE(p); EXPECT_NEAR(p->position.x, t * 1.0, epsilon); EXPECT_NEAR(p->position.y, 0.0, epsilon); EXPECT_NEAR(p->position.z, 0.0, epsilon); @@ -108,30 +110,30 @@ TEST(predicted_path_utils, testCalcInterpolatedPose) // Negative time { const auto p = calcInterpolatedPose(path, -1.0); - EXPECT_EQ(p, boost::none); + EXPECT_FALSE(p); } // Over the time horizon { const auto p = calcInterpolatedPose(path, 11.0); - EXPECT_EQ(p, boost::none); + EXPECT_FALSE(p); } // Empty Path { PredictedPath empty_path; const auto p = calcInterpolatedPose(empty_path, 5.0); - EXPECT_EQ(p, boost::none); + EXPECT_FALSE(p); } } } TEST(predicted_path_utils, resamplePredictedPath_by_vector) { + using autoware::object_recognition_utils::resamplePredictedPath; using autoware::universe_utils::createQuaternionFromRPY; using autoware::universe_utils::createQuaternionFromYaw; using autoware::universe_utils::deg2rad; - using object_recognition_utils::resamplePredictedPath; const auto path = createTestPredictedPath(10, 1.0, 1.0); @@ -205,10 +207,10 @@ TEST(predicted_path_utils, resamplePredictedPath_by_vector) TEST(predicted_path_utils, resamplePredictedPath_by_sampling_time) { + using autoware::object_recognition_utils::resamplePredictedPath; using autoware::universe_utils::createQuaternionFromRPY; using autoware::universe_utils::createQuaternionFromYaw; using autoware::universe_utils::deg2rad; - using object_recognition_utils::resamplePredictedPath; const auto path = createTestPredictedPath(10, 1.0, 1.0); diff --git a/common/osqp_interface/CMakeLists.txt b/common/autoware_osqp_interface/CMakeLists.txt similarity index 78% rename from common/osqp_interface/CMakeLists.txt rename to common/autoware_osqp_interface/CMakeLists.txt index 5a4231c15e8fd..e9ed0ce25f2f8 100644 --- a/common/osqp_interface/CMakeLists.txt +++ b/common/autoware_osqp_interface/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(osqp_interface) +project(autoware_osqp_interface) find_package(autoware_cmake REQUIRED) autoware_package() @@ -17,9 +17,9 @@ set(OSQP_INTERFACE_LIB_SRC ) set(OSQP_INTERFACE_LIB_HEADERS - include/osqp_interface/csc_matrix_conv.hpp - include/osqp_interface/osqp_interface.hpp - include/osqp_interface/visibility_control.hpp + 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 @@ -28,18 +28,18 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast -Wno-error=useless-cast) -target_include_directories(osqp_interface +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC "${OSQP_INCLUDE_DIR}" "${EIGEN3_INCLUDE_DIR}" ) -ament_target_dependencies(osqp_interface +ament_target_dependencies(${PROJECT_NAME} Eigen3 osqp_vendor ) -# crucial so downstream package builds because osqp_interface exposes osqp.hpp +# 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) diff --git a/common/osqp_interface/design/osqp_interface-design.md b/common/autoware_osqp_interface/design/osqp_interface-design.md similarity index 96% rename from common/osqp_interface/design/osqp_interface-design.md rename to common/autoware_osqp_interface/design/osqp_interface-design.md index e9ff4a2a3bc3a..887a4b4d9af3f 100644 --- a/common/osqp_interface/design/osqp_interface-design.md +++ b/common/autoware_osqp_interface/design/osqp_interface-design.md @@ -1,6 +1,6 @@ # Interface for the OSQP library -This is the design document for the `osqp_interface` package. +This is the design document for the `autoware_osqp_interface` package. ## Purpose / Use cases diff --git a/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp similarity index 83% rename from common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp rename to common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp index a919bc1f1c038..8c21553152d70 100644 --- a/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp +++ b/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp @@ -12,21 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ -#define OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ +#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 "osqp_interface/visibility_control.hpp" #include #include -namespace autoware -{ -namespace common -{ -namespace osqp +namespace autoware::osqp_interface { /// \brief Compressed-Column-Sparse Matrix struct OSQP_INTERFACE_PUBLIC CSC_Matrix @@ -46,8 +42,6 @@ OSQP_INTERFACE_PUBLIC CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & /// \brief Print the given CSC matrix to the standard output OSQP_INTERFACE_PUBLIC void printCSCMatrix(const CSC_Matrix & csc_mat); -} // namespace osqp -} // namespace common -} // namespace autoware +} // namespace autoware::osqp_interface -#endif // OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ +#endif // AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ diff --git a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp similarity index 96% rename from common/osqp_interface/include/osqp_interface/osqp_interface.hpp rename to common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp index f126ba9329d3e..ff3013bd61514 100644 --- a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp +++ b/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OSQP_INTERFACE__OSQP_INTERFACE_HPP_ -#define OSQP_INTERFACE__OSQP_INTERFACE_HPP_ +#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 "osqp_interface/csc_matrix_conv.hpp" -#include "osqp_interface/visibility_control.hpp" #include #include @@ -28,11 +28,7 @@ #include #include -namespace autoware -{ -namespace common -{ -namespace osqp +namespace autoware::osqp_interface { constexpr c_float INF = 1e30; @@ -193,8 +189,6 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface void logUnsolvedStatus(const std::string & prefix_message = "") const; }; -} // namespace osqp -} // namespace common -} // namespace autoware +} // namespace autoware::osqp_interface -#endif // OSQP_INTERFACE__OSQP_INTERFACE_HPP_ +#endif // AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/osqp_interface/include/osqp_interface/visibility_control.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp similarity index 89% rename from common/osqp_interface/include/osqp_interface/visibility_control.hpp rename to common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp index 070a5c5f1542c..a6cdaa8a0a74e 100644 --- a/common/osqp_interface/include/osqp_interface/visibility_control.hpp +++ b/common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ -#define OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ +#ifndef AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ //////////////////////////////////////////////////////////////////////////////// #if defined(__WIN32) @@ -34,4 +34,4 @@ #error "Unsupported Build Configuration" #endif -#endif // OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ +#endif // AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ diff --git a/common/osqp_interface/package.xml b/common/autoware_osqp_interface/package.xml similarity index 96% rename from common/osqp_interface/package.xml rename to common/autoware_osqp_interface/package.xml index 41ee5bb9055a6..d49ce63bd8c93 100644 --- a/common/osqp_interface/package.xml +++ b/common/autoware_osqp_interface/package.xml @@ -1,7 +1,7 @@ - osqp_interface + autoware_osqp_interface 1.0.0 Interface for the OSQP solver Maxime CLEMENT diff --git a/common/osqp_interface/src/csc_matrix_conv.cpp b/common/autoware_osqp_interface/src/csc_matrix_conv.cpp similarity index 94% rename from common/osqp_interface/src/csc_matrix_conv.cpp rename to common/autoware_osqp_interface/src/csc_matrix_conv.cpp index 1944face4516b..c6e1a0a42d938 100644 --- a/common/osqp_interface/src/csc_matrix_conv.cpp +++ b/common/autoware_osqp_interface/src/csc_matrix_conv.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "osqp_interface/csc_matrix_conv.hpp" +#include "autoware/osqp_interface/csc_matrix_conv.hpp" #include #include @@ -21,11 +21,7 @@ #include #include -namespace autoware -{ -namespace common -{ -namespace osqp +namespace autoware::osqp_interface { CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) { @@ -136,6 +132,4 @@ void printCSCMatrix(const CSC_Matrix & csc_mat) std::cout << "]\n"; } -} // namespace osqp -} // namespace common -} // namespace autoware +} // namespace autoware::osqp_interface diff --git a/common/osqp_interface/src/osqp_interface.cpp b/common/autoware_osqp_interface/src/osqp_interface.cpp similarity index 98% rename from common/osqp_interface/src/osqp_interface.cpp rename to common/autoware_osqp_interface/src/osqp_interface.cpp index f0dbc3ab22e4a..9ebf132f65028 100644 --- a/common/osqp_interface/src/osqp_interface.cpp +++ b/common/autoware_osqp_interface/src/osqp_interface.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "osqp_interface/osqp_interface.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" +#include "autoware/osqp_interface/csc_matrix_conv.hpp" #include "osqp/osqp.h" -#include "osqp_interface/csc_matrix_conv.hpp" #include #include @@ -25,11 +25,7 @@ #include #include -namespace autoware -{ -namespace common -{ -namespace osqp +namespace autoware::osqp_interface { OSQPInterface::OSQPInterface(const c_float eps_abs, const bool polish) : m_work{nullptr, OSQPWorkspaceDeleter} @@ -436,6 +432,4 @@ void OSQPInterface::logUnsolvedStatus(const std::string & prefix_message) const // log with warning RCLCPP_WARN(rclcpp::get_logger("osqp_interface"), output_message.c_str()); } -} // namespace osqp -} // namespace common -} // namespace autoware +} // namespace autoware::osqp_interface diff --git a/common/osqp_interface/test/test_csc_matrix_conv.cpp b/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp similarity index 93% rename from common/osqp_interface/test/test_csc_matrix_conv.cpp rename to common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp index 765f1a1afed3b..a63f979a966bf 100644 --- a/common/osqp_interface/test/test_csc_matrix_conv.cpp +++ b/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp @@ -12,8 +12,8 @@ // 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 "osqp_interface/csc_matrix_conv.hpp" #include @@ -23,8 +23,8 @@ TEST(TestCscMatrixConv, Nominal) { - using autoware::common::osqp::calCSCMatrix; - using autoware::common::osqp::CSC_Matrix; + using autoware::osqp_interface::calCSCMatrix; + using autoware::osqp_interface::CSC_Matrix; Eigen::MatrixXd rect1(1, 2); rect1 << 0.0, 1.0; @@ -117,8 +117,8 @@ TEST(TestCscMatrixConv, Nominal) } TEST(TestCscMatrixConv, Trapezoidal) { - using autoware::common::osqp::calCSCMatrixTrapezoidal; - using autoware::common::osqp::CSC_Matrix; + using autoware::osqp_interface::calCSCMatrixTrapezoidal; + using autoware::osqp_interface::CSC_Matrix; Eigen::MatrixXd square1(2, 2); Eigen::MatrixXd square2(3, 3); @@ -166,10 +166,10 @@ TEST(TestCscMatrixConv, Trapezoidal) } TEST(TestCscMatrixConv, Print) { - using autoware::common::osqp::calCSCMatrix; - using autoware::common::osqp::calCSCMatrixTrapezoidal; - using autoware::common::osqp::CSC_Matrix; - using autoware::common::osqp::printCSCMatrix; + 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; diff --git a/common/osqp_interface/test/test_osqp_interface.cpp b/common/autoware_osqp_interface/test/test_osqp_interface.cpp similarity index 85% rename from common/osqp_interface/test/test_osqp_interface.cpp rename to common/autoware_osqp_interface/test/test_osqp_interface.cpp index 4644ec6c3e019..d03713f82566f 100644 --- a/common/osqp_interface/test/test_osqp_interface.cpp +++ b/common/autoware_osqp_interface/test/test_osqp_interface.cpp @@ -12,8 +12,8 @@ // 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 "osqp_interface/osqp_interface.hpp" #include @@ -39,9 +39,9 @@ namespace TEST(TestOsqpInterface, BasicQp) { - using autoware::common::osqp::calCSCMatrix; - using autoware::common::osqp::calCSCMatrixTrapezoidal; - using autoware::common::osqp::CSC_Matrix; + 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) { @@ -66,12 +66,12 @@ TEST(TestOsqpInterface, BasicQp) 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::common::osqp::INF}; - const std::vector u = {1.0, 0.7, 0.7, autoware::common::osqp::INF}; + 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::common::osqp::OSQPInterface osqp; + autoware::osqp_interface::OSQPInterface osqp; std::tuple, std::vector, int, int, int> result = osqp.optimize(P, A, q, l, u); check_result(result); @@ -79,7 +79,7 @@ TEST(TestOsqpInterface, BasicQp) { // Define problem during initialization - autoware::common::osqp::OSQPInterface osqp(P, A, q, l, u, 1e-6); + 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); } @@ -92,7 +92,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - autoware::common::osqp::OSQPInterface osqp(P_ini, A_ini, q_ini, l_ini, u_ini, 1e-6); + autoware::osqp_interface::OSQPInterface osqp(P_ini, A_ini, q_ini, l_ini, u_ini, 1e-6); osqp.optimize(); // Redefine problem before optimization @@ -105,7 +105,7 @@ TEST(TestOsqpInterface, BasicQp) // Define problem during initialization with csc matrix CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); CSC_Matrix A_csc = calCSCMatrix(A); - autoware::common::osqp::OSQPInterface osqp(P_csc, A_csc, q, l, u, 1e-6); + 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); } @@ -118,7 +118,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - autoware::common::osqp::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); + 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 @@ -138,7 +138,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - autoware::common::osqp::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); + 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 diff --git a/common/polar_grid/CMakeLists.txt b/common/autoware_polar_grid/CMakeLists.txt similarity index 85% rename from common/polar_grid/CMakeLists.txt rename to common/autoware_polar_grid/CMakeLists.txt index 9932dc594d552..efa068a3f64a4 100644 --- a/common/polar_grid/CMakeLists.txt +++ b/common/autoware_polar_grid/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(polar_grid) +project(autoware_polar_grid) find_package(autoware_cmake REQUIRED) autoware_package() @@ -10,12 +10,12 @@ set(CMAKE_AUTOMOC ON) set(CMAKE_INCLUDE_CURRENT_DIR ON) add_definitions(-DQT_NO_KEYWORDS) -ament_auto_add_library(polar_grid SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/polar_grid_display.cpp src/polar_grid_display.hpp ) -target_link_libraries(polar_grid +target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ) diff --git a/common/polar_grid/Readme.md b/common/autoware_polar_grid/Readme.md similarity index 100% rename from common/polar_grid/Readme.md rename to common/autoware_polar_grid/Readme.md diff --git a/common/polar_grid/icons/classes/PolarGridDisplay.png b/common/autoware_polar_grid/icons/classes/PolarGridDisplay.png similarity index 100% rename from common/polar_grid/icons/classes/PolarGridDisplay.png rename to common/autoware_polar_grid/icons/classes/PolarGridDisplay.png diff --git a/common/polar_grid/package.xml b/common/autoware_polar_grid/package.xml similarity index 90% rename from common/polar_grid/package.xml rename to common/autoware_polar_grid/package.xml index ece156669c85b..3b376b3a75185 100644 --- a/common/polar_grid/package.xml +++ b/common/autoware_polar_grid/package.xml @@ -1,9 +1,9 @@ - polar_grid + autoware_polar_grid 0.1.0 - The polar_grid package + The autoware_polar_grid package Yukihiro Saito Apache License 2.0 diff --git a/common/polar_grid/plugins/plugin_description.xml b/common/autoware_polar_grid/plugins/plugin_description.xml similarity index 81% rename from common/polar_grid/plugins/plugin_description.xml rename to common/autoware_polar_grid/plugins/plugin_description.xml index 1ccaf8725d4de..a09fb5ed6a5d4 100644 --- a/common/polar_grid/plugins/plugin_description.xml +++ b/common/autoware_polar_grid/plugins/plugin_description.xml @@ -1,4 +1,4 @@ - + diff --git a/common/polar_grid/src/polar_grid_display.cpp b/common/autoware_polar_grid/src/polar_grid_display.cpp similarity index 100% rename from common/polar_grid/src/polar_grid_display.cpp rename to common/autoware_polar_grid/src/polar_grid_display.cpp diff --git a/common/polar_grid/src/polar_grid_display.hpp b/common/autoware_polar_grid/src/polar_grid_display.hpp similarity index 100% rename from common/polar_grid/src/polar_grid_display.hpp rename to common/autoware_polar_grid/src/polar_grid_display.hpp diff --git a/common/autoware_signal_processing/test/src/lowpass_filter_1d_test.cpp b/common/autoware_signal_processing/test/src/lowpass_filter_1d_test.cpp index 44e8a1ffce437..ba43219f325ff 100644 --- a/common/autoware_signal_processing/test/src/lowpass_filter_1d_test.cpp +++ b/common/autoware_signal_processing/test/src/lowpass_filter_1d_test.cpp @@ -14,6 +14,8 @@ #include "autoware/signal_processing/lowpass_filter_1d.hpp" +#include + #include constexpr double epsilon = 1e-6; @@ -25,7 +27,7 @@ TEST(lowpass_filter_1d, filter) LowpassFilter1d lowpass_filter_1d(0.1); // initial state - EXPECT_EQ(lowpass_filter_1d.getValue(), boost::none); + EXPECT_FALSE(lowpass_filter_1d.getValue()); // random filter EXPECT_NEAR(lowpass_filter_1d.filter(0.0), 0.0, epsilon); @@ -35,7 +37,7 @@ TEST(lowpass_filter_1d, filter) // reset without value lowpass_filter_1d.reset(); - EXPECT_EQ(lowpass_filter_1d.getValue(), boost::none); + EXPECT_FALSE(lowpass_filter_1d.getValue()); // reset with value lowpass_filter_1d.reset(-1.1); diff --git a/common/autoware_signal_processing/test/src/lowpass_filter_test.cpp b/common/autoware_signal_processing/test/src/lowpass_filter_test.cpp index 864378719dd5c..83ccc0f5dab03 100644 --- a/common/autoware_signal_processing/test/src/lowpass_filter_test.cpp +++ b/common/autoware_signal_processing/test/src/lowpass_filter_test.cpp @@ -14,6 +14,8 @@ #include "autoware/signal_processing/lowpass_filter.hpp" +#include + #include constexpr double epsilon = 1e-6; @@ -41,7 +43,7 @@ TEST(lowpass_filter_twist, filter) LowpassFilterTwist lowpass_filter_(0.1); { // initial state - EXPECT_EQ(lowpass_filter_.getValue(), boost::none); + EXPECT_FALSE(lowpass_filter_.getValue()); } { // random filter @@ -58,7 +60,7 @@ TEST(lowpass_filter_twist, filter) { // reset without value lowpass_filter_.reset(); - EXPECT_EQ(lowpass_filter_.getValue(), boost::none); + EXPECT_FALSE(lowpass_filter_.getValue()); } { // reset with value diff --git a/common/object_recognition_utils/include/object_recognition_utils/object_recognition_utils.hpp b/common/object_recognition_utils/include/object_recognition_utils/object_recognition_utils.hpp deleted file mode 100644 index 4a4ef4f246453..0000000000000 --- a/common/object_recognition_utils/include/object_recognition_utils/object_recognition_utils.hpp +++ /dev/null @@ -1,25 +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 OBJECT_RECOGNITION_UTILS__OBJECT_RECOGNITION_UTILS_HPP_ -#define OBJECT_RECOGNITION_UTILS__OBJECT_RECOGNITION_UTILS_HPP_ - -#include "object_recognition_utils/conversion.hpp" -#include "object_recognition_utils/geometry.hpp" -#include "object_recognition_utils/matching.hpp" -#include "object_recognition_utils/object_classification.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" -#include "object_recognition_utils/transform.hpp" - -#endif // OBJECT_RECOGNITION_UTILS__OBJECT_RECOGNITION_UTILS_HPP_ diff --git a/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp b/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp index 6691c1fb9e97d..9e11f5f220492 100644 --- a/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp +++ b/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp @@ -184,12 +184,21 @@ class TrtCommon // NOLINT */ void setup(); +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8500 + void setupBindings(std::vector & bindings); +#endif + bool isInitialized(); 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 + 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 diff --git a/common/tensorrt_common/package.xml b/common/tensorrt_common/package.xml index f5a3896b55881..f124dc4c29dc6 100644 --- a/common/tensorrt_common/package.xml +++ b/common/tensorrt_common/package.xml @@ -8,6 +8,8 @@ Daisuke Nishimatsu Dan Umeda Manato Hirabayashi + Amadeusz Szymko + Kenzo Lobos-Tsunekawa Apache License 2.0 diff --git a/common/tensorrt_common/src/tensorrt_common.cpp b/common/tensorrt_common/src/tensorrt_common.cpp index 74fd331b40ae7..9024207d9fe50 100644 --- a/common/tensorrt_common/src/tensorrt_common.cpp +++ b/common/tensorrt_common/src/tensorrt_common.cpp @@ -225,6 +225,16 @@ void TrtCommon::setup() is_initialized_ = true; } +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8500 +void TrtCommon::setupBindings(std::vector & bindings) +{ + for (int32_t i = 0, e = engine_->getNbIOTensors(); i < e; i++) { + auto const name = engine_->getIOTensorName(i); + context_->setTensorAddress(name, bindings.at(i)); + } +} +#endif + bool TrtCommon::loadEngine(const std::string & engine_file_path) { std::ifstream engine_file(engine_file_path); @@ -303,8 +313,7 @@ void TrtCommon::printNetworkInfo(const std::string & onnx_file_path) total_gflops += gflops; total_params += num_weights; std::cout << "L" << i << " [conv " << k_dims.d[0] << "x" << k_dims.d[1] << " (" << groups - << ") " - << "/" << s_dims.d[0] << "] " << dim_in.d[3] << "x" << dim_in.d[2] << "x" + << ") " << "/" << s_dims.d[0] << "] " << dim_in.d[3] << "x" << dim_in.d[2] << "x" << dim_in.d[1] << " -> " << dim_out.d[3] << "x" << dim_out.d[2] << "x" << dim_out.d[1]; std::cout << " weights:" << num_weights; @@ -369,8 +378,7 @@ bool TrtCommon::buildEngineFromOnnx( if (num_available_dla > 0) { std::cout << "###" << num_available_dla << " DLAs are supported! ###" << std::endl; } else { - std::cout << "###Warning : " - << "No DLA is supported! ###" << std::endl; + std::cout << "###Warning : " << "No DLA is supported! ###" << std::endl; } config->setDefaultDeviceType(nvinfer1::DeviceType::kDLA); config->setDLACore(build_config_->dla_core_id); @@ -567,6 +575,24 @@ bool TrtCommon::setBindingDimensions(const int32_t index, const nvinfer1::Dims & return context_->setBindingDimensions(index, dimensions); } +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8500 +bool TrtCommon::enqueueV3(cudaStream_t stream) +{ + if (build_config_->profile_per_layer) { + auto inference_start = std::chrono::high_resolution_clock::now(); + + bool ret = context_->enqueueV3(stream); + + auto inference_end = std::chrono::high_resolution_clock::now(); + host_profiler_.reportLayerTime( + "inference", + std::chrono::duration(inference_end - inference_start).count()); + return ret; + } + 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) { @@ -583,6 +609,7 @@ bool TrtCommon::enqueueV2(void ** bindings, cudaStream_t stream, cudaEvent_t * i return context_->enqueueV2(bindings, stream, input_consumed); } } +#endif void TrtCommon::printProfiling() { diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp index 25f890799465d..1fcdacef17697 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -60,7 +61,10 @@ std::unique_ptr gradation( } std::unique_ptr setColorDependsOnVelocity( - const double vel_max, const double cmd_vel) + const double vel_max, const double cmd_vel, + const rviz_common::properties::ColorProperty & color_min, + const rviz_common::properties::ColorProperty & color_mid, + const rviz_common::properties::ColorProperty & color_max) { const double cmd_vel_abs = std::fabs(cmd_vel); const double vel_min = 0.0; @@ -68,16 +72,17 @@ std::unique_ptr setColorDependsOnVelocity( std::unique_ptr color_ptr(new Ogre::ColourValue()); if (vel_min < cmd_vel_abs && cmd_vel_abs <= (vel_max / 2.0)) { double ratio = (cmd_vel_abs - vel_min) / (vel_max / 2.0 - vel_min); - color_ptr = gradation(Qt::red, Qt::yellow, ratio); + color_ptr = gradation(color_min.getColor(), color_mid.getColor(), ratio); } else if ((vel_max / 2.0) < cmd_vel_abs && cmd_vel_abs <= vel_max) { double ratio = (cmd_vel_abs - vel_max / 2.0) / (vel_max - vel_max / 2.0); - color_ptr = gradation(Qt::yellow, Qt::green, ratio); + color_ptr = gradation(color_mid.getColor(), color_max.getColor(), ratio); } else if (vel_max < cmd_vel_abs) { - *color_ptr = Ogre::ColourValue::Green; + // Use max color when velocity exceeds max + *color_ptr = rviz_common::properties::qtToOgre(color_max.getColor()); } else { - *color_ptr = Ogre::ColourValue::Red; + // Use min color when velocity is below min + *color_ptr = rviz_common::properties::qtToOgre(color_min.getColor()); } - return color_ptr; } @@ -109,8 +114,10 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay property_path_width_view_{"Constant Width", false, "", &property_path_view_}, property_path_width_{"Width", 2.0, "", &property_path_view_}, property_path_alpha_{"Alpha", 1.0, "", &property_path_view_}, - property_path_color_view_{"Constant Color", false, "", &property_path_view_}, - property_path_color_{"Color", Qt::black, "", &property_path_view_}, + property_min_color_("Min Velocity Color", QColor("#3F2EE3"), "", &property_path_view_), + property_mid_color_("Mid Velocity Color", QColor("#208AAE"), "", &property_path_view_), + property_max_color_("Max Velocity Color", QColor("#00E678"), "", &property_path_view_), + property_fade_out_distance_{"Fade Out Distance", 0.0, "[m]", &property_path_view_}, property_vel_max_{"Color Border Vel Max", 3.0, "[m/s]", this}, // velocity property_velocity_view_{"View Velocity", true, "", this}, @@ -356,6 +363,34 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay const float right = property_path_width_view_.getBool() ? property_path_width_.getFloat() / 2.0 : info->width / 2.0; + // Initialize alphas with the default alpha value + std::vector alphas(msg_ptr->points.size(), property_path_alpha_.getFloat()); + + // Backward iteration to adjust alpha values for the last x meters + if (property_fade_out_distance_.getFloat() > std::numeric_limits::epsilon()) { + alphas.back() = 0.0f; + float cumulative_distance = 0.0f; + for (size_t point_idx = msg_ptr->points.size() - 1; point_idx > 0; point_idx--) { + const auto & curr_point = autoware::universe_utils::getPose(msg_ptr->points.at(point_idx)); + const auto & prev_point = + autoware::universe_utils::getPose(msg_ptr->points.at(point_idx - 1)); + float distance = std::sqrt(autoware::universe_utils::calcSquaredDistance2d( + prev_point.position, curr_point.position)); + cumulative_distance += distance; + + if (cumulative_distance <= property_fade_out_distance_.getFloat()) { + auto ratio = + static_cast(cumulative_distance / property_fade_out_distance_.getFloat()); + float alpha = property_path_alpha_.getFloat() * ratio; + alphas.at(point_idx - 1) = alpha; + } else { + // If the distance exceeds the fade out distance, break the loop + break; + } + } + } + + // Forward iteration to visualize path for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { const auto & path_point = msg_ptr->points.at(point_idx); const auto & pose = autoware::universe_utils::getPose(path_point); @@ -364,15 +399,14 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay // path if (property_path_view_.getBool()) { Ogre::ColourValue color; - if (property_path_color_view_.getBool()) { - color = rviz_common::properties::qtToOgre(property_path_color_.getColor()); - } else { - // color change depending on velocity - std::unique_ptr dynamic_color_ptr = - setColorDependsOnVelocity(property_vel_max_.getFloat(), velocity); - color = *dynamic_color_ptr; - } - color.a = property_path_alpha_.getFloat(); + + // color change depending on velocity + std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( + property_vel_max_.getFloat(), velocity, property_min_color_, property_mid_color_, + property_max_color_); + color = *dynamic_color_ptr; + color.a = alphas.at(point_idx); + Eigen::Vector3f vec_in; Eigen::Vector3f vec_out; Eigen::Quaternionf quat_yaw_reverse(0, 0, 0, 1); @@ -413,8 +447,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay color = rviz_common::properties::qtToOgre(property_velocity_color_.getColor()); } else { /* color change depending on velocity */ - std::unique_ptr dynamic_color_ptr = - setColorDependsOnVelocity(property_vel_max_.getFloat(), velocity); + std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( + property_vel_max_.getFloat(), velocity, property_min_color_, property_mid_color_, + property_max_color_); color = *dynamic_color_ptr; } color.a = property_velocity_alpha_.getFloat(); @@ -616,8 +651,13 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay rviz_common::properties::BoolProperty property_path_width_view_; rviz_common::properties::FloatProperty property_path_width_; rviz_common::properties::FloatProperty property_path_alpha_; - rviz_common::properties::BoolProperty property_path_color_view_; - rviz_common::properties::ColorProperty property_path_color_; + // Gradient points for velocity color + rviz_common::properties::ColorProperty property_min_color_; + rviz_common::properties::ColorProperty property_mid_color_; + rviz_common::properties::ColorProperty property_max_color_; + // Last x meters of the path will fade out to transparent + rviz_common::properties::FloatProperty property_fade_out_distance_; + rviz_common::properties::FloatProperty property_vel_max_; rviz_common::properties::BoolProperty property_velocity_view_; rviz_common::properties::FloatProperty property_velocity_alpha_; diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 214a6dc309210..32bedd15c5c53 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -341,7 +341,7 @@ class AEB : public rclcpp::Node // publisher rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; rclcpp::Publisher::SharedPtr debug_marker_publisher_; - rclcpp::Publisher::SharedPtr info_marker_publisher_; + rclcpp::Publisher::SharedPtr virtual_wall_publisher_; rclcpp::Publisher::SharedPtr debug_processing_time_detail_pub_; rclcpp::Publisher::SharedPtr debug_rss_distance_publisher_; diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index b3988ba238fe7..df78ffc156802 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -138,7 +138,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) pub_obstacle_pointcloud_ = this->create_publisher("~/debug/obstacle_pointcloud", 1); debug_marker_publisher_ = this->create_publisher("~/debug/markers", 1); - info_marker_publisher_ = this->create_publisher("~/info/markers", 1); + virtual_wall_publisher_ = this->create_publisher("~/virtual_wall", 1); debug_rss_distance_publisher_ = this->create_publisher("~/debug/rss_distance", 1); } @@ -398,7 +398,7 @@ bool AEB::fetchLatestData() void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) { MarkerArray debug_markers; - MarkerArray info_markers; + MarkerArray virtual_wall_marker; checkCollision(debug_markers); if (!collision_data_keeper_.checkCollisionExpired()) { @@ -414,7 +414,7 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) addCollisionMarker(data.value(), debug_markers); } } - addVirtualStopWallMarker(info_markers); + addVirtualStopWallMarker(virtual_wall_marker); } else { const std::string error_msg = "[AEB]: No Collision"; const auto diag_level = DiagnosticStatus::OK; @@ -423,7 +423,7 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) // publish debug markers debug_marker_publisher_->publish(debug_markers); - info_marker_publisher_->publish(info_markers); + virtual_wall_publisher_->publish(virtual_wall_marker); } bool AEB::checkCollision(MarkerArray & debug_markers) 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 56a1e39391b10..551ee08aa3327 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 @@ -20,12 +20,14 @@ #include "autoware_vehicle_info_utils/vehicle_info.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" +#include #include #include #include #include #include +#include #include #include @@ -143,6 +145,7 @@ 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_; // system parameters int64_t diag_error_count_threshold_ = 0; @@ -167,6 +170,8 @@ class ControlValidator : public rclcpp::Node Odometry::ConstSharedPtr current_kinematics_; + autoware::universe_utils::StopWatch stop_watch; + std::shared_ptr debug_pose_publisher_; }; } // namespace autoware::control_validator diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp index 2370d274fecc8..d601319d47bc5 100644 --- a/control/autoware_control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -47,6 +47,9 @@ ControlValidator::ControlValidator(const rclcpp::NodeOptions & options) pub_markers_ = create_publisher("~/output/markers", 1); + pub_processing_time_ = + this->create_publisher("~/debug/processing_time_ms", 1); + debug_pose_publisher_ = std::make_shared(this); setup_parameters(); @@ -134,6 +137,8 @@ bool ControlValidator::is_data_ready() void ControlValidator::on_predicted_trajectory(const Trajectory::ConstSharedPtr msg) { + stop_watch.tic(); + current_predicted_trajectory_ = msg; current_reference_trajectory_ = sub_reference_traj_->takeData(); current_kinematics_ = sub_kinematics_->takeData(); @@ -162,6 +167,12 @@ void ControlValidator::publish_debug_info() debug_pose_publisher_->push_warning_msg(front_pose, "INVALID CONTROL"); } debug_pose_publisher_->publish(); + + // Publish ProcessingTime + tier4_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); } void ControlValidator::validate( diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp index 1e1295cb06b1e..ab9b3392449ee 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ #include "autoware/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" -#include "osqp_interface/osqp_interface.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" namespace autoware::motion::control::mpc_lateral_controller @@ -58,7 +58,7 @@ class QPSolverOSQP : public QPSolverInterface double getObjVal() const override { return osqpsolver_.getObjVal(); } private: - autoware::common::osqp::OSQPInterface osqpsolver_; + autoware::osqp_interface::OSQPInterface osqpsolver_; rclcpp::Logger logger_; }; } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/autoware_mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml index 5c5f8886d6ed3..ee9ede3598256 100644 --- a/control/autoware_mpc_lateral_controller/package.xml +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -20,6 +20,7 @@ autoware_control_msgs autoware_interpolation autoware_motion_utils + autoware_osqp_interface autoware_planning_msgs autoware_trajectory_follower_base autoware_universe_utils @@ -29,7 +30,6 @@ diagnostic_updater eigen geometry_msgs - osqp_interface rclcpp rclcpp_components std_msgs diff --git a/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index 32d97a783627c..c0052d4ff1b3e 100644 --- a/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -83,29 +83,25 @@ MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInWorldCoordin // create initial state in the world coordinate Eigen::VectorXd state_w = [&]() { - Eigen::VectorXd state = Eigen::VectorXd::Zero(4); + Eigen::VectorXd state = Eigen::VectorXd::Zero(3); const auto lateral_error_0 = x0(0); const auto yaw_error_0 = x0(1); state(0, 0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0; // world-x state(1, 0) = t.y.at(0) + std::cos(t.yaw.at(0)) * lateral_error_0; // world-y state(2, 0) = t.yaw.at(0) + yaw_error_0; // world-yaw - state(3, 0) = x0(2); // steering return state; }(); // update state in the world coordinate const auto updateState = [&]( - const Eigen::VectorXd & state_w, const Eigen::MatrixXd & input, - const double dt, const double velocity) { + const Eigen::VectorXd & state_w, const double & input, const double dt, + const double velocity) { const auto yaw = state_w(2); - const auto steer = state_w(3); - const auto desired_steer = input(0); Eigen::VectorXd dstate = Eigen::VectorXd::Zero(4); dstate(0) = velocity * std::cos(yaw); dstate(1) = velocity * std::sin(yaw); - dstate(2) = velocity * std::tan(steer) / m_wheelbase; - dstate(3) = -(steer - desired_steer) / m_steer_tau; + dstate(2) = velocity * std::tan(input) / m_wheelbase; // Note: don't do "return state_w + dstate * dt", which does not work due to the lazy evaluation // in Eigen. @@ -117,7 +113,7 @@ MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInWorldCoordin const auto DIM_U = getDimU(); for (size_t i = 0; i < reference_trajectory.size(); ++i) { - state_w = updateState(state_w, Uex.block(i * DIM_U, 0, DIM_U, 1), dt, t.vx.at(i)); + state_w = updateState(state_w, Uex(i * DIM_U, 0), dt, t.vx.at(i)); mpc_predicted_trajectory.push_back( state_w(0), state_w(1), t.z.at(i), state_w(2), t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i)); diff --git a/control/obstacle_collision_checker/CMakeLists.txt b/control/autoware_obstacle_collision_checker/CMakeLists.txt similarity index 72% rename from control/obstacle_collision_checker/CMakeLists.txt rename to control/autoware_obstacle_collision_checker/CMakeLists.txt index a57aeb093332b..cfe2546617140 100644 --- a/control/obstacle_collision_checker/CMakeLists.txt +++ b/control/autoware_obstacle_collision_checker/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(obstacle_collision_checker) +project(autoware_obstacle_collision_checker) find_package(autoware_cmake REQUIRED) autoware_package() @@ -12,12 +12,11 @@ include_directories( ) ament_auto_add_library(obstacle_collision_checker SHARED - src/obstacle_collision_checker_node/obstacle_collision_checker.cpp - src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp + DIRECTORY src ) rclcpp_components_register_node(obstacle_collision_checker - PLUGIN "obstacle_collision_checker::ObstacleCollisionCheckerNode" + PLUGIN "autoware::obstacle_collision_checker::ObstacleCollisionCheckerNode" EXECUTABLE obstacle_collision_checker_node ) diff --git a/control/obstacle_collision_checker/README.md b/control/autoware_obstacle_collision_checker/README.md similarity index 100% rename from control/obstacle_collision_checker/README.md rename to control/autoware_obstacle_collision_checker/README.md diff --git a/control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml b/control/autoware_obstacle_collision_checker/config/obstacle_collision_checker.param.yaml similarity index 100% rename from control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml rename to control/autoware_obstacle_collision_checker/config/obstacle_collision_checker.param.yaml diff --git a/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/debug.hpp b/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/debug.hpp new file mode 100644 index 0000000000000..017e64ab15e02 --- /dev/null +++ b/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/debug.hpp @@ -0,0 +1,32 @@ +// Copyright 2024 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__OBSTACLE_COLLISION_CHECKER__DEBUG_HPP_ +#define AUTOWARE__OBSTACLE_COLLISION_CHECKER__DEBUG_HPP_ + +#include "autoware/obstacle_collision_checker/obstacle_collision_checker.hpp" + +#include + +namespace autoware::obstacle_collision_checker +{ +/// @brief create debug markers of the given output +/// @param output structure with output data calculated by the obstacle_collision_checker module +/// @param base_link_z current z value of the base_link in map frame +/// @param now current time +/// @return debug markers +visualization_msgs::msg::MarkerArray create_marker_array( + const Output & output, const double base_link_z, const rclcpp::Time & now); +} // namespace autoware::obstacle_collision_checker +#endif // AUTOWARE__OBSTACLE_COLLISION_CHECKER__DEBUG_HPP_ diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp b/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker.hpp similarity index 50% rename from control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp rename to control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker.hpp index 8cce8b88a0e7f..2a3d8cf8778b2 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp +++ b/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. All rights reserved. +// Copyright 2020-2024 Tier IV, Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,10 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ -#define OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ +#ifndef AUTOWARE__OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ +#define AUTOWARE__OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ -#include #include #include @@ -24,8 +23,6 @@ #include #include -#include - #include #include @@ -33,7 +30,7 @@ #include #include -namespace obstacle_collision_checker +namespace autoware::obstacle_collision_checker { using autoware::universe_utils::LinearRing2d; @@ -54,6 +51,8 @@ struct Input geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory; + autoware::vehicle_info_utils::VehicleInfo vehicle_info; + Param param; }; struct Output @@ -65,43 +64,31 @@ struct Output std::vector vehicle_passing_areas; }; -class ObstacleCollisionChecker -{ -public: - explicit ObstacleCollisionChecker(rclcpp::Node & node); - Output update(const Input & input); - - void setParam(const Param & param) { param_ = param; } +Output check_for_collisions(const Input & input); -private: - Param param_; - autoware::vehicle_info_utils::VehicleInfo vehicle_info_; +//! This function assumes the input trajectory is sampled dense enough +autoware_planning_msgs::msg::Trajectory resample_trajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval); - //! This function assumes the input trajectory is sampled dense enough - static autoware_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval); +autoware_planning_msgs::msg::Trajectory cut_trajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double length); - static autoware_planning_msgs::msg::Trajectory cutTrajectory( - const autoware_planning_msgs::msg::Trajectory & trajectory, const double length); +std::vector create_vehicle_footprints( + const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); - static std::vector createVehicleFootprints( - const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); +std::vector create_vehicle_passing_areas( + const std::vector & vehicle_footprints); - static std::vector createVehiclePassingAreas( - const std::vector & vehicle_footprints); +LinearRing2d create_hull_from_footprints(const LinearRing2d & area1, const LinearRing2d & area2); - static LinearRing2d createHullFromFootprints( - const LinearRing2d & area1, const LinearRing2d & area2); +bool will_collide( + const pcl::PointCloud & obstacle_pointcloud, + const std::vector & vehicle_footprints); - static bool willCollide( - const pcl::PointCloud & obstacle_pointcloud, - const std::vector & vehicle_footprints); - - static bool hasCollision( - const pcl::PointCloud & obstacle_pointcloud, - const LinearRing2d & vehicle_footprint); -}; -} // namespace obstacle_collision_checker +bool has_collision( + const pcl::PointCloud & obstacle_pointcloud, + const LinearRing2d & vehicle_footprint); +} // namespace autoware::obstacle_collision_checker -#endif // OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ +#endif // AUTOWARE__OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp b/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker_node.hpp similarity index 71% rename from control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp rename to control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker_node.hpp index b08ecccd57282..86fe8e06872b9 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp +++ b/control/autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/obstacle_collision_checker_node.hpp @@ -12,16 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_NODE_HPP_ -#define OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_NODE_HPP_ +#ifndef AUTOWARE__OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_NODE_HPP_ +#define AUTOWARE__OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_NODE_HPP_ -#include "obstacle_collision_checker/obstacle_collision_checker.hpp" +#include "autoware/obstacle_collision_checker/obstacle_collision_checker.hpp" #include #include #include #include #include +#include #include #include @@ -29,16 +30,15 @@ #include #include #include -#include #include #include -namespace obstacle_collision_checker +namespace autoware::obstacle_collision_checker { struct NodeParam { - double update_rate; + double update_rate{}; }; class ObstacleCollisionCheckerNode : public rclcpp::Node @@ -64,12 +64,13 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform_; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; // Callback - void onObstaclePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - void onReferenceTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); - void onPredictedTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); - void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); + void on_obstacle_pointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg); + void on_reference_trajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); + void on_predicted_trajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); + void on_odom(const nav_msgs::msg::Odometry::SharedPtr msg); // Publisher std::shared_ptr debug_publisher_; @@ -77,34 +78,29 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node // Timer rclcpp::TimerBase::SharedPtr timer_; - void initTimer(double period_s); + void init_timer(double period_s); - bool isDataReady(); - bool isDataTimeout(); - void onTimer(); + bool is_data_ready(); + bool is_data_timeout(); + void on_timer(); // Parameter NodeParam node_param_; - Param param_; // Dynamic Reconfigure OnSetParametersCallbackHandle::SharedPtr set_param_res_; - rcl_interfaces::msg::SetParametersResult paramCallback( + rcl_interfaces::msg::SetParametersResult param_callback( const std::vector & parameters); // Core Input input_; Output output_; - std::unique_ptr obstacle_collision_checker_; // Diagnostic Updater diagnostic_updater::Updater updater_; - void checkLaneDeparture(diagnostic_updater::DiagnosticStatusWrapper & stat); - - // Visualization - visualization_msgs::msg::MarkerArray createMarkerArray() const; + void check_lane_departure(diagnostic_updater::DiagnosticStatusWrapper & stat); }; -} // namespace obstacle_collision_checker +} // namespace autoware::obstacle_collision_checker -#endif // OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_NODE_HPP_ +#endif // AUTOWARE__OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_NODE_HPP_ diff --git a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml b/control/autoware_obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml similarity index 79% rename from control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml rename to control/autoware_obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml index f82384534e040..c2cb75415a0f3 100755 --- a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml +++ b/control/autoware_obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml @@ -4,11 +4,11 @@ - + - + diff --git a/control/obstacle_collision_checker/package.xml b/control/autoware_obstacle_collision_checker/package.xml similarity index 97% rename from control/obstacle_collision_checker/package.xml rename to control/autoware_obstacle_collision_checker/package.xml index 45b142fe601fe..9dc29ce6fd387 100644 --- a/control/obstacle_collision_checker/package.xml +++ b/control/autoware_obstacle_collision_checker/package.xml @@ -1,7 +1,7 @@ - obstacle_collision_checker + autoware_obstacle_collision_checker 0.1.0 The obstacle_collision_checker package Taiki Tanaka diff --git a/control/obstacle_collision_checker/schema/obstacle_collision_checker.json b/control/autoware_obstacle_collision_checker/schema/autoware_obstacle_collision_checker.json similarity index 100% rename from control/obstacle_collision_checker/schema/obstacle_collision_checker.json rename to control/autoware_obstacle_collision_checker/schema/autoware_obstacle_collision_checker.json diff --git a/control/autoware_obstacle_collision_checker/src/debug.cpp b/control/autoware_obstacle_collision_checker/src/debug.cpp new file mode 100644 index 0000000000000..ac847264d3b66 --- /dev/null +++ b/control/autoware_obstacle_collision_checker/src/debug.cpp @@ -0,0 +1,89 @@ +// Copyright 2024 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/obstacle_collision_checker/debug.hpp" + +#include + +namespace autoware::obstacle_collision_checker +{ +visualization_msgs::msg::MarkerArray create_marker_array( + const Output & output, const double base_link_z, const rclcpp::Time & now) +{ + using autoware::universe_utils::createDefaultMarker; + using autoware::universe_utils::createMarkerColor; + using autoware::universe_utils::createMarkerScale; + + visualization_msgs::msg::MarkerArray marker_array; + + if (output.resampled_trajectory.points.size() >= 2) { + // Line of resampled_trajectory + { + auto marker = createDefaultMarker( + "map", now, "resampled_trajectory_line", 0, visualization_msgs::msg::Marker::LINE_STRIP, + createMarkerScale(0.05, 0, 0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + + for (const auto & p : output.resampled_trajectory.points) { + marker.points.push_back(p.pose.position); + marker.colors.push_back(marker.color); + } + + marker_array.markers.push_back(marker); + } + + // Points of resampled_trajectory + { + auto marker = createDefaultMarker( + "map", now, "resampled_trajectory_points", 0, visualization_msgs::msg::Marker::SPHERE_LIST, + createMarkerScale(0.1, 0.1, 0.1), createMarkerColor(0.0, 1.0, 0.0, 0.999)); + + for (const auto & p : output.resampled_trajectory.points) { + marker.points.push_back(p.pose.position); + marker.colors.push_back(marker.color); + } + + marker_array.markers.push_back(marker); + } + } + + // Vehicle Footprints + { + const auto color_ok = createMarkerColor(0.0, 1.0, 0.0, 0.5); + const auto color_will_collide = createMarkerColor(1.0, 0.0, 0.0, 0.5); + + auto color = color_ok; + if (output.will_collide) { + color = color_will_collide; + } + + auto marker = createDefaultMarker( + "map", now, "vehicle_footprints", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.05, 0, 0), color); + + for (const auto & vehicle_footprint : output.vehicle_footprints) { + for (size_t i = 0; i < vehicle_footprint.size() - 1; ++i) { + const auto & p1 = vehicle_footprint.at(i); + const auto & p2 = vehicle_footprint.at(i + 1); + + marker.points.push_back(toMsg(p1.to_3d(base_link_z))); + marker.points.push_back(toMsg(p2.to_3d(base_link_z))); + } + } + + marker_array.markers.push_back(marker); + } + + return marker_array; +} +} // namespace autoware::obstacle_collision_checker diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp b/control/autoware_obstacle_collision_checker/src/obstacle_collision_checker.cpp similarity index 72% rename from control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp rename to control/autoware_obstacle_collision_checker/src/obstacle_collision_checker.cpp index 9fb3657b957c7..bc8cb373a60df 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp +++ b/control/autoware_obstacle_collision_checker/src/obstacle_collision_checker.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_collision_checker/obstacle_collision_checker.hpp" +#include "autoware/obstacle_collision_checker/obstacle_collision_checker.hpp" #include #include @@ -20,24 +20,18 @@ #include #include #include +#include #include #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include #include namespace { -pcl::PointCloud getTransformedPointCloud( +pcl::PointCloud get_transformed_point_cloud( const sensor_msgs::msg::PointCloud2 & pointcloud_msg, const geometry_msgs::msg::Transform & transform) { @@ -52,7 +46,7 @@ pcl::PointCloud getTransformedPointCloud( return transformed_pointcloud; } -pcl::PointCloud filterPointCloudByTrajectory( +pcl::PointCloud filter_point_cloud_by_trajectory( const pcl::PointCloud & pointcloud, const autoware_planning_msgs::msg::Trajectory & trajectory, const double radius) { @@ -70,7 +64,7 @@ pcl::PointCloud filterPointCloudByTrajectory( return filtered_pointcloud; } -double calcBrakingDistance( +double calc_braking_distance( const double abs_velocity, const double max_deceleration, const double delay_time) { const double idling_distance = abs_velocity * delay_time; @@ -80,14 +74,9 @@ double calcBrakingDistance( } // namespace -namespace obstacle_collision_checker -{ -ObstacleCollisionChecker::ObstacleCollisionChecker(rclcpp::Node & node) -: vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) +namespace autoware::obstacle_collision_checker { -} - -Output ObstacleCollisionChecker::update(const Input & input) +Output check_for_collisions(const Input & input) { Output output; autoware::universe_utils::StopWatch stop_watch; @@ -97,31 +86,32 @@ Output ObstacleCollisionChecker::update(const Input & input) const auto & raw_abs_velocity = std::abs(input.current_twist->linear.x); const auto abs_velocity = raw_abs_velocity < min_velocity ? 0.0 : raw_abs_velocity; const auto braking_distance = - calcBrakingDistance(abs_velocity, param_.max_deceleration, param_.delay_time); - output.resampled_trajectory = cutTrajectory( - resampleTrajectory(*input.predicted_trajectory, param_.resample_interval), braking_distance); + calc_braking_distance(abs_velocity, input.param.max_deceleration, input.param.delay_time); + output.resampled_trajectory = cut_trajectory( + resample_trajectory(*input.predicted_trajectory, input.param.resample_interval), + braking_distance); output.processing_time_map["resampleTrajectory"] = stop_watch.toc(true); // resample pointcloud const auto obstacle_pointcloud = - getTransformedPointCloud(*input.obstacle_pointcloud, input.obstacle_transform->transform); - const auto filtered_obstacle_pointcloud = filterPointCloudByTrajectory( - obstacle_pointcloud, output.resampled_trajectory, param_.search_radius); + get_transformed_point_cloud(*input.obstacle_pointcloud, input.obstacle_transform->transform); + const auto filtered_obstacle_pointcloud = filter_point_cloud_by_trajectory( + obstacle_pointcloud, output.resampled_trajectory, input.param.search_radius); output.vehicle_footprints = - createVehicleFootprints(output.resampled_trajectory, param_, vehicle_info_); + create_vehicle_footprints(output.resampled_trajectory, input.param, input.vehicle_info); output.processing_time_map["createVehicleFootprints"] = stop_watch.toc(true); - output.vehicle_passing_areas = createVehiclePassingAreas(output.vehicle_footprints); + output.vehicle_passing_areas = create_vehicle_passing_areas(output.vehicle_footprints); output.processing_time_map["createVehiclePassingAreas"] = stop_watch.toc(true); - output.will_collide = willCollide(filtered_obstacle_pointcloud, output.vehicle_passing_areas); + output.will_collide = will_collide(filtered_obstacle_pointcloud, output.vehicle_passing_areas); output.processing_time_map["willCollide"] = stop_watch.toc(true); return output; } -autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajectory( +autoware_planning_msgs::msg::Trajectory resample_trajectory( const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval) { autoware_planning_msgs::msg::Trajectory resampled; @@ -131,11 +121,9 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajec for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { const auto & point = trajectory.points.at(i); - const auto p1 = - autoware::universe_utils::fromMsg(resampled.points.back().pose.position).to_2d(); - const auto p2 = autoware::universe_utils::fromMsg(point.pose.position).to_2d(); - - if (boost::geometry::distance(p1, p2) > interval) { + const auto distance = + autoware::universe_utils::calcDistance2d(resampled.points.back(), point.pose.position); + if (distance > interval) { resampled.points.push_back(point); } } @@ -144,7 +132,7 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajec return resampled; } -autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( +autoware_planning_msgs::msg::Trajectory cut_trajectory( const autoware_planning_msgs::msg::Trajectory & trajectory, const double length) { autoware_planning_msgs::msg::Trajectory cut; @@ -157,8 +145,8 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( const auto p1 = autoware::universe_utils::fromMsg(cut.points.back().pose.position); const auto p2 = autoware::universe_utils::fromMsg(point.pose.position); - const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); + const auto points_distance = boost::geometry::distance(p1, p2); const auto remain_distance = length - total_length; // Over length @@ -187,7 +175,7 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( return cut; } -std::vector ObstacleCollisionChecker::createVehicleFootprints( +std::vector create_vehicle_footprints( const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { @@ -205,7 +193,7 @@ std::vector ObstacleCollisionChecker::createVehicleFootprints( return vehicle_footprints; } -std::vector ObstacleCollisionChecker::createVehiclePassingAreas( +std::vector create_vehicle_passing_areas( const std::vector & vehicle_footprints) { // Create hull from two adjacent vehicle footprints @@ -213,14 +201,13 @@ std::vector ObstacleCollisionChecker::createVehiclePassingAreas( for (size_t i = 0; i < vehicle_footprints.size() - 1; ++i) { const auto & footprint1 = vehicle_footprints.at(i); const auto & footprint2 = vehicle_footprints.at(i + 1); - areas.push_back(createHullFromFootprints(footprint1, footprint2)); + areas.push_back(create_hull_from_footprints(footprint1, footprint2)); } return areas; } -LinearRing2d ObstacleCollisionChecker::createHullFromFootprints( - const LinearRing2d & area1, const LinearRing2d & area2) +LinearRing2d create_hull_from_footprints(const LinearRing2d & area1, const LinearRing2d & area2) { autoware::universe_utils::MultiPoint2d combined; for (const auto & p : area1) { @@ -234,16 +221,15 @@ LinearRing2d ObstacleCollisionChecker::createHullFromFootprints( return hull; } -bool ObstacleCollisionChecker::willCollide( +bool will_collide( const pcl::PointCloud & obstacle_pointcloud, const std::vector & vehicle_footprints) { for (size_t i = 1; i < vehicle_footprints.size(); i++) { // skip first footprint because surround obstacle checker handle it const auto & vehicle_footprint = vehicle_footprints.at(i); - if (hasCollision(obstacle_pointcloud, vehicle_footprint)) { - RCLCPP_WARN( - rclcpp::get_logger("obstacle_collision_checker"), "ObstacleCollisionChecker::willCollide"); + if (has_collision(obstacle_pointcloud, vehicle_footprint)) { + RCLCPP_WARN(rclcpp::get_logger("obstacle_collision_checker"), "willCollide"); return true; } } @@ -251,7 +237,7 @@ bool ObstacleCollisionChecker::willCollide( return false; } -bool ObstacleCollisionChecker::hasCollision( +bool has_collision( const pcl::PointCloud & obstacle_pointcloud, const LinearRing2d & vehicle_footprint) { @@ -259,12 +245,12 @@ bool ObstacleCollisionChecker::hasCollision( if (boost::geometry::within( autoware::universe_utils::Point2d{point.x, point.y}, vehicle_footprint)) { RCLCPP_WARN( - rclcpp::get_logger("obstacle_collision_checker"), - "[ObstacleCollisionChecker] Collide to Point x: %f y: %f", point.x, point.y); + rclcpp::get_logger("obstacle_collision_checker"), "Collide to Point x: %f y: %f", point.x, + point.y); return true; } } return false; } -} // namespace obstacle_collision_checker +} // namespace autoware::obstacle_collision_checker diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp b/control/autoware_obstacle_collision_checker/src/obstacle_collision_checker_node.cpp similarity index 53% rename from control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp rename to control/autoware_obstacle_collision_checker/src/obstacle_collision_checker_node.cpp index 9af5fe24b17ea..598ae592f4c8d 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp +++ b/control/autoware_obstacle_collision_checker/src/obstacle_collision_checker_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. All rights reserved. +// Copyright 2020-2024 Tier IV, Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,42 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_collision_checker/obstacle_collision_checker_node.hpp" +#include "autoware/obstacle_collision_checker/obstacle_collision_checker_node.hpp" + +#include "autoware/obstacle_collision_checker/debug.hpp" #include #include -#include +#include #include #include #include -#include #include -namespace -{ -template -bool update_param( - const std::vector & params, const std::string & name, T & value) -{ - const auto itr = std::find_if( - params.cbegin(), params.cend(), - [&name](const rclcpp::Parameter & p) { return p.get_name() == name; }); - - // Not found - if (itr == params.cend()) { - return false; - } - - value = itr->template get_value(); - return true; -} -} // namespace - -namespace obstacle_collision_checker +namespace autoware::obstacle_collision_checker { ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOptions & node_options) -: Node("obstacle_collision_checker_node", node_options), updater_(this) +: Node("obstacle_collision_checker_node", node_options), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), + updater_(this) { using std::placeholders::_1; @@ -55,19 +38,15 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt node_param_.update_rate = declare_parameter("update_rate"); // Core Parameter - param_.delay_time = declare_parameter("delay_time"); - param_.footprint_margin = declare_parameter("footprint_margin"); - param_.max_deceleration = declare_parameter("max_deceleration"); - param_.resample_interval = declare_parameter("resample_interval"); - param_.search_radius = declare_parameter("search_radius"); + input_.param.delay_time = declare_parameter("delay_time"); + input_.param.footprint_margin = declare_parameter("footprint_margin"); + input_.param.max_deceleration = declare_parameter("max_deceleration"); + input_.param.resample_interval = declare_parameter("resample_interval"); + input_.param.search_radius = declare_parameter("search_radius"); // Dynamic Reconfigure - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&ObstacleCollisionCheckerNode::paramCallback, this, _1)); - - // Core - obstacle_collision_checker_ = std::make_unique(*this); - obstacle_collision_checker_->setParam(param_); + set_param_res_ = this->add_on_set_parameters_callback(std::bind( + &autoware::obstacle_collision_checker::ObstacleCollisionCheckerNode::param_callback, this, _1)); // Subscriber self_pose_listener_ = std::make_shared(this); @@ -75,15 +54,15 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt sub_obstacle_pointcloud_ = create_subscription( "input/obstacle_pointcloud", rclcpp::SensorDataQoS(), - std::bind(&ObstacleCollisionCheckerNode::onObstaclePointcloud, this, _1)); + std::bind(&ObstacleCollisionCheckerNode::on_obstacle_pointcloud, this, _1)); sub_reference_trajectory_ = create_subscription( "input/reference_trajectory", 1, - std::bind(&ObstacleCollisionCheckerNode::onReferenceTrajectory, this, _1)); + std::bind(&ObstacleCollisionCheckerNode::on_reference_trajectory, this, _1)); sub_predicted_trajectory_ = create_subscription( "input/predicted_trajectory", 1, - std::bind(&ObstacleCollisionCheckerNode::onPredictedTrajectory, this, _1)); + std::bind(&ObstacleCollisionCheckerNode::on_predicted_trajectory, this, _1)); sub_odom_ = create_subscription( - "input/odometry", 1, std::bind(&ObstacleCollisionCheckerNode::onOdom, this, _1)); + "input/odometry", 1, std::bind(&ObstacleCollisionCheckerNode::on_odom, this, _1)); // Publisher debug_publisher_ = @@ -94,47 +73,47 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt updater_.setHardwareID("obstacle_collision_checker"); updater_.add( - "obstacle_collision_checker", this, &ObstacleCollisionCheckerNode::checkLaneDeparture); + "obstacle_collision_checker", this, &ObstacleCollisionCheckerNode::check_lane_departure); // Wait for first self pose self_pose_listener_->waitForFirstPose(); // Timer - initTimer(1.0 / node_param_.update_rate); + init_timer(1.0 / node_param_.update_rate); } -void ObstacleCollisionCheckerNode::onObstaclePointcloud( +void ObstacleCollisionCheckerNode::on_obstacle_pointcloud( const sensor_msgs::msg::PointCloud2::SharedPtr msg) { obstacle_pointcloud_ = msg; } -void ObstacleCollisionCheckerNode::onReferenceTrajectory( +void ObstacleCollisionCheckerNode::on_reference_trajectory( const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { reference_trajectory_ = msg; } -void ObstacleCollisionCheckerNode::onPredictedTrajectory( +void ObstacleCollisionCheckerNode::on_predicted_trajectory( const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { predicted_trajectory_ = msg; } -void ObstacleCollisionCheckerNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg) +void ObstacleCollisionCheckerNode::on_odom(const nav_msgs::msg::Odometry::SharedPtr msg) { current_twist_ = std::make_shared(msg->twist.twist); } -void ObstacleCollisionCheckerNode::initTimer(double period_s) +void ObstacleCollisionCheckerNode::init_timer(double period_s) { const auto period_ns = std::chrono::duration_cast(std::chrono::duration(period_s)); timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&ObstacleCollisionCheckerNode::onTimer, this)); + this, get_clock(), period_ns, std::bind(&ObstacleCollisionCheckerNode::on_timer, this)); } -bool ObstacleCollisionCheckerNode::isDataReady() +bool ObstacleCollisionCheckerNode::is_data_ready() { if (!current_pose_) { RCLCPP_INFO_THROTTLE( @@ -178,7 +157,7 @@ bool ObstacleCollisionCheckerNode::isDataReady() return true; } -bool ObstacleCollisionCheckerNode::isDataTimeout() +bool ObstacleCollisionCheckerNode::is_data_timeout() { const auto now = this->now(); @@ -193,7 +172,7 @@ bool ObstacleCollisionCheckerNode::isDataTimeout() return false; } -void ObstacleCollisionCheckerNode::onTimer() +void ObstacleCollisionCheckerNode::on_timer() { current_pose_ = self_pose_listener_->getCurrentPose(); if (obstacle_pointcloud_) { @@ -209,11 +188,11 @@ void ObstacleCollisionCheckerNode::onTimer() } } - if (!isDataReady()) { + if (!is_data_ready()) { return; } - if (isDataTimeout()) { + if (is_data_timeout()) { return; } @@ -223,17 +202,19 @@ void ObstacleCollisionCheckerNode::onTimer() input_.reference_trajectory = reference_trajectory_; input_.predicted_trajectory = predicted_trajectory_; input_.current_twist = current_twist_; + input_.vehicle_info = vehicle_info_; - output_ = obstacle_collision_checker_->update(input_); + output_ = check_for_collisions(input_); updater_.force_update(); - debug_publisher_->publish("marker_array", createMarkerArray()); + debug_publisher_->publish( + "marker_array", create_marker_array(output_, current_pose_->pose.position.z, this->now())); time_publisher_->publish(output_.processing_time_map); } -rcl_interfaces::msg::SetParametersResult ObstacleCollisionCheckerNode::paramCallback( +rcl_interfaces::msg::SetParametersResult ObstacleCollisionCheckerNode::param_callback( const std::vector & parameters) { rcl_interfaces::msg::SetParametersResult result; @@ -241,25 +222,22 @@ rcl_interfaces::msg::SetParametersResult ObstacleCollisionCheckerNode::paramCall result.reason = "success"; try { + using autoware::universe_utils::updateParam; // Node Parameter { auto & p = node_param_; // Update params - update_param(parameters, "update_rate", p.update_rate); + updateParam(parameters, "update_rate", p.update_rate); } - auto & p = param_; - - update_param(parameters, "delay_time", p.delay_time); - update_param(parameters, "footprint_margin", p.footprint_margin); - update_param(parameters, "max_deceleration", p.max_deceleration); - update_param(parameters, "resample_interval", p.resample_interval); - update_param(parameters, "search_radius", p.search_radius); + auto & p = input_.param; - if (obstacle_collision_checker_) { - obstacle_collision_checker_->setParam(param_); - } + updateParam(parameters, "delay_time", p.delay_time); + updateParam(parameters, "footprint_margin", p.footprint_margin); + updateParam(parameters, "max_deceleration", p.max_deceleration); + updateParam(parameters, "resample_interval", p.resample_interval); + updateParam(parameters, "search_radius", p.search_radius); } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); @@ -267,7 +245,7 @@ rcl_interfaces::msg::SetParametersResult ObstacleCollisionCheckerNode::paramCall return result; } -void ObstacleCollisionCheckerNode::checkLaneDeparture( +void ObstacleCollisionCheckerNode::check_lane_departure( diagnostic_updater::DiagnosticStatusWrapper & stat) { int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -280,79 +258,7 @@ void ObstacleCollisionCheckerNode::checkLaneDeparture( stat.summary(level, msg); } - -visualization_msgs::msg::MarkerArray ObstacleCollisionCheckerNode::createMarkerArray() const -{ - using autoware::universe_utils::createDefaultMarker; - using autoware::universe_utils::createMarkerColor; - using autoware::universe_utils::createMarkerScale; - - visualization_msgs::msg::MarkerArray marker_array; - - const auto base_link_z = current_pose_->pose.position.z; - - if (output_.resampled_trajectory.points.size() >= 2) { - // Line of resampled_trajectory - { - auto marker = createDefaultMarker( - "map", this->now(), "resampled_trajectory_line", 0, - visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.05, 0, 0), - createMarkerColor(1.0, 1.0, 1.0, 0.999)); - - for (const auto & p : output_.resampled_trajectory.points) { - marker.points.push_back(p.pose.position); - marker.colors.push_back(marker.color); - } - - marker_array.markers.push_back(marker); - } - - // Points of resampled_trajectory - { - auto marker = createDefaultMarker( - "map", this->now(), "resampled_trajectory_points", 0, - visualization_msgs::msg::Marker::SPHERE_LIST, createMarkerScale(0.1, 0.1, 0.1), - createMarkerColor(0.0, 1.0, 0.0, 0.999)); - - for (const auto & p : output_.resampled_trajectory.points) { - marker.points.push_back(p.pose.position); - marker.colors.push_back(marker.color); - } - - marker_array.markers.push_back(marker); - } - } - - // Vehicle Footprints - { - const auto color_ok = createMarkerColor(0.0, 1.0, 0.0, 0.5); - const auto color_will_collide = createMarkerColor(1.0, 0.0, 0.0, 0.5); - - auto color = color_ok; - if (output_.will_collide) { - color = color_will_collide; - } - - auto marker = createDefaultMarker( - "map", this->now(), "vehicle_footprints", 0, visualization_msgs::msg::Marker::LINE_LIST, - createMarkerScale(0.05, 0, 0), color); - - for (const auto & vehicle_footprint : output_.vehicle_footprints) { - for (size_t i = 0; i < vehicle_footprint.size() - 1; ++i) { - const auto p1 = vehicle_footprint.at(i); - const auto p2 = vehicle_footprint.at(i + 1); - - marker.points.push_back(toMsg(p1.to_3d(base_link_z))); - marker.points.push_back(toMsg(p2.to_3d(base_link_z))); - } - } - - marker_array.markers.push_back(marker); - } - - return marker_array; -} -} // namespace obstacle_collision_checker +} // namespace autoware::obstacle_collision_checker #include -RCLCPP_COMPONENTS_REGISTER_NODE(obstacle_collision_checker::ObstacleCollisionCheckerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::obstacle_collision_checker::ObstacleCollisionCheckerNode) diff --git a/control/autoware_obstacle_collision_checker/test/test_obstacle_collision_checker.cpp b/control/autoware_obstacle_collision_checker/test/test_obstacle_collision_checker.cpp new file mode 100644 index 0000000000000..97b26162812ef --- /dev/null +++ b/control/autoware_obstacle_collision_checker/test/test_obstacle_collision_checker.cpp @@ -0,0 +1,254 @@ +// Copyright 2022-2024 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/obstacle_collision_checker.cpp" // NOLINT +#include "gtest/gtest.h" + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace +{ +pcl::PointXYZ pcl_point(const float x, const float y) +{ + pcl::PointXYZ p; + p.x = x; + p.y = y; + p.z = 0.0; + return p; +} + +pcl::PointCloud pcl_pointcloud(const std::vector> & points) +{ + pcl::PointCloud pcl; + for (const auto & p : points) { + pcl.push_back(pcl_point(p.first, p.second)); + } + return pcl; +} + +bool point_in_pcl_pointcloud(const pcl::PointXYZ & pt, const pcl::PointCloud & pcd) +{ + for (const auto & p : pcd) { + if (p.x == pt.x && p.y == pt.y && p.z == pt.z) { + return true; + } + } + return false; +} +} // namespace + +TEST(test_obstacle_collision_checker, filterPointCloudByTrajectory) +{ + pcl::PointCloud pcl; + autoware_planning_msgs::msg::Trajectory trajectory; + pcl::PointXYZ pcl_point; + autoware_planning_msgs::msg::TrajectoryPoint traj_point; + pcl_point.y = 0.0; + traj_point.pose.position.y = 0.99; + for (float x = 0.0; x < 10.0; x += 1.0) { + pcl_point.x = x; + traj_point.pose.position.x = x; + trajectory.points.push_back(traj_point); + pcl.push_back(pcl_point); + } + // radius < 1: all points are filtered + for (auto radius = 0.0; radius <= 0.99; radius += 0.1) { + const auto filtered_pcl = filter_point_cloud_by_trajectory(pcl, trajectory, radius); + EXPECT_EQ(filtered_pcl.size(), 0ul); + } + // radius >= 1.0: all points are kept + for (auto radius = 1.0; radius < 10.0; radius += 0.1) { + const auto filtered_pcl = filter_point_cloud_by_trajectory(pcl, trajectory, radius); + ASSERT_EQ(pcl.size(), filtered_pcl.size()); + for (size_t i = 0; i < pcl.size(); ++i) { + EXPECT_EQ(pcl[i].x, filtered_pcl[i].x); + EXPECT_EQ(pcl[i].y, filtered_pcl[i].y); + } + } +} + +TEST(test_obstacle_collision_checker, getTransformedPointCloud) +{ + sensor_msgs::msg::PointCloud2 pcd_msg; + const auto pcl_pcd = pcl_pointcloud({ + {0.0, 0.0}, + {1.0, 1.0}, + {-2.0, 3.0}, + }); + pcl::toROSMsg(pcl_pcd, pcd_msg); + + { // empty transform, expect same points + geometry_msgs::msg::Transform transform; + const auto transformed_pcd = get_transformed_point_cloud(pcd_msg, transform); + EXPECT_EQ(pcl_pcd.size(), transformed_pcd.size()); + for (const auto & p : transformed_pcd.points) { + EXPECT_TRUE(point_in_pcl_pointcloud(p, pcl_pcd)); + } + } + + { // translation + geometry_msgs::msg::Transform transform; + transform.translation.x = 2.0; + transform.translation.y = 1.5; + const auto transformed_pcd = get_transformed_point_cloud(pcd_msg, transform); + EXPECT_EQ(pcl_pcd.size(), transformed_pcd.size()); + for (const auto & p : transformed_pcd.points) { + auto transformed_p = p; + transformed_p.x -= static_cast(transform.translation.x); + transformed_p.y -= static_cast(transform.translation.y); + EXPECT_TRUE(point_in_pcl_pointcloud(transformed_p, pcl_pcd)); + } + } + { // rotation + geometry_msgs::msg::Transform transform; + transform.rotation = autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, M_PI); + const auto transformed_pcd = get_transformed_point_cloud(pcd_msg, transform); + EXPECT_EQ(pcl_pcd.size(), transformed_pcd.size()); + EXPECT_TRUE(point_in_pcl_pointcloud(pcl_point(0.0, 0.0), transformed_pcd)); + EXPECT_TRUE(point_in_pcl_pointcloud(pcl_point(-1.0, -1.0), transformed_pcd)); + EXPECT_TRUE(point_in_pcl_pointcloud(pcl_point(2.0, -3.0), transformed_pcd)); + } + { // translation + rotation + geometry_msgs::msg::Transform transform; + transform.translation.x = 0.5; + transform.translation.y = -0.5; + transform.rotation = autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, M_PI); + const auto transformed_pcd = get_transformed_point_cloud(pcd_msg, transform); + EXPECT_EQ(pcl_pcd.size(), transformed_pcd.size()); + EXPECT_TRUE(point_in_pcl_pointcloud(pcl_point(0.5, -0.5), transformed_pcd)); + EXPECT_TRUE(point_in_pcl_pointcloud(pcl_point(-0.5, -1.5), transformed_pcd)); + EXPECT_TRUE(point_in_pcl_pointcloud(pcl_point(2.5, -3.5), transformed_pcd)); + } +} + +TEST(test_obstacle_collision_checker, calcBrakingDistance) +{ + EXPECT_TRUE(std::isnan(calc_braking_distance(0.0, 0.0, 0.0))); + // if we cannot decelerate (max_decel = 0.0), then the result is infinity + EXPECT_DOUBLE_EQ(calc_braking_distance(1.0, 0.0, 1.0), std::numeric_limits::infinity()); + EXPECT_DOUBLE_EQ( + calc_braking_distance(1.0, 1.0, 1.0), + // 1s * 1m/s = 1m for the delay, then 1->0m/s at 1m/s² = 0.5m + 1.0 + 0.5); +} + +TEST(test_obstacle_collision_checker, check_for_collisions) +{ + autoware::obstacle_collision_checker::Input input; + // call with empty input causes a segfault + // const auto output = check_for_collisions(input); + input.param.delay_time = 1.0; + input.param.footprint_margin = 0.0; + input.param.max_deceleration = 1.0; + input.param.resample_interval = 1.0; + input.param.search_radius = 10.0; + geometry_msgs::msg::PoseStamped ego_pose; // default (0,0) ego pose + geometry_msgs::msg::TransformStamped tf; // (0,0) transform + tf.transform.rotation.w = 1.0; + input.current_pose = std::make_shared(ego_pose); + input.obstacle_transform = std::make_shared(tf); + // 2mx2m footprint + input.vehicle_info.front_overhang_m = 1.0; + input.vehicle_info.wheel_base_m = 0.0; + input.vehicle_info.rear_overhang_m = 1.0; + input.vehicle_info.left_overhang_m = 1.0; + input.vehicle_info.right_overhang_m = 1.0; + autoware_planning_msgs::msg::Trajectory trajectory; + autoware_planning_msgs::msg::TrajectoryPoint point; + point.pose.position.y = 0.0; + for (auto x = 0.0; x < 6.0; x += 0.1) { + point.pose.position.x = x; + trajectory.points.push_back(point); + } + input.predicted_trajectory = + std::make_shared(trajectory); + input.reference_trajectory = {}; // TODO(someone): the reference trajectory is not used + { + // obstacle point on the trajectory + sensor_msgs::msg::PointCloud2 pcd_msg; + const auto pcl_pcd = pcl_pointcloud({ + {5.0, 0.0}, + }); + pcl::toROSMsg(pcl_pcd, pcd_msg); + input.obstacle_pointcloud = std::make_shared(pcd_msg); + geometry_msgs::msg::Twist twist; // no velocity -> no collision + twist.linear.x = 0.0; + input.current_twist = std::make_shared(twist); + const auto output = check_for_collisions(input); + EXPECT_FALSE(output.will_collide); + // zero velocity: only the 1st point of the trajectory is kept + EXPECT_EQ(output.resampled_trajectory.points.size(), 1UL); + } + { + // moderate velocity: short braking distance so the trajectory is cut before the collision + geometry_msgs::msg::Twist twist; + twist.linear.x = 1.0; + input.current_twist = std::make_shared(twist); + const auto output = check_for_collisions(input); + EXPECT_FALSE(output.will_collide); + // 1s * 1m/s = 1m for the delay, then 1->0m/s at 1m/s² = 0.5m -> 1.5m braking distance + EXPECT_DOUBLE_EQ( + autoware::universe_utils::calcDistance2d( + output.resampled_trajectory.points.front(), output.resampled_trajectory.points.back()), + 1.5); + } + { + // high velocity -> collision + geometry_msgs::msg::Twist twist; + twist.linear.x = 10.0; + input.current_twist = std::make_shared(twist); + const auto output = check_for_collisions(input); + EXPECT_TRUE(output.will_collide); + // high velocity: the full trajectory is resampled (original interval = 0.1, resample interval + // = 1.0) + EXPECT_EQ( + output.resampled_trajectory.points.size(), + input.predicted_trajectory->points.size() / 10 + 1); + } + { + // obstacle point on the side of the trajectory but inside the ego footprint -> collision + sensor_msgs::msg::PointCloud2 pcd_msg; + const auto pcl_pcd = pcl_pointcloud({ + {5.0, 0.5}, + }); + pcl::toROSMsg(pcl_pcd, pcd_msg); + input.obstacle_pointcloud = std::make_shared(pcd_msg); + const auto output = check_for_collisions(input); + EXPECT_TRUE(output.will_collide); + } + { + // obstacle point on the side of the trajectory and outside the ego footprint -> no collision + sensor_msgs::msg::PointCloud2 pcd_msg; + const auto pcl_pcd = pcl_pointcloud({ + {5.0, 1.5}, + }); + pcl::toROSMsg(pcl_pcd, pcd_msg); + input.obstacle_pointcloud = std::make_shared(pcd_msg); + const auto output = check_for_collisions(input); + EXPECT_FALSE(output.will_collide); + } +} diff --git a/control/autoware_trajectory_follower_base/package.xml b/control/autoware_trajectory_follower_base/package.xml index 548129eb06f8c..c4b6fbfeb4ca6 100644 --- a/control/autoware_trajectory_follower_base/package.xml +++ b/control/autoware_trajectory_follower_base/package.xml @@ -23,6 +23,7 @@ autoware_control_msgs autoware_interpolation autoware_motion_utils + autoware_osqp_interface autoware_planning_msgs autoware_universe_utils autoware_vehicle_info_utils @@ -31,7 +32,6 @@ diagnostic_updater eigen geometry_msgs - osqp_interface rclcpp rclcpp_components std_msgs diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index bf61b69857849..b028455ab3354 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -77,6 +77,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) engage_pub_ = create_publisher("output/engage", durable_qos); pub_external_emergency_ = create_publisher("output/external_emergency", durable_qos); operation_mode_pub_ = create_publisher("output/operation_mode", durable_qos); + processing_time_pub_ = + this->create_publisher("~/debug/processing_time_ms", 1); is_filter_activated_pub_ = create_publisher("~/is_filter_activated", durable_qos); @@ -375,6 +377,8 @@ T VehicleCmdGate::getContinuousTopic( void VehicleCmdGate::onTimer() { + stop_watch.tic(); + // Subscriber for auto const auto msg_auto_command_turn_indicator = auto_turn_indicator_cmd_sub_.takeData(); if (msg_auto_command_turn_indicator) @@ -573,12 +577,18 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) vehicle_cmd_emergency.emergency = (use_emergency_handling_ && is_system_emergency_); vehicle_cmd_emergency.stamp = filtered_control.stamp; + // ProcessingTime + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + // Publish commands vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency); control_cmd_pub_->publish(filtered_control); published_time_publisher_->publish_if_subscribed(control_cmd_pub_, filtered_control.stamp); adapi_pause_->publish(); moderate_stop_interface_->publish(); + processing_time_pub_->publish(processing_time_msg); // Save ControlCmd to steering angle when disengaged prev_control_cmd_ = filtered_control; @@ -616,12 +626,18 @@ void VehicleCmdGate::publishEmergencyStopControlCommands() vehicle_cmd_emergency.stamp = stamp; vehicle_cmd_emergency.emergency = true; + // ProcessingTime + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + // Publish topics vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency); control_cmd_pub_->publish(control_cmd); turn_indicator_cmd_pub_->publish(turn_indicator); hazard_light_cmd_pub_->publish(hazard_light); gear_cmd_pub_->publish(gear); + processing_time_pub_->publish(processing_time_msg); } void VehicleCmdGate::publishStatus() @@ -638,12 +654,18 @@ void VehicleCmdGate::publishStatus() external_emergency.stamp = stamp; external_emergency.emergency = is_external_emergency_stop_; + // ProcessingTime + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + gate_mode_pub_->publish(current_gate_mode_); engage_pub_->publish(autoware_engage); pub_external_emergency_->publish(external_emergency); operation_mode_pub_->publish(current_operation_mode_); adapi_pause_->publish(); moderate_stop_interface_->publish(); + processing_time_pub_->publish(processing_time_msg); } Control VehicleCmdGate::filterControlCommand(const Control & in) diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 6863015443363..c2716ea3baa6a 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -41,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -114,6 +116,7 @@ class VehicleCmdGate : public rclcpp::Node rclcpp::Publisher::SharedPtr filter_activated_marker_pub_; rclcpp::Publisher::SharedPtr filter_activated_marker_raw_pub_; rclcpp::Publisher::SharedPtr filter_activated_flag_pub_; + rclcpp::Publisher::SharedPtr processing_time_pub_; // Parameter callback OnSetParametersCallbackHandle::SharedPtr set_param_res_; rcl_interfaces::msg::SetParametersResult onParameter( @@ -268,6 +271,9 @@ class VehicleCmdGate : public rclcpp::Node std::unique_ptr vehicle_stop_checker_; double stop_check_duration_; + // processing time + autoware::universe_utils::StopWatch stop_watch; + // debug MarkerArray createMarkerArray(const IsFilterActivated & filter_activated); void publishMarkers(const IsFilterActivated & filter_activated); diff --git a/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp b/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp deleted file mode 100644 index 705bff754d3d9..0000000000000 --- a/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2022 Tier IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "../src/obstacle_collision_checker_node/obstacle_collision_checker.cpp" // NOLINT -#include "gtest/gtest.h" - -TEST(test_obstacle_collision_checker, filterPointCloudByTrajectory) -{ - pcl::PointCloud pcl; - autoware_planning_msgs::msg::Trajectory trajectory; - pcl::PointXYZ pcl_point; - autoware_planning_msgs::msg::TrajectoryPoint traj_point; - pcl_point.y = 0.0; - traj_point.pose.position.y = 0.99; - for (double x = 0.0; x < 10.0; x += 1.0) { - pcl_point.x = x; - traj_point.pose.position.x = x; - trajectory.points.push_back(traj_point); - pcl.push_back(pcl_point); - } - // radius < 1: all points are filtered - for (auto radius = 0.0; radius <= 0.99; radius += 0.1) { - const auto filtered_pcl = filterPointCloudByTrajectory(pcl, trajectory, radius); - EXPECT_EQ(filtered_pcl.size(), 0ul); - } - // radius >= 1.0: all points are kept - for (auto radius = 1.0; radius < 10.0; radius += 0.1) { - const auto filtered_pcl = filterPointCloudByTrajectory(pcl, trajectory, radius); - ASSERT_EQ(pcl.size(), filtered_pcl.size()); - for (size_t i = 0; i < pcl.size(); ++i) { - EXPECT_EQ(pcl[i].x, filtered_pcl[i].x); - EXPECT_EQ(pcl[i].y, filtered_pcl[i].y); - } - } -} diff --git a/evaluator/perception_online_evaluator/package.xml b/evaluator/perception_online_evaluator/package.xml index 23b1210063705..0103200511e73 100644 --- a/evaluator/perception_online_evaluator/package.xml +++ b/evaluator/perception_online_evaluator/package.xml @@ -19,6 +19,7 @@ autoware_lanelet2_extension autoware_motion_utils + autoware_object_recognition_utils autoware_perception_msgs autoware_universe_utils autoware_vehicle_info_utils @@ -27,7 +28,6 @@ geometry_msgs glog nav_msgs - object_recognition_utils pluginlib rclcpp rclcpp_components diff --git a/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp b/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp index 5859bf055d4aa..ab69ea9cb8c7b 100644 --- a/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp +++ b/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp @@ -14,8 +14,8 @@ #include "perception_online_evaluator/metrics/detection_count.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include "perception_online_evaluator/utils/objects_filtering.hpp" #include @@ -72,7 +72,8 @@ void DetectionCounter::addObjects( for (const auto & object : objects.objects) { const auto uuid = toHexString(object.object_id); - const auto label = object_recognition_utils::getHighestProbLabel(object.classification); + const auto label = + autoware::object_recognition_utils::getHighestProbLabel(object.classification); if (!isCountObject(label, parameters_->object_parameters)) { continue; } diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index 212af8711a62e..fea4316fa6820 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -15,16 +15,16 @@ #include "perception_online_evaluator/metrics_calculator.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/object_recognition_utils/object_classification.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "object_recognition_utils/object_classification.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include namespace perception_diagnostics { +using autoware::object_recognition_utils::convertLabelToString; using autoware::universe_utils::inverseTransformPoint; -using object_recognition_utils::convertLabelToString; std::optional MetricsCalculator::calculate(const Metric & metric) const { diff --git a/launch/tier4_control_launch/launch/control.launch.xml b/launch/tier4_control_launch/launch/control.launch.xml index 0eda6faaf4e05..9b2efd2da8961 100644 --- a/launch/tier4_control_launch/launch/control.launch.xml +++ b/launch/tier4_control_launch/launch/control.launch.xml @@ -174,7 +174,7 @@ - + @@ -192,7 +192,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py index 4fda3521c07e9..16d48313eb2c9 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py @@ -35,15 +35,6 @@ def __init__(self, context): with open(pointcloud_map_filter_param_path, "r") as f: self.pointcloud_map_filter_param = yaml.safe_load(f)["/**"]["ros__parameters"] self.voxel_size = self.pointcloud_map_filter_param["down_sample_voxel_size"] - self.distance_threshold = self.pointcloud_map_filter_param["distance_threshold"] - self.downsize_ratio_z_axis = self.pointcloud_map_filter_param["downsize_ratio_z_axis"] - self.timer_interval_ms = self.pointcloud_map_filter_param["timer_interval_ms"] - self.use_dynamic_map_loading = self.pointcloud_map_filter_param["use_dynamic_map_loading"] - self.map_update_distance_threshold = self.pointcloud_map_filter_param[ - "map_update_distance_threshold" - ] - self.map_loader_radius = self.pointcloud_map_filter_param["map_loader_radius"] - self.publish_debug_pcd = self.pointcloud_map_filter_param["publish_debug_pcd"] self.use_pointcloud_map = LaunchConfiguration("use_pointcloud_map").perform(context) def create_pipeline(self): @@ -116,16 +107,10 @@ def create_compare_map_pipeline(self): ("kinematic_state", "/localization/kinematic_state"), ], parameters=[ + self.pointcloud_map_filter_param, { - "distance_threshold": self.distance_threshold, - "downsize_ratio_z_axis": self.downsize_ratio_z_axis, - "timer_interval_ms": self.timer_interval_ms, - "use_dynamic_map_loading": self.use_dynamic_map_loading, - "map_update_distance_threshold": self.map_update_distance_threshold, - "map_loader_radius": self.map_loader_radius, - "publish_debug_pcd": self.publish_debug_pcd, "input_frame": "map", - } + }, ], extra_arguments=[ {"use_intra_process_comms": False}, diff --git a/localization/autoware_pose_covariance_modifier/README.md b/localization/autoware_pose_covariance_modifier/README.md index 60ecf4bde3508..201fb8bb37103 100644 --- a/localization/autoware_pose_covariance_modifier/README.md +++ b/localization/autoware_pose_covariance_modifier/README.md @@ -80,12 +80,12 @@ graph TD pc1{{"gnss_pose_yaw
stddev"}} pc1 -->|"<= 0.3 rad"| pc2{{"gnss_pose_z
stddev"}} pc2 -->|"<= 0.1 m"| pc3{{"gnss_pose_xy
stddev"}} - pc2 -->|"> 0.1 m"| ndt_pose("NDT Pose") + pc2 -->|"> 0.1 m"| ndt_pose("NDT Pose") pc3 -->|"<= 0.1 m"| gnss_pose("GNSS Pose") pc3 -->|"0.1 m < x <= 0.2 m"| gnss_ndt_pose("`Both GNSS and NDT Pose (_with modified covariance_)`") - pc3 -->|"> 0.2 m"| ndt_pose - pc1 -->|"> 0.3 rad"| ndt_pose + pc3 -->|"> 0.2 m"| ndt_pose + pc1 -->|"> 0.3 rad"| ndt_pose end pose_covariance_modifier_node -->|"/localization/pose_estimator/pose_with_covariance"| ekf_localizer["ekf_localizer"] diff --git a/perception/autoware_cluster_merger/package.xml b/perception/autoware_cluster_merger/package.xml index 879cf1da2a565..ff3eb60ec9c45 100644 --- a/perception/autoware_cluster_merger/package.xml +++ b/perception/autoware_cluster_merger/package.xml @@ -12,11 +12,11 @@ ament_cmake_auto + autoware_object_recognition_utils autoware_test_utils autoware_universe_utils geometry_msgs message_filters - object_recognition_utils rclcpp rclcpp_components tier4_perception_msgs diff --git a/perception/autoware_cluster_merger/src/cluster_merger_node.cpp b/perception/autoware_cluster_merger/src/cluster_merger_node.cpp index 554196787c028..42e0ef005057f 100644 --- a/perception/autoware_cluster_merger/src/cluster_merger_node.cpp +++ b/perception/autoware_cluster_merger/src/cluster_merger_node.cpp @@ -14,7 +14,7 @@ #include "cluster_merger_node.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -52,9 +52,9 @@ void ClusterMergerNode::objectsCallback( DetectedObjectsWithFeature transformed_objects0; DetectedObjectsWithFeature transformed_objects1; if ( - !object_recognition_utils::transformObjectsWithFeature( + !autoware::object_recognition_utils::transformObjectsWithFeature( *input_objects0_msg, output_frame_id_, tf_buffer_, transformed_objects0) || - !object_recognition_utils::transformObjectsWithFeature( + !autoware::object_recognition_utils::transformObjectsWithFeature( *input_objects1_msg, output_frame_id_, tf_buffer_, transformed_objects1)) { return; } diff --git a/perception/autoware_detected_object_validation/package.xml b/perception/autoware_detected_object_validation/package.xml index b46c182d2cb57..4f136ad6270c8 100644 --- a/perception/autoware_detected_object_validation/package.xml +++ b/perception/autoware_detected_object_validation/package.xml @@ -19,12 +19,12 @@ autoware_lanelet2_extension autoware_map_msgs + autoware_object_recognition_utils autoware_perception_msgs autoware_test_utils autoware_universe_utils message_filters nav_msgs - object_recognition_utils pcl_conversions rclcpp rclcpp_components 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 59de872ae9b90..e54ffd5452e7c 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 @@ -14,11 +14,11 @@ #include "lanelet_filter.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_lanelet2_extension/utility/message_conversion.hpp" #include "autoware_lanelet2_extension/utility/query.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -97,7 +97,7 @@ void ObjectLaneletFilterNode::objectCallback( return; } autoware_perception_msgs::msg::DetectedObjects transformed_objects; - if (!object_recognition_utils::transformObjects( + if (!autoware::object_recognition_utils::transformObjects( *input_msg, lanelet_frame_id_, tf_buffer_, transformed_objects)) { RCLCPP_ERROR(get_logger(), "Failed transform to %s.", lanelet_frame_id_.c_str()); return; 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 7f55c86080fd2..e3889ae8f6513 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 @@ -16,8 +16,8 @@ #include "obstacle_pointcloud_validator.hpp" +#include #include -#include #include @@ -325,7 +325,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( // Transform to pointcloud frame autoware_perception_msgs::msg::DetectedObjects transformed_objects; - if (!object_recognition_utils::transformObjects( + if (!autoware::object_recognition_utils::transformObjects( *input_objects, input_obstacle_pointcloud->header.frame_id, tf_buffer_, transformed_objects)) { // objects_pub_->publish(*input_objects); diff --git a/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp b/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp index 51df1929a300c..a78e2bf90d8c2 100644 --- a/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp +++ b/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp @@ -16,9 +16,9 @@ #include "occupancy_grid_map_validator.hpp" +#include "autoware/object_recognition_utils/object_classification.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "object_recognition_utils/object_classification.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -68,7 +68,7 @@ void OccupancyGridBasedValidator::onObjectsAndOccGrid( // Transform to occ grid frame autoware_perception_msgs::msg::DetectedObjects transformed_objects; - if (!object_recognition_utils::transformObjects( + if (!autoware::object_recognition_utils::transformObjects( *input_objects, input_occ_grid->header.frame_id, tf_buffer_, transformed_objects)) return; @@ -80,7 +80,7 @@ void OccupancyGridBasedValidator::onObjectsAndOccGrid( const auto & transformed_object = transformed_objects.objects.at(i); const auto & object = input_objects->objects.at(i); const auto & label = object.classification.front().label; - if (object_recognition_utils::isCarLikeVehicle(label)) { + if (autoware::object_recognition_utils::isCarLikeVehicle(label)) { auto mask = getMask(*input_occ_grid, transformed_object); const float mean = mask ? cv::mean(occ_grid, mask.value())[0] * 0.01 : 1.0; if (mean_threshold_ < mean) output.objects.push_back(object); @@ -159,7 +159,7 @@ void OccupancyGridBasedValidator::showDebugImage( // Get vehicle mask image and calculate mean within mask. for (const auto & object : objects.objects) { const auto & label = object.classification.front().label; - if (object_recognition_utils::isCarLikeVehicle(label)) { + if (autoware::object_recognition_utils::isCarLikeVehicle(label)) { auto mask = getMask(ros_occ_grid, object); const float mean = mask ? cv::mean(occ_grid, mask.value())[0] * 0.01 : 1.0; if (mean_threshold_ < mean) { diff --git a/perception/autoware_detection_by_tracker/package.xml b/perception/autoware_detection_by_tracker/package.xml index 6f23baeefbd3e..e84e13c484e16 100644 --- a/perception/autoware_detection_by_tracker/package.xml +++ b/perception/autoware_detection_by_tracker/package.xml @@ -14,10 +14,10 @@ eigen3_cmake_module autoware_euclidean_cluster + autoware_object_recognition_utils autoware_shape_estimation autoware_universe_utils eigen - object_recognition_utils rclcpp rclcpp_components tf2 diff --git a/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.cpp b/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.cpp index 4c65500d96aaa..571b6682eb5b0 100644 --- a/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.cpp +++ b/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.cpp @@ -16,9 +16,9 @@ #include "detection_by_tracker_node.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -161,14 +161,14 @@ void DetectionByTracker::onObjects( tracker_handler_.estimateTrackedObjects(input_msg->header.stamp, objects); if ( !available_trackers || - !object_recognition_utils::transformObjects( + !autoware::object_recognition_utils::transformObjects( objects, input_msg->header.frame_id, tf_buffer_, transformed_objects)) { objects_pub_->publish(detected_objects); published_time_publisher_->publish_if_subscribed(objects_pub_, detected_objects.header.stamp); return; } // to simplify post processes, convert tracked_objects to DetectedObjects message. - tracked_objects = object_recognition_utils::toDetectedObjects(transformed_objects); + tracked_objects = autoware::object_recognition_utils::toDetectedObjects(transformed_objects); } debugger_->publishInitialObjects(*input_msg); debugger_->publishTrackedObjects(tracked_objects); @@ -233,9 +233,9 @@ void DetectionByTracker::divideUnderSegmentedObjects( } // detect under segmented cluster const float recall = - object_recognition_utils::get2dRecall(initial_object.object, tracked_object); + autoware::object_recognition_utils::get2dRecall(initial_object.object, tracked_object); const float precision = - object_recognition_utils::get2dPrecision(initial_object.object, tracked_object); + autoware::object_recognition_utils::get2dPrecision(initial_object.object, tracked_object); const bool is_under_segmented = (recall_min_threshold < recall && precision < precision_max_threshold); if (!is_under_segmented) { @@ -310,7 +310,7 @@ float DetectionByTracker::optimizeUnderSegmentedObject( if (!is_shape_estimated) { continue; } - const float iou = object_recognition_utils::get2dIoU( + const float iou = autoware::object_recognition_utils::get2dIoU( highest_iou_object_in_current_iter.object, target_object); if (highest_iou_in_current_iter < iou) { highest_iou_in_current_iter = iou; @@ -332,7 +332,7 @@ float DetectionByTracker::optimizeUnderSegmentedObject( // build output highest_iou_object.object.classification = target_object.classification; highest_iou_object.object.existence_probability = - object_recognition_utils::get2dIoU(target_object, highest_iou_object.object); + autoware::object_recognition_utils::get2dIoU(target_object, highest_iou_object.object); output = highest_iou_object; return highest_iou; @@ -370,8 +370,8 @@ void DetectionByTracker::mergeOverSegmentedObjects( } // If there is an initial object in the tracker, it will be merged. - const float precision = - object_recognition_utils::get2dPrecision(initial_object.object, extended_tracked_object); + const float precision = autoware::object_recognition_utils::get2dPrecision( + initial_object.object, extended_tracked_object); if (precision < precision_threshold) { continue; } @@ -401,7 +401,7 @@ void DetectionByTracker::mergeOverSegmentedObjects( } feature_object.object.existence_probability = - object_recognition_utils::get2dIoU(tracked_object, feature_object.object); + autoware::object_recognition_utils::get2dIoU(tracked_object, feature_object.object); setClusterInObjectWithFeature(in_cluster_objects.header, pcl_merged_cluster, feature_object); out_objects.feature_objects.push_back(feature_object); } diff --git a/perception/autoware_ground_segmentation/docs/scan-ground-filter.md b/perception/autoware_ground_segmentation/docs/scan-ground-filter.md index 392bed89ac902..6b2f6d6b57571 100644 --- a/perception/autoware_ground_segmentation/docs/scan-ground-filter.md +++ b/perception/autoware_ground_segmentation/docs/scan-ground-filter.md @@ -8,14 +8,16 @@ The purpose of this node is that remove the ground points from the input pointcl This algorithm works by following steps, -1. Divide whole pointclouds into groups by horizontal angle and sort by xy-distance. -2. Divide sorted pointclouds of each ray into grids -3. Check the xy distance to previous pointcloud, if the distance is large and previous pointcloud is "no ground" and the height level of current point greater than previous point, the current pointcloud is classified as no ground. -4. Check vertical angle of the point compared with previous ground grid -5. Check the height of the point compared with predicted ground level -6. If vertical angle is greater than local_slope_max and related height to predicted ground level is greater than "non ground height threshold", the point is classified as "non ground" -7. If the vertical angle is in range of [-local_slope_max, local_slope_max] or related height to predicted ground level is smaller than non_ground_height_threshold, the point is classified as "ground" -8. If the vertical angle is lower than -local_slope_max or the related height to ground level is greater than detection_range_z_max, the point will be classified as out of range +1. Divide whole pointclouds into groups by azimuth angle (so-called ray) +2. Sort points by radial distance (xy-distance), on each ray. +3. Divide pointcloud into grids, on each ray. +4. Classify the point + 1. Check radial distance to previous pointcloud, if the distance is large and previous pointcloud is "no ground" and the height level of current point greater than previous point, the current pointcloud is classified as no ground. + 2. Check vertical angle of the point compared with previous ground grid + 3. Check the height of the point compared with predicted ground level + 4. If vertical angle is greater than local_slope_max and related height to predicted ground level is greater than "non ground height threshold", the point is classified as "non ground" + 5. If the vertical angle is in range of [-local_slope_max, local_slope_max] or related height to predicted ground level is smaller than non_ground_height_threshold, the point is classified as "ground" + 6. If the vertical angle is lower than -local_slope_max or the related height to ground level is greater than detection_range_z_max, the point will be classified as out of range ## Inputs / Outputs diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/grid.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/grid.hpp new file mode 100644 index 0000000000000..cd55d4a548211 --- /dev/null +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/grid.hpp @@ -0,0 +1,111 @@ +// 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 SCAN_GROUND_FILTER__GRID_HPP_ +#define SCAN_GROUND_FILTER__GRID_HPP_ + +#include +#include + +#include + +namespace autoware::ground_segmentation +{ + +class ScanGroundGrid +{ +public: + ScanGroundGrid() = default; + ~ScanGroundGrid() = default; + + void initialize( + const float grid_size_m, const float grid_mode_switch_radius, const float virtual_lidar_z) + { + grid_size_m_ = grid_size_m; + mode_switch_radius_ = grid_mode_switch_radius; + virtual_lidar_z_ = virtual_lidar_z; + + // calculate parameters + inv_grid_size_m_ = 1.0f / grid_size_m_; + mode_switch_grid_id_ = mode_switch_radius_ * inv_grid_size_m_; + mode_switch_angle_rad_ = std::atan2(mode_switch_radius_, virtual_lidar_z_); + + grid_size_rad_ = universe_utils::normalizeRadian( + std::atan2(mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - + universe_utils::normalizeRadian(mode_switch_angle_rad_); + inv_grid_size_rad_ = 1.0f / grid_size_rad_; + tan_grid_size_rad_ = std::tan(grid_size_rad_); + grid_id_offset_ = mode_switch_grid_id_ - mode_switch_angle_rad_ * inv_grid_size_rad_; + + is_initialized_ = true; + } + + float getGridSize(const float radius, const size_t grid_id) const + { + // check if initialized + if (!is_initialized_) { + throw std::runtime_error("ScanGroundGrid is not initialized."); + } + + float grid_size = grid_size_m_; + constexpr uint16_t back_steps_num = 1; + + if (radius > mode_switch_radius_ && grid_id > mode_switch_grid_id_ + back_steps_num) { + // equivalent to grid_size = (std::tan(gamma) - std::tan(gamma - grid_size_rad_)) * + // virtual_lidar_z_ + // where gamma = normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f) + grid_size = radius - (radius - tan_grid_size_rad_ * virtual_lidar_z_) / + (1 + radius * tan_grid_size_rad_ / virtual_lidar_z_); + } + return grid_size; + } + + uint16_t getGridId(const float radius) const + { + // check if initialized + if (!is_initialized_) { + throw std::runtime_error("ScanGroundGrid is not initialized."); + } + + uint16_t grid_id = 0; + if (radius <= mode_switch_radius_) { + grid_id = static_cast(radius * inv_grid_size_m_); + } else { + auto gamma{universe_utils::normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f)}; + grid_id = grid_id_offset_ + gamma * inv_grid_size_rad_; + } + return grid_id; + } + +private: + bool is_initialized_ = false; + + // configured parameters + float grid_size_m_ = 0.0f; + float mode_switch_radius_ = 0.0f; + float virtual_lidar_z_ = 0.0f; + + // calculated parameters + float inv_grid_size_m_ = 0.0f; + float grid_size_rad_ = 0.0f; + float inv_grid_size_rad_ = 0.0f; + float tan_grid_size_rad_ = 0.0f; + float mode_switch_grid_id_ = 0.0f; + float mode_switch_angle_rad_ = 0.0f; + float grid_id_offset_ = 0.0f; +}; + +} // namespace autoware::ground_segmentation + +#endif // SCAN_GROUND_FILTER__GRID_HPP_ 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 72481d13c41fb..c0419c7121c64 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp @@ -14,10 +14,10 @@ #include "node.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" +#include +#include +#include +#include #include #include @@ -38,40 +38,48 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & { // set initial parameters { - low_priority_region_x_ = declare_parameter("low_priority_region_x"); - detection_range_z_max_ = declare_parameter("detection_range_z_max"); - center_pcl_shift_ = declare_parameter("center_pcl_shift"); - non_ground_height_threshold_ = declare_parameter("non_ground_height_threshold"); - grid_mode_switch_radius_ = declare_parameter("grid_mode_switch_radius"); - - grid_size_m_ = declare_parameter("grid_size_m"); - gnd_grid_buffer_size_ = declare_parameter("gnd_grid_buffer_size"); + // modes elevation_grid_mode_ = declare_parameter("elevation_grid_mode"); + + // common parameters + radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg")); + radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_); + + // common thresholds global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg")); local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg")); global_slope_max_ratio_ = std::tan(global_slope_max_angle_rad_); local_slope_max_ratio_ = std::tan(local_slope_max_angle_rad_); - radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg")); split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance"); split_points_distance_tolerance_square_ = split_points_distance_tolerance_ * split_points_distance_tolerance_; - split_height_distance_ = declare_parameter("split_height_distance"); + + // vehicle info + vehicle_info_ = VehicleInfoUtils(*this).getVehicleInfo(); + + // non-grid parameters use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point"); + split_height_distance_ = declare_parameter("split_height_distance"); + + // grid mode parameters use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster"); use_lowest_point_ = declare_parameter("use_lowest_point"); - radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_); - vehicle_info_ = VehicleInfoUtils(*this).getVehicleInfo(); + detection_range_z_max_ = declare_parameter("detection_range_z_max"); + low_priority_region_x_ = declare_parameter("low_priority_region_x"); + center_pcl_shift_ = declare_parameter("center_pcl_shift"); + non_ground_height_threshold_ = declare_parameter("non_ground_height_threshold"); - grid_mode_switch_grid_id_ = - grid_mode_switch_radius_ / grid_size_m_; // changing the mode of grid division + // grid parameters + grid_size_m_ = declare_parameter("grid_size_m"); + grid_mode_switch_radius_ = declare_parameter("grid_mode_switch_radius"); + gnd_grid_buffer_size_ = declare_parameter("gnd_grid_buffer_size"); virtual_lidar_z_ = vehicle_info_.vehicle_height_m; - grid_mode_switch_angle_rad_ = std::atan2(grid_mode_switch_radius_, virtual_lidar_z_); - grid_size_rad_ = - normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - - normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); - tan_grid_size_rad_ = std::tan(grid_size_rad_); - offset_initialized_ = false; + // initialize grid + grid_.initialize(grid_size_m_, grid_mode_switch_radius_, virtual_lidar_z_); + + // data access + data_offset_initialized_ = false; } using std::placeholders::_1; @@ -98,31 +106,32 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & } } -inline void ScanGroundFilterComponent::set_field_offsets(const PointCloud2ConstPtr & input) +inline void ScanGroundFilterComponent::set_field_index_offsets(const PointCloud2ConstPtr & input) { - x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; - y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; - z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; + data_offset_x_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; + data_offset_y_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; + data_offset_z_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; int intensity_index = pcl::getFieldIndex(*input, "intensity"); if (intensity_index != -1) { - intensity_offset_ = input->fields[intensity_index].offset; + data_offset_intensity_ = input->fields[intensity_index].offset; intensity_type_ = input->fields[intensity_index].datatype; } else { - intensity_offset_ = -1; + data_offset_intensity_ = -1; } - offset_initialized_ = true; + data_offset_initialized_ = true; } -inline void ScanGroundFilterComponent::get_point_from_global_offset( - const PointCloud2ConstPtr & input, pcl::PointXYZ & point, size_t global_offset) +inline void ScanGroundFilterComponent::get_point_from_data_index( + const PointCloud2ConstPtr & input, const size_t data_index, pcl::PointXYZ & point) const { - point.x = *reinterpret_cast(&input->data[global_offset + x_offset_]); - point.y = *reinterpret_cast(&input->data[global_offset + y_offset_]); - point.z = *reinterpret_cast(&input->data[global_offset + z_offset_]); + point.x = *reinterpret_cast(&input->data[data_index + data_offset_x_]); + point.y = *reinterpret_cast(&input->data[data_index + data_offset_y_]); + point.z = *reinterpret_cast(&input->data[data_index + data_offset_z_]); } void ScanGroundFilterComponent::convertPointcloudGridScan( - const PointCloud2ConstPtr & in_cloud, std::vector & out_radial_ordered_points) + const PointCloud2ConstPtr & in_cloud, + std::vector & out_radial_ordered_points) const { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -131,52 +140,41 @@ void ScanGroundFilterComponent::convertPointcloudGridScan( PointData current_point; const auto inv_radial_divider_angle_rad = 1.0f / radial_divider_angle_rad_; - const auto inv_grid_size_rad = 1.0f / grid_size_rad_; - const auto inv_grid_size_m = 1.0f / grid_size_m_; - - const auto grid_id_offset = - grid_mode_switch_grid_id_ - grid_mode_switch_angle_rad_ * inv_grid_size_rad; const auto x_shift = vehicle_info_.wheel_base_m / 2.0f + center_pcl_shift_; const size_t in_cloud_data_size = in_cloud->data.size(); const size_t in_cloud_point_step = in_cloud->point_step; - { // grouping pointcloud by its horizontal angle + { // grouping pointcloud by its azimuth angle std::unique_ptr inner_st_ptr; if (time_keeper_) - inner_st_ptr = std::make_unique("horizontal_angle_grouping", *time_keeper_); + inner_st_ptr = std::make_unique("azimuth_angle_grouping", *time_keeper_); size_t point_index = 0; pcl::PointXYZ input_point; - for (size_t global_offset = 0; global_offset + in_cloud_point_step <= in_cloud_data_size; - global_offset += in_cloud_point_step) { - get_point_from_global_offset(in_cloud, input_point, global_offset); + for (size_t data_index = 0; data_index + in_cloud_point_step <= in_cloud_data_size; + data_index += in_cloud_point_step) { + // Get Point + get_point_from_data_index(in_cloud, data_index, input_point); + // determine the azimuth angle group auto x{input_point.x - x_shift}; // base on front wheel center auto radius{static_cast(std::hypot(x, input_point.y))}; auto theta{normalizeRadian(std::atan2(x, input_point.y), 0.0)}; - - // divide by vertical angle auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; - uint16_t grid_id = 0; - if (radius <= grid_mode_switch_radius_) { - grid_id = static_cast(radius * inv_grid_size_m); - } else { - auto gamma{normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f)}; - grid_id = grid_id_offset + gamma * inv_grid_size_rad; - } - current_point.grid_id = grid_id; + current_point.radius = radius; current_point.point_state = PointLabel::INIT; current_point.orig_index = point_index; + current_point.grid_id = grid_.getGridId(radius); - // radial divisions + // store the point in the corresponding radial division out_radial_ordered_points[radial_div].emplace_back(current_point); ++point_index; } } - { // sorting pointcloud by distance, on each horizontal angle group + { // sorting pointcloud by distance, on each azimuth angle group std::unique_ptr inner_st_ptr; if (time_keeper_) inner_st_ptr = std::make_unique("sort", *time_keeper_); @@ -189,7 +187,8 @@ void ScanGroundFilterComponent::convertPointcloudGridScan( } void ScanGroundFilterComponent::convertPointcloud( - const PointCloud2ConstPtr & in_cloud, std::vector & out_radial_ordered_points) + const PointCloud2ConstPtr & in_cloud, + std::vector & out_radial_ordered_points) const { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -202,18 +201,19 @@ void ScanGroundFilterComponent::convertPointcloud( const size_t in_cloud_data_size = in_cloud->data.size(); const size_t in_cloud_point_step = in_cloud->point_step; - { // grouping pointcloud by its horizontal angle + { // grouping pointcloud by its azimuth angle std::unique_ptr inner_st_ptr; if (time_keeper_) - inner_st_ptr = std::make_unique("horizontal_angle_grouping", *time_keeper_); + inner_st_ptr = std::make_unique("azimuth_angle_grouping", *time_keeper_); size_t point_index = 0; pcl::PointXYZ input_point; - for (size_t global_offset = 0; global_offset + in_cloud_point_step <= in_cloud_data_size; - global_offset += in_cloud_point_step) { - // Point - get_point_from_global_offset(in_cloud, input_point, global_offset); + for (size_t data_index = 0; data_index + in_cloud_point_step <= in_cloud_data_size; + data_index += in_cloud_point_step) { + // Get Point + get_point_from_data_index(in_cloud, data_index, input_point); + // determine the azimuth angle group auto radius{static_cast(std::hypot(input_point.x, input_point.y))}; auto theta{normalizeRadian(std::atan2(input_point.x, input_point.y), 0.0)}; auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; @@ -222,13 +222,13 @@ void ScanGroundFilterComponent::convertPointcloud( current_point.point_state = PointLabel::INIT; current_point.orig_index = point_index; - // radial divisions + // store the point in the corresponding radial division out_radial_ordered_points[radial_div].emplace_back(current_point); ++point_index; } } - { // sorting pointcloud by distance, on each horizontal angle group + { // sorting pointcloud by distance, on each azimuth angle group std::unique_ptr inner_st_ptr; if (time_keeper_) inner_st_ptr = std::make_unique("sort", *time_keeper_); @@ -240,30 +240,15 @@ void ScanGroundFilterComponent::convertPointcloud( } } -void ScanGroundFilterComponent::calcVirtualGroundOrigin(pcl::PointXYZ & point) +void ScanGroundFilterComponent::calcVirtualGroundOrigin(pcl::PointXYZ & point) const { point.x = vehicle_info_.wheel_base_m; point.y = 0; point.z = 0; } -inline float ScanGroundFilterComponent::calcGridSize(const PointData & p) -{ - float grid_size = grid_size_m_; - uint16_t back_steps_num = 1; - - if ( - p.radius > grid_mode_switch_radius_ && p.grid_id > grid_mode_switch_grid_id_ + back_steps_num) { - // equivalent to grid_size = (std::tan(gamma) - std::tan(gamma - grid_size_rad_)) * - // virtual_lidar_z_ when gamma = normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f) - grid_size = p.radius - (p.radius - tan_grid_size_rad_ * virtual_lidar_z_) / - (1 + p.radius * tan_grid_size_rad_ / virtual_lidar_z_); - } - return grid_size; -} - void ScanGroundFilterComponent::initializeFirstGndGrids( - const float h, const float r, const uint16_t id, std::vector & gnd_grids) + const float h, const float r, const uint16_t id, std::vector & gnd_grids) const { GridCenter curr_gnd_grid; for (int ind_grid = id - 1 - gnd_grid_buffer_size_; ind_grid < id - 1; ++ind_grid) { @@ -282,7 +267,8 @@ void ScanGroundFilterComponent::initializeFirstGndGrids( } void ScanGroundFilterComponent::checkContinuousGndGrid( - PointData & p, const pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) + PointData & pd, const pcl::PointXYZ & point_curr, + const std::vector & gnd_grids_list) const { float next_gnd_z = 0.0f; float curr_gnd_slope_ratio = 0.0f; @@ -307,56 +293,58 @@ void ScanGroundFilterComponent::checkContinuousGndGrid( curr_gnd_slope_ratio = curr_gnd_slope_ratio > global_slope_max_ratio_ ? global_slope_max_ratio_ : curr_gnd_slope_ratio; - next_gnd_z = curr_gnd_slope_ratio * (p.radius - gnd_buff_radius) + gnd_buff_z_mean; + next_gnd_z = curr_gnd_slope_ratio * (pd.radius - gnd_buff_radius) + gnd_buff_z_mean; - float gnd_z_local_thresh = std::tan(DEG2RAD(5.0)) * (p.radius - gnd_grids_list.back().radius); + float gnd_z_local_thresh = std::tan(DEG2RAD(5.0)) * (pd.radius - gnd_grids_list.back().radius); - tmp_delta_mean_z = p_orig_point.z - (gnd_grids_list.end() - 2)->avg_height; - tmp_delta_radius = p.radius - (gnd_grids_list.end() - 2)->radius; + tmp_delta_mean_z = point_curr.z - (gnd_grids_list.end() - 2)->avg_height; + tmp_delta_radius = pd.radius - (gnd_grids_list.end() - 2)->radius; float local_slope_ratio = tmp_delta_mean_z / tmp_delta_radius; if ( - abs(p_orig_point.z - next_gnd_z) <= non_ground_height_threshold_ + gnd_z_local_thresh || + abs(point_curr.z - next_gnd_z) <= non_ground_height_threshold_ + gnd_z_local_thresh || abs(local_slope_ratio) <= local_slope_max_ratio_) { - p.point_state = PointLabel::GROUND; - } else if (p_orig_point.z - next_gnd_z > non_ground_height_threshold_ + gnd_z_local_thresh) { - p.point_state = PointLabel::NON_GROUND; + pd.point_state = PointLabel::GROUND; + } else if (point_curr.z - next_gnd_z > non_ground_height_threshold_ + gnd_z_local_thresh) { + pd.point_state = PointLabel::NON_GROUND; } } void ScanGroundFilterComponent::checkDiscontinuousGndGrid( - PointData & p, const pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) + PointData & pd, const pcl::PointXYZ & point_curr, + const std::vector & gnd_grids_list) const { - float tmp_delta_max_z = p_orig_point.z - gnd_grids_list.back().max_height; - float tmp_delta_avg_z = p_orig_point.z - gnd_grids_list.back().avg_height; - float tmp_delta_radius = p.radius - gnd_grids_list.back().radius; + float tmp_delta_max_z = point_curr.z - gnd_grids_list.back().max_height; + float tmp_delta_avg_z = point_curr.z - gnd_grids_list.back().avg_height; + float tmp_delta_radius = pd.radius - gnd_grids_list.back().radius; float local_slope_ratio = tmp_delta_avg_z / tmp_delta_radius; if ( abs(local_slope_ratio) < local_slope_max_ratio_ || abs(tmp_delta_avg_z) < non_ground_height_threshold_ || abs(tmp_delta_max_z) < non_ground_height_threshold_) { - p.point_state = PointLabel::GROUND; + pd.point_state = PointLabel::GROUND; } else if (local_slope_ratio > global_slope_max_ratio_) { - p.point_state = PointLabel::NON_GROUND; + pd.point_state = PointLabel::NON_GROUND; } } void ScanGroundFilterComponent::checkBreakGndGrid( - PointData & p, const pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) + PointData & pd, const pcl::PointXYZ & point_curr, + const std::vector & gnd_grids_list) const { - float tmp_delta_avg_z = p_orig_point.z - gnd_grids_list.back().avg_height; - float tmp_delta_radius = p.radius - gnd_grids_list.back().radius; + float tmp_delta_avg_z = point_curr.z - gnd_grids_list.back().avg_height; + float tmp_delta_radius = pd.radius - gnd_grids_list.back().radius; float local_slope_ratio = tmp_delta_avg_z / tmp_delta_radius; if (abs(local_slope_ratio) < global_slope_max_ratio_) { - p.point_state = PointLabel::GROUND; + pd.point_state = PointLabel::GROUND; } else if (local_slope_ratio > global_slope_max_ratio_) { - p.point_state = PointLabel::NON_GROUND; + pd.point_state = PointLabel::NON_GROUND; } } void ScanGroundFilterComponent::recheckGroundCluster( - PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point, - pcl::PointIndices & non_ground_indices) + const PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point, + pcl::PointIndices & non_ground_indices) const { float reference_height = use_lowest_point ? gnd_cluster.getMinHeight() : gnd_cluster.getAverageHeight(); @@ -370,131 +358,157 @@ void ScanGroundFilterComponent::recheckGroundCluster( } void ScanGroundFilterComponent::classifyPointCloudGridScan( - const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, - pcl::PointIndices & out_no_ground_indices) + const PointCloud2ConstPtr & in_cloud, + const std::vector & in_radial_ordered_clouds, + pcl::PointIndices & out_no_ground_indices) const { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); out_no_ground_indices.indices.clear(); + + // run the classification algorithm for each ray (azimuth division) for (size_t i = 0; i < in_radial_ordered_clouds.size(); ++i) { - PointsCentroid ground_cluster; - ground_cluster.initialize(); + PointsCentroid centroid_bin; + centroid_bin.initialize(); std::vector gnd_grids; - GridCenter curr_gnd_grid; // check empty ray if (in_radial_ordered_clouds[i].size() == 0) { continue; } - // check the first point in ray - auto * p = &in_radial_ordered_clouds[i][0]; - bool initialized_first_gnd_grid = false; bool prev_list_init = false; - pcl::PointXYZ p_orig_point, prev_p_orig_point; - for (auto & point : in_radial_ordered_clouds[i]) { - auto * prev_p = p; // for checking the distance to prev point - prev_p_orig_point = p_orig_point; - p = &point; - get_point_from_global_offset(in_cloud, p_orig_point, in_cloud->point_step * p->orig_index); - float global_slope_ratio_p = p_orig_point.z / p->radius; + + PointData pd_curr, pd_prev; + pcl::PointXYZ point_curr, point_prev; + + // initialize the previous point + pd_curr = in_radial_ordered_clouds[i][0]; + + // iterate over the points in the ray + for (const auto & point : in_radial_ordered_clouds[i]) { + // set the previous point + point_prev = point_curr; + pd_prev = pd_curr; + + // set the current point + pd_curr = point; + const size_t data_index = in_cloud->point_step * pd_curr.orig_index; + get_point_from_data_index(in_cloud, data_index, point_curr); + + // set the thresholds + const float global_slope_ratio_p = point_curr.z / pd_curr.radius; float non_ground_height_threshold_local = non_ground_height_threshold_; - if (p_orig_point.x < low_priority_region_x_) { + if (point_curr.x < low_priority_region_x_) { non_ground_height_threshold_local = - non_ground_height_threshold_ * abs(p_orig_point.x / low_priority_region_x_); - } - // classify first grid's point cloud - if ( - !initialized_first_gnd_grid && global_slope_ratio_p >= global_slope_max_ratio_ && - p_orig_point.z > non_ground_height_threshold_local) { - out_no_ground_indices.indices.push_back(p->orig_index); - p->point_state = PointLabel::NON_GROUND; - continue; - } - - if ( - !initialized_first_gnd_grid && abs(global_slope_ratio_p) < global_slope_max_ratio_ && - abs(p_orig_point.z) < non_ground_height_threshold_local) { - ground_cluster.addPoint(p->radius, p_orig_point.z, p->orig_index); - p->point_state = PointLabel::GROUND; - initialized_first_gnd_grid = static_cast(p->grid_id - prev_p->grid_id); - continue; + non_ground_height_threshold_ * abs(point_curr.x / low_priority_region_x_); } if (!initialized_first_gnd_grid) { + // classify first grid's point cloud + if ( + global_slope_ratio_p >= global_slope_max_ratio_ && + point_curr.z > non_ground_height_threshold_local) { + out_no_ground_indices.indices.push_back(pd_curr.orig_index); + pd_curr.point_state = PointLabel::NON_GROUND; + } else if ( + abs(global_slope_ratio_p) < global_slope_max_ratio_ && + abs(point_curr.z) < non_ground_height_threshold_local) { + centroid_bin.addPoint(pd_curr.radius, point_curr.z, pd_curr.orig_index); + pd_curr.point_state = PointLabel::GROUND; + // if the gird id is not the initial grid_id, then the first gnd grid is initialized + initialized_first_gnd_grid = static_cast(pd_curr.grid_id - pd_prev.grid_id); + } + // else, the point is not classified continue; } - // initialize lists of previous gnd grids + // initialize gnd_grids based on the initial centroid_bin if (!prev_list_init) { - float h = ground_cluster.getAverageHeight(); - float r = ground_cluster.getAverageRadius(); - initializeFirstGndGrids(h, r, p->grid_id, gnd_grids); + float h = centroid_bin.getAverageHeight(); + float r = centroid_bin.getAverageRadius(); + initializeFirstGndGrids(h, r, pd_curr.grid_id, gnd_grids); prev_list_init = true; } - // move to new grid - if (p->grid_id > prev_p->grid_id && ground_cluster.getAverageRadius() > 0.0) { + // finalize the current centroid_bin and update the gnd_grids + if (pd_curr.grid_id > pd_prev.grid_id && centroid_bin.getAverageRadius() > 0.0) { // check if the prev grid have ground point cloud if (use_recheck_ground_cluster_) { recheckGroundCluster( - ground_cluster, non_ground_height_threshold_, use_lowest_point_, out_no_ground_indices); + centroid_bin, non_ground_height_threshold_, use_lowest_point_, out_no_ground_indices); + // centroid_bin is not modified. should be rechecked by out_no_ground_indices? } - curr_gnd_grid.radius = ground_cluster.getAverageRadius(); - curr_gnd_grid.avg_height = ground_cluster.getAverageHeight(); - curr_gnd_grid.max_height = ground_cluster.getMaxHeight(); - curr_gnd_grid.grid_id = prev_p->grid_id; + // convert the centroid_bin to grid-center and add it to the gnd_grids + GridCenter curr_gnd_grid; + curr_gnd_grid.radius = centroid_bin.getAverageRadius(); + curr_gnd_grid.avg_height = centroid_bin.getAverageHeight(); + curr_gnd_grid.max_height = centroid_bin.getMaxHeight(); + curr_gnd_grid.grid_id = pd_prev.grid_id; gnd_grids.push_back(curr_gnd_grid); - ground_cluster.initialize(); + // clear the centroid_bin + centroid_bin.initialize(); } - // classify - if (p_orig_point.z - gnd_grids.back().avg_height > detection_range_z_max_) { - p->point_state = PointLabel::OUT_OF_RANGE; + + // 1: height is out-of-range + if (point_curr.z - gnd_grids.back().avg_height > detection_range_z_max_) { + pd_curr.point_state = PointLabel::OUT_OF_RANGE; continue; } + + // 2: continuously non-ground float points_xy_distance_square = - (p_orig_point.x - prev_p_orig_point.x) * (p_orig_point.x - prev_p_orig_point.x) + - (p_orig_point.y - prev_p_orig_point.y) * (p_orig_point.y - prev_p_orig_point.y); + (point_curr.x - point_prev.x) * (point_curr.x - point_prev.x) + + (point_curr.y - point_prev.y) * (point_curr.y - point_prev.y); if ( - prev_p->point_state == PointLabel::NON_GROUND && + pd_prev.point_state == PointLabel::NON_GROUND && points_xy_distance_square < split_points_distance_tolerance_square_ && - p_orig_point.z > prev_p_orig_point.z) { - p->point_state = PointLabel::NON_GROUND; - out_no_ground_indices.indices.push_back(p->orig_index); + point_curr.z > point_prev.z) { + pd_curr.point_state = PointLabel::NON_GROUND; + out_no_ground_indices.indices.push_back(pd_curr.orig_index); continue; } + + // 3: the angle is exceed the global slope threshold if (global_slope_ratio_p > global_slope_max_ratio_) { - out_no_ground_indices.indices.push_back(p->orig_index); + out_no_ground_indices.indices.push_back(pd_curr.orig_index); continue; } - // gnd grid is continuous, the last gnd grid is close + uint16_t next_gnd_grid_id_thresh = (gnd_grids.end() - gnd_grid_buffer_size_)->grid_id + gnd_grid_buffer_size_ + gnd_grid_continual_thresh_; - float curr_grid_size = calcGridSize(*p); + float curr_grid_size = grid_.getGridSize(pd_curr.radius, pd_curr.grid_id); if ( - p->grid_id < next_gnd_grid_id_thresh && - p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) { - checkContinuousGndGrid(*p, p_orig_point, gnd_grids); + // 4: the point is continuous with the previous grid + pd_curr.grid_id < next_gnd_grid_id_thresh && + pd_curr.radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) { + checkContinuousGndGrid(pd_curr, point_curr, gnd_grids); } else if ( - p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) { - checkDiscontinuousGndGrid(*p, p_orig_point, gnd_grids); + // 5: the point is discontinuous with the previous grid + pd_curr.radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) { + checkDiscontinuousGndGrid(pd_curr, point_curr, gnd_grids); } else { - checkBreakGndGrid(*p, p_orig_point, gnd_grids); + // 6: the point is break the previous grid + checkBreakGndGrid(pd_curr, point_curr, gnd_grids); } - if (p->point_state == PointLabel::NON_GROUND) { - out_no_ground_indices.indices.push_back(p->orig_index); - } else if (p->point_state == PointLabel::GROUND) { - ground_cluster.addPoint(p->radius, p_orig_point.z, p->orig_index); + + // update the point label and update the ground cluster + if (pd_curr.point_state == PointLabel::NON_GROUND) { + out_no_ground_indices.indices.push_back(pd_curr.orig_index); + } else if (pd_curr.point_state == PointLabel::GROUND) { + centroid_bin.addPoint(pd_curr.radius, point_curr.z, pd_curr.orig_index); } + // else, the point is not classified } } } void ScanGroundFilterComponent::classifyPointCloud( - const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, - pcl::PointIndices & out_no_ground_indices) + const PointCloud2ConstPtr & in_cloud, + const std::vector & in_radial_ordered_clouds, + pcl::PointIndices & out_no_ground_indices) const { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -505,24 +519,32 @@ void ScanGroundFilterComponent::classifyPointCloud( pcl::PointXYZ virtual_ground_point(0, 0, 0); calcVirtualGroundOrigin(virtual_ground_point); - // point classification algorithm - // sweep through each radial division + // run the classification algorithm for each ray (azimuth division) for (size_t i = 0; i < in_radial_ordered_clouds.size(); ++i) { float prev_gnd_radius = 0.0f; float prev_gnd_slope = 0.0f; PointsCentroid ground_cluster, non_ground_cluster; - PointLabel prev_point_label = PointLabel::INIT; - pcl::PointXYZ prev_gnd_point(0, 0, 0), p_orig_point, prev_p_orig_point; + PointLabel point_label_curr = PointLabel::INIT; + + pcl::PointXYZ prev_gnd_point(0, 0, 0), point_curr, point_prev; - // loop through each point in the radial div + // iterate over the points in the ray for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); ++j) { float points_distance = 0.0f; const float local_slope_max_angle = local_slope_max_angle_rad_; - prev_p_orig_point = p_orig_point; - auto * p = &in_radial_ordered_clouds[i][j]; - get_point_from_global_offset(in_cloud, p_orig_point, in_cloud->point_step * p->orig_index); + + // set the previous point + point_prev = point_curr; + PointLabel point_label_prev = point_label_curr; + + // set the current point + const PointData & pd = in_radial_ordered_clouds[i][j]; + point_label_curr = pd.point_state; + + const size_t data_index = in_cloud->point_step * pd.orig_index; + get_point_from_data_index(in_cloud, data_index, point_curr); if (j == 0) { - bool is_front_side = (p_orig_point.x > virtual_ground_point.x); + bool is_front_side = (point_curr.x > virtual_ground_point.x); if (use_virtual_ground_point_ && is_front_side) { prev_gnd_point = virtual_ground_point; } else { @@ -532,78 +554,78 @@ void ScanGroundFilterComponent::classifyPointCloud( prev_gnd_slope = 0.0f; ground_cluster.initialize(); non_ground_cluster.initialize(); - points_distance = calcDistance3d(p_orig_point, prev_gnd_point); + points_distance = calcDistance3d(point_curr, prev_gnd_point); } else { - points_distance = calcDistance3d(p_orig_point, prev_p_orig_point); + points_distance = calcDistance3d(point_curr, point_prev); } - float radius_distance_from_gnd = p->radius - prev_gnd_radius; - float height_from_gnd = p_orig_point.z - prev_gnd_point.z; - float height_from_obj = p_orig_point.z - non_ground_cluster.getAverageHeight(); + float radius_distance_from_gnd = pd.radius - prev_gnd_radius; + float height_from_gnd = point_curr.z - prev_gnd_point.z; + float height_from_obj = point_curr.z - non_ground_cluster.getAverageHeight(); bool calculate_slope = false; bool is_point_close_to_prev = (points_distance < - (p->radius * radial_divider_angle_rad_ + split_points_distance_tolerance_)); + (pd.radius * radial_divider_angle_rad_ + split_points_distance_tolerance_)); - float global_slope_ratio = p_orig_point.z / p->radius; + float global_slope_ratio = point_curr.z / pd.radius; // check points which is far enough from previous point if (global_slope_ratio > global_slope_max_ratio_) { - p->point_state = PointLabel::NON_GROUND; + point_label_curr = PointLabel::NON_GROUND; calculate_slope = false; } else if ( - (prev_point_label == PointLabel::NON_GROUND) && + (point_label_prev == PointLabel::NON_GROUND) && (std::abs(height_from_obj) >= split_height_distance_)) { calculate_slope = true; } else if (is_point_close_to_prev && std::abs(height_from_gnd) < split_height_distance_) { // close to the previous point, set point follow label - p->point_state = PointLabel::POINT_FOLLOW; + point_label_curr = PointLabel::POINT_FOLLOW; calculate_slope = false; } else { calculate_slope = true; } if (is_point_close_to_prev) { - height_from_gnd = p_orig_point.z - ground_cluster.getAverageHeight(); - radius_distance_from_gnd = p->radius - ground_cluster.getAverageRadius(); + height_from_gnd = point_curr.z - ground_cluster.getAverageHeight(); + radius_distance_from_gnd = pd.radius - ground_cluster.getAverageRadius(); } if (calculate_slope) { // far from the previous point auto local_slope = std::atan2(height_from_gnd, radius_distance_from_gnd); if (local_slope - prev_gnd_slope > local_slope_max_angle) { // the point is outside of the local slope threshold - p->point_state = PointLabel::NON_GROUND; + point_label_curr = PointLabel::NON_GROUND; } else { - p->point_state = PointLabel::GROUND; + point_label_curr = PointLabel::GROUND; } } - if (p->point_state == PointLabel::GROUND) { + if (point_label_curr == PointLabel::GROUND) { ground_cluster.initialize(); non_ground_cluster.initialize(); } - if (p->point_state == PointLabel::NON_GROUND) { - out_no_ground_indices.indices.push_back(p->orig_index); + if (point_label_curr == PointLabel::NON_GROUND) { + out_no_ground_indices.indices.push_back(pd.orig_index); } else if ( // NOLINT - (prev_point_label == PointLabel::NON_GROUND) && - (p->point_state == PointLabel::POINT_FOLLOW)) { - p->point_state = PointLabel::NON_GROUND; - out_no_ground_indices.indices.push_back(p->orig_index); + (point_label_prev == PointLabel::NON_GROUND) && + (point_label_curr == PointLabel::POINT_FOLLOW)) { + point_label_curr = PointLabel::NON_GROUND; + out_no_ground_indices.indices.push_back(pd.orig_index); } else if ( // NOLINT - (prev_point_label == PointLabel::GROUND) && (p->point_state == PointLabel::POINT_FOLLOW)) { - p->point_state = PointLabel::GROUND; + (point_label_prev == PointLabel::GROUND) && + (point_label_curr == PointLabel::POINT_FOLLOW)) { + point_label_curr = PointLabel::GROUND; } else { } // update the ground state - prev_point_label = p->point_state; - if (p->point_state == PointLabel::GROUND) { - prev_gnd_radius = p->radius; - prev_gnd_point = pcl::PointXYZ(p_orig_point.x, p_orig_point.y, p_orig_point.z); - ground_cluster.addPoint(p->radius, p_orig_point.z); + if (point_label_curr == PointLabel::GROUND) { + prev_gnd_radius = pd.radius; + prev_gnd_point = pcl::PointXYZ(point_curr.x, point_curr.y, point_curr.z); + ground_cluster.addPoint(pd.radius, point_curr.z); prev_gnd_slope = ground_cluster.getAverageSlope(); } // update the non ground state - if (p->point_state == PointLabel::NON_GROUND) { - non_ground_cluster.addPoint(p->radius, p_orig_point.z); + if (point_label_curr == PointLabel::NON_GROUND) { + non_ground_cluster.addPoint(pd.radius, point_curr.z); } } } @@ -611,7 +633,7 @@ void ScanGroundFilterComponent::classifyPointCloud( void ScanGroundFilterComponent::extractObjectPoints( const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices, - PointCloud2 & out_object_cloud) + PointCloud2 & out_object_cloud) const { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -637,8 +659,8 @@ void ScanGroundFilterComponent::faster_filter( std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); - if (!offset_initialized_) { - set_field_offsets(input); + if (!data_offset_initialized_) { + set_field_index_offsets(input); } std::vector radial_ordered_points; @@ -684,37 +706,16 @@ void ScanGroundFilterComponent::filter( } rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( - const std::vector & p) + const std::vector & param) { - if (get_param(p, "grid_size_m", grid_size_m_)) { - grid_mode_switch_grid_id_ = grid_mode_switch_radius_ / grid_size_m_; - grid_size_rad_ = - normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - - normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); - tan_grid_size_rad_ = std::tan(grid_size_rad_); - RCLCPP_DEBUG(get_logger(), "Setting grid_size_m to: %f.", grid_size_m_); - RCLCPP_DEBUG( - get_logger(), "Setting grid_mode_switch_grid_id to: %f.", grid_mode_switch_grid_id_); - RCLCPP_DEBUG(get_logger(), "Setting grid_size_rad to: %f.", grid_size_rad_); - RCLCPP_DEBUG(get_logger(), "Setting tan_grid_size_rad to: %f.", tan_grid_size_rad_); + if (get_param(param, "grid_size_m", grid_size_m_)) { + grid_.initialize(grid_size_m_, grid_mode_switch_radius_, virtual_lidar_z_); } - if (get_param(p, "grid_mode_switch_radius", grid_mode_switch_radius_)) { - grid_mode_switch_grid_id_ = grid_mode_switch_radius_ / grid_size_m_; - grid_mode_switch_angle_rad_ = std::atan2(grid_mode_switch_radius_, virtual_lidar_z_); - grid_size_rad_ = - normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - - normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); - tan_grid_size_rad_ = std::tan(grid_size_rad_); - RCLCPP_DEBUG(get_logger(), "Setting grid_mode_switch_radius to: %f.", grid_mode_switch_radius_); - RCLCPP_DEBUG( - get_logger(), "Setting grid_mode_switch_grid_id to: %f.", grid_mode_switch_grid_id_); - RCLCPP_DEBUG( - get_logger(), "Setting grid_mode_switch_angle_rad to: %f.", grid_mode_switch_angle_rad_); - RCLCPP_DEBUG(get_logger(), "Setting grid_size_rad to: %f.", grid_size_rad_); - RCLCPP_DEBUG(get_logger(), "Setting tan_grid_size_rad to: %f.", tan_grid_size_rad_); + if (get_param(param, "grid_mode_switch_radius", grid_mode_switch_radius_)) { + grid_.initialize(grid_size_m_, grid_mode_switch_radius_, virtual_lidar_z_); } double global_slope_max_angle_deg{get_parameter("global_slope_max_angle_deg").as_double()}; - if (get_param(p, "global_slope_max_angle_deg", global_slope_max_angle_deg)) { + if (get_param(param, "global_slope_max_angle_deg", global_slope_max_angle_deg)) { global_slope_max_angle_rad_ = deg2rad(global_slope_max_angle_deg); global_slope_max_ratio_ = std::tan(global_slope_max_angle_rad_); RCLCPP_DEBUG( @@ -722,7 +723,7 @@ rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( RCLCPP_DEBUG(get_logger(), "Setting global_slope_max_ratio to: %f.", global_slope_max_ratio_); } double local_slope_max_angle_deg{get_parameter("local_slope_max_angle_deg").as_double()}; - if (get_param(p, "local_slope_max_angle_deg", local_slope_max_angle_deg)) { + if (get_param(param, "local_slope_max_angle_deg", local_slope_max_angle_deg)) { local_slope_max_angle_rad_ = deg2rad(local_slope_max_angle_deg); local_slope_max_ratio_ = std::tan(local_slope_max_angle_rad_); RCLCPP_DEBUG( @@ -730,14 +731,14 @@ rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( RCLCPP_DEBUG(get_logger(), "Setting local_slope_max_ratio to: %f.", local_slope_max_ratio_); } double radial_divider_angle_deg{get_parameter("radial_divider_angle_deg").as_double()}; - if (get_param(p, "radial_divider_angle_deg", radial_divider_angle_deg)) { + if (get_param(param, "radial_divider_angle_deg", radial_divider_angle_deg)) { radial_divider_angle_rad_ = deg2rad(radial_divider_angle_deg); radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_); RCLCPP_DEBUG( get_logger(), "Setting radial_divider_angle_rad to: %f.", radial_divider_angle_rad_); RCLCPP_DEBUG(get_logger(), "Setting radial_dividers_num to: %zu.", radial_dividers_num_); } - if (get_param(p, "split_points_distance_tolerance", split_points_distance_tolerance_)) { + if (get_param(param, "split_points_distance_tolerance", split_points_distance_tolerance_)) { split_points_distance_tolerance_square_ = split_points_distance_tolerance_ * split_points_distance_tolerance_; RCLCPP_DEBUG( @@ -747,15 +748,15 @@ rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( get_logger(), "Setting split_points_distance_tolerance_square to: %f.", split_points_distance_tolerance_square_); } - if (get_param(p, "split_height_distance", split_height_distance_)) { + if (get_param(param, "split_height_distance", split_height_distance_)) { RCLCPP_DEBUG(get_logger(), "Setting split_height_distance to: %f.", split_height_distance_); } - if (get_param(p, "use_virtual_ground_point", use_virtual_ground_point_)) { + if (get_param(param, "use_virtual_ground_point", use_virtual_ground_point_)) { RCLCPP_DEBUG_STREAM( get_logger(), "Setting use_virtual_ground_point to: " << std::boolalpha << use_virtual_ground_point_); } - if (get_param(p, "use_recheck_ground_cluster", use_recheck_ground_cluster_)) { + if (get_param(param, "use_recheck_ground_cluster", use_recheck_ground_cluster_)) { RCLCPP_DEBUG_STREAM( get_logger(), "Setting use_recheck_ground_cluster to: " << std::boolalpha << use_recheck_ground_cluster_); diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp index 12590afd5fb86..bccef138e754e 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp @@ -19,6 +19,7 @@ #include "autoware/pointcloud_preprocessor/transform_info.hpp" #include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info.hpp" +#include "grid.hpp" #include @@ -134,7 +135,7 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt addPoint(radius, height); } - float getAverageSlope() { return std::atan2(height_avg, radius_avg); } + float getAverageSlope() const { return std::atan2(height_avg, radius_avg); } float getAverageHeight() const { return height_avg; } @@ -144,8 +145,8 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt float getMinHeight() const { return height_min; } - pcl::PointIndices & getIndicesRef() { return pcl_indices; } - std::vector & getHeightListRef() { return height_list; } + const pcl::PointIndices & getIndicesRef() const { return pcl_indices; } + const std::vector & getHeightListRef() const { return height_list; } }; void filter( @@ -160,47 +161,56 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt tf2_ros::Buffer tf_buffer_{get_clock()}; tf2_ros::TransformListener tf_listener_{tf_buffer_}; - int x_offset_; - int y_offset_; - int z_offset_; - int intensity_offset_; + int data_offset_x_; + int data_offset_y_; + int data_offset_z_; + int data_offset_intensity_; int intensity_type_; - bool offset_initialized_; - - void set_field_offsets(const PointCloud2ConstPtr & input); - - void get_point_from_global_offset( - const PointCloud2ConstPtr & input, pcl::PointXYZ & point, size_t global_offset); + bool data_offset_initialized_; const uint16_t gnd_grid_continual_thresh_ = 3; bool elevation_grid_mode_; float non_ground_height_threshold_; - float grid_size_rad_; - float tan_grid_size_rad_; - float grid_size_m_; float low_priority_region_x_; - uint16_t gnd_grid_buffer_size_; - float grid_mode_switch_grid_id_; - float grid_mode_switch_angle_rad_; - float virtual_lidar_z_; - float detection_range_z_max_; - float center_pcl_shift_; // virtual center of pcl to center mass - float grid_mode_switch_radius_; // non linear grid size switching distance + float center_pcl_shift_; // virtual center of pcl to center mass + + // common parameters + double radial_divider_angle_rad_; // distance in rads between dividers + size_t radial_dividers_num_; + VehicleInfo vehicle_info_; + + // common thresholds double global_slope_max_angle_rad_; // radians double local_slope_max_angle_rad_; // radians double global_slope_max_ratio_; double local_slope_max_ratio_; - double radial_divider_angle_rad_; // distance in rads between dividers double split_points_distance_tolerance_; // distance in meters between concentric divisions double split_points_distance_tolerance_square_; + + // non-grid mode parameters + bool use_virtual_ground_point_; double // minimum height threshold regardless the slope, split_height_distance_; // useful for close points - bool use_virtual_ground_point_; + + // grid mode parameters bool use_recheck_ground_cluster_; // to enable recheck ground cluster bool use_lowest_point_; // to select lowest point for reference in recheck ground cluster, // otherwise select middle point - size_t radial_dividers_num_; - VehicleInfo vehicle_info_; + float detection_range_z_max_; + + // grid parameters + float grid_size_m_; + float grid_mode_switch_radius_; // non linear grid size switching distance + uint16_t gnd_grid_buffer_size_; + float virtual_lidar_z_; + + // grid data + ScanGroundGrid grid_; + + // data access methods + void set_field_index_offsets(const PointCloud2ConstPtr & input); + void get_point_from_data_index( + const PointCloud2ConstPtr & input, const size_t data_index, pcl::PointXYZ & point) const; // time keeper related rclcpp::Publisher::SharedPtr @@ -224,17 +234,15 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt */ void convertPointcloud( const PointCloud2ConstPtr & in_cloud, - std::vector & out_radial_ordered_points); + std::vector & out_radial_ordered_points) const; void convertPointcloudGridScan( const PointCloud2ConstPtr & in_cloud, - std::vector & out_radial_ordered_points); + std::vector & out_radial_ordered_points) const; /*! * Output ground center of front wheels as the virtual ground point * @param[out] point Virtual ground origin point */ - void calcVirtualGroundOrigin(pcl::PointXYZ & point); - - float calcGridSize(const PointData & p); + void calcVirtualGroundOrigin(pcl::PointXYZ & point) const; /*! * Classifies Points in the PointCloud as Ground and Not Ground @@ -245,23 +253,25 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt */ void initializeFirstGndGrids( - const float h, const float r, const uint16_t id, std::vector & gnd_grids); + const float h, const float r, const uint16_t id, std::vector & gnd_grids) const; void checkContinuousGndGrid( - PointData & p, const pcl::PointXYZ & p_orig_point, - const std::vector & gnd_grids_list); + PointData & pd, const pcl::PointXYZ & point_curr, + const std::vector & gnd_grids_list) const; void checkDiscontinuousGndGrid( - PointData & p, const pcl::PointXYZ & p_orig_point, - const std::vector & gnd_grids_list); + PointData & pd, const pcl::PointXYZ & point_curr, + const std::vector & gnd_grids_list) const; void checkBreakGndGrid( - PointData & p, const pcl::PointXYZ & p_orig_point, - const std::vector & gnd_grids_list); + PointData & pd, const pcl::PointXYZ & point_curr, + const std::vector & gnd_grids_list) const; void classifyPointCloud( - const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, - pcl::PointIndices & out_no_ground_indices); + const PointCloud2ConstPtr & in_cloud, + const std::vector & in_radial_ordered_clouds, + pcl::PointIndices & out_no_ground_indices) const; void classifyPointCloudGridScan( - const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, - pcl::PointIndices & out_no_ground_indices); + const PointCloud2ConstPtr & in_cloud, + const std::vector & in_radial_ordered_clouds, + pcl::PointIndices & out_no_ground_indices) const; /*! * Re-classifies point of ground cluster based on their height * @param gnd_cluster Input ground cluster for re-checking @@ -269,8 +279,8 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt * @param non_ground_indices Output non-ground PointCloud indices */ void recheckGroundCluster( - PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point, - pcl::PointIndices & non_ground_indices); + const PointsCentroid & gnd_cluster, const float non_ground_threshold, + const bool use_lowest_point, pcl::PointIndices & non_ground_indices) const; /*! * Returns the resulting complementary PointCloud, one with the points kept * and the other removed as indicated in the indices @@ -280,13 +290,14 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt */ void extractObjectPoints( const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices, - PointCloud2 & out_object_cloud); + PointCloud2 & out_object_cloud) const; /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult onParameter(const std::vector & p); + rcl_interfaces::msg::SetParametersResult onParameter( + const std::vector & param); // debugger std::unique_ptr> stop_watch_ptr_{ diff --git a/perception/autoware_image_projection_based_fusion/package.xml b/perception/autoware_image_projection_based_fusion/package.xml index f08ab04bc6616..b494f2d13887d 100644 --- a/perception/autoware_image_projection_based_fusion/package.xml +++ b/perception/autoware_image_projection_based_fusion/package.xml @@ -18,6 +18,7 @@ autoware_euclidean_cluster autoware_lidar_centerpoint + autoware_object_recognition_utils autoware_perception_msgs autoware_point_types autoware_universe_utils @@ -25,7 +26,6 @@ image_geometry image_transport message_filters - object_recognition_utils perception_utils rclcpp rclcpp_components 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 39835b465d8af..043e648403f3b 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 @@ -14,7 +14,7 @@ #include "autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -50,8 +50,9 @@ void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) ignored_object_flags.resize(output_msg.objects.size()); for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) { const auto & object = output_msg.objects.at(obj_i); - const auto label = object_recognition_utils::getHighestProbLabel(object.classification); - const auto pos = object_recognition_utils::getPose(object).position; + const auto label = + autoware::object_recognition_utils::getHighestProbLabel(object.classification); + const auto pos = autoware::object_recognition_utils::getPose(object).position; const auto object_sqr_dist = pos.x * pos.x + pos.y * pos.y; const auto prob_threshold = fusion_params_.passthrough_lower_bound_probability_thresholds.at(label); @@ -222,9 +223,9 @@ void RoiDetectedObjectFusionNode::fuseObjectsOnImage( for (const auto & image_roi : image_rois) { const auto & object_roi = object_pair.second; const auto object_roi_label = - object_recognition_utils::getHighestProbLabel(object_roi.object.classification); + autoware::object_recognition_utils::getHighestProbLabel(object_roi.object.classification); const auto image_roi_label = - object_recognition_utils::getHighestProbLabel(image_roi.object.classification); + autoware::object_recognition_utils::getHighestProbLabel(image_roi.object.classification); if (!fusion_params_.can_assign_matrix(object_roi_label, image_roi_label)) { continue; } diff --git a/perception/autoware_lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp b/perception/autoware_lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp index f7dd85b019250..8f3f0fa9b20e2 100644 --- a/perception/autoware_lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/autoware_lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp @@ -14,9 +14,9 @@ #include "autoware/lidar_centerpoint/postprocess/non_maximum_suppression.hpp" +#include +#include #include -#include -#include namespace autoware::lidar_centerpoint { @@ -41,8 +41,10 @@ bool NonMaximumSuppression::isTargetLabel(const uint8_t label) bool NonMaximumSuppression::isTargetPairObject( const DetectedObject & object1, const DetectedObject & object2) { - const auto label1 = object_recognition_utils::getHighestProbLabel(object1.classification); - const auto label2 = object_recognition_utils::getHighestProbLabel(object2.classification); + const auto label1 = + autoware::object_recognition_utils::getHighestProbLabel(object1.classification); + const auto label2 = + autoware::object_recognition_utils::getHighestProbLabel(object2.classification); if (isTargetLabel(label1) && isTargetLabel(label2)) { return true; @@ -50,7 +52,8 @@ bool NonMaximumSuppression::isTargetPairObject( const auto search_sqr_dist_2d = params_.search_distance_2d_ * params_.search_distance_2d_; const auto sqr_dist_2d = autoware::universe_utils::calcSquaredDistance2d( - object_recognition_utils::getPose(object1), object_recognition_utils::getPose(object2)); + autoware::object_recognition_utils::getPose(object1), + autoware::object_recognition_utils::getPose(object2)); return sqr_dist_2d <= search_sqr_dist_2d; } @@ -69,7 +72,7 @@ Eigen::MatrixXd NonMaximumSuppression::generateIoUMatrix( } if (params_.nms_type_ == NMS_TYPE::IoU_BEV) { - const double iou = object_recognition_utils::get2dIoU(target_obj, source_obj); + const double iou = autoware::object_recognition_utils::get2dIoU(target_obj, source_obj); triangular_matrix(target_i, source_i) = iou; // NOTE: If the target object has any objects with iou > iou_threshold, it // will be suppressed regardless of later results. diff --git a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp index feeab969e88bd..c76fa6567ab51 100644 --- a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp @@ -14,9 +14,9 @@ #include "autoware/lidar_centerpoint/ros_utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/constants.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" namespace autoware::lidar_centerpoint { @@ -41,7 +41,7 @@ void box3DToDetectedObject( rclcpp::get_logger("lidar_centerpoint"), "Unexpected label: UNKNOWN is set."); } - if (object_recognition_utils::isCarLikeVehicle(classification.label)) { + if (autoware::object_recognition_utils::isCarLikeVehicle(classification.label)) { obj.kinematics.orientation_availability = autoware_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; } diff --git a/perception/autoware_lidar_centerpoint/package.xml b/perception/autoware_lidar_centerpoint/package.xml index 07f67f7e3c11e..29a7bc122707e 100644 --- a/perception/autoware_lidar_centerpoint/package.xml +++ b/perception/autoware_lidar_centerpoint/package.xml @@ -12,9 +12,9 @@ ament_cmake_python autoware_cmake + autoware_object_recognition_utils autoware_perception_msgs autoware_universe_utils - object_recognition_utils pcl_ros rclcpp rclcpp_components diff --git a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/utils.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/utils.hpp index feea2216c5f78..f3a7cecf9ce00 100644 --- a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/utils.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/utils.hpp @@ -36,7 +36,7 @@ struct Box3D float yaw; }; -enum NetworkIO { voxels = 0, num_points, coors, cls_score, dir_pred, bbox_pred, ENUM_SIZE }; +enum NetworkIO { voxels = 0, num_points, coors, cls_score, bbox_pred, dir_pred, ENUM_SIZE }; // cspell: ignore divup template diff --git a/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp b/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp index 3eacf005cff4e..48e8c1014199c 100644 --- a/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp +++ b/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp @@ -23,6 +23,20 @@ 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 = ""; @@ -253,8 +267,14 @@ bool NetworkTRT::validateNetworkIO() << ". 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) { - tensors_names_.push_back(engine->getIOTensorName(i)); + const char * name = engine->getIOTensorName(i); + tensors_names_[nameToNetworkIO(name)] = name; } // Log the network IO diff --git a/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp b/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp index 6e563f706e3b2..1ce151d2960ef 100644 --- a/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp @@ -14,9 +14,9 @@ #include "autoware/lidar_transfusion/postprocess/non_maximum_suppression.hpp" +#include +#include #include -#include -#include namespace autoware::lidar_transfusion { @@ -41,8 +41,10 @@ bool NonMaximumSuppression::isTargetLabel(const uint8_t label) bool NonMaximumSuppression::isTargetPairObject( const DetectedObject & object1, const DetectedObject & object2) { - const auto label1 = object_recognition_utils::getHighestProbLabel(object1.classification); - const auto label2 = object_recognition_utils::getHighestProbLabel(object2.classification); + const auto label1 = + autoware::object_recognition_utils::getHighestProbLabel(object1.classification); + const auto label2 = + autoware::object_recognition_utils::getHighestProbLabel(object2.classification); if (isTargetLabel(label1) && isTargetLabel(label2)) { return true; @@ -50,7 +52,8 @@ bool NonMaximumSuppression::isTargetPairObject( const auto search_sqr_dist_2d = params_.search_distance_2d_ * params_.search_distance_2d_; const auto sqr_dist_2d = autoware::universe_utils::calcSquaredDistance2d( - object_recognition_utils::getPose(object1), object_recognition_utils::getPose(object2)); + autoware::object_recognition_utils::getPose(object1), + autoware::object_recognition_utils::getPose(object2)); return sqr_dist_2d <= search_sqr_dist_2d; } @@ -69,7 +72,7 @@ Eigen::MatrixXd NonMaximumSuppression::generateIoUMatrix( } if (params_.nms_type_ == NMS_TYPE::IoU_BEV) { - const double iou = object_recognition_utils::get2dIoU(target_obj, source_obj); + const double iou = autoware::object_recognition_utils::get2dIoU(target_obj, source_obj); triangular_matrix(target_i, source_i) = iou; // NOTE: If the target object has any objects with iou > iou_threshold, it // will be suppressed regardless of later results. diff --git a/perception/autoware_lidar_transfusion/lib/postprocess/postprocess_kernel.cu b/perception/autoware_lidar_transfusion/lib/postprocess/postprocess_kernel.cu index 905b4ea6f39f4..cfd3bffcc18c9 100644 --- a/perception/autoware_lidar_transfusion/lib/postprocess/postprocess_kernel.cu +++ b/perception/autoware_lidar_transfusion/lib/postprocess/postprocess_kernel.cu @@ -83,8 +83,8 @@ __global__ void generateBoxes3D_kernel( det_boxes3d[point_idx].y = box_output[point_idx + num_proposals] * num_point_values * voxel_size_y + min_y_range; det_boxes3d[point_idx].z = box_output[point_idx + 2 * num_proposals]; - det_boxes3d[point_idx].width = expf(box_output[point_idx + 3 * num_proposals]); - det_boxes3d[point_idx].length = expf(box_output[point_idx + 4 * num_proposals]); + det_boxes3d[point_idx].length = expf(box_output[point_idx + 3 * num_proposals]); + det_boxes3d[point_idx].width = expf(box_output[point_idx + 4 * num_proposals]); det_boxes3d[point_idx].height = expf(box_output[point_idx + 5 * num_proposals]); det_boxes3d[point_idx].yaw = atan2f(dir_cls_output[point_idx], dir_cls_output[point_idx + num_proposals]); diff --git a/perception/autoware_lidar_transfusion/lib/ros_utils.cpp b/perception/autoware_lidar_transfusion/lib/ros_utils.cpp index dce4028759de5..88f6497f8d656 100644 --- a/perception/autoware_lidar_transfusion/lib/ros_utils.cpp +++ b/perception/autoware_lidar_transfusion/lib/ros_utils.cpp @@ -14,9 +14,9 @@ #include "autoware/lidar_transfusion/ros_utils.hpp" +#include #include #include -#include namespace autoware::lidar_transfusion { @@ -40,7 +40,7 @@ void box3DToDetectedObject( rclcpp::get_logger("lidar_transfusion"), "Unexpected label: UNKNOWN is set."); } - if (object_recognition_utils::isCarLikeVehicle(classification.label)) { + if (autoware::object_recognition_utils::isCarLikeVehicle(classification.label)) { obj.kinematics.orientation_availability = autoware_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; } @@ -48,12 +48,10 @@ void box3DToDetectedObject( obj.classification.emplace_back(classification); // pose and shape - // mmdet3d yaw format to ros yaw format - float yaw = box3d.yaw + autoware::universe_utils::pi / 2; obj.kinematics.pose_with_covariance.pose.position = autoware::universe_utils::createPoint(box3d.x, box3d.y, box3d.z); obj.kinematics.pose_with_covariance.pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(yaw); + autoware::universe_utils::createQuaternionFromYaw(box3d.yaw); obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; obj.shape.dimensions = autoware::universe_utils::createTranslation(box3d.length, box3d.width, box3d.height); diff --git a/perception/autoware_lidar_transfusion/package.xml b/perception/autoware_lidar_transfusion/package.xml index 834afba1221ad..791832ef44bda 100644 --- a/perception/autoware_lidar_transfusion/package.xml +++ b/perception/autoware_lidar_transfusion/package.xml @@ -12,11 +12,11 @@ ament_cmake_auto autoware_cmake + autoware_object_recognition_utils autoware_perception_msgs autoware_point_types autoware_universe_utils launch_ros - object_recognition_utils pcl_ros rclcpp rclcpp_components diff --git a/perception/autoware_multi_object_tracker/CMakeLists.txt b/perception/autoware_multi_object_tracker/CMakeLists.txt index fe4546cc9bc60..73edabaa09429 100644 --- a/perception/autoware_multi_object_tracker/CMakeLists.txt +++ b/perception/autoware_multi_object_tracker/CMakeLists.txt @@ -44,6 +44,7 @@ set(${PROJECT_NAME}_lib lib/tracker/model/pedestrian_and_bicycle_tracker.cpp lib/tracker/model/unknown_tracker.cpp lib/tracker/model/pass_through_tracker.cpp + lib/uncertainty/uncertainty_processor.cpp ) ament_auto_add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_src} diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/object_model.hpp similarity index 93% rename from perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/object_model.hpp index b49464109eec8..4fea038d00667 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/object_model.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Taekjin Lee // -#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ -#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ #include @@ -53,6 +53,7 @@ namespace object_model { enum class ObjectModelType { NormalVehicle, BigVehicle, Bicycle, Pedestrian, Unknown }; + struct ObjectSize { double length{0.0}; // [m] @@ -287,7 +288,16 @@ class ObjectModel measurement_covariance.pos_x = sq(0.4); measurement_covariance.pos_y = sq(0.4); measurement_covariance.yaw = sq(deg2rad(30.0)); + measurement_covariance.vel_long = sq(kmph2mps(5.0)); + break; + case ObjectModelType::Unknown: + // measurement noise model + measurement_covariance.pos_x = sq(1.0); + measurement_covariance.pos_y = sq(1.0); + measurement_covariance.yaw = sq(deg2rad(360.0)); + measurement_covariance.vel_long = sq(kmph2mps(10.0)); + measurement_covariance.vel_lat = sq(kmph2mps(10.0)); break; default: @@ -302,8 +312,9 @@ static const ObjectModel normal_vehicle(ObjectModelType::NormalVehicle); static const ObjectModel big_vehicle(ObjectModelType::BigVehicle); static const ObjectModel bicycle(ObjectModelType::Bicycle); static const ObjectModel pedestrian(ObjectModelType::Pedestrian); +static const ObjectModel unknown(ObjectModelType::Unknown); } // namespace object_model } // namespace autoware::multi_object_tracker -#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 7cb2963d38ef1..8501c68b0cf23 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -20,9 +20,9 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ #include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/object_model.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 227e6cd01f4dc..489f656f750cb 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -20,9 +20,9 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ #include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/object_model.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 8f5bab65c6aed..0bfc065c7cc68 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -20,9 +20,9 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ #include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/object_model.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index 500148ba41081..f20b38f73e95f 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -20,9 +20,9 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ #include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/object_model.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" -#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index bc52e49f763fd..0b9ea9c44a6cf 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp @@ -21,7 +21,7 @@ #define EIGEN_MPL2_ONLY #include "autoware/multi_object_tracker/utils/utils.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -79,7 +79,7 @@ class Tracker } std::uint8_t getHighestProbLabel() const { - return object_recognition_utils::getHighestProbLabel(classification_); + return autoware::object_recognition_utils::getHighestProbLabel(classification_); } // existence states diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp index 123eb30e63d6c..821e470054f04 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -45,24 +45,27 @@ class BicycleMotionModel : public MotionModel double lf_; double lr_; - // motion parameters + // motion parameters: process noise and motion limits struct MotionParams { - double q_stddev_acc_long; - double q_stddev_acc_lat; - double q_cov_acc_long; - double q_cov_acc_lat; - double q_stddev_yaw_rate_min; - double q_stddev_yaw_rate_max; - double q_cov_slip_rate_min; - double q_cov_slip_rate_max; - double q_max_slip_angle; - double lf_ratio; - double lr_ratio; - double lf_min; - double lr_min; - double max_vel; - double max_slip; + double q_stddev_acc_long = 3.43; // [m/s^2] uncertain longitudinal acceleration, 0.35G + double q_stddev_acc_lat = 1.47; // [m/s^2] uncertain longitudinal acceleration, 0.15G + double q_cov_acc_long = 11.8; // [m/s^2] uncertain longitudinal acceleration, 0.35G + double q_cov_acc_lat = 2.16; // [m/s^2] uncertain lateral acceleration, 0.15G + double q_stddev_yaw_rate_min = 0.02618; // [rad/s] uncertain yaw change rate, 1.5deg/s + double q_stddev_yaw_rate_max = 0.2618; // [rad/s] uncertain yaw change rate, 15deg/s + double q_cov_slip_rate_min = + 2.7416e-5; // [rad^2/s^2] uncertain slip angle change rate, 0.3 deg/s + double q_cov_slip_rate_max = 0.03046; // [rad^2/s^2] uncertain slip angle change rate, 10 deg/s + double q_max_slip_angle = 0.5236; // [rad] max slip angle, 30deg + double lf_ratio = 0.3; // [-] ratio of the distance from the center to the front wheel + double lr_ratio = 0.25; // [-] ratio of the distance from the center to the rear wheel + double lf_min = 1.0; // [m] minimum distance from the center to the front wheel + double lr_min = 1.0; // [m] minimum distance from the center to the rear wheel + double max_vel = 27.8; // [m/s] maximum velocity, 100km/h + double max_slip = 0.5236; // [rad] maximum slip angle, 30deg + double max_reverse_vel = + -1.389; // [m/s] maximum reverse velocity, -5km/h. The value is expected to be negative } motion_params_; public: @@ -76,8 +79,6 @@ class BicycleMotionModel : public MotionModel const std::array & pose_cov, const double & vel, const double & vel_cov, const double & slip, const double & slip_cov, const double & length); - void setDefaultParams(); - void setMotionParams( const double & q_stddev_acc_long, const double & q_stddev_acc_lat, const double & q_stddev_yaw_rate_min, const double & q_stddev_yaw_rate_max, diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp index 2632d99047053..8165b126eda8e 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp @@ -42,16 +42,17 @@ class CTRVMotionModel : public MotionModel // attributes rclcpp::Logger logger_; - // motion parameters + // motion parameters: process noise and motion limits struct MotionParams { - double q_cov_x; - double q_cov_y; - double q_cov_yaw; - double q_cov_vel; - double q_cov_wz; - double max_vel; - double max_wz; + double q_cov_x = 0.025; // [m^2/s^2] uncertain position in x, 0.5m/s + double q_cov_y = 0.025; // [m^2/s^2] uncertain position in y, 0.5m/s + double q_cov_yaw = 0.1218; // [rad^2/s^2] uncertain yaw angle, 20deg/s + double q_cov_vel = 8.6436; // [m^2/s^4] uncertain velocity, 0.3G m/s^2 + double q_cov_wz = 0.5236; // [rad^2/s^4] uncertain yaw rate, 30deg/s^2 + double max_vel = 2.78; // [m/s] maximum velocity + double max_wz = 0.5236; // [rad/s] maximum yaw rate, 30deg/s + double max_reverse_vel = -1.38; // [m/s] maximum reverse velocity, -5km/h } motion_params_; public: @@ -65,8 +66,6 @@ class CTRVMotionModel : public MotionModel const std::array & pose_cov, const double & vel, const double & vel_cov, const double & wz, const double & wz_cov); - void setDefaultParams(); - void setMotionParams( const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_yaw, const double & q_stddev_vx, const double & q_stddev_wz); diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp index 26799f1916741..dad1b1024879a 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp @@ -41,15 +41,15 @@ class CVMotionModel : public MotionModel // attributes rclcpp::Logger logger_; - // motion parameters + // motion parameters: process noise and motion limits struct MotionParams { - double q_cov_x; - double q_cov_y; - double q_cov_vx; - double q_cov_vy; - double max_vx; - double max_vy; + double q_cov_x = 0.025; // [m^2/s^2] uncertain position in x, 0.5m/s + double q_cov_y = 0.025; // [m^2/s^2] uncertain position in y, 0.5m/s + double q_cov_vx = 8.6436; // [m^2/s^4] uncertain velocity, 0.3G m/s^2 + double q_cov_vy = 8.6436; // [m^2/s^4] uncertain velocity, 0.3G m/s^2 + double max_vx = 16.67; // [m/s] maximum velocity, 60km/h + double max_vy = 16.67; // [m/s] maximum velocity, 60km/h } motion_params_; public: @@ -63,8 +63,6 @@ class CVMotionModel : public MotionModel const std::array & pose_cov, const double & vx, const double & vy, const std::array & twist_cov); - void setDefaultParams(); - void setMotionParams( const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_vx, const double & q_stddev_vy); diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp new file mode 100644 index 0000000000000..c67e71b38a6f9 --- /dev/null +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp @@ -0,0 +1,51 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee + +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__UNCERTAINTY__UNCERTAINTY_PROCESSOR_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__UNCERTAINTY__UNCERTAINTY_PROCESSOR_HPP_ + +#include "autoware/multi_object_tracker/object_model/object_model.hpp" + +#include + +#include + +namespace autoware::multi_object_tracker +{ + +namespace uncertainty +{ + +using autoware::multi_object_tracker::object_model::ObjectModel; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::ObjectClassification; + +ObjectModel decodeObjectModel(const ObjectClassification & object_class); + +DetectedObjects modelUncertainty(const DetectedObjects & detected_objects); + +object_model::StateCovariance covarianceFromObjectClass( + const DetectedObject & detected_object, const ObjectClassification & object_class); + +void normalizeUncertainty(DetectedObjects & detected_objects); + +} // namespace uncertainty + +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__UNCERTAINTY__UNCERTAINTY_PROCESSOR_HPP_ diff --git a/perception/autoware_multi_object_tracker/lib/association/association.cpp b/perception/autoware_multi_object_tracker/lib/association/association.cpp index ac31cad4cc13c..0c809ee5f086d 100644 --- a/perception/autoware_multi_object_tracker/lib/association/association.cpp +++ b/perception/autoware_multi_object_tracker/lib/association/association.cpp @@ -16,7 +16,7 @@ #include "autoware/multi_object_tracker/association/solver/gnn_solver.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -165,7 +165,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( const autoware_perception_msgs::msg::DetectedObject & measurement_object = measurements.objects.at(measurement_idx); const std::uint8_t measurement_label = - object_recognition_utils::getHighestProbLabel(measurement_object.classification); + autoware::object_recognition_utils::getHighestProbLabel(measurement_object.classification); double score = 0.0; if (can_assign_matrix_(tracker_label, measurement_label)) { @@ -210,7 +210,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double min_iou = min_iou_matrix_(tracker_label, measurement_label); const double min_union_iou_area = 1e-2; - const double iou = object_recognition_utils::get2dIoU( + const double iou = autoware::object_recognition_utils::get2dIoU( measurement_object, tracked_object, min_union_iou_area); if (iou < min_iou) passed_gate = false; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index ba53ccec1ad43..098ad39dd3df9 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -20,11 +20,11 @@ #include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -162,42 +162,6 @@ autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( } } - // UNCERTAINTY MODEL - if (!object.kinematics.has_position_covariance) { - // measurement noise covariance - auto r_cov_x = object_model_.measurement_covariance.pos_x; - auto r_cov_y = object_model_.measurement_covariance.pos_y; - - // yaw angle fix - const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const bool is_yaw_available = - object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; - - // fill covariance matrix - using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; - const double cos_yaw = std::cos(pose_yaw); - const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0 * pose_yaw); - pose_cov[XYZRPY_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - pose_cov[XYZRPY_COV_IDX::Y_Y] = - r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x - pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw - pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw - pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x - pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y - pose_cov[XYZRPY_COV_IDX::YAW_YAW] = object_model_.measurement_covariance.yaw; // yaw - yaw - if (!is_yaw_available) { - pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value - } - auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; - twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long; // vel - vel - } - return updating_object; } @@ -282,7 +246,9 @@ bool BicycleTracker::measure( // update classification const auto & current_classification = getClassification(); - if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + if ( + autoware::object_recognition_utils::getHighestProbLabel(object.classification) == + Label::UNKNOWN) { setClassification(current_classification); } @@ -308,7 +274,7 @@ bool BicycleTracker::measure( bool BicycleTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - object = object_recognition_utils::toTrackedObject(object_); + object = autoware::object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp index 0593b7fc9dc12..349ffb1eec634 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp @@ -20,11 +20,11 @@ #include "autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -193,50 +193,6 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje bounding_box_.width, bounding_box_.length, nearest_corner_index, bbox_object, tracked_yaw, updating_object, tracking_offset_); - // UNCERTAINTY MODEL - if (!object.kinematics.has_position_covariance) { - // measurement noise covariance - auto r_cov_x = object_model_.measurement_covariance.pos_x; - auto r_cov_y = object_model_.measurement_covariance.pos_y; - const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (label == autoware_perception_msgs::msg::ObjectClassification::CAR) { - // if label is changed, enlarge the measurement noise covariance - constexpr float r_stddev_x = 2.0; // [m] - constexpr float r_stddev_y = 2.0; // [m] - r_cov_x = r_stddev_x * r_stddev_x; - r_cov_y = r_stddev_y * r_stddev_y; - } - - // yaw angle fix - const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const bool is_yaw_available = - object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; - - // fill covariance matrix - using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; - const double cos_yaw = std::cos(pose_yaw); - const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0 * pose_yaw); - pose_cov[XYZRPY_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - pose_cov[XYZRPY_COV_IDX::Y_Y] = - r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x - pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw - pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw - pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x - pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y - pose_cov[XYZRPY_COV_IDX::YAW_YAW] = object_model_.measurement_covariance.yaw; // yaw - yaw - if (!is_yaw_available) { - pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value - } - auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; - twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long; // vel - vel - } - return updating_object; } @@ -345,7 +301,9 @@ bool BigVehicleTracker::measure( // update classification const auto & current_classification = getClassification(); - if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + if ( + autoware::object_recognition_utils::getHighestProbLabel(object.classification) == + Label::UNKNOWN) { setClassification(current_classification); } @@ -371,7 +329,7 @@ bool BigVehicleTracker::measure( bool BigVehicleTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - object = object_recognition_utils::toTrackedObject(object_); + object = autoware::object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp index a6e5021e4fe7d..4751e3d1c894f 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp @@ -48,7 +48,9 @@ bool MultipleVehicleTracker::measure( { big_vehicle_tracker_.measure(object, time, self_transform); normal_vehicle_tracker_.measure(object, time, self_transform); - if (object_recognition_utils::getHighestProbLabel(object.classification) != Label::UNKNOWN) + if ( + autoware::object_recognition_utils::getHighestProbLabel(object.classification) != + Label::UNKNOWN) updateClassification(object.classification); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp index ef345692b32ca..b4be5e42b0397 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp @@ -20,11 +20,11 @@ #include "autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -195,50 +195,6 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO bounding_box_.width, bounding_box_.length, nearest_corner_index, bbox_object, tracked_yaw, updating_object, tracking_offset_); - // UNCERTAINTY MODEL - if (!object.kinematics.has_position_covariance) { - // measurement noise covariance - auto r_cov_x = object_model_.measurement_covariance.pos_x; - auto r_cov_y = object_model_.measurement_covariance.pos_y; - const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (utils::isLargeVehicleLabel(label)) { - // if label is changed, enlarge the measurement noise covariance - constexpr float r_stddev_x = 2.0; // [m] - constexpr float r_stddev_y = 2.0; // [m] - r_cov_x = r_stddev_x * r_stddev_x; - r_cov_y = r_stddev_y * r_stddev_y; - } - - // yaw angle fix - const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const bool is_yaw_available = - object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; - - // fill covariance matrix - using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; - const double cos_yaw = std::cos(pose_yaw); - const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0 * pose_yaw); - pose_cov[XYZRPY_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - pose_cov[XYZRPY_COV_IDX::Y_Y] = - r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x - pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw - pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw - pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x - pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y - pose_cov[XYZRPY_COV_IDX::YAW_YAW] = object_model_.measurement_covariance.yaw; // yaw - yaw - if (!is_yaw_available) { - pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value - } - auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; - twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long; // vel - vel - } - return updating_object; } @@ -347,7 +303,9 @@ bool NormalVehicleTracker::measure( // update classification const auto & current_classification = getClassification(); - if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + if ( + autoware::object_recognition_utils::getHighestProbLabel(object.classification) == + Label::UNKNOWN) { setClassification(current_classification); } @@ -373,7 +331,7 @@ bool NormalVehicleTracker::measure( bool NormalVehicleTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - object = object_recognition_utils::toTrackedObject(object_); + object = autoware::object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp index c872944fab3d7..f66e616241ae0 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp @@ -19,8 +19,8 @@ #include "autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -91,7 +91,7 @@ bool PassThroughTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - object = object_recognition_utils::toTrackedObject(object_); + object = autoware::object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X] = 0.0; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp index 8c665e6078a2b..1b8018351f5a5 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -48,7 +48,9 @@ bool PedestrianAndBicycleTracker::measure( { pedestrian_tracker_.measure(object, time, self_transform); bicycle_tracker_.measure(object, time, self_transform); - if (object_recognition_utils::getHighestProbLabel(object.classification) != Label::UNKNOWN) + if ( + autoware::object_recognition_utils::getHighestProbLabel(object.classification) != + Label::UNKNOWN) updateClassification(object.classification); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index ee50c2e5449ed..2135514df8485 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -20,11 +20,11 @@ #include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -154,42 +154,6 @@ autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObje { autoware_perception_msgs::msg::DetectedObject updating_object = object; - // UNCERTAINTY MODEL - if (!object.kinematics.has_position_covariance) { - // measurement noise covariance - auto r_cov_x = object_model_.measurement_covariance.pos_x; - auto r_cov_y = object_model_.measurement_covariance.pos_y; - - // yaw angle fix - const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const bool is_yaw_available = - object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; - - // fill covariance matrix - using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; - const double cos_yaw = std::cos(pose_yaw); - const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0 * pose_yaw); - pose_cov[XYZRPY_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - pose_cov[XYZRPY_COV_IDX::Y_Y] = - r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x - pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw - pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw - pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x - pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y - pose_cov[XYZRPY_COV_IDX::YAW_YAW] = object_model_.measurement_covariance.yaw; // yaw - yaw - if (!is_yaw_available) { - pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value - } - auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; - twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long; // vel - vel - } - return updating_object; } @@ -278,7 +242,9 @@ bool PedestrianTracker::measure( object_ = object; const auto & current_classification = getClassification(); - if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + if ( + autoware::object_recognition_utils::getHighestProbLabel(object.classification) == + Label::UNKNOWN) { setClassification(current_classification); } @@ -305,7 +271,7 @@ bool PedestrianTracker::measure( bool PedestrianTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - object = object_recognition_utils::toTrackedObject(object_); + object = autoware::object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index 0648007e0b807..2805e43b88323 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -34,7 +34,7 @@ #else #include #endif -#include "object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" namespace autoware::multi_object_tracker { @@ -217,7 +217,7 @@ bool UnknownTracker::measure( bool UnknownTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - object = object_recognition_utils::toTrackedObject(object_); + object = autoware::object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index 215e55cb4ac62..399634b63bffe 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -38,40 +38,6 @@ using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; BicycleMotionModel::BicycleMotionModel() : logger_(rclcpp::get_logger("BicycleMotionModel")) { - // Initialize motion parameters - setDefaultParams(); -} - -void BicycleMotionModel::setDefaultParams() -{ - // set default motion parameters - constexpr double q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - constexpr double q_stddev_yaw_rate_min = - autoware::universe_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate - constexpr double q_stddev_yaw_rate_max = - autoware::universe_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate - constexpr double q_stddev_slip_rate_min = - autoware::universe_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate - constexpr double q_stddev_slip_rate_max = - autoware::universe_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate - constexpr double q_max_slip_angle = - autoware::universe_utils::deg2rad(30.0); // [rad] max slip angle - // extended state parameters - constexpr double lf_ratio = 0.3; // 30% front from the center - constexpr double lf_min = 1.0; // minimum of 1.0m - constexpr double lr_ratio = 0.25; // 25% rear from the center - constexpr double lr_min = 1.0; // minimum of 1.0m - setMotionParams( - q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, - q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, - lr_min); - - // set motion limitations - constexpr double max_vel = autoware::universe_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30.0; // [deg] maximum slip angle - setMotionLimits(max_vel, max_slip); - // set prediction parameters constexpr double dt_max = 0.11; // [s] maximum time interval for prediction setMaxDeltaTime(dt_max); @@ -101,8 +67,8 @@ void BicycleMotionModel::setMotionParams( logger_, "BicycleMotionModel::setMotionParams: minimum wheel position should be greater than 0.01m."); } - motion_params_.lf_min = (lf_min < minimum_wheel_pos) ? minimum_wheel_pos : lf_min; - motion_params_.lr_min = (lr_min < minimum_wheel_pos) ? minimum_wheel_pos : lr_min; + motion_params_.lf_min = std::max(minimum_wheel_pos, lf_min); + motion_params_.lr_min = std::max(minimum_wheel_pos, lr_min); motion_params_.lf_ratio = lf_ratio; motion_params_.lr_ratio = lr_ratio; } @@ -260,15 +226,26 @@ bool BicycleMotionModel::limitStates() Eigen::MatrixXd P_t(DIM, DIM); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); + + // maximum reverse velocity + if (motion_params_.max_reverse_vel < 0 && X_t(IDX::VEL) < motion_params_.max_reverse_vel) { + // rotate the object orientation by 180 degrees + X_t(IDX::VEL) = -X_t(IDX::VEL); + X_t(IDX::YAW) = X_t(IDX::YAW) + M_PI; + } + // maximum velocity if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; } + // maximum slip angle if (!(-motion_params_.max_slip <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= motion_params_.max_slip)) { X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -motion_params_.max_slip : motion_params_.max_slip; } - ekf_.init(X_t, P_t); + // normalize yaw + X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); + // overwrite state + ekf_.init(X_t, P_t); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp index bf5950bdd4023..6f63ecbdce06d 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp @@ -36,26 +36,6 @@ using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; CTRVMotionModel::CTRVMotionModel() : logger_(rclcpp::get_logger("CTRVMotionModel")) { - // Initialize motion parameters - setDefaultParams(); -} - -void CTRVMotionModel::setDefaultParams() -{ - // process noise covariance - constexpr double q_stddev_x = 0.5; // [m/s] - constexpr double q_stddev_y = 0.5; // [m/s] - constexpr double q_stddev_yaw = autoware::universe_utils::deg2rad(20); // [rad/s] - constexpr double q_stddev_vel = 9.8 * 0.3; // [m/(s*s)] - constexpr double q_stddev_wz = autoware::universe_utils::deg2rad(30); // [rad/(s*s)] - - setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vel, q_stddev_wz); - - // set motion limitations - constexpr double max_vel = autoware::universe_utils::kmph2mps(10); // [m/s] maximum velocity - constexpr double max_wz = 30.0; // [deg] maximum yaw rate - setMotionLimits(max_vel, max_wz); - // set prediction parameters constexpr double dt_max = 0.11; // [s] maximum time interval for prediction setMaxDeltaTime(dt_max); @@ -77,7 +57,7 @@ void CTRVMotionModel::setMotionLimits(const double & max_vel, const double & max { // set motion limitations motion_params_.max_vel = max_vel; - motion_params_.max_wz = autoware::universe_utils::deg2rad(max_wz); + motion_params_.max_wz = max_wz; } bool CTRVMotionModel::initialize( @@ -223,15 +203,26 @@ bool CTRVMotionModel::limitStates() Eigen::MatrixXd P_t(DIM, DIM); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); + + // maximum reverse velocity + if (X_t(IDX::VEL) < 0 && X_t(IDX::VEL) < motion_params_.max_reverse_vel) { + // rotate the object orientation by 180 degrees + X_t(IDX::VEL) = -X_t(IDX::VEL); + X_t(IDX::YAW) = X_t(IDX::YAW) + M_PI; + } + // maximum velocity if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; } + // maximum yaw rate if (!(-motion_params_.max_wz <= X_t(IDX::WZ) && X_t(IDX::WZ) <= motion_params_.max_wz)) { X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -motion_params_.max_wz : motion_params_.max_wz; } - ekf_.init(X_t, P_t); + // normalize yaw + X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); + // overwrite state + ekf_.init(X_t, P_t); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp index 2e2ba660a6e0d..e139419197d79 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp @@ -38,24 +38,6 @@ using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; CVMotionModel::CVMotionModel() : logger_(rclcpp::get_logger("CVMotionModel")) { - // Initialize motion parameters - setDefaultParams(); -} - -void CVMotionModel::setDefaultParams() -{ - // process noise covariance - constexpr double q_stddev_x = 0.5; // [m/s] standard deviation of x - constexpr double q_stddev_y = 0.5; // [m/s] standard deviation of y - constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] standard deviation of vx - constexpr double q_stddev_vy = 9.8 * 0.3; // [m/(s*s)] standard deviation of vy - setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy); - - // set motion limitations - constexpr double max_vx = autoware::universe_utils::kmph2mps(60); // [m/s] maximum x velocity - constexpr double max_vy = autoware::universe_utils::kmph2mps(60); // [m/s] maximum y velocity - setMotionLimits(max_vx, max_vy); - // set prediction parameters constexpr double dt_max = 0.11; // [s] maximum time interval for prediction setMaxDeltaTime(dt_max); diff --git a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp new file mode 100644 index 0000000000000..840879f9bd7aa --- /dev/null +++ b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp @@ -0,0 +1,139 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee + +#include "autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp" + +namespace autoware::multi_object_tracker +{ +namespace uncertainty +{ +using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + +object_model::StateCovariance covarianceFromObjectClass(const ObjectClassification & object_class) +{ + const auto & label = object_class.label; + ObjectModel obj_class_model(object_model::ObjectModelType::Unknown); + switch (label) { + case ObjectClassification::CAR: + obj_class_model = object_model::normal_vehicle; + break; + case ObjectClassification::BUS: + case ObjectClassification::TRUCK: + case ObjectClassification::TRAILER: + obj_class_model = object_model::big_vehicle; + break; + case ObjectClassification::BICYCLE: + case ObjectClassification::MOTORCYCLE: + obj_class_model = object_model::bicycle; + break; + case ObjectClassification::PEDESTRIAN: + obj_class_model = object_model::pedestrian; + break; + default: + obj_class_model = object_model::normal_vehicle; + break; + } + return obj_class_model.measurement_covariance; +} + +DetectedObject modelUncertaintyByClass( + const DetectedObject & object, const ObjectClassification & object_class) +{ + DetectedObject updating_object = object; + + // measurement noise covariance + const object_model::StateCovariance measurement_covariance = + covarianceFromObjectClass(object_class); + + const auto & r_cov_x = measurement_covariance.pos_x; + const auto & r_cov_y = measurement_covariance.pos_y; + + // yaw angle + const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + + // fill position covariance matrix + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + const double cos_yaw = std::cos(pose_yaw); + const double sin_yaw = std::sin(pose_yaw); + const double sin_2yaw = std::sin(2.0 * pose_yaw); + pose_cov[XYZRPY_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[XYZRPY_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x + pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw + pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw + pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x + pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = measurement_covariance.yaw; // yaw - yaw + const bool is_yaw_available = + object.kinematics.orientation_availability != + autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + if (!is_yaw_available) { + pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value + } + + // fill twist covariance matrix + auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; + twist_cov[XYZRPY_COV_IDX::X_X] = measurement_covariance.vel_long; + twist_cov[XYZRPY_COV_IDX::X_Y] = 0.0; + twist_cov[XYZRPY_COV_IDX::Y_X] = 0.0; + twist_cov[XYZRPY_COV_IDX::Y_Y] = measurement_covariance.vel_lat; + + return updating_object; +} + +DetectedObjects modelUncertainty(const DetectedObjects & detected_objects) +{ + DetectedObjects updating_objects; + updating_objects.header = detected_objects.header; + for (const auto & object : detected_objects.objects) { + if (object.kinematics.has_position_covariance) { + updating_objects.objects.push_back(object); + continue; + } + const ObjectClassification & object_class = + autoware::object_recognition_utils::getHighestProbClassification(object.classification); + updating_objects.objects.push_back(modelUncertaintyByClass(object, object_class)); + } + return updating_objects; +} + +void normalizeUncertainty(DetectedObjects & detected_objects) +{ + constexpr double min_cov_dist = 1e-4; + constexpr double min_cov_rad = 1e-6; + constexpr double min_cov_vel = 1e-4; + + for (auto & object : detected_objects.objects) { + // normalize position covariance + auto & pose_cov = object.kinematics.pose_with_covariance.covariance; + pose_cov[XYZRPY_COV_IDX::X_X] = std::max(pose_cov[XYZRPY_COV_IDX::X_X], min_cov_dist); + pose_cov[XYZRPY_COV_IDX::Y_Y] = std::max(pose_cov[XYZRPY_COV_IDX::Y_Y], min_cov_dist); + pose_cov[XYZRPY_COV_IDX::Z_Z] = std::max(pose_cov[XYZRPY_COV_IDX::Z_Z], min_cov_dist); + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = std::max(pose_cov[XYZRPY_COV_IDX::YAW_YAW], min_cov_rad); + + // normalize twist covariance + auto & twist_cov = object.kinematics.twist_with_covariance.covariance; + twist_cov[XYZRPY_COV_IDX::X_X] = std::max(twist_cov[XYZRPY_COV_IDX::X_X], min_cov_vel); + twist_cov[XYZRPY_COV_IDX::Y_Y] = std::max(twist_cov[XYZRPY_COV_IDX::Y_Y], min_cov_vel); + } +} + +} // namespace uncertainty +} // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/package.xml b/perception/autoware_multi_object_tracker/package.xml index 405ce7eced625..d7250293df28e 100644 --- a/perception/autoware_multi_object_tracker/package.xml +++ b/perception/autoware_multi_object_tracker/package.xml @@ -14,13 +14,13 @@ eigen3_cmake_module autoware_kalman_filter + autoware_object_recognition_utils autoware_perception_msgs autoware_universe_utils diagnostic_updater eigen glog mussp - object_recognition_utils rclcpp rclcpp_components tf2 diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index ce7f8157a31b6..24bf4ef9c123d 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -17,6 +17,7 @@ #include "multi_object_tracker_node.hpp" +#include "autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" #include @@ -260,11 +261,11 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list) } void MultiObjectTracker::runProcess( - const DetectedObjects & input_objects_msg, const uint & channel_index) + const DetectedObjects & input_objects, const uint & channel_index) { // Get the time of the measurement const rclcpp::Time measurement_time = - rclcpp::Time(input_objects_msg.header.stamp, this->now().get_clock_type()); + rclcpp::Time(input_objects.header.stamp, this->now().get_clock_type()); // Get the transform of the self frame const auto self_transform = @@ -273,10 +274,16 @@ void MultiObjectTracker::runProcess( return; } + // Model the object uncertainty if it is empty + DetectedObjects input_objects_with_uncertainty = uncertainty::modelUncertainty(input_objects); + + // Normalize the object uncertainty + uncertainty::normalizeUncertainty(input_objects_with_uncertainty); + // Transform the objects to the world frame DetectedObjects transformed_objects; - if (!object_recognition_utils::transformObjects( - input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) { + if (!autoware::object_recognition_utils::transformObjects( + input_objects_with_uncertainty, world_frame_id_, tf_buffer_, transformed_objects)) { return; } diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp index 3c23e3f066488..04d83ebd47acb 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp @@ -100,7 +100,7 @@ class MultiObjectTracker : public rclcpp::Node void onMessage(const ObjectsList & objects_list); // publish processes - void runProcess(const DetectedObjects & input_objects_msg, const uint & channel_index); + void runProcess(const DetectedObjects & input_objects, const uint & channel_index); void checkAndPublish(const rclcpp::Time & time); void publish(const rclcpp::Time & time) const; inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index 17871c89b5258..ba9b3d17fd20f 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -15,7 +15,7 @@ #include "processor.hpp" #include "autoware/multi_object_tracker/tracker/tracker.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware_perception_msgs/msg/tracked_objects.hpp" @@ -90,7 +90,8 @@ std::shared_ptr TrackerProcessor::createNewTracker( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const { - const std::uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); + const std::uint8_t label = + autoware::object_recognition_utils::getHighestProbLabel(object.classification); if (tracker_map_.count(label) != 0) { const auto tracker = tracker_map_.at(label); if (tracker == "bicycle_tracker") @@ -168,7 +169,8 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time) // Check the Intersection over Union (IoU) between the two objects const double min_union_iou_area = 1e-2; - const auto iou = object_recognition_utils::get2dIoU(object1, object2, min_union_iou_area); + const auto iou = + autoware::object_recognition_utils::get2dIoU(object1, object2, min_union_iou_area); const auto & label1 = (*itr1)->getHighestProbLabel(); const auto & label2 = (*itr2)->getHighestProbLabel(); bool should_delete_tracker1 = false; diff --git a/perception/autoware_object_merger/package.xml b/perception/autoware_object_merger/package.xml index e87ef8d81b01b..77c30aa6d4556 100644 --- a/perception/autoware_object_merger/package.xml +++ b/perception/autoware_object_merger/package.xml @@ -13,11 +13,11 @@ autoware_cmake eigen3_cmake_module + autoware_object_recognition_utils autoware_perception_msgs autoware_universe_utils eigen mussp - object_recognition_utils rclcpp rclcpp_components tf2 diff --git a/perception/autoware_object_merger/src/association/data_association.cpp b/perception/autoware_object_merger/src/association/data_association.cpp index 8b40178b592f8..bec2d48469562 100644 --- a/perception/autoware_object_merger/src/association/data_association.cpp +++ b/perception/autoware_object_merger/src/association/data_association.cpp @@ -15,8 +15,8 @@ #include "autoware/object_merger/association/data_association.hpp" #include "autoware/object_merger/association/solver/gnn_solver.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -124,13 +124,13 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( const autoware_perception_msgs::msg::DetectedObject & object1 = objects1.objects.at(objects1_idx); const std::uint8_t object1_label = - object_recognition_utils::getHighestProbLabel(object1.classification); + autoware::object_recognition_utils::getHighestProbLabel(object1.classification); for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) { const autoware_perception_msgs::msg::DetectedObject & object0 = objects0.objects.at(objects0_idx); const std::uint8_t object0_label = - object_recognition_utils::getHighestProbLabel(object0.classification); + autoware::object_recognition_utils::getHighestProbLabel(object0.classification); double score = 0.0; if (can_assign_matrix_(object1_label, object0_label)) { @@ -158,7 +158,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( const double min_iou = min_iou_matrix_(object1_label, object0_label); const double min_union_iou_area = 1e-2; const double iou = - object_recognition_utils::get2dIoU(object0, object1, min_union_iou_area); + autoware::object_recognition_utils::get2dIoU(object0, object1, min_union_iou_area); if (iou < min_iou) passed_gate = false; } 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 663a338649d3c..af9d0e15f4894 100644 --- a/perception/autoware_object_merger/src/object_association_merger_node.cpp +++ b/perception/autoware_object_merger/src/object_association_merger_node.cpp @@ -16,8 +16,8 @@ #include "autoware/object_merger/object_association_merger_node.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -40,18 +40,19 @@ bool isUnknownObjectOverlapped( const std::map & generalized_iou_threshold_map) { const double generalized_iou_threshold = generalized_iou_threshold_map.at( - object_recognition_utils::getHighestProbLabel(known_object.classification)); + autoware::object_recognition_utils::getHighestProbLabel(known_object.classification)); const double distance_threshold = distance_threshold_map.at( - object_recognition_utils::getHighestProbLabel(known_object.classification)); + autoware::object_recognition_utils::getHighestProbLabel(known_object.classification)); const double sq_distance_threshold = std::pow(distance_threshold, 2.0); const double sq_distance = autoware::universe_utils::calcSquaredDistance2d( unknown_object.kinematics.pose_with_covariance.pose, known_object.kinematics.pose_with_covariance.pose); if (sq_distance_threshold < sq_distance) return false; - const auto precision = object_recognition_utils::get2dPrecision(unknown_object, known_object); - const auto recall = object_recognition_utils::get2dRecall(unknown_object, known_object); + const auto precision = + autoware::object_recognition_utils::get2dPrecision(unknown_object, known_object); + const auto recall = autoware::object_recognition_utils::get2dRecall(unknown_object, known_object); const auto generalized_iou = - object_recognition_utils::get2dGeneralizedIoU(unknown_object, known_object); + autoware::object_recognition_utils::get2dGeneralizedIoU(unknown_object, known_object); return precision > precision_threshold || recall > recall_threshold || generalized_iou > generalized_iou_threshold; } @@ -142,9 +143,9 @@ void ObjectAssociationMergerNode::objectsCallback( /* transform to base_link coordinate */ autoware_perception_msgs::msg::DetectedObjects transformed_objects0, transformed_objects1; if ( - !object_recognition_utils::transformObjects( + !autoware::object_recognition_utils::transformObjects( *input_objects0_msg, base_link_frame_id_, tf_buffer_, transformed_objects0) || - !object_recognition_utils::transformObjects( + !autoware::object_recognition_utils::transformObjects( *input_objects1_msg, base_link_frame_id_, tf_buffer_, transformed_objects1)) { return; } @@ -198,7 +199,9 @@ void ObjectAssociationMergerNode::objectsCallback( unknown_objects.reserve(output_msg.objects.size()); known_objects.reserve(output_msg.objects.size()); for (const auto & object : output_msg.objects) { - if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + if ( + autoware::object_recognition_utils::getHighestProbLabel(object.classification) == + Label::UNKNOWN) { unknown_objects.push_back(object); } else { known_objects.push_back(object); diff --git a/perception/autoware_radar_object_clustering/package.xml b/perception/autoware_radar_object_clustering/package.xml index d9c3464e02b18..00106ee38e82f 100644 --- a/perception/autoware_radar_object_clustering/package.xml +++ b/perception/autoware_radar_object_clustering/package.xml @@ -14,10 +14,10 @@ ament_cmake_auto + autoware_object_recognition_utils autoware_perception_msgs autoware_universe_utils geometry_msgs - object_recognition_utils rclcpp rclcpp_components tf2 diff --git a/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.cpp b/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.cpp index 4db4c578f9f89..cf686e3ae1055 100644 --- a/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.cpp +++ b/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.cpp @@ -14,9 +14,9 @@ #include "radar_object_clustering_node.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -162,7 +162,7 @@ void RadarObjectClusteringNode::onObjects(const DetectedObjects::ConstSharedPtr // Fixed label correction if (node_param_.is_fixed_label) { clustered_output_object.classification.at(0).label = - object_recognition_utils::toLabel(node_param_.fixed_label); + autoware::object_recognition_utils::toLabel(node_param_.fixed_label); } // Fixed size correction diff --git a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp index efde1e6763cdd..ad53f8f9d7bf0 100644 --- a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp +++ b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp @@ -21,9 +21,9 @@ #define EIGEN_MPL2_ONLY +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware_radar_object_tracker/association/solver/gnn_solver.hpp" #include "autoware_radar_object_tracker/tracker/tracker.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include diff --git a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp index caea725ef8f81..cc51af599cdd9 100644 --- a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp @@ -20,8 +20,8 @@ #define AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ #define EIGEN_MPL2_ONLY +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware_radar_object_tracker/utils/utils.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -68,7 +68,7 @@ class Tracker } std::uint8_t getHighestProbLabel() const { - return object_recognition_utils::getHighestProbLabel(classification_); + return autoware::object_recognition_utils::getHighestProbLabel(classification_); } int getNoMeasurementCount() const { return no_measurement_count_; } int getTotalNoMeasurementCount() const { return total_no_measurement_count_; } diff --git a/perception/autoware_radar_object_tracker/package.xml b/perception/autoware_radar_object_tracker/package.xml index 6829c26f797c7..3a18b93451d39 100644 --- a/perception/autoware_radar_object_tracker/package.xml +++ b/perception/autoware_radar_object_tracker/package.xml @@ -15,13 +15,13 @@ autoware_kalman_filter autoware_lanelet2_extension + autoware_object_recognition_utils autoware_perception_msgs autoware_universe_utils eigen glog mussp nlohmann-json-dev - object_recognition_utils rclcpp rclcpp_components tf2 diff --git a/perception/autoware_radar_object_tracker/src/association/data_association.cpp b/perception/autoware_radar_object_tracker/src/association/data_association.cpp index 2d906ce55bcec..673ae084fb242 100644 --- a/perception/autoware_radar_object_tracker/src/association/data_association.cpp +++ b/perception/autoware_radar_object_tracker/src/association/data_association.cpp @@ -176,7 +176,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( const autoware_perception_msgs::msg::DetectedObject & measurement_object = measurements.objects.at(measurement_idx); const std::uint8_t measurement_label = - object_recognition_utils::getHighestProbLabel(measurement_object.classification); + autoware::object_recognition_utils::getHighestProbLabel(measurement_object.classification); // Create a JSON object to hold the log data for this pair nlohmann::json pair_log_data; @@ -259,7 +259,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double min_iou = min_iou_matrix_(tracker_label, measurement_label); const double min_union_iou_area = 1e-2; - const double iou = object_recognition_utils::get2dIoU( + const double iou = autoware::object_recognition_utils::get2dIoU( measurement_object, tracked_object, min_union_iou_area); if (iou < min_iou) { passed_gate = false; diff --git a/perception/autoware_radar_object_tracker/src/radar_object_tracker_node.cpp b/perception/autoware_radar_object_tracker/src/radar_object_tracker_node.cpp index 10bf11e46abb9..1815b8ed64b04 100644 --- a/perception/autoware_radar_object_tracker/src/radar_object_tracker_node.cpp +++ b/perception/autoware_radar_object_tracker/src/radar_object_tracker_node.cpp @@ -150,7 +150,7 @@ void RadarObjectTrackerNode::onMeasurement( /* transform to world coordinate */ autoware_perception_msgs::msg::DetectedObjects transformed_objects; - if (!object_recognition_utils::transformObjects( + if (!autoware::object_recognition_utils::transformObjects( *input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) { return; } @@ -215,7 +215,8 @@ std::shared_ptr RadarObjectTrackerNode::createNewTracker( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & /*self_transform*/) const { - const std::uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); + const std::uint8_t label = + autoware::object_recognition_utils::getHighestProbLabel(object.classification); if (tracker_map_.count(label) != 0) { const auto tracker = tracker_map_.at(label); @@ -347,7 +348,8 @@ void RadarObjectTrackerNode::sanitizeTracker( } const double min_union_iou_area = 1e-2; - const auto iou = object_recognition_utils::get2dIoU(object1, object2, min_union_iou_area); + const auto iou = + autoware::object_recognition_utils::get2dIoU(object1, object2, min_union_iou_area); const auto & label1 = (*itr1)->getHighestProbLabel(); const auto & label2 = (*itr2)->getHighestProbLabel(); bool should_delete_tracker1 = false; diff --git a/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp b/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp index f30e4f984f8a1..47b9430616fc9 100644 --- a/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp +++ b/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp @@ -497,7 +497,9 @@ bool ConstantTurnRateMotionTracker::measure( { const auto & current_classification = getClassification(); object_ = object; - if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + if ( + autoware::object_recognition_utils::getHighestProbLabel(object.classification) == + Label::UNKNOWN) { setClassification(current_classification); } @@ -517,7 +519,7 @@ bool ConstantTurnRateMotionTracker::measure( bool ConstantTurnRateMotionTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - object = object_recognition_utils::toTrackedObject(object_); + object = autoware::object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp index dbadb3366ba0c..6facac3707c56 100644 --- a/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp +++ b/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp @@ -516,7 +516,9 @@ bool LinearMotionTracker::measure( { const auto & current_classification = getClassification(); object_ = object; - if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + if ( + autoware::object_recognition_utils::getHighestProbLabel(object.classification) == + Label::UNKNOWN) { setClassification(current_classification); } @@ -536,7 +538,7 @@ bool LinearMotionTracker::measure( bool LinearMotionTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - object = object_recognition_utils::toTrackedObject(object_); + object = autoware::object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp index 2e317139f27c0..905957178ebd6 100644 --- a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp +++ b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__TENSORRT_YOLOX__TENSORRT_YOLOX_NODE_HPP_ #define AUTOWARE__TENSORRT_YOLOX__TENSORRT_YOLOX_NODE_HPP_ -#include "object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include #include diff --git a/perception/autoware_tensorrt_yolox/package.xml b/perception/autoware_tensorrt_yolox/package.xml index cd9dc6bac015d..749624bcbe5a3 100644 --- a/perception/autoware_tensorrt_yolox/package.xml +++ b/perception/autoware_tensorrt_yolox/package.xml @@ -19,12 +19,12 @@ cudnn_cmake_module tensorrt_cmake_module + autoware_object_recognition_utils autoware_perception_msgs cuda_utils cv_bridge image_transport libopencv-dev - object_recognition_utils perception_utils rclcpp rclcpp_components diff --git a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp index 2455411618ceb..753e51d51265c 100644 --- a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp +++ b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp @@ -14,7 +14,7 @@ #include "autoware/tensorrt_yolox/tensorrt_yolox_node.hpp" -#include "object_recognition_utils/object_classification.hpp" +#include "autoware/object_recognition_utils/object_classification.hpp" #include "perception_utils/run_length_encoder.hpp" #include @@ -168,8 +168,8 @@ void TrtYoloXNode::onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg) object.feature.roi.width = yolox_object.width; object.feature.roi.height = yolox_object.height; object.object.existence_probability = yolox_object.score; - object.object.classification = - object_recognition_utils::toObjectClassifications(label_map_[yolox_object.type], 1.0f); + object.object.classification = autoware::object_recognition_utils::toObjectClassifications( + label_map_[yolox_object.type], 1.0f); out_objects.feature_objects.push_back(object); const auto left = std::max(0, static_cast(object.feature.roi.x_offset)); const auto top = std::max(0, static_cast(object.feature.roi.y_offset)); diff --git a/perception/autoware_tracking_object_merger/package.xml b/perception/autoware_tracking_object_merger/package.xml index 321016ac0ff48..ce4cf5276ab80 100644 --- a/perception/autoware_tracking_object_merger/package.xml +++ b/perception/autoware_tracking_object_merger/package.xml @@ -13,12 +13,12 @@ autoware_cmake eigen3_cmake_module + autoware_object_recognition_utils autoware_perception_msgs autoware_universe_utils eigen glog mussp - object_recognition_utils rclcpp rclcpp_components tf2 diff --git a/perception/autoware_tracking_object_merger/src/association/data_association.cpp b/perception/autoware_tracking_object_merger/src/association/data_association.cpp index 119c95c86daf8..771fcb5f1a484 100644 --- a/perception/autoware_tracking_object_merger/src/association/data_association.cpp +++ b/perception/autoware_tracking_object_merger/src/association/data_association.cpp @@ -14,10 +14,10 @@ #include "autoware/tracking_object_merger/association/data_association.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/tracking_object_merger/association/solver/gnn_solver.hpp" #include "autoware/tracking_object_merger/utils/utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -178,9 +178,9 @@ double DataAssociation::calcScoreBetweenObjects( const autoware_perception_msgs::msg::TrackedObject & object1) const { const std::uint8_t object1_label = - object_recognition_utils::getHighestProbLabel(object1.classification); + autoware::object_recognition_utils::getHighestProbLabel(object1.classification); const std::uint8_t object0_label = - object_recognition_utils::getHighestProbLabel(object0.classification); + autoware::object_recognition_utils::getHighestProbLabel(object0.classification); double score = 0.0; if (can_assign_matrix_(object1_label, object0_label)) { @@ -206,7 +206,8 @@ double DataAssociation::calcScoreBetweenObjects( if (passed_gate) { const double min_iou = min_iou_matrix_(object1_label, object0_label); const double min_union_iou_area = 1e-2; - const double iou = object_recognition_utils::get2dIoU(object0, object1, min_union_iou_area); + const double iou = + autoware::object_recognition_utils::get2dIoU(object0, object1, min_union_iou_area); if (iou < min_iou) passed_gate = false; } // max velocity diff gate diff --git a/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp b/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp index 295a350071ac8..55d31a1b890cd 100644 --- a/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp +++ b/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp @@ -16,9 +16,9 @@ #include "autoware/tracking_object_merger/decorative_tracker_merger_node.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/tracking_object_merger/association/solver/ssp.hpp" #include "autoware/tracking_object_merger/utils/utils.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -200,7 +200,7 @@ void DecorativeTrackerMergerNode::mainObjectsCallback( /* transform to target merge coordinate */ TrackedObjects transformed_objects; - if (!object_recognition_utils::transformObjects( + if (!autoware::object_recognition_utils::transformObjects( *main_objects, merge_frame_id_, tf_buffer_, transformed_objects)) { return; } @@ -258,7 +258,7 @@ void DecorativeTrackerMergerNode::subObjectsCallback(const TrackedObjects::Const { /* transform to target merge coordinate */ TrackedObjects transformed_objects; - if (!object_recognition_utils::transformObjects( + if (!autoware::object_recognition_utils::transformObjects( *msg, merge_frame_id_, tf_buffer_, transformed_objects)) { return; } 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 b08d16c68fa6f..434440359f282 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 @@ -49,6 +49,8 @@ #include "autoware_costmap_generator/points_to_costmap.hpp" #include "costmap_generator_node_parameters.hpp" +#include +#include #include #include #include @@ -58,6 +60,7 @@ #include #include #include +#include #include #include @@ -91,6 +94,7 @@ 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::Subscription::SharedPtr sub_points_; rclcpp::Subscription::SharedPtr sub_objects_; @@ -175,6 +179,9 @@ class CostmapGenerator : public rclcpp::Node /// \brief calculate cost for final output grid_map::Matrix generateCombinedCostmap(); + + /// \brief measure processing time + autoware::universe_utils::StopWatch stop_watch; }; } // namespace autoware::costmap_generator diff --git a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp index 9012b7f3250b4..07bf9eb33ffbe 100644 --- a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp @@ -192,6 +192,8 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) pub_processing_time_ = 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); // Timer const auto period_ns = rclcpp::Rate(param_->update_rate).period(); @@ -277,7 +279,13 @@ void CostmapGenerator::onScenario(const tier4_planning_msgs::msg::Scenario::Cons void CostmapGenerator::onTimer() { autoware::universe_utils::ScopedTimeTrack scoped_time_track(__func__, *time_keeper_); + stop_watch.tic(); if (!isActive()) { + // Publish ProcessingTime + tier4_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); return; } @@ -467,6 +475,12 @@ void CostmapGenerator::publishCostmap(const grid_map::GridMap & costmap) auto out_gridmap_msg = grid_map::GridMapRosConverter::toMessage(costmap); out_gridmap_msg->header = header; pub_costmap_->publish(*out_gridmap_msg); + + // Publish ProcessingTime + tier4_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); } } // namespace autoware::costmap_generator diff --git a/planning/autoware_freespace_planner/CMakeLists.txt b/planning/autoware_freespace_planner/CMakeLists.txt index 7901be70fbc63..5c9e5cf15f9b0 100644 --- a/planning/autoware_freespace_planner/CMakeLists.txt +++ b/planning/autoware_freespace_planner/CMakeLists.txt @@ -6,6 +6,7 @@ autoware_package() ament_auto_add_library(freespace_planner_node SHARED src/autoware_freespace_planner/freespace_planner_node.cpp + src/autoware_freespace_planner/utils.cpp ) rclcpp_components_register_node(freespace_planner_node @@ -16,6 +17,8 @@ rclcpp_components_register_node(freespace_planner_node if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_freespace_planner_node_interface.cpp + test/test_freespace_planner_utils.cpp + test/test_freespace_planner.cpp ) target_link_libraries(test_${PROJECT_NAME} freespace_planner_node 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 b381f4dc1d0e7..55d7bf966b24f 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 @@ -64,6 +64,8 @@ #include #include +class TestFreespacePlanner; + namespace autoware::freespace_planner { using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; @@ -159,22 +161,50 @@ class FreespacePlannerNode : public rclcpp::Node void onRoute(const LaneletRoute::ConstSharedPtr msg); void onOdometry(const Odometry::ConstSharedPtr msg); + void onTimer(); void updateData(); + void reset(); + void planTrajectory(); + void initializePlanningAlgorithm(); bool isDataReady(); - void onTimer(); - - void reset(); + /** + * @brief Checks if a new trajectory planning is required. + * @details A new trajectory planning is required if: + * - Current trajectory points are empty, or + * - Current trajectory collides with an object, or + * - Ego deviates from current trajectory + * @return true if any of the conditions are met. + */ bool isPlanRequired(); - void planTrajectory(); + + /** + * @brief Sets the target index along the current trajectory points + * @details if Ego is stopped AND is near the current target index along the trajectory, + * then will get the next target index along the trajectory. + * If the new target index is the same as the current target index, then + * is_complete_ is set to true, and will publish is_completed_msg. + * Otherwise will update prev_target_index_ and target_index_, to continue + * following the trajectory. + */ void updateTargetIndex(); - void initializePlanningAlgorithm(); + /** + * @brief Checks if current trajectory is colliding with an object. + * @details Will check if an obstacle exists along the current trajectory, + * if there is no obstacle along the current trajectory, will reset obs_found_time_. + * If an obstacle exists and the variable obs_found_time_ is not initialized, + * will initialize with the current time. + * @return true if there is an obstacle along current trajectory, AND duration since + * obs_found_time_ exceeds the parameter th_obstacle_time_sec + */ bool checkCurrentTrajectoryCollision(); TransformStamped getTransform(const std::string & from, const std::string & to); std::unique_ptr logger_configure_; + + friend class ::TestFreespacePlanner; }; } // namespace autoware::freespace_planner diff --git a/planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp b/planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp new file mode 100644 index 0000000000000..4a7f3b1cd6c2b --- /dev/null +++ b/planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp @@ -0,0 +1,78 @@ +// 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__FREESPACE_PLANNER__UTILS_HPP_ +#define AUTOWARE__FREESPACE_PLANNER__UTILS_HPP_ + +#include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace autoware::freespace_planner::utils +{ +using autoware::freespace_planning_algorithms::PlannerWaypoint; +using autoware::freespace_planning_algorithms::PlannerWaypoints; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseArray; +using geometry_msgs::msg::PoseStamped; +using geometry_msgs::msg::TransformStamped; +using nav_msgs::msg::Odometry; +using tier4_planning_msgs::msg::Scenario; + +PoseArray trajectory_to_pose_array(const Trajectory & trajectory); + +double calc_distance_2d(const Trajectory & trajectory, const Pose & pose); + +Pose transform_pose(const Pose & pose, const TransformStamped & transform); + +bool is_active(const Scenario::ConstSharedPtr & scenario); + +std::vector get_reversing_indices(const Trajectory & trajectory); + +size_t get_next_target_index( + const size_t trajectory_size, const std::vector & reversing_indices, + const size_t current_target_index); + +Trajectory get_partial_trajectory( + const Trajectory & trajectory, const size_t start_index, const size_t end_index); + +Trajectory create_trajectory( + const PoseStamped & current_pose, const PlannerWaypoints & planner_waypoints, + const double & velocity); + +Trajectory create_stop_trajectory(const PoseStamped & current_pose); + +Trajectory create_stop_trajectory(const Trajectory & trajectory); + +bool is_stopped( + const std::deque & odom_buffer, const double th_stopped_velocity_mps); + +bool is_near_target( + const Pose & target_pose, const Pose & current_pose, const double th_distance_m); +} // namespace autoware::freespace_planner::utils + +#endif // AUTOWARE__FREESPACE_PLANNER__UTILS_HPP_ 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 267fd9932e781..a2a4d1f079b3c 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 @@ -30,6 +30,7 @@ #include "autoware/freespace_planner/freespace_planner_node.hpp" +#include "autoware/freespace_planner/utils.hpp" #include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" #include @@ -43,181 +44,6 @@ #include #include -namespace -{ -using autoware_planning_msgs::msg::Trajectory; -using autoware_planning_msgs::msg::TrajectoryPoint; -using TrajectoryPoints = std::vector; -using autoware::freespace_planning_algorithms::AstarSearch; -using autoware::freespace_planning_algorithms::PlannerWaypoint; -using autoware::freespace_planning_algorithms::PlannerWaypoints; -using autoware::freespace_planning_algorithms::RRTStar; -using geometry_msgs::msg::Pose; -using geometry_msgs::msg::PoseArray; -using geometry_msgs::msg::PoseStamped; -using geometry_msgs::msg::TransformStamped; -using geometry_msgs::msg::Twist; -using nav_msgs::msg::Odometry; -using tier4_planning_msgs::msg::Scenario; - -bool isActive(const Scenario::ConstSharedPtr & scenario) -{ - if (!scenario) { - return false; - } - - const auto & s = scenario->activating_scenarios; - if (std::find(std::begin(s), std::end(s), Scenario::PARKING) != std::end(s)) { - return true; - } - - return false; -} - -PoseArray trajectory2PoseArray(const Trajectory & trajectory) -{ - PoseArray pose_array; - pose_array.header = trajectory.header; - - for (const auto & point : trajectory.points) { - pose_array.poses.push_back(point.pose); - } - - return pose_array; -} - -std::vector getReversingIndices(const Trajectory & trajectory) -{ - std::vector indices; - - for (size_t i = 0; i < trajectory.points.size() - 1; ++i) { - if ( - trajectory.points.at(i).longitudinal_velocity_mps * - trajectory.points.at(i + 1).longitudinal_velocity_mps < - 0) { - indices.push_back(i); - } - } - - return indices; -} - -size_t getNextTargetIndex( - const size_t trajectory_size, const std::vector & reversing_indices, - const size_t current_target_index) -{ - if (!reversing_indices.empty()) { - for (const auto reversing_index : reversing_indices) { - if (reversing_index > current_target_index) { - return reversing_index; - } - } - } - - return trajectory_size - 1; -} - -Trajectory getPartialTrajectory( - const Trajectory & trajectory, const size_t start_index, const size_t end_index) -{ - Trajectory partial_trajectory; - partial_trajectory.header = trajectory.header; - partial_trajectory.header.stamp = rclcpp::Clock().now(); - - partial_trajectory.points.reserve(trajectory.points.size()); - for (size_t i = start_index; i <= end_index; ++i) { - partial_trajectory.points.push_back(trajectory.points.at(i)); - } - - // Modify velocity at start/end point - if (partial_trajectory.points.size() >= 2) { - partial_trajectory.points.front().longitudinal_velocity_mps = - partial_trajectory.points.at(1).longitudinal_velocity_mps; - } - if (!partial_trajectory.points.empty()) { - partial_trajectory.points.back().longitudinal_velocity_mps = 0; - } - - return partial_trajectory; -} - -double calcDistance2d(const Trajectory & trajectory, const Pose & pose) -{ - const auto idx = autoware::motion_utils::findNearestIndex(trajectory.points, pose.position); - return autoware::universe_utils::calcDistance2d(trajectory.points.at(idx), pose); -} - -Pose transformPose(const Pose & pose, const TransformStamped & transform) -{ - PoseStamped transformed_pose; - PoseStamped orig_pose; - orig_pose.pose = pose; - tf2::doTransform(orig_pose, transformed_pose, transform); - - return transformed_pose.pose; -} - -Trajectory createTrajectory( - const PoseStamped & current_pose, const PlannerWaypoints & planner_waypoints, - const double & velocity) -{ - Trajectory trajectory; - trajectory.header = planner_waypoints.header; - - for (const auto & awp : planner_waypoints.waypoints) { - TrajectoryPoint point; - - point.pose = awp.pose.pose; - - point.pose.position.z = current_pose.pose.position.z; // height = const - point.longitudinal_velocity_mps = velocity / 3.6; // velocity = const - - // switch sign by forward/backward - point.longitudinal_velocity_mps = (awp.is_back ? -1 : 1) * point.longitudinal_velocity_mps; - - trajectory.points.push_back(point); - } - - return trajectory; -} - -Trajectory createStopTrajectory(const PoseStamped & current_pose) -{ - PlannerWaypoints waypoints; - PlannerWaypoint waypoint; - - waypoints.header.stamp = rclcpp::Clock().now(); - waypoints.header.frame_id = current_pose.header.frame_id; - waypoint.pose.header = waypoints.header; - waypoint.pose.pose = current_pose.pose; - waypoint.is_back = false; - waypoints.waypoints.push_back(waypoint); - - return createTrajectory(current_pose, waypoints, 0.0); -} - -Trajectory createStopTrajectory(const Trajectory & trajectory) -{ - Trajectory stop_trajectory = trajectory; - for (size_t i = 0; i < trajectory.points.size(); ++i) { - stop_trajectory.points.at(i).longitudinal_velocity_mps = 0.0; - } - return stop_trajectory; -} - -bool isStopped( - const std::deque & odom_buffer, const double th_stopped_velocity_mps) -{ - for (const auto & odom : odom_buffer) { - if (std::abs(odom->twist.twist.linear.x) > th_stopped_velocity_mps) { - return false; - } - } - return true; -} - -} // namespace - namespace autoware::freespace_planner { FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_options) @@ -321,8 +147,8 @@ bool FreespacePlannerNode::isPlanRequired() } if (node_param_.replan_when_course_out) { - const bool is_course_out = - calcDistance2d(trajectory_, current_pose_.pose) > node_param_.th_course_out_distance_m; + const bool is_course_out = utils::calc_distance_2d(trajectory_, current_pose_.pose) > + node_param_.th_course_out_distance_m; if (is_course_out) { RCLCPP_INFO(get_logger(), "Course out"); return true; @@ -340,10 +166,10 @@ bool FreespacePlannerNode::checkCurrentTrajectoryCollision() partial_trajectory_.points, current_pose_.pose.position); const size_t end_index_partial = partial_trajectory_.points.size() - 1; const auto forward_trajectory = - getPartialTrajectory(partial_trajectory_, nearest_index_partial, end_index_partial); + utils::get_partial_trajectory(partial_trajectory_, nearest_index_partial, end_index_partial); const bool is_obs_found = - algo_->hasObstacleOnTrajectory(trajectory2PoseArray(forward_trajectory)); + algo_->hasObstacleOnTrajectory(utils::trajectory_to_pose_array(forward_trajectory)); if (!is_obs_found) { obs_found_time_ = {}; @@ -357,28 +183,30 @@ bool FreespacePlannerNode::checkCurrentTrajectoryCollision() void FreespacePlannerNode::updateTargetIndex() { - const double long_disp_to_target = autoware::universe_utils::calcLongitudinalDeviation( - trajectory_.points.at(target_index_).pose, current_pose_.pose.position); - const auto is_near_target = std::abs(long_disp_to_target) < node_param_.th_arrived_distance_m; - - const auto is_stopped = isStopped(odom_buffer_, node_param_.th_stopped_velocity_mps); - - if (is_near_target && is_stopped) { - const auto new_target_index = - getNextTargetIndex(trajectory_.points.size(), reversing_indices_, target_index_); - - if (new_target_index == target_index_) { - // Finished publishing all partial trajectories - is_completed_ = true; - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "Freespace planning completed"); - std_msgs::msg::Bool is_completed_msg; - is_completed_msg.data = is_completed_; - parking_state_pub_->publish(is_completed_msg); - } else { - // Switch to next partial trajectory - prev_target_index_ = target_index_; - target_index_ = new_target_index; - } + if (!utils::is_stopped(odom_buffer_, node_param_.th_stopped_velocity_mps)) { + return; + } + + const auto is_near_target = utils::is_near_target( + trajectory_.points.at(target_index_).pose, current_pose_.pose, + node_param_.th_arrived_distance_m); + + if (!is_near_target) return; + + const auto new_target_index = + utils::get_next_target_index(trajectory_.points.size(), reversing_indices_, target_index_); + + if (new_target_index == target_index_) { + // Finished publishing all partial trajectories + is_completed_ = true; + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "Freespace planning completed"); + std_msgs::msg::Bool is_completed_msg; + is_completed_msg.data = is_completed_; + parking_state_pub_->publish(is_completed_msg); + } else { + // Switch to next partial trajectory + prev_target_index_ = target_index_; + target_index_ = new_target_index; } } @@ -450,7 +278,7 @@ bool FreespacePlannerNode::isDataReady() void FreespacePlannerNode::onTimer() { scenario_ = scenario_sub_.takeData(); - if (!isActive(scenario_)) { + if (!utils::is_active(scenario_)) { reset(); return; } @@ -463,7 +291,7 @@ void FreespacePlannerNode::onTimer() if (is_completed_) { partial_trajectory_.header = odom_->header; - const auto stop_trajectory = createStopTrajectory(partial_trajectory_); + const auto stop_trajectory = utils::create_stop_trajectory(partial_trajectory_); trajectory_pub_->publish(stop_trajectory); return; } @@ -483,11 +311,11 @@ void FreespacePlannerNode::onTimer() // stops. if (!is_new_parking_cycle_) { const auto stop_trajectory = partial_trajectory_.points.empty() - ? createStopTrajectory(current_pose_) - : createStopTrajectory(partial_trajectory_); + ? utils::create_stop_trajectory(current_pose_) + : utils::create_stop_trajectory(partial_trajectory_); trajectory_pub_->publish(stop_trajectory); - debug_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory)); - debug_partial_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory)); + debug_pose_array_pub_->publish(utils::trajectory_to_pose_array(stop_trajectory)); + debug_partial_pose_array_pub_->publish(utils::trajectory_to_pose_array(stop_trajectory)); } reset(); @@ -496,8 +324,9 @@ void FreespacePlannerNode::onTimer() } if (reset_in_progress_) { - const auto is_stopped = isStopped(odom_buffer_, node_param_.th_stopped_velocity_mps); - if (is_stopped) { + const auto is_ego_stopped = + utils::is_stopped(odom_buffer_, node_param_.th_stopped_velocity_mps); + if (is_ego_stopped) { // Plan new trajectory planTrajectory(); reset_in_progress_ = false; @@ -517,12 +346,13 @@ void FreespacePlannerNode::onTimer() // Update partial trajectory updateTargetIndex(); - partial_trajectory_ = getPartialTrajectory(trajectory_, prev_target_index_, target_index_); + partial_trajectory_ = + utils::get_partial_trajectory(trajectory_, prev_target_index_, target_index_); // Publish messages trajectory_pub_->publish(partial_trajectory_); - debug_pose_array_pub_->publish(trajectory2PoseArray(trajectory_)); - debug_partial_pose_array_pub_->publish(trajectory2PoseArray(partial_trajectory_)); + debug_pose_array_pub_->publish(utils::trajectory_to_pose_array(trajectory_)); + debug_partial_pose_array_pub_->publish(utils::trajectory_to_pose_array(partial_trajectory_)); is_new_parking_cycle_ = false; } @@ -537,11 +367,11 @@ void FreespacePlannerNode::planTrajectory() algo_->setMap(*occupancy_grid_); // Calculate poses in costmap frame - const auto current_pose_in_costmap_frame = transformPose( + const auto current_pose_in_costmap_frame = utils::transform_pose( current_pose_.pose, getTransform(occupancy_grid_->header.frame_id, current_pose_.header.frame_id)); - const auto goal_pose_in_costmap_frame = transformPose( + const auto goal_pose_in_costmap_frame = utils::transform_pose( goal_pose_.pose, getTransform(occupancy_grid_->header.frame_id, goal_pose_.header.frame_id)); // execute planning @@ -559,12 +389,12 @@ void FreespacePlannerNode::planTrajectory() if (result) { RCLCPP_DEBUG(get_logger(), "Found goal!"); - trajectory_ = - createTrajectory(current_pose_, algo_->getWaypoints(), node_param_.waypoints_velocity); - reversing_indices_ = getReversingIndices(trajectory_); + trajectory_ = utils::create_trajectory( + current_pose_, algo_->getWaypoints(), node_param_.waypoints_velocity); + reversing_indices_ = utils::get_reversing_indices(trajectory_); prev_target_index_ = 0; - target_index_ = - getNextTargetIndex(trajectory_.points.size(), reversing_indices_, prev_target_index_); + target_index_ = utils::get_next_target_index( + trajectory_.points.size(), reversing_indices_, prev_target_index_); } else { RCLCPP_INFO(get_logger(), "Can't find goal: %s", error_msg.c_str()); reset(); diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/utils.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/utils.cpp new file mode 100644 index 0000000000000..badd9c824468a --- /dev/null +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/utils.cpp @@ -0,0 +1,178 @@ +// 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/freespace_planner/utils.hpp" + +#include + +namespace autoware::freespace_planner::utils +{ + +PoseArray trajectory_to_pose_array(const Trajectory & trajectory) +{ + PoseArray pose_array; + pose_array.header = trajectory.header; + + for (const auto & point : trajectory.points) { + pose_array.poses.push_back(point.pose); + } + + return pose_array; +} + +double calc_distance_2d(const Trajectory & trajectory, const Pose & pose) +{ + const auto idx = autoware::motion_utils::findNearestIndex(trajectory.points, pose.position); + return autoware::universe_utils::calcDistance2d(trajectory.points.at(idx), pose); +} + +Pose transform_pose(const Pose & pose, const TransformStamped & transform) +{ + PoseStamped transformed_pose; + PoseStamped orig_pose; + orig_pose.pose = pose; + tf2::doTransform(orig_pose, transformed_pose, transform); + + return transformed_pose.pose; +} + +bool is_active(const Scenario::ConstSharedPtr & scenario) +{ + if (!scenario) return false; + + const auto & s = scenario->activating_scenarios; + return std::find(std::begin(s), std::end(s), Scenario::PARKING) != std::end(s); +} + +std::vector get_reversing_indices(const Trajectory & trajectory) +{ + std::vector indices; + + for (size_t i = 0; i < trajectory.points.size() - 1; ++i) { + if ( + trajectory.points.at(i).longitudinal_velocity_mps * + trajectory.points.at(i + 1).longitudinal_velocity_mps < + 0) { + indices.push_back(i); + } + } + + return indices; +} + +size_t get_next_target_index( + const size_t trajectory_size, const std::vector & reversing_indices, + const size_t current_target_index) +{ + if (!reversing_indices.empty()) { + for (const auto reversing_index : reversing_indices) { + if (reversing_index > current_target_index) { + return reversing_index; + } + } + } + + return trajectory_size - 1; +} + +Trajectory get_partial_trajectory( + const Trajectory & trajectory, const size_t start_index, const size_t end_index) +{ + Trajectory partial_trajectory; + partial_trajectory.header = trajectory.header; + partial_trajectory.header.stamp = rclcpp::Clock().now(); + + partial_trajectory.points.reserve(trajectory.points.size()); + for (size_t i = start_index; i <= end_index; ++i) { + partial_trajectory.points.push_back(trajectory.points.at(i)); + } + + // Modify velocity at start/end point + if (partial_trajectory.points.size() >= 2) { + partial_trajectory.points.front().longitudinal_velocity_mps = + partial_trajectory.points.at(1).longitudinal_velocity_mps; + } + if (!partial_trajectory.points.empty()) { + partial_trajectory.points.back().longitudinal_velocity_mps = 0; + } + + return partial_trajectory; +} + +Trajectory create_trajectory( + const PoseStamped & current_pose, const PlannerWaypoints & planner_waypoints, + const double & velocity) +{ + Trajectory trajectory; + trajectory.header = planner_waypoints.header; + + for (const auto & awp : planner_waypoints.waypoints) { + TrajectoryPoint point; + + point.pose = awp.pose.pose; + + point.pose.position.z = current_pose.pose.position.z; // height = const + point.longitudinal_velocity_mps = velocity / 3.6; // velocity = const + + // switch sign by forward/backward + point.longitudinal_velocity_mps = (awp.is_back ? -1 : 1) * point.longitudinal_velocity_mps; + + trajectory.points.push_back(point); + } + + return trajectory; +} + +Trajectory create_stop_trajectory(const PoseStamped & current_pose) +{ + PlannerWaypoints waypoints; + PlannerWaypoint waypoint; + + waypoints.header.stamp = rclcpp::Clock().now(); + waypoints.header.frame_id = current_pose.header.frame_id; + waypoint.pose.header = waypoints.header; + waypoint.pose.pose = current_pose.pose; + waypoint.is_back = false; + waypoints.waypoints.push_back(waypoint); + + return create_trajectory(current_pose, waypoints, 0.0); +} + +Trajectory create_stop_trajectory(const Trajectory & trajectory) +{ + Trajectory stop_trajectory = trajectory; + for (size_t i = 0; i < trajectory.points.size(); ++i) { + stop_trajectory.points.at(i).longitudinal_velocity_mps = 0.0; + } + return stop_trajectory; +} + +bool is_stopped( + const std::deque & odom_buffer, const double th_stopped_velocity_mps) +{ + for (const auto & odom : odom_buffer) { + if (std::abs(odom->twist.twist.linear.x) > th_stopped_velocity_mps) { + return false; + } + } + return true; +} + +bool is_near_target(const Pose & target_pose, const Pose & current_pose, const double th_distance_m) +{ + const double long_disp_to_target = + autoware::universe_utils::calcLongitudinalDeviation(target_pose, current_pose.position); + return std::abs(long_disp_to_target) < th_distance_m; +} +} // namespace autoware::freespace_planner::utils diff --git a/planning/autoware_freespace_planner/test/test_freespace_planner.cpp b/planning/autoware_freespace_planner/test/test_freespace_planner.cpp new file mode 100644 index 0000000000000..37ea52c07734b --- /dev/null +++ b/planning/autoware_freespace_planner/test/test_freespace_planner.cpp @@ -0,0 +1,226 @@ +// 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/freespace_planner/freespace_planner_node.hpp" +#include "autoware/freespace_planner/utils.hpp" + +#include +#include +#include + +#include +#include + +#include +#include + +using autoware::freespace_planner::FreespacePlannerNode; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Pose; +using nav_msgs::msg::OccupancyGrid; +using nav_msgs::msg::Odometry; + +class TestFreespacePlanner : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + set_up_node(); + set_up_costmap(); + set_up_trajectory(); + } + + void set_up_node() + { + auto node_options = rclcpp::NodeOptions{}; + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); + const auto freespace_planner_dir = + ament_index_cpp::get_package_share_directory("autoware_freespace_planner"); + node_options.arguments( + {"--ros-args", "--params-file", + autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", + freespace_planner_dir + "/config/freespace_planner.param.yaml"}); + freespace_planner_ = std::make_shared(node_options); + } + + void set_up_costmap() + { + const size_t width = 80; + const size_t height = 80; + const double resolution = 0.5; + costmap_ = autoware::test_utils::makeCostMapMsg(width, height, resolution); + } + + void set_up_trajectory() + { + trajectory_.points.clear(); + + const double y = 0.5 * costmap_.info.height * costmap_.info.resolution; + + // forward trajectory for 20.0 m + double x = 5.0; + for (; x < 25.0 + std::numeric_limits::epsilon(); x += costmap_.info.resolution) { + TrajectoryPoint point; + point.pose.position.x = x; + point.pose.position.y = y; + point.longitudinal_velocity_mps = 1.0; + trajectory_.points.push_back(point); + } + + // backward trajectory for 10.0 m + x = trajectory_.points.back().pose.position.x - costmap_.info.resolution; + for (; x > 15.0 - std::numeric_limits::epsilon(); x -= costmap_.info.resolution) { + TrajectoryPoint point; + point.pose.position.x = x; + point.pose.position.y = y; + point.longitudinal_velocity_mps = -1.0; + trajectory_.points.push_back(point); + } + + // forward trajectory for 20.0 m + x = trajectory_.points.back().pose.position.x + costmap_.info.resolution; + for (; x < 35.0 + std::numeric_limits::epsilon(); x += costmap_.info.resolution) { + TrajectoryPoint point; + point.pose.position.x = x; + point.pose.position.y = y; + point.longitudinal_velocity_mps = 1.0; + trajectory_.points.push_back(point); + } + + reversing_indices = autoware::freespace_planner::utils::get_reversing_indices(trajectory_); + } + + [[nodiscard]] OccupancyGrid get_blocked_costmap() const + { + auto costmap = costmap_; + const auto block_y = 0.5 * costmap.info.height * costmap.info.resolution; + const auto block_x = 20.0; + const int index_x = std::round(block_x / costmap.info.resolution); + const int index_y = std::round(block_y / costmap.info.resolution); + const auto id = index_y * costmap.info.width + index_x; + costmap.data.at(id) = 100; + return costmap; + } + + bool test_is_plan_required( + const bool empty_traj = false, const bool colliding = false, const bool out_of_course = false) + { + freespace_planner_->trajectory_ = Trajectory(); + freespace_planner_->partial_trajectory_ = Trajectory(); + freespace_planner_->reversing_indices_.clear(); + freespace_planner_->obs_found_time_ = {}; + + freespace_planner_->occupancy_grid_ = std::make_shared(costmap_); + + if (empty_traj) { + return freespace_planner_->isPlanRequired(); + } + + freespace_planner_->trajectory_ = trajectory_; + freespace_planner_->reversing_indices_ = reversing_indices; + freespace_planner_->partial_trajectory_ = + autoware::freespace_planner::utils::get_partial_trajectory( + trajectory_, 0, reversing_indices.front()); + freespace_planner_->current_pose_.pose = trajectory_.points.front().pose; + + if (colliding) { + freespace_planner_->obs_found_time_ = freespace_planner_->get_clock()->now(); + freespace_planner_->occupancy_grid_ = std::make_shared(get_blocked_costmap()); + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + if (out_of_course) { + freespace_planner_->current_pose_.pose.position.y += + (freespace_planner_->node_param_.th_course_out_distance_m + 0.1); + } + + return freespace_planner_->isPlanRequired(); + } + + std::pair test_update_target_index( + const bool stopped = false, const bool near_target = false) + { + freespace_planner_->trajectory_ = trajectory_; + freespace_planner_->reversing_indices_ = reversing_indices; + freespace_planner_->prev_target_index_ = 0; + freespace_planner_->target_index_ = autoware::freespace_planner::utils::get_next_target_index( + trajectory_.points.size(), reversing_indices, freespace_planner_->prev_target_index_); + freespace_planner_->partial_trajectory_ = + autoware::freespace_planner::utils::get_partial_trajectory( + trajectory_, freespace_planner_->prev_target_index_, freespace_planner_->target_index_); + + Odometry odom; + odom.pose.pose = trajectory_.points.front().pose; + odom.twist.twist.linear.x = 1.0; + + if (stopped) odom.twist.twist.linear.x = 0.0; + if (near_target) odom.pose.pose = trajectory_.points.at(freespace_planner_->target_index_).pose; + + freespace_planner_->odom_buffer_.clear(); + freespace_planner_->odom_ = std::make_shared(odom); + freespace_planner_->odom_buffer_.push_back(freespace_planner_->odom_); + freespace_planner_->current_pose_.pose = odom.pose.pose; + + freespace_planner_->updateTargetIndex(); + + return {freespace_planner_->prev_target_index_, freespace_planner_->target_index_}; + } + + void TearDown() override + { + freespace_planner_ = nullptr; + trajectory_ = Trajectory(); + costmap_ = OccupancyGrid(); + rclcpp::shutdown(); + } + + std::shared_ptr freespace_planner_; + OccupancyGrid costmap_; + Trajectory trajectory_; + std::vector reversing_indices; +}; + +TEST_F(TestFreespacePlanner, testIsPlanRequired) +{ + EXPECT_FALSE(test_is_plan_required()); + // test with empty current trajectory + EXPECT_TRUE(test_is_plan_required(true)); + // test with blocked trajectory + EXPECT_TRUE(test_is_plan_required(false, true)); + // test with deviation from trajectory + EXPECT_TRUE(test_is_plan_required(false, false, true)); +} + +TEST_F(TestFreespacePlanner, testUpdateTargetIndex) +{ + size_t prev_target_index, target_index; + std::tie(prev_target_index, target_index) = test_update_target_index(); + EXPECT_EQ(prev_target_index, 0ul); + EXPECT_EQ(target_index, reversing_indices.front()); + + std::tie(prev_target_index, target_index) = test_update_target_index(true); + EXPECT_EQ(prev_target_index, 0ul); + EXPECT_EQ(target_index, reversing_indices.front()); + + std::tie(prev_target_index, target_index) = test_update_target_index(false, true); + EXPECT_EQ(prev_target_index, 0ul); + EXPECT_EQ(target_index, reversing_indices.front()); + + std::tie(prev_target_index, target_index) = test_update_target_index(true, true); + EXPECT_EQ(prev_target_index, reversing_indices.front()); + EXPECT_EQ(target_index, *std::next(reversing_indices.begin())); +} diff --git a/planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp b/planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp new file mode 100644 index 0000000000000..071f7e2cfcaa0 --- /dev/null +++ b/planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp @@ -0,0 +1,282 @@ +// 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/freespace_planner/utils.hpp" + +#include + +#include + +#include +#include + +using autoware::freespace_planner::utils::Odometry; +using autoware::freespace_planner::utils::Scenario; +using autoware::freespace_planner::utils::Trajectory; +using autoware::freespace_planner::utils::TrajectoryPoint; +using autoware::freespace_planning_algorithms::PlannerWaypoint; +using autoware::freespace_planning_algorithms::PlannerWaypoints; + +Trajectory get_trajectory(const size_t n_switches) +{ + const auto get_traj_point = [](const double x, const double y, const float v) { + TrajectoryPoint point; + point.pose.position.x = x; + point.pose.position.y = y; + point.longitudinal_velocity_mps = v; + return point; + }; + + Trajectory trajectory; + constexpr double eps = 0.001; + constexpr double res = 0.5; + + const auto add_points = [&](const double length, const bool forward) { + auto x = trajectory.points.empty() ? 0.0 : trajectory.points.back().pose.position.x; + + if (forward) { + const auto target = x + length; + for (; x < target + eps; x += res) { + trajectory.points.push_back(get_traj_point(x, 0.0, 1.0)); + } + } else { + const auto target = x - length; + for (; x > target - eps; x -= res) { + trajectory.points.push_back(get_traj_point(x, 0.0, -1.0)); + } + } + }; + + bool forward = true; + for (size_t i = 0; i <= n_switches; ++i) { + const auto length = forward ? 5.0 : 3.0; + add_points(length, forward); + forward = !forward; + } + + return trajectory; +} + +PlannerWaypoints get_waypoints(const size_t n_switches) +{ + const auto get_waypoint = [](const double x, const double y, const bool is_back) { + PlannerWaypoint point; + point.pose.pose.position.x = x; + point.pose.pose.position.y = y; + point.is_back = is_back; + return point; + }; + + PlannerWaypoints waypoints; + constexpr double eps = 0.001; + constexpr double res = 0.5; + + const auto add_points = [&](const double length, const bool forward) { + auto x = waypoints.waypoints.empty() ? 0.0 : waypoints.waypoints.back().pose.pose.position.x; + + if (forward) { + const auto target = x + length; + for (; x < target + eps; x += res) { + waypoints.waypoints.push_back(get_waypoint(x, 0.0, !forward)); + } + } else { + const auto target = x - length; + for (; x > target - eps; x -= res) { + waypoints.waypoints.push_back(get_waypoint(x, 0.0, !forward)); + } + } + }; + + bool forward = true; + for (size_t i = 0; i <= n_switches; ++i) { + const auto length = forward ? 5.0 : 3.0; + add_points(length, forward); + forward = !forward; + } + + return waypoints; +} + +TEST(FreespacePlannerUtilsTest, testIsActive) +{ + Scenario::ConstSharedPtr scenario_ptr; + + EXPECT_FALSE(autoware::freespace_planner::utils::is_active(scenario_ptr)); + + Scenario scenario; + scenario.current_scenario = Scenario::EMPTY; + scenario_ptr = std::make_shared(scenario); + EXPECT_FALSE(autoware::freespace_planner::utils::is_active(scenario_ptr)); + + scenario.current_scenario = Scenario::LANEDRIVING; + scenario_ptr = std::make_shared(scenario); + EXPECT_FALSE(autoware::freespace_planner::utils::is_active(scenario_ptr)); + + scenario.current_scenario = Scenario::PARKING; + scenario.activating_scenarios.push_back(Scenario::PARKING); + scenario_ptr = std::make_shared(scenario); + EXPECT_TRUE(autoware::freespace_planner::utils::is_active(scenario_ptr)); +} + +TEST(FreespacePlannerUtilsTest, testIsStopped) +{ + std::deque odometry_buffer; + const double th_stopped_velocity_mps = 0.01; + EXPECT_TRUE( + autoware::freespace_planner::utils::is_stopped(odometry_buffer, th_stopped_velocity_mps)); + + Odometry odometry; + odometry.twist.twist.linear.x = 0.0; + odometry_buffer.push_back(std::make_shared(odometry)); + EXPECT_TRUE( + autoware::freespace_planner::utils::is_stopped(odometry_buffer, th_stopped_velocity_mps)); + + odometry.twist.twist.linear.x = 1.0; + odometry_buffer.push_back(std::make_shared(odometry)); + EXPECT_FALSE( + autoware::freespace_planner::utils::is_stopped(odometry_buffer, th_stopped_velocity_mps)); +} + +TEST(FreespacePlannerUtilsTest, testIsNearTarget) +{ + const auto trajectory = get_trajectory(0ul); + const auto target_pose = trajectory.points.back().pose; + + auto current_pose = target_pose; + current_pose.position.x -= 1.0; + current_pose.position.y += 1.0; + + const double th_distance_m = 0.5; + + EXPECT_FALSE( + autoware::freespace_planner::utils::is_near_target(target_pose, current_pose, th_distance_m)); + + current_pose.position.x += 0.6; + EXPECT_TRUE( + autoware::freespace_planner::utils::is_near_target(target_pose, current_pose, th_distance_m)); +} + +TEST(FreespacePlannerUtilsTest, testGetReversingIndices) +{ + auto trajectory = get_trajectory(0ul); + auto reversing_indices = autoware::freespace_planner::utils::get_reversing_indices(trajectory); + EXPECT_EQ(reversing_indices.size(), 0ul); + + trajectory = get_trajectory(1ul); + reversing_indices = autoware::freespace_planner::utils::get_reversing_indices(trajectory); + EXPECT_EQ(reversing_indices.size(), 1ul); + if (!reversing_indices.empty()) { + EXPECT_EQ(reversing_indices.front(), 10ul); + } + + trajectory = get_trajectory(2ul); + reversing_indices = autoware::freespace_planner::utils::get_reversing_indices(trajectory); + EXPECT_EQ(reversing_indices.size(), 2ul); + if (!reversing_indices.empty()) { + EXPECT_EQ(reversing_indices.front(), 10ul); + EXPECT_EQ(reversing_indices.back(), 17ul); + } +} + +TEST(FreespacePlannerUtilsTest, testGetNextTargetIndex) +{ + auto trajectory = get_trajectory(0ul); + auto reversing_indices = autoware::freespace_planner::utils::get_reversing_indices(trajectory); + auto current_target_index = 0ul; + auto next_target_index = autoware::freespace_planner::utils::get_next_target_index( + trajectory.points.size(), reversing_indices, current_target_index); + EXPECT_EQ(next_target_index, trajectory.points.size() - 1); + + trajectory = get_trajectory(2ul); + reversing_indices = autoware::freespace_planner::utils::get_reversing_indices(trajectory); + ASSERT_EQ(reversing_indices.size(), 2ul); + + next_target_index = autoware::freespace_planner::utils::get_next_target_index( + trajectory.points.size(), reversing_indices, current_target_index); + EXPECT_EQ(next_target_index, reversing_indices.front()); + + current_target_index = reversing_indices.front(); + next_target_index = autoware::freespace_planner::utils::get_next_target_index( + trajectory.points.size(), reversing_indices, current_target_index); + EXPECT_EQ(next_target_index, reversing_indices.back()); +} + +TEST(FreespacePlannerUtilsTest, testGetPartialTrajectory) +{ + const auto trajectory = get_trajectory(2ul); + const auto reversing_indices = + autoware::freespace_planner::utils::get_reversing_indices(trajectory); + + ASSERT_EQ(reversing_indices.size(), 2ul); + + auto partial_traj = autoware::freespace_planner::utils::get_partial_trajectory( + trajectory, 0ul, reversing_indices.front()); + ASSERT_FALSE(partial_traj.points.empty()); + auto expected_size = reversing_indices.front() + 1ul; + EXPECT_EQ(partial_traj.points.size(), expected_size); + EXPECT_TRUE(partial_traj.points.front().longitudinal_velocity_mps > 0.0); + EXPECT_FLOAT_EQ(partial_traj.points.back().longitudinal_velocity_mps, 0.0); + + partial_traj = autoware::freespace_planner::utils::get_partial_trajectory( + trajectory, reversing_indices.front(), reversing_indices.back()); + ASSERT_FALSE(partial_traj.points.empty()); + expected_size = reversing_indices.back() - reversing_indices.front() + 1ul; + EXPECT_EQ(partial_traj.points.size(), expected_size); + EXPECT_TRUE(partial_traj.points.front().longitudinal_velocity_mps < 0.0); + EXPECT_FLOAT_EQ(partial_traj.points.back().longitudinal_velocity_mps, 0.0); +} + +TEST(FreespacePlannerUtilsTest, testCreateTrajectory) +{ + const auto waypoints = get_waypoints(1ul); + + geometry_msgs::msg::PoseStamped current_pose; + current_pose.pose.position.z = 1.0; + const double velocity = 1.0; + + const auto trajectory = + autoware::freespace_planner::utils::create_trajectory(current_pose, waypoints, velocity); + + ASSERT_FALSE(trajectory.points.empty()); + EXPECT_EQ(trajectory.points.size(), waypoints.waypoints.size()); + EXPECT_EQ(trajectory.points.front().pose.position.z, current_pose.pose.position.z); + EXPECT_EQ(trajectory.points.back().pose.position.z, current_pose.pose.position.z); + EXPECT_TRUE(trajectory.points.front().longitudinal_velocity_mps > 0.0); + EXPECT_TRUE(trajectory.points.back().longitudinal_velocity_mps < 0.0); +} + +TEST(FreespacePlannerUtilsTest, testCreateStopTrajectory) +{ + geometry_msgs::msg::PoseStamped current_pose; + current_pose.pose.position.x = 1.0; + + auto stop_traj = autoware::freespace_planner::utils::create_stop_trajectory(current_pose); + EXPECT_EQ(stop_traj.points.size(), 1ul); + if (!stop_traj.points.empty()) { + EXPECT_DOUBLE_EQ(stop_traj.points.front().pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(stop_traj.points.front().longitudinal_velocity_mps, 0.0); + } + + const auto trajectory = get_trajectory(0ul); + stop_traj = autoware::freespace_planner::utils::create_stop_trajectory(trajectory); + EXPECT_EQ(stop_traj.points.size(), trajectory.points.size()); + if (!stop_traj.points.empty()) { + EXPECT_DOUBLE_EQ( + stop_traj.points.front().pose.position.x, trajectory.points.front().pose.position.x); + EXPECT_DOUBLE_EQ( + stop_traj.points.back().pose.position.x, trajectory.points.back().pose.position.x); + EXPECT_DOUBLE_EQ(stop_traj.points.front().longitudinal_velocity_mps, 0.0); + EXPECT_DOUBLE_EQ(stop_traj.points.back().longitudinal_velocity_mps, 0.0); + } +} diff --git a/planning/autoware_mission_planner/CMakeLists.txt b/planning/autoware_mission_planner/CMakeLists.txt index 74bc8ddbc0a32..939903c2999ff 100644 --- a/planning/autoware_mission_planner/CMakeLists.txt +++ b/planning/autoware_mission_planner/CMakeLists.txt @@ -38,7 +38,8 @@ pluginlib_export_plugin_description_file(autoware_mission_planner plugins/plugin if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - test/test_lanelet2_plugins_default_planner.cpp + test/test_lanelet2_plugins_default_planner.cpp + test/test_utility_functions.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}_lanelet2_plugins diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp index 3cf6e73bc2bc6..50a6becf2e124 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -41,96 +41,6 @@ #include #include -namespace -{ -using RouteSections = std::vector; - -bool is_in_lane(const lanelet::ConstLanelet & lanelet, const lanelet::ConstPoint3d & point) -{ - // check if goal is on a lane at appropriate angle - const auto distance = boost::geometry::distance( - lanelet.polygon2d().basicPolygon(), lanelet::utils::to2D(point).basicPoint()); - constexpr double th_distance = std::numeric_limits::epsilon(); - return distance < th_distance; -} - -bool is_in_parking_space( - const lanelet::ConstLineStrings3d & parking_spaces, const lanelet::ConstPoint3d & point) -{ - for (const auto & parking_space : parking_spaces) { - lanelet::ConstPolygon3d parking_space_polygon; - if (!lanelet::utils::lineStringWithWidthToPolygon(parking_space, &parking_space_polygon)) { - continue; - } - - const double distance = boost::geometry::distance( - lanelet::utils::to2D(parking_space_polygon).basicPolygon(), - lanelet::utils::to2D(point).basicPoint()); - constexpr double th_distance = std::numeric_limits::epsilon(); - if (distance < th_distance) { - return true; - } - } - return false; -} - -bool is_in_parking_lot( - const lanelet::ConstPolygons3d & parking_lots, const lanelet::ConstPoint3d & point) -{ - for (const auto & parking_lot : parking_lots) { - const double distance = boost::geometry::distance( - lanelet::utils::to2D(parking_lot).basicPolygon(), lanelet::utils::to2D(point).basicPoint()); - constexpr double th_distance = std::numeric_limits::epsilon(); - if (distance < th_distance) { - return true; - } - } - return false; -} - -double project_goal_to_map( - const lanelet::ConstLanelet & lanelet_component, const lanelet::ConstPoint3d & goal_point) -{ - const lanelet::ConstLineString3d center_line = - lanelet::utils::generateFineCenterline(lanelet_component); - lanelet::BasicPoint3d project = lanelet::geometry::project(center_line, goal_point.basicPoint()); - return project.z(); -} - -geometry_msgs::msg::Pose get_closest_centerline_pose( - const lanelet::ConstLanelets & road_lanelets, const geometry_msgs::msg::Pose & point, - autoware::vehicle_info_utils::VehicleInfo vehicle_info) -{ - lanelet::Lanelet closest_lanelet; - if (!lanelet::utils::query::getClosestLaneletWithConstrains( - road_lanelets, point, &closest_lanelet, 0.0)) { - // point is not on any lanelet. - return point; - } - - const auto refined_center_line = lanelet::utils::generateFineCenterline(closest_lanelet, 1.0); - closest_lanelet.setCenterline(refined_center_line); - - const double lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, point.position); - - const auto nearest_idx = autoware::motion_utils::findNearestIndex( - convertCenterlineToPoints(closest_lanelet), point.position); - const auto nearest_point = closest_lanelet.centerline()[nearest_idx]; - - // shift nearest point on its local y axis so that vehicle's right and left edges - // would have approx the same clearance from road border - const auto shift_length = (vehicle_info.right_overhang_m - vehicle_info.left_overhang_m) / 2.0; - const auto delta_x = -shift_length * std::sin(lane_yaw); - const auto delta_y = shift_length * std::cos(lane_yaw); - - lanelet::BasicPoint3d refined_point( - nearest_point.x() + delta_x, nearest_point.y() + delta_y, nearest_point.z()); - - return convertBasicPoint3dToPose(refined_point, lane_yaw); -} - -} // anonymous namespace - namespace autoware::mission_planner::lanelet2 { @@ -395,8 +305,7 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) std::stringstream log_ss; for (const auto & point : points) { - log_ss << "x: " << point.position.x << " " - << "y: " << point.position.y << std::endl; + log_ss << "x: " << point.position.x << " " << "y: " << point.position.y << std::endl; } RCLCPP_DEBUG_STREAM( logger, "start planning route with check points: " << std::endl diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp index 34b16ef35f603..08345282a9264 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -14,10 +14,18 @@ #include "utility_functions.hpp" +#include +#include +#include + #include #include +#include + +namespace autoware::mission_planner::lanelet2 +{ autoware::universe_utils::Polygon2d convert_linear_ring_to_polygon( autoware::universe_utils::LinearRing2d footprint) { @@ -64,3 +72,89 @@ geometry_msgs::msg::Pose convertBasicPoint3dToPose( return pose; } + +bool is_in_lane(const lanelet::ConstLanelet & lanelet, const lanelet::ConstPoint3d & point) +{ + // check if goal is on a lane at appropriate angle + const auto distance = boost::geometry::distance( + lanelet.polygon2d().basicPolygon(), lanelet::utils::to2D(point).basicPoint()); + constexpr double th_distance = std::numeric_limits::epsilon(); + return distance < th_distance; +} + +bool is_in_parking_space( + const lanelet::ConstLineStrings3d & parking_spaces, const lanelet::ConstPoint3d & point) +{ + for (const auto & parking_space : parking_spaces) { + lanelet::ConstPolygon3d parking_space_polygon; + if (!lanelet::utils::lineStringWithWidthToPolygon(parking_space, &parking_space_polygon)) { + continue; + } + + const double distance = boost::geometry::distance( + lanelet::utils::to2D(parking_space_polygon).basicPolygon(), + lanelet::utils::to2D(point).basicPoint()); + constexpr double th_distance = std::numeric_limits::epsilon(); + if (distance < th_distance) { + return true; + } + } + return false; +} + +bool is_in_parking_lot( + const lanelet::ConstPolygons3d & parking_lots, const lanelet::ConstPoint3d & point) +{ + for (const auto & parking_lot : parking_lots) { + const double distance = boost::geometry::distance( + lanelet::utils::to2D(parking_lot).basicPolygon(), lanelet::utils::to2D(point).basicPoint()); + constexpr double th_distance = std::numeric_limits::epsilon(); + if (distance < th_distance) { + return true; + } + } + return false; +} + +double project_goal_to_map( + const lanelet::ConstLanelet & lanelet_component, const lanelet::ConstPoint3d & goal_point) +{ + const lanelet::ConstLineString3d center_line = + lanelet::utils::generateFineCenterline(lanelet_component); + lanelet::BasicPoint3d project = lanelet::geometry::project(center_line, goal_point.basicPoint()); + return project.z(); +} + +geometry_msgs::msg::Pose get_closest_centerline_pose( + const lanelet::ConstLanelets & road_lanelets, const geometry_msgs::msg::Pose & point, + autoware::vehicle_info_utils::VehicleInfo vehicle_info) +{ + lanelet::Lanelet closest_lanelet; + if (!lanelet::utils::query::getClosestLaneletWithConstrains( + road_lanelets, point, &closest_lanelet, 0.0)) { + // point is not on any lanelet. + return point; + } + + const auto refined_center_line = lanelet::utils::generateFineCenterline(closest_lanelet, 1.0); + closest_lanelet.setCenterline(refined_center_line); + + const double lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, point.position); + + const auto nearest_idx = autoware::motion_utils::findNearestIndex( + convertCenterlineToPoints(closest_lanelet), point.position); + const auto nearest_point = closest_lanelet.centerline()[nearest_idx]; + + // shift nearest point on its local y axis so that vehicle's right and left edges + // would have approx the same clearance from road border + const auto shift_length = (vehicle_info.right_overhang_m - vehicle_info.left_overhang_m) / 2.0; + const auto delta_x = -shift_length * std::sin(lane_yaw); + const auto delta_y = shift_length * std::cos(lane_yaw); + + lanelet::BasicPoint3d refined_point( + nearest_point.x() + delta_x, nearest_point.y() + delta_y, nearest_point.z()); + + return convertBasicPoint3dToPose(refined_point, lane_yaw); +} + +} // namespace autoware::mission_planner::lanelet2 diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp index 6e3d2a0e88941..36a2e17fb5ff0 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -29,6 +29,10 @@ #include +namespace autoware::mission_planner::lanelet2 +{ +using RouteSections = std::vector; + template bool exists(const std::vector & vectors, const T & item) { @@ -48,4 +52,17 @@ void insert_marker_array( std::vector convertCenterlineToPoints(const lanelet::Lanelet & lanelet); geometry_msgs::msg::Pose convertBasicPoint3dToPose( const lanelet::BasicPoint3d & point, const double lane_yaw); + +bool is_in_lane(const lanelet::ConstLanelet & lanelet, const lanelet::ConstPoint3d & point); +bool is_in_parking_space( + const lanelet::ConstLineStrings3d & parking_spaces, const lanelet::ConstPoint3d & point); +bool is_in_parking_lot( + const lanelet::ConstPolygons3d & parking_lots, const lanelet::ConstPoint3d & point); +double project_goal_to_map( + const lanelet::ConstLanelet & lanelet_component, const lanelet::ConstPoint3d & goal_point); +geometry_msgs::msg::Pose get_closest_centerline_pose( + const lanelet::ConstLanelets & road_lanelets, const geometry_msgs::msg::Pose & point, + autoware::vehicle_info_utils::VehicleInfo vehicle_info); + +} // namespace autoware::mission_planner::lanelet2 #endif // LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ diff --git a/planning/autoware_mission_planner/test/test_lanelet2_plugins_default_planner.cpp b/planning/autoware_mission_planner/test/test_lanelet2_plugins_default_planner.cpp index 78db6a5072a50..124a04e3e4ed2 100644 --- a/planning/autoware_mission_planner/test/test_lanelet2_plugins_default_planner.cpp +++ b/planning/autoware_mission_planner/test/test_lanelet2_plugins_default_planner.cpp @@ -13,22 +13,33 @@ // limitations under the License. #include <../src/lanelet2_plugins/default_planner.hpp> +#include #include +#include #include #include #include #include +#include #include #include #include #include +#include -struct DefaultPlanner : protected autoware::mission_planner::lanelet2::DefaultPlanner +using autoware::universe_utils::calcOffsetPose; +using autoware::universe_utils::createQuaternionFromRPY; +using autoware_planning_msgs::msg::LaneletRoute; +using geometry_msgs::msg::Pose; +using RoutePoints = std::vector; + +// inherit DefaultPlanner to access protected methods and make wrapper to private methods +struct DefaultPlanner : public autoware::mission_planner::lanelet2::DefaultPlanner { - void set_front_offset(const double offset) { vehicle_info_.max_longitudinal_offset_m = offset; } - void set_dummy_map() { route_handler_.setMap(autoware::test_utils::makeMapBinMsg()); } + // todo(someone): create tests with various kinds of maps + void set_default_test_map() { route_handler_.setMap(autoware::test_utils::makeMapBinMsg()); } [[nodiscard]] bool check_goal_inside_lanes( const lanelet::ConstLanelet & closest_lanelet_to_goal, const lanelet::ConstLanelets & path_lanelets, @@ -37,15 +48,61 @@ struct DefaultPlanner : protected autoware::mission_planner::lanelet2::DefaultPl return check_goal_footprint_inside_lanes( closest_lanelet_to_goal, path_lanelets, goal_footprint); } + bool is_goal_valid_wrapper( + const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelets & path_lanelets) + { + return is_goal_valid(goal, path_lanelets); + } + + lanelet::ConstLanelets get_lanelets_from_ids(const std::vector & ids) + { + const auto lanelet_map_ptr = route_handler_.getLaneletMapPtr(); + + lanelet::ConstLanelets lanelets; + + for (const auto & id : ids) { + lanelets.push_back(lanelet_map_ptr->laneletLayer.get(id)); + } + return lanelets; + } +}; + +class DefaultPlannerTest : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + + rclcpp::NodeOptions options; + + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); + const auto mission_planner_dir = + ament_index_cpp::get_package_share_directory("autoware_mission_planner"); + options.arguments( + {"--ros-args", "--params-file", + autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", + mission_planner_dir + "/config/mission_planner.param.yaml"}); + // NOTE: vehicle width and length set by test_vehicle_info.param.yaml are as follows + // vehicle_width: 1.83, vehicle_length: 4.77 + + node_ = std::make_shared("test_node", options); + planner_.initialize(node_.get()); + } + + ~DefaultPlannerTest() override { rclcpp::shutdown(); } + + std::shared_ptr node_; + + DefaultPlanner planner_; }; -TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) +TEST_F(DefaultPlannerTest, checkGoalInsideLane) { // Test with dummy map such that only the lanelets provided as inputs are used for the ego lane - DefaultPlanner planner; - planner.set_dummy_map(); + planner_.set_default_test_map(); // vehicle max longitudinal offset is used to retrieve additional lanelets from the map - planner.set_front_offset(0.0); lanelet::LineString3d left_bound; lanelet::LineString3d right_bound; left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); @@ -63,7 +120,7 @@ TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) goal_footprint.outer().emplace_back(0.5, 0.5); goal_footprint.outer().emplace_back(0.5, 0); goal_footprint.outer().emplace_back(0, 0); - EXPECT_TRUE(planner.check_goal_inside_lanes(goal_lanelet, {goal_lanelet}, goal_footprint)); + EXPECT_TRUE(planner_.check_goal_inside_lanes(goal_lanelet, {goal_lanelet}, goal_footprint)); // the footprint touches the border of the lanelet goal_footprint.clear(); @@ -72,7 +129,7 @@ TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) goal_footprint.outer().emplace_back(1, 1); goal_footprint.outer().emplace_back(1, 0); goal_footprint.outer().emplace_back(0, 0); - EXPECT_TRUE(planner.check_goal_inside_lanes(goal_lanelet, {goal_lanelet}, goal_footprint)); + EXPECT_TRUE(planner_.check_goal_inside_lanes(goal_lanelet, {goal_lanelet}, goal_footprint)); // add lanelets such that the footprint touches the linestring shared by the combined lanelets lanelet::LineString3d next_left_bound; @@ -83,7 +140,7 @@ TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) next_right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 2, 1}); lanelet::ConstLanelet next_lanelet{lanelet::InvalId, next_left_bound, next_right_bound}; EXPECT_TRUE( - planner.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); + planner_.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); // the footprint is inside the other lanelet goal_footprint.clear(); @@ -93,7 +150,7 @@ TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) goal_footprint.outer().emplace_back(1.6, -0.5); goal_footprint.outer().emplace_back(1.1, -0.5); EXPECT_TRUE( - planner.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); + planner_.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); // the footprint is completely outside of the lanelets goal_footprint.clear(); @@ -103,7 +160,7 @@ TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) goal_footprint.outer().emplace_back(1.6, 1.5); goal_footprint.outer().emplace_back(1.1, 1.5); EXPECT_FALSE( - planner.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); + planner_.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); // the footprint is outside of the lanelets but touches an edge goal_footprint.clear(); @@ -113,7 +170,7 @@ TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) goal_footprint.outer().emplace_back(1.6, 1.0); goal_footprint.outer().emplace_back(1.1, 1.0); EXPECT_FALSE( - planner.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); + planner_.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); // the footprint is outside of the lanelets but share a point goal_footprint.clear(); @@ -123,7 +180,7 @@ TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) goal_footprint.outer().emplace_back(3.0, 1.0); goal_footprint.outer().emplace_back(2.0, 1.0); EXPECT_FALSE( - planner.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); + planner_.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); // ego footprint that overlaps both lanelets goal_footprint.clear(); @@ -133,7 +190,7 @@ TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) goal_footprint.outer().emplace_back(1.5, -0.5); goal_footprint.outer().emplace_back(-0.5, -0.5); EXPECT_TRUE( - planner.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); + planner_.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); // ego footprint that goes further than the next lanelet goal_footprint.clear(); @@ -143,5 +200,215 @@ TEST(TestLanelet2PluginsDefaultPlanner, checkGoalInsideLane) goal_footprint.outer().emplace_back(2.5, -0.5); goal_footprint.outer().emplace_back(-0.5, -0.5); EXPECT_FALSE( - planner.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); + planner_.check_goal_inside_lanes(goal_lanelet, {goal_lanelet, next_lanelet}, goal_footprint)); +} + +TEST_F(DefaultPlannerTest, isValidGoal) +{ + planner_.set_default_test_map(); + + std::vector path_lanelet_ids = {9102, 9540, 9546, 9178, 52, 124}; + const auto path_lanelets = planner_.get_lanelets_from_ids(path_lanelet_ids); + + const double yaw_threshold = 0.872665; // 50 degrees + + /** + * 1) goal pose within the lanelet + */ + // goal pose within the lanelet + Pose goal_pose; + goal_pose.position.x = 3810.24951171875; + goal_pose.position.y = 73769.2578125; + goal_pose.position.z = 0.0; + goal_pose.orientation.x = 0.0; + goal_pose.orientation.y = 0.0; + goal_pose.orientation.z = 0.23908402523702438; + goal_pose.orientation.w = 0.9709988820160721; + const double yaw = tf2::getYaw(goal_pose.orientation); + EXPECT_TRUE(planner_.is_goal_valid_wrapper(goal_pose, path_lanelets)); + + // move 1m to the right to make the goal outside of the lane + Pose right_offset_goal_pose = calcOffsetPose(goal_pose, 0.0, 1.0, 0.0); + EXPECT_FALSE(planner_.is_goal_valid_wrapper(right_offset_goal_pose, path_lanelets)); + + // move 1m to the left to make the goal outside of the lane + Pose left_offset_goal_pose = calcOffsetPose(goal_pose, 0.0, -1.0, 0.0); + EXPECT_FALSE(planner_.is_goal_valid_wrapper(left_offset_goal_pose, path_lanelets)); + + // rotate to the right + Pose right_rotated_goal_pose = goal_pose; + right_rotated_goal_pose.orientation = createQuaternionFromRPY(0.0, 0.0, yaw + yaw_threshold); + EXPECT_FALSE(planner_.is_goal_valid_wrapper(right_rotated_goal_pose, path_lanelets)); + + // rotate to the left + Pose left_rotated_goal_pose = goal_pose; + left_rotated_goal_pose.orientation = createQuaternionFromRPY(0.0, 0.0, yaw - yaw_threshold); + EXPECT_FALSE(planner_.is_goal_valid_wrapper(left_rotated_goal_pose, path_lanelets)); + + /** + * 2) goal pose on the road shoulder + */ + std::vector path_lanelet_to_road_shoulder_ids = {9102, 9540, 9546}; + lanelet::Id target_road_shoulder_id = 10304; // left road shoulder of 9546 + const auto path_lanelets_to_road_shoulder = + planner_.get_lanelets_from_ids(path_lanelet_to_road_shoulder_ids); + const auto target_road_shoulder = + planner_.get_lanelets_from_ids({target_road_shoulder_id}).front(); + + Pose goal_pose_on_road_shoulder; + goal_pose_on_road_shoulder.position.x = 3742.3825513144147; + goal_pose_on_road_shoulder.position.y = 73737.75783925217; + goal_pose_on_road_shoulder.position.z = 0.0; + goal_pose_on_road_shoulder.orientation.x = 0.0; + goal_pose_on_road_shoulder.orientation.y = 0.0; + goal_pose_on_road_shoulder.orientation.z = 0.23721495449671745; + goal_pose_on_road_shoulder.orientation.w = 0.9714571865826721; + EXPECT_TRUE( + planner_.is_goal_valid_wrapper(goal_pose_on_road_shoulder, path_lanelets_to_road_shoulder)); + + // put goal on the left bound of the road shoulder + // if the goal is on the road shoulder, the footprint can be outside of the lane, and only the + // angle is checked + const auto left_bound = target_road_shoulder.leftBound(); + Pose goal_pose_on_road_shoulder_left_bound = goal_pose_on_road_shoulder; + goal_pose_on_road_shoulder_left_bound.position.x = left_bound.front().x(); + goal_pose_on_road_shoulder_left_bound.position.y = left_bound.front().y(); + EXPECT_TRUE(planner_.is_goal_valid_wrapper( + goal_pose_on_road_shoulder_left_bound, path_lanelets_to_road_shoulder)); + + // move goal pose outside of the road shoulder + Pose goal_pose_outside_road_shoulder = + calcOffsetPose(goal_pose_on_road_shoulder_left_bound, 0.0, 0.1, 0.0); + EXPECT_FALSE(planner_.is_goal_valid_wrapper( + goal_pose_outside_road_shoulder, path_lanelets_to_road_shoulder)); + + // rotate goal to the right + Pose right_rotated_goal_pose_on_road_shoulder = goal_pose_on_road_shoulder; + right_rotated_goal_pose_on_road_shoulder.orientation = + createQuaternionFromRPY(0.0, 0.0, yaw + yaw_threshold); + EXPECT_FALSE(planner_.is_goal_valid_wrapper( + right_rotated_goal_pose_on_road_shoulder, path_lanelets_to_road_shoulder)); + + /** + * todo(someone): add more test for parking area + */ +} + +TEST_F(DefaultPlannerTest, plan) +{ + planner_.set_default_test_map(); + + /** + * 1) goal pose within the lanelet + */ + RoutePoints route_points; + Pose start_pose; + start_pose.position.x = 3717.239501953125; + start_pose.position.y = 73720.84375; + start_pose.position.z = 0.0; + start_pose.orientation.x = 0.00012620055018808463; + start_pose.orientation.y = -0.0005077247816171834; + start_pose.orientation.z = 0.2412209576008544; + + Pose goal_pose; + goal_pose.position.x = 3810.24951171875; + goal_pose.position.y = 73769.2578125; + goal_pose.position.z = 0.0; + goal_pose.orientation.x = 0.0; + goal_pose.orientation.y = 0.0; + goal_pose.orientation.z = 0.23908402523702438; + goal_pose.orientation.w = 0.9709988820160721; + + route_points.push_back(start_pose); + route_points.push_back(goal_pose); + const auto route = planner_.plan(route_points); + + EXPECT_EQ(route.start_pose.position.x, start_pose.position.x); + EXPECT_EQ(route.start_pose.position.y, start_pose.position.y); + EXPECT_EQ(route.goal_pose.position.x, goal_pose.position.x); + EXPECT_EQ(route.goal_pose.position.y, goal_pose.position.y); + std::vector path_lanelet_ids = {9102, 9540, 9546, 9178, 52, 124}; + EXPECT_EQ(route.segments.size(), path_lanelet_ids.size()); + for (size_t i = 0; i < route.segments.size(); i++) { + EXPECT_EQ(route.segments[i].preferred_primitive.id, path_lanelet_ids[i]); + const auto & primitives = route.segments[i].primitives; + EXPECT_TRUE(std::find_if(primitives.begin(), primitives.end(), [&](const auto & primitive) { + return primitive.id == path_lanelet_ids[i]; + }) != primitives.end()); + } + + /** + * 2) goal pose on the road shoulder + */ + Pose goal_pose_on_road_shoulder; + goal_pose_on_road_shoulder.position.x = 3742.3825513144147; + goal_pose_on_road_shoulder.position.y = 73737.75783925217; + goal_pose_on_road_shoulder.position.z = 0.0; + goal_pose_on_road_shoulder.orientation.x = 0.0; + goal_pose_on_road_shoulder.orientation.y = 0.0; + goal_pose_on_road_shoulder.orientation.z = 0.23721495449671745; + goal_pose_on_road_shoulder.orientation.w = 0.9714571865826721; + + RoutePoints route_points_to_road_shoulder; + route_points_to_road_shoulder.push_back(start_pose); + route_points_to_road_shoulder.push_back(goal_pose_on_road_shoulder); + const auto route_to_road_shoulder = planner_.plan(route_points_to_road_shoulder); + + EXPECT_EQ(route_to_road_shoulder.start_pose.position.x, start_pose.position.x); + EXPECT_EQ(route_to_road_shoulder.start_pose.position.y, start_pose.position.y); + EXPECT_EQ(route_to_road_shoulder.goal_pose.position.x, goal_pose_on_road_shoulder.position.x); + EXPECT_EQ(route_to_road_shoulder.goal_pose.position.y, goal_pose_on_road_shoulder.position.y); + std::vector path_lanelet_to_road_shoulder_ids = {9102, 9540, 9546}; + EXPECT_EQ(route_to_road_shoulder.segments.size(), path_lanelet_to_road_shoulder_ids.size()); + for (size_t i = 0; i < route_to_road_shoulder.segments.size(); i++) { + EXPECT_EQ( + route_to_road_shoulder.segments[i].preferred_primitive.id, + path_lanelet_to_road_shoulder_ids[i]); + const auto & primitives = route_to_road_shoulder.segments[i].primitives; + EXPECT_TRUE(std::find_if(primitives.begin(), primitives.end(), [&](const auto & primitive) { + return primitive.id == path_lanelet_to_road_shoulder_ids[i]; + }) != primitives.end()); + } +} + +// `visualize` function is used for user too, so it is more important than debug functions +TEST_F(DefaultPlannerTest, visualize) +{ + DefaultPlanner planner; + planner_.set_default_test_map(); + const LaneletRoute route = autoware::test_utils::makeBehaviorNormalRoute(); + const auto marker_array = planner_.visualize(route); + // clang-format off + const std::vector expected_ns = { + // 1) lanelet::visualization::laneletsBoundaryAsMarkerArray + // center line is not set in the test lanelets, + // so "center_lane_line" and "center_line_arrows" are not visualized + "left_lane_bound", "right_lane_bound", "lane_start_bound", + // 2) lanelet::visualization::laneletsAsMarkerArray + "route_lanelets", + // 3) lanelet::visualization::laneletsAsMarkerArray + "goal_lanelets"}; + // clang-format on + for (const auto & marker : marker_array.markers) { + EXPECT_TRUE(std::find(expected_ns.begin(), expected_ns.end(), marker.ns) != expected_ns.end()); + } +} + +TEST_F(DefaultPlannerTest, visualizeDebugFootprint) +{ + DefaultPlanner planner; + planner_.set_default_test_map(); + + autoware::universe_utils::LinearRing2d footprint; + footprint.push_back({1.0, 1.0}); + footprint.push_back({1.0, -1.0}); + footprint.push_back({0.0, -1.0}); + footprint.push_back({-1.0, -1.0}); + footprint.push_back({-1.0, 1.0}); + footprint.push_back({0.0, 1.0}); + footprint.push_back({1.0, 1.0}); + + const auto marker_array = planner_.visualize_debug_footprint(footprint); + EXPECT_EQ(marker_array.markers.size(), 1); + EXPECT_EQ(marker_array.markers[0].points.size(), 7); } diff --git a/planning/autoware_mission_planner/test/test_utility_functions.cpp b/planning/autoware_mission_planner/test/test_utility_functions.cpp new file mode 100644 index 0000000000000..6cde09b7664a2 --- /dev/null +++ b/planning/autoware_mission_planner/test/test_utility_functions.cpp @@ -0,0 +1,423 @@ +// Copyright 2024 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include <../src/lanelet2_plugins/utility_functions.hpp> +#include +#include +#include + +#include +#include +#include +#include +#include + +using autoware::mission_planner::lanelet2::convert_linear_ring_to_polygon; +using autoware::mission_planner::lanelet2::convertBasicPoint3dToPose; +using autoware::mission_planner::lanelet2::convertCenterlineToPoints; +using autoware::mission_planner::lanelet2::exists; +using autoware::mission_planner::lanelet2::get_closest_centerline_pose; +using autoware::mission_planner::lanelet2::insert_marker_array; +using autoware::mission_planner::lanelet2::is_in_lane; +using autoware::mission_planner::lanelet2::is_in_parking_lot; +using autoware::mission_planner::lanelet2::is_in_parking_space; +using autoware::mission_planner::lanelet2::project_goal_to_map; + +using autoware::vehicle_info_utils::VehicleInfo; +using geometry_msgs::msg::Pose; + +TEST(TestUtilityFunctions, convertLinearRingToPolygon) +{ + // clockwise + { + autoware::universe_utils::LinearRing2d footprint; + footprint.push_back({1.0, 1.0}); + footprint.push_back({1.0, -1.0}); + footprint.push_back({0.0, -1.0}); + footprint.push_back({-1.0, -1.0}); + footprint.push_back({-1.0, 1.0}); + footprint.push_back({0.0, 1.0}); + footprint.push_back({1.0, 1.0}); + autoware::universe_utils::Polygon2d polygon = convert_linear_ring_to_polygon(footprint); + + ASSERT_EQ(polygon.outer().size(), footprint.size()); + for (std::size_t i = 0; i < footprint.size(); ++i) { + EXPECT_DOUBLE_EQ( + boost::geometry::get<0>(polygon.outer()[i]), boost::geometry::get<0>(footprint[i])); + EXPECT_DOUBLE_EQ( + boost::geometry::get<1>(polygon.outer()[i]), boost::geometry::get<1>(footprint[i])); + } + + EXPECT_EQ(polygon.outer().front(), polygon.outer().back()); + + const double area = boost::geometry::area(polygon); + EXPECT_GT(area, 0.0); + } + + // counterclockwise + { + autoware::universe_utils::LinearRing2d footprint; + footprint.push_back({1.0, 1.0}); + footprint.push_back({0.0, 1.0}); + footprint.push_back({-1.0, 1.0}); + footprint.push_back({-1.0, -1.0}); + footprint.push_back({0.0, -1.0}); + footprint.push_back({1.0, -1.0}); + footprint.push_back({1.0, 1.0}); + autoware::universe_utils::Polygon2d polygon = convert_linear_ring_to_polygon(footprint); + + ASSERT_EQ(polygon.outer().size(), footprint.size()); + + // polygon is always clockwise + for (std::size_t i = 0; i < footprint.size(); ++i) { + const std::size_t j = footprint.size() - i - 1; + EXPECT_DOUBLE_EQ( + boost::geometry::get<0>(polygon.outer()[i]), boost::geometry::get<0>(footprint[j])); + EXPECT_DOUBLE_EQ( + boost::geometry::get<1>(polygon.outer()[i]), boost::geometry::get<1>(footprint[j])); + } + + const double area = boost::geometry::area(polygon); + EXPECT_GT(area, 0.0); + } +} + +TEST(TestUtilityFunctions, convertCenterlineToPoints) +{ + lanelet::LineString3d left_bound; + lanelet::LineString3d right_bound; + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1}); + lanelet::Lanelet lanelet{lanelet::InvalId, left_bound, right_bound}; + + lanelet::LineString3d centerline; + centerline.push_back(lanelet::Point3d{lanelet::InvalId, -1, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 0, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 1, 0}); + lanelet.setCenterline(centerline); + + const std::vector points = convertCenterlineToPoints(lanelet); + + ASSERT_EQ(points.size(), centerline.size()); + for (std::size_t i = 0; i < centerline.size(); ++i) { + EXPECT_DOUBLE_EQ(points[i].x, centerline[i].x()); + EXPECT_DOUBLE_EQ(points[i].y, centerline[i].y()); + EXPECT_DOUBLE_EQ(points[i].z, centerline[i].z()); + } +} + +TEST(TestUtilityFunctions, insertMarkerArray) +{ + visualization_msgs::msg::MarkerArray a1; + visualization_msgs::msg::MarkerArray a2; + a1.markers.resize(1); + a2.markers.resize(2); + a1.markers[0].id = 0; + a2.markers[0].id = 1; + a2.markers[1].id = 2; + + insert_marker_array(&a1, a2); + + ASSERT_EQ(a1.markers.size(), 3); + EXPECT_EQ(a1.markers[0].id, 0); + EXPECT_EQ(a1.markers[1].id, 1); + EXPECT_EQ(a1.markers[2].id, 2); +} + +TEST(TestUtilityFunctions, convertBasicPoint3dToPose) +{ + { + const lanelet::BasicPoint3d point(1.0, 2.0, 3.0); + const double lane_yaw = 0.0; + const Pose pose = convertBasicPoint3dToPose(point, lane_yaw); + EXPECT_DOUBLE_EQ(pose.position.x, point.x()); + EXPECT_DOUBLE_EQ(pose.position.y, point.y()); + EXPECT_DOUBLE_EQ(pose.position.z, point.z()); + EXPECT_DOUBLE_EQ(pose.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.z, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.w, 1.0); + } + + { + const lanelet::BasicPoint3d point(1.0, 2.0, 3.0); + const double lane_yaw = M_PI_2; + const Pose pose = convertBasicPoint3dToPose(point, lane_yaw); + EXPECT_DOUBLE_EQ(pose.position.x, point.x()); + EXPECT_DOUBLE_EQ(pose.position.y, point.y()); + EXPECT_DOUBLE_EQ(pose.position.z, point.z()); + EXPECT_DOUBLE_EQ(pose.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.z, 0.7071067811865476); + EXPECT_DOUBLE_EQ(pose.orientation.w, 0.7071067811865476); + } +} + +TEST(TestUtilityFunctions, is_in_lane) +{ + lanelet::LineString3d left_bound; + lanelet::LineString3d right_bound; + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1}); + lanelet::Lanelet lanelet{lanelet::InvalId, left_bound, right_bound}; + + lanelet::LineString3d centerline; + centerline.push_back(lanelet::Point3d{lanelet::InvalId, -1, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 0, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 1, 0}); + lanelet.setCenterline(centerline); + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 0}; + EXPECT_TRUE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 1}; + EXPECT_TRUE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -1}; + EXPECT_TRUE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 2}; + EXPECT_FALSE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -2}; + EXPECT_FALSE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 2, 0}; + EXPECT_FALSE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -2, 0}; + EXPECT_FALSE(is_in_lane(lanelet, point)); + } +} + +TEST(TestUtilityFunctions, is_in_parking_lot) +{ + lanelet::Polygon3d parking_lot; + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1}); + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1}); + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1}); + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 0}; + EXPECT_TRUE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 1, 0}; + EXPECT_TRUE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 1}; + EXPECT_TRUE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 2, 0}; + EXPECT_FALSE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 2}; + EXPECT_FALSE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -2, 0}; + EXPECT_FALSE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -2}; + EXPECT_FALSE(is_in_parking_lot({parking_lot}, point)); + } +} + +TEST(TestUtilityFunctions, is_in_parking_space) +{ + lanelet::LineString3d parking_space; + parking_space.push_back(lanelet::Point3d{lanelet::InvalId, -1, 0}); + parking_space.push_back(lanelet::Point3d{lanelet::InvalId, 1, 0}); + parking_space.setAttribute("width", 2.0); + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 0}; + EXPECT_TRUE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 1, 0}; + EXPECT_TRUE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -1, 0}; + EXPECT_TRUE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 2, 0}; + EXPECT_FALSE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 2}; + EXPECT_FALSE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -2, 0}; + EXPECT_FALSE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -2}; + EXPECT_FALSE(is_in_parking_space({parking_space}, point)); + } +} + +TEST(TestUtilityFunctions, project_goal_to_map) +{ + const auto create_lane = [&](const double height) -> lanelet::Lanelet { + lanelet::LineString3d left_bound; + lanelet::LineString3d right_bound; + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1, height}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, -1, height}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1, height}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1, height}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, 1, height}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1, height}); + const lanelet::Lanelet lanelet{lanelet::InvalId, left_bound, right_bound}; + return lanelet; + }; + + const auto check_height = [&](const double height) -> void { + const auto lanelet = create_lane(height); + lanelet::Point3d goal_point{lanelet::InvalId, 0, 0, height}; + EXPECT_DOUBLE_EQ(project_goal_to_map(lanelet, goal_point), height); + }; + + check_height(0.0); + check_height(1.0); + check_height(-1.0); +} + +TEST(TestUtilityFunctions, TestUtilityFunctions) +{ + lanelet::LineString3d left_bound; + lanelet::LineString3d right_bound; + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1}); + lanelet::Lanelet lanelet{lanelet::InvalId, left_bound, right_bound}; + + lanelet::LineString3d centerline; + centerline.push_back(lanelet::Point3d{lanelet::InvalId, -1, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 0, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 1, 0}); + lanelet.setCenterline(centerline); + + VehicleInfo vehicle_info; + vehicle_info.left_overhang_m = 0.5; + vehicle_info.right_overhang_m = 0.5; + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 1, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -1, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, -1.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 2, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 2.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -2, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, -2.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 1}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -1}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } +} diff --git a/planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 2844ff6a54e3d..75adf228cc7ca 100644 --- a/planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -29,15 +29,26 @@ suppress_sudden_obstacle_stop: false stop_obstacle_type: - unknown: true - car: true - truck: true - bus: true - trailer: true - motorcycle: true - bicycle: true - pedestrian: true pointcloud: false + inside: + unknown: true + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: true + + outside: + unknown: false + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: true cruise_obstacle_type: inside: @@ -99,6 +110,12 @@ stop: max_lat_margin: 0.3 # lateral margin between the obstacles except for unknown and ego's footprint max_lat_margin_against_unknown: 0.3 # lateral margin between the unknown obstacles and ego's footprint + min_velocity_to_reach_collision_point: 2.0 # minimum velocity margin to calculate time to reach collision [m/s] + outside_obstacle: + max_lateral_time_margin: 4.0 # time threshold of lateral margin between the obstacles and ego's footprint [s] + num_of_predicted_paths: 3 # number of the highest confidence predicted path to check collision between obstacle and ego + pedestrian_deceleration_rate: 0.5 # planner perceives pedestrians will stop with this rate to avoid unnecessary stops [m/ss] + bicycle_deceleration_rate: 0.5 # planner perceives bicycles will stop with this rate to avoid unnecessary stops [m/ss] crossing_obstacle: collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] @@ -108,6 +125,8 @@ obstacle_velocity_threshold : 3.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] ego_obstacle_overlap_time_threshold : 2.0 # time threshold to decide cut-in obstacle for cruise or stop [s] max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego + max_lateral_time_margin: 5.0 # time threshold of lateral margin between obstacle and trajectory band with ego's width [s] + num_of_predicted_paths: 3 # number of the highest confidence predicted path to check collision between obstacle and ego yield: enable_yield: true lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp index f2a475c494aba..0f35960c350a8 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp @@ -58,10 +58,13 @@ struct Obstacle Obstacle( const rclcpp::Time & arg_stamp, const PredictedObject & object, const geometry_msgs::msg::Pose & arg_pose, const double ego_to_obstacle_distance, - const double lat_dist_from_obstacle_to_traj) + const double lat_dist_from_obstacle_to_traj, const double longitudinal_velocity, + const double approach_velocity) : stamp(arg_stamp), ego_to_obstacle_distance(ego_to_obstacle_distance), lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj), + longitudinal_velocity(longitudinal_velocity), + approach_velocity(approach_velocity), pose(arg_pose), orientation_reliable(true), twist(object.kinematics.initial_twist_with_covariance.twist), @@ -94,6 +97,8 @@ struct Obstacle rclcpp::Time stamp; // This is not the current stamp, but when the object was observed. double ego_to_obstacle_distance; double lat_dist_from_obstacle_to_traj; + double longitudinal_velocity; + double approach_velocity; // for PredictedObject geometry_msgs::msg::Pose pose; // interpolated with the current stamp diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index 35cbf31239272..629dba73e85dc 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -37,6 +37,7 @@ #include #include #include +#include #include namespace autoware::motion_planning @@ -79,24 +80,29 @@ class ObstacleCruisePlannerNode : public rclcpp::Node const Odometry & odometry, const std::vector & traj_points, const std::vector & traj_polys, const Obstacle & obstacle, const double precise_lateral_dist) const; + std::optional> + createCollisionPointForOutsideStopObstacle( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & traj_polys, const Obstacle & obstacle, + const PredictedPath & resampled_predicted_path, double max_lat_margin_for_stop) const; std::optional createStopObstacleForPointCloud( const std::vector & traj_points, const Obstacle & obstacle, const double precise_lateral_dist) const; + bool isInsideStopObstacle(const uint8_t label) const; + bool isOutsideStopObstacle(const uint8_t label) const; bool isStopObstacle(const uint8_t label) const; bool isInsideCruiseObstacle(const uint8_t label) const; bool isOutsideCruiseObstacle(const uint8_t label) const; bool isCruiseObstacle(const uint8_t label) const; bool isSlowDownObstacle(const uint8_t label) const; - std::optional createCollisionPointForStopObstacle( - const std::vector & traj_points, const std::vector & traj_polys, - const Obstacle & obstacle) const; std::optional createYieldCruiseObstacle( const Obstacle & obstacle, const std::vector & traj_points); std::optional> findYieldCruiseObstacles( const std::vector & obstacles, const std::vector & traj_points); std::optional createCruiseObstacle( - const std::vector & traj_points, const std::vector & traj_polys, - const Obstacle & obstacle, const double precise_lat_dist); + const Odometry & odometry, const std::vector & traj_points, + const std::vector & traj_polys, const Obstacle & obstacle, + const double precise_lat_dist); std::optional> createCollisionPointsForInsideCruiseObstacle( const std::vector & traj_points, const std::vector & traj_polys, const Obstacle & obstacle) const; @@ -129,7 +135,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool isFrontCollideObstacle( const std::vector & traj_points, const Obstacle & obstacle, const size_t first_collision_idx) const; - + double calcTimeToReachCollisionPoint( + const Odometry & odometry, const geometry_msgs::msg::Point & collision_point, + const std::vector & traj_points, const double abs_ego_offset) const; bool enable_debug_info_; bool enable_calculation_time_info_; bool use_pointcloud_for_stop_; @@ -140,7 +148,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double min_safe_distance_margin_on_curve_; bool suppress_sudden_obstacle_stop_; - std::vector stop_obstacle_types_; + std::vector inside_stop_obstacle_types_; + std::vector outside_stop_obstacle_types_; std::vector inside_cruise_obstacle_types_; std::vector outside_cruise_obstacle_types_; std::vector slow_down_obstacle_types_; @@ -226,11 +235,20 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double ego_obstacle_overlap_time_threshold; double max_prediction_time_for_collision_check; double crossing_obstacle_traj_angle_threshold; + int num_of_predicted_paths_for_outside_cruise_obstacle; + int num_of_predicted_paths_for_outside_stop_obstacle; + double pedestrian_deceleration_rate; + double bicycle_deceleration_rate; // obstacle hold double stop_obstacle_hold_time_threshold; + // reach collision point + double min_velocity_to_reach_collision_point; // prediction resampling double prediction_resampling_time_interval; double prediction_resampling_time_horizon; + // max lateral time margin + double max_lat_time_margin_for_stop; + double max_lat_time_margin_for_cruise; // max lateral margin double max_lat_margin_for_stop; double max_lat_margin_for_stop_against_unknown; diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp index a2ac86ee6c86b..ca17b428e369a 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp @@ -15,7 +15,7 @@ #define AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ #include "autoware/obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp" -#include "osqp_interface/osqp_interface.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" #include @@ -69,7 +69,7 @@ class VelocityOptimizer double over_j_weight_; // QPSolver - autoware::common::osqp::OSQPInterface qp_solver_; + autoware::osqp_interface::OSQPInterface qp_solver_; }; #endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp index fa6ee17d117be..f84aa26480765 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp @@ -43,6 +43,11 @@ std::optional> getCollisionPoint( const Obstacle & obstacle, const bool is_driving_forward, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); +std::optional> getCollisionPoint( + const std::vector & traj_points, const size_t collision_idx, + const std::vector & collision_points, const bool is_driving_forward, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); + std::vector getCollisionPoints( const std::vector & traj_points, const std::vector & traj_polygons, const rclcpp::Time & obstacle_stamp, const PredictedPath & predicted_path, const Shape & shape, diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index 9999be9937111..d1c3b8b00e069 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -8,6 +8,7 @@ Kosuke Takeuchi Satoshi Ota Yuki Takagi + Berkay Karaman Apache License 2.0 @@ -21,6 +22,8 @@ autoware_interpolation autoware_lanelet2_extension autoware_motion_utils + autoware_object_recognition_utils + autoware_osqp_interface autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager @@ -29,8 +32,6 @@ autoware_vehicle_info_utils geometry_msgs nav_msgs - object_recognition_utils - osqp_interface rclcpp rclcpp_components std_msgs diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index cbb3f98385774..37b82ecb66e74 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -16,18 +16,16 @@ #include "autoware/motion_utils/resample/resample.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/obstacle_cruise_planner/polygon_utils.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" #include "autoware/universe_utils/ros/update_param.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" #include #include -#include - #include #include #include @@ -72,18 +70,39 @@ std::optional calcDistanceToFrontVehicle( return ego_to_obstacle_distance; } -PredictedPath resampleHighestConfidencePredictedPath( +std::vector resampleHighestConfidencePredictedPaths( const std::vector & predicted_paths, const double time_interval, - const double time_horizon) + const double time_horizon, const size_t num_paths) { - // get highest confidence path - const auto reliable_path = std::max_element( - predicted_paths.begin(), predicted_paths.end(), - [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); - - // resample - return object_recognition_utils::resamplePredictedPath( - *reliable_path, time_interval, time_horizon); + std::vector sorted_paths = predicted_paths; + + // Sort paths by descending confidence + std::sort( + sorted_paths.begin(), sorted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence > b.confidence; }); + + std::vector selected_paths; + size_t path_count = 0; + + // Select paths that meet the confidence thresholds + for (const auto & path : sorted_paths) { + if (path_count < num_paths) { + selected_paths.push_back(path); + ++path_count; + } + } + + // Resample each selected path + std::vector resampled_paths; + for (const auto & path : selected_paths) { + if (path.path.size() < 2) { + continue; + } + resampled_paths.push_back( + autoware::object_recognition_utils::resamplePredictedPath(path, time_interval, time_horizon)); + } + + return resampled_paths; } double calcDiffAngleAgainstTrajectory( @@ -99,20 +118,48 @@ double calcDiffAngleAgainstTrajectory( return diff_yaw; } -std::pair projectObstacleVelocityToTrajectory( - const std::vector & traj_points, const Obstacle & obstacle) +/** + * @brief Calculates the obstacle's longitudinal and approach velocities relative to the trajectory. + * + * This function calculates the obstacle's velocity components relative to the trajectory. + * It returns the longitudinal and approach components of the obstacle's velocity + * with respect to the trajectory. Negative approach velocity indicates that the + * obstacle is getting far away from the trajectory. + * + * @param traj_points The trajectory points. + * @param obstacle_pose The current pose of the obstacle. + * @param obstacle_twist The twist (velocity) of the obstacle. + * @return A pair containing the longitudinal and approach velocity components. + */ +std::pair calculateObstacleVelocitiesRelativeToTrajectory( + const std::vector & traj_points, const geometry_msgs::msg::Pose & obstacle_pose, + const geometry_msgs::msg::Twist & obstacle_twist) { const size_t object_idx = - autoware::motion_utils::findNearestIndex(traj_points, obstacle.pose.position); - const double traj_yaw = tf2::getYaw(traj_points.at(object_idx).pose.orientation); + autoware::motion_utils::findNearestIndex(traj_points, obstacle_pose.position); + + const auto & nearest_point = traj_points.at(object_idx); + + const double traj_yaw = tf2::getYaw(nearest_point.pose.orientation); + const double obstacle_yaw = tf2::getYaw(obstacle_pose.orientation); + const Eigen::Rotation2Dd R_ego_to_obstacle( + autoware::universe_utils::normalizeRadian(obstacle_yaw - traj_yaw)); + + // Calculate the trajectory direction and the vector from the trajectory to the obstacle + const Eigen::Vector2d traj_direction(std::cos(traj_yaw), std::sin(traj_yaw)); + const Eigen::Vector2d traj_to_obstacle( + obstacle_pose.position.x - nearest_point.pose.position.x, + obstacle_pose.position.y - nearest_point.pose.position.y); - const double obstacle_yaw = tf2::getYaw(obstacle.pose.orientation); + // Determine if the obstacle is to the left or right of the trajectory using the cross product + const double cross_product = + traj_direction.x() * traj_to_obstacle.y() - traj_direction.y() * traj_to_obstacle.x(); + const int sign = (cross_product > 0) ? -1 : 1; - const Eigen::Rotation2Dd R_ego_to_obstacle(obstacle_yaw - traj_yaw); - const Eigen::Vector2d obstacle_velocity(obstacle.twist.linear.x, obstacle.twist.linear.y); + const Eigen::Vector2d obstacle_velocity(obstacle_twist.linear.x, obstacle_twist.linear.y); const Eigen::Vector2d projected_velocity = R_ego_to_obstacle * obstacle_velocity; - return std::make_pair(projected_velocity[0], projected_velocity[1]); + return std::make_pair(projected_velocity[0], sign * projected_velocity[1]); } double calcObstacleMaxLength(const Shape & shape) @@ -311,6 +358,20 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara "behavior_determination.consider_current_pose.enable_to_consider_current_pose"); time_to_convergence = node.declare_parameter( "behavior_determination.consider_current_pose.time_to_convergence"); + min_velocity_to_reach_collision_point = node.declare_parameter( + "behavior_determination.stop.min_velocity_to_reach_collision_point"); + max_lat_time_margin_for_stop = node.declare_parameter( + "behavior_determination.stop.outside_obstacle.max_lateral_time_margin"); + max_lat_time_margin_for_cruise = node.declare_parameter( + "behavior_determination.cruise.outside_obstacle.max_lateral_time_margin"); + num_of_predicted_paths_for_outside_cruise_obstacle = node.declare_parameter( + "behavior_determination.cruise.outside_obstacle.num_of_predicted_paths"); + num_of_predicted_paths_for_outside_stop_obstacle = node.declare_parameter( + "behavior_determination.stop.outside_obstacle.num_of_predicted_paths"); + pedestrian_deceleration_rate = node.declare_parameter( + "behavior_determination.stop.outside_obstacle.pedestrian_deceleration_rate"); + bicycle_deceleration_rate = node.declare_parameter( + "behavior_determination.stop.outside_obstacle.bicycle_deceleration_rate"); } void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( @@ -403,6 +464,27 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( autoware::universe_utils::updateParam( parameters, "behavior_determination.consider_current_pose.time_to_convergence", time_to_convergence); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.stop.min_velocity_to_reach_collision_point", + min_velocity_to_reach_collision_point); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.stop.outside_obstacle.max_lateral_time_margin", + max_lat_time_margin_for_stop); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.cruise.outside_obstacle.max_lateral_time_margin", + max_lat_time_margin_for_cruise); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.cruise.outside_obstacle.num_of_predicted_paths", + num_of_predicted_paths_for_outside_cruise_obstacle); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.stop.outside_obstacle.num_of_predicted_paths", + num_of_predicted_paths_for_outside_stop_obstacle); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.stop.outside_obstacle.pedestrian_deceleration_rate", + pedestrian_deceleration_rate); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.stop.outside_obstacle.bicycle_deceleration_rate", + bicycle_deceleration_rate); } ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options) @@ -488,7 +570,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & } { // stop/cruise/slow down obstacle type - stop_obstacle_types_ = getTargetObjectType(*this, "common.stop_obstacle_type."); + inside_stop_obstacle_types_ = getTargetObjectType(*this, "common.stop_obstacle_type.inside."); + outside_stop_obstacle_types_ = getTargetObjectType(*this, "common.stop_obstacle_type.outside."); inside_cruise_obstacle_types_ = getTargetObjectType(*this, "common.cruise_obstacle_type.inside."); outside_cruise_obstacle_types_ = @@ -738,10 +821,21 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( const auto obj_stamp = rclcpp::Time(objects.header.stamp); const auto & p = behavior_determination_param_; + constexpr double epsilon = 1e-6; + const double max_lat_margin = std::max( + {p.max_lat_margin_for_stop, p.max_lat_margin_for_stop_against_unknown, + p.max_lat_margin_for_cruise, p.max_lat_margin_for_slow_down}); + const double max_lat_time_margin = + std::max({p.max_lat_time_margin_for_stop, p.max_lat_time_margin_for_cruise}); + const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); + std::vector target_obstacles; for (const auto & predicted_object : objects.objects) { const auto & object_id = autoware::universe_utils::toHexString(predicted_object.object_id).substr(0, 4); + + // brkay54: When use_prediction is true, we observed wrong orientation for the object in + // scenario simulator. const auto & current_obstacle_pose = obstacle_cruise_utils::getCurrentObjectPose(predicted_object, obj_stamp, now(), true); @@ -756,8 +850,11 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( continue; } + const auto projected_vel = calculateObstacleVelocitiesRelativeToTrajectory( + traj_points, current_obstacle_pose.pose, + predicted_object.kinematics.initial_twist_with_covariance.twist); + // 2. Check if the obstacle is in front of the ego. - const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); const auto ego_to_obstacle_distance = calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position); if (!ego_to_obstacle_distance) { @@ -767,7 +864,8 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( continue; } - // 3. Check if rough lateral distance is smaller than the threshold + // 3. Check if rough lateral distance and time to reach trajectory are smaller than the + // threshold const double lat_dist_from_obstacle_to_traj = autoware::motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position); @@ -777,19 +875,27 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( obstacle_max_length; }(); - const double max_lat_margin = std::max( - {p.max_lat_margin_for_stop, p.max_lat_margin_for_stop_against_unknown, - p.max_lat_margin_for_cruise, p.max_lat_margin_for_slow_down}); if (max_lat_margin < min_lat_dist_to_traj_poly) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "Ignore obstacle (%s) since it is too far from the trajectory.", object_id.c_str()); - continue; + if (projected_vel.second > 0.0) { + const auto time_to_traj = + min_lat_dist_to_traj_poly / std::max(epsilon, projected_vel.second); + if (time_to_traj > max_lat_time_margin) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "Ignore obstacle (%s) since it is too far from the trajectory.", object_id.c_str()); + continue; + } + } else { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "Ignore obstacle (%s) since it is too far from the trajectory.", object_id.c_str()); + continue; + } } const auto target_obstacle = Obstacle( - obj_stamp, predicted_object, current_obstacle_pose.pose, ego_to_obstacle_distance.value(), - lat_dist_from_obstacle_to_traj); + obj_stamp, predicted_object, current_obstacle_pose.pose, *ego_to_obstacle_distance, + lat_dist_from_obstacle_to_traj, projected_vel.first, projected_vel.second); target_obstacles.push_back(target_obstacle); } @@ -919,12 +1025,23 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( return target_obstacles; } -bool ObstacleCruisePlannerNode::isStopObstacle(const uint8_t label) const +bool ObstacleCruisePlannerNode::isInsideStopObstacle(const uint8_t label) const +{ + const auto & types = inside_stop_obstacle_types_; + return std::find(types.begin(), types.end(), label) != types.end(); +} + +bool ObstacleCruisePlannerNode::isOutsideStopObstacle(const uint8_t label) const { - const auto & types = stop_obstacle_types_; + const auto & types = outside_stop_obstacle_types_; return std::find(types.begin(), types.end(), label) != types.end(); } +bool ObstacleCruisePlannerNode::isStopObstacle(const uint8_t label) const +{ + return isInsideStopObstacle(label) || isOutsideStopObstacle(label); +} + bool ObstacleCruisePlannerNode::isInsideCruiseObstacle(const uint8_t label) const { const auto & types = inside_cruise_obstacle_types_; @@ -992,8 +1109,8 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstPredictedObjectObstacles( } // Filter obstacles for cruise, stop and slow down - const auto cruise_obstacle = - createCruiseObstacle(decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); + const auto cruise_obstacle = createCruiseObstacle( + odometry, decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); if (cruise_obstacle) { cruise_obstacles.push_back(*cruise_obstacle); continue; @@ -1141,8 +1258,9 @@ std::vector ObstacleCruisePlannerNode::decimateTrajectoryPoints } std::optional ObstacleCruisePlannerNode::createCruiseObstacle( - const std::vector & traj_points, const std::vector & traj_polys, - const Obstacle & obstacle, const double precise_lat_dist) + const Odometry & odometry, const std::vector & traj_points, + const std::vector & traj_polys, const Obstacle & obstacle, + const double precise_lat_dist) { const auto & object_id = obstacle.uuid.substr(0, 4); const auto & p = behavior_determination_param_; @@ -1152,12 +1270,24 @@ std::optional ObstacleCruisePlannerNode::createCruiseObstacle( if (!isCruiseObstacle(obstacle.classification.label) || !is_driving_forward_) { return std::nullopt; } - if (p.max_lat_margin_for_cruise < precise_lat_dist) { + + if (obstacle.longitudinal_velocity < 0.0) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, - "[Cruise] Ignore obstacle (%s) since it's far from trajectory.", object_id.c_str()); + "[Cruise] Ignore obstacle (%s) since it's driving in opposite direction.", object_id.c_str()); return std::nullopt; } + + if (p.max_lat_margin_for_cruise < precise_lat_dist) { + const auto time_to_traj = precise_lat_dist / std::max(1e-6, obstacle.approach_velocity); + if (time_to_traj > p.max_lat_time_margin_for_cruise) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Cruise] Ignore obstacle (%s) since it's far from trajectory.", object_id.c_str()); + return std::nullopt; + } + } + if (isObstacleCrossing(traj_points, obstacle)) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, @@ -1173,15 +1303,23 @@ std::optional ObstacleCruisePlannerNode::createCruiseObstacle( return createCollisionPointsForInsideCruiseObstacle(traj_points, traj_polys, obstacle); } // obstacle is outside the trajectory + // If the ego is stopping, do not plan cruise for outside obstacles. Stop will be planned. + if (odometry.twist.twist.linear.x < 0.1) { + return std::nullopt; + } return createCollisionPointsForOutsideCruiseObstacle(traj_points, traj_polys, obstacle); }(); if (!collision_points) { return std::nullopt; } - const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); - return CruiseObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose, - tangent_vel, normal_vel, *collision_points}; + return CruiseObstacle{ + obstacle.uuid, + obstacle.stamp, + obstacle.pose, + obstacle.longitudinal_velocity, + obstacle.approach_velocity, + *collision_points}; } std::optional ObstacleCruisePlannerNode::createYieldCruiseObstacle( @@ -1232,11 +1370,15 @@ std::optional ObstacleCruisePlannerNode::createYieldCruiseObstac }(); if (!collision_points) return std::nullopt; - const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); // check if obstacle is driving on the opposite direction - if (tangent_vel < 0.0) return std::nullopt; - return CruiseObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose, - tangent_vel, normal_vel, collision_points.value()}; + if (obstacle.longitudinal_velocity < 0.0) return std::nullopt; + return CruiseObstacle{ + obstacle.uuid, + obstacle.stamp, + obstacle.pose, + obstacle.longitudinal_velocity, + obstacle.approach_velocity, + collision_points.value()}; } std::optional> ObstacleCruisePlannerNode::findYieldCruiseObstacles( @@ -1332,22 +1474,20 @@ ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle( } { // consider hysteresis - const auto [obstacle_tangent_vel, obstacle_normal_vel] = - projectObstacleVelocityToTrajectory(traj_points, obstacle); // const bool is_prev_obstacle_stop = getObstacleFromUuid(prev_stop_obstacles_, // obstacle.uuid).has_value(); const bool is_prev_obstacle_cruise = getObstacleFromUuid(prev_cruise_object_obstacles_, obstacle.uuid).has_value(); if (is_prev_obstacle_cruise) { - if (obstacle_tangent_vel < p.obstacle_velocity_threshold_from_cruise_to_stop) { + if (obstacle.longitudinal_velocity < p.obstacle_velocity_threshold_from_cruise_to_stop) { return std::nullopt; } // NOTE: else is keeping cruise } else { // if (is_prev_obstacle_stop) { // TODO(murooka) consider hysteresis for slow down // If previous obstacle is stop or does not exist. - if (obstacle_tangent_vel < p.obstacle_velocity_threshold_from_stop_to_cruise) { + if (obstacle.longitudinal_velocity < p.obstacle_velocity_threshold_from_stop_to_cruise) { return std::nullopt; } // NOTE: else is cruise from stop @@ -1355,15 +1495,19 @@ ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle( } // Get highest confidence predicted path - const auto resampled_predicted_path = resampleHighestConfidencePredictedPath( + const auto resampled_predicted_paths = resampleHighestConfidencePredictedPaths( obstacle.predicted_paths, p.prediction_resampling_time_interval, - p.prediction_resampling_time_horizon); + p.prediction_resampling_time_horizon, 1); + + if (resampled_predicted_paths.empty()) { + return std::nullopt; + } // calculate nearest collision point std::vector collision_index; const auto collision_points = polygon_utils::getCollisionPoints( - traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), - is_driving_forward_, collision_index, + traj_points, traj_polys, obstacle.stamp, resampled_predicted_paths.front(), obstacle.shape, + now(), is_driving_forward_, collision_index, calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + std::hypot( vehicle_info_.vehicle_length_m, @@ -1397,21 +1541,32 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle( return std::nullopt; } - // Get highest confidence predicted path - const auto resampled_predicted_path = resampleHighestConfidencePredictedPath( + // Get the highest confidence predicted paths + const auto resampled_predicted_paths = resampleHighestConfidencePredictedPaths( obstacle.predicted_paths, p.prediction_resampling_time_interval, - p.prediction_resampling_time_horizon); + p.prediction_resampling_time_horizon, p.num_of_predicted_paths_for_outside_cruise_obstacle); // calculate collision condition for cruise std::vector collision_index; - const auto collision_points = polygon_utils::getCollisionPoints( - traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), - is_driving_forward_, collision_index, - calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + - std::hypot( - vehicle_info_.vehicle_length_m, - vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_cruise), - p.max_prediction_time_for_collision_check); + const auto getCollisionPoints = [&]() -> std::vector { + for (const auto & predicted_path : resampled_predicted_paths) { + const auto collision_points = polygon_utils::getCollisionPoints( + traj_points, traj_polys, obstacle.stamp, predicted_path, obstacle.shape, now(), + is_driving_forward_, collision_index, + calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + + std::hypot( + vehicle_info_.vehicle_length_m, + vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_cruise), + p.max_prediction_time_for_collision_check); + if (!collision_points.empty()) { + return collision_points; + } + } + return {}; + }; + + const auto collision_points = getCollisionPoints(); + if (collision_points.empty()) { // Ignore vehicle obstacles outside the trajectory without collision RCLCPP_INFO_EXPRESSION( @@ -1446,6 +1601,49 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle( return collision_points; } +std::optional> +ObstacleCruisePlannerNode::createCollisionPointForOutsideStopObstacle( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & traj_polys, const Obstacle & obstacle, + const PredictedPath & resampled_predicted_path, double max_lat_margin_for_stop) const +{ + const auto & object_id = obstacle.uuid.substr(0, 4); + const auto & p = behavior_determination_param_; + + std::vector collision_index; + const auto collision_points = polygon_utils::getCollisionPoints( + traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), + is_driving_forward_, collision_index, + calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + + std::hypot( + vehicle_info_.vehicle_length_m, + vehicle_info_.vehicle_width_m * 0.5 + max_lat_margin_for_stop)); + if (collision_points.empty()) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Stop] Ignore outside obstacle (%s) since there is no collision point between the " + "predicted path " + "and the ego.", + object_id.c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + return std::nullopt; + } + + const double collision_time_margin = + calcCollisionTimeMargin(odometry, collision_points, traj_points, is_driving_forward_); + if (p.collision_time_margin < collision_time_margin) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Stop] Ignore outside obstacle (%s) since it will not collide with the ego.", + object_id.c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + return std::nullopt; + } + + return polygon_utils::getCollisionPoint( + traj_points, collision_index.front(), collision_points, is_driving_forward_, vehicle_info_); +} + std::optional ObstacleCruisePlannerNode::createStopObstacleForPredictedObject( const Odometry & odometry, const std::vector & traj_points, const std::vector & traj_polys, const Obstacle & obstacle, @@ -1454,71 +1652,81 @@ std::optional ObstacleCruisePlannerNode::createStopObstacleForPred const auto & p = behavior_determination_param_; const auto & object_id = obstacle.uuid.substr(0, 4); - // NOTE: consider all target obstacles when driving backward if (!isStopObstacle(obstacle.classification.label)) { return std::nullopt; } - const double max_lat_margin_for_stop = (obstacle.classification.label == ObjectClassification::UNKNOWN) ? p.max_lat_margin_for_stop_against_unknown : p.max_lat_margin_for_stop; + // Obstacle that is not inside of trajectory if (precise_lat_dist > std::max(max_lat_margin_for_stop, 1e-3)) { - return std::nullopt; - } - - // check obstacle velocity - // NOTE: If precise_lat_dist is 0, always plan stop - constexpr double epsilon = 1e-6; - if (epsilon < precise_lat_dist) { - const auto [obstacle_tangent_vel, obstacle_normal_vel] = - projectObstacleVelocityToTrajectory(traj_points, obstacle); - if (p.obstacle_velocity_threshold_from_stop_to_cruise <= obstacle_tangent_vel) { + if (!isOutsideStopObstacle(obstacle.classification.label)) { return std::nullopt; } - } - // NOTE: Dynamic obstacles for stop are crossing ego's trajectory with high speed, - // and the collision between ego and obstacles are within the margin threshold. - const bool is_obstacle_crossing = isObstacleCrossing(traj_points, obstacle); - const bool has_high_speed = p.crossing_obstacle_velocity_threshold < - std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); - if (is_obstacle_crossing && has_high_speed) { - // Get highest confidence predicted path - const auto resampled_predicted_path = resampleHighestConfidencePredictedPath( - obstacle.predicted_paths, p.prediction_resampling_time_interval, - p.prediction_resampling_time_horizon); - - std::vector collision_index; - const auto collision_points = polygon_utils::getCollisionPoints( - traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), - is_driving_forward_, collision_index, - calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + - std::hypot( - vehicle_info_.vehicle_length_m, - vehicle_info_.vehicle_width_m * 0.5 + max_lat_margin_for_stop)); - if (collision_points.empty()) { + const auto time_to_traj = precise_lat_dist / std::max(1e-6, obstacle.approach_velocity); + if (time_to_traj > p.max_lat_time_margin_for_stop) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, - "[Stop] Ignore inside obstacle (%s) since there is no collision point between the " - "predicted path " - "and the ego.", - object_id.c_str()); - debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + "[Stop] Ignore outside obstacle (%s) since it's far from trajectory.", object_id.c_str()); return std::nullopt; } - const double collision_time_margin = - calcCollisionTimeMargin(odometry, collision_points, traj_points, is_driving_forward_); - if (p.collision_time_margin < collision_time_margin) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "[Stop] Ignore inside obstacle (%s) since it will not collide with the ego.", - object_id.c_str()); - debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + // brkay54: For the pedestrians and bicycles, we need to check the collision point by thinking + // they will stop with a predefined deceleration rate to avoid unnecessary stops. + double resample_time_horizon = p.prediction_resampling_time_horizon; + if (obstacle.classification.label == ObjectClassification::PEDESTRIAN) { + resample_time_horizon = + std::sqrt(std::pow(obstacle.twist.linear.x, 2) + std::pow(obstacle.twist.linear.y, 2)) / + (2.0 * p.pedestrian_deceleration_rate); + } else if (obstacle.classification.label == ObjectClassification::BICYCLE) { + resample_time_horizon = + std::sqrt(std::pow(obstacle.twist.linear.x, 2) + std::pow(obstacle.twist.linear.y, 2)) / + (2.0 * p.bicycle_deceleration_rate); + } + + // Get the highest confidence predicted path + const auto resampled_predicted_paths = resampleHighestConfidencePredictedPaths( + obstacle.predicted_paths, p.prediction_resampling_time_interval, resample_time_horizon, + p.num_of_predicted_paths_for_outside_stop_obstacle); + if (resampled_predicted_paths.empty()) { return std::nullopt; } + + const auto getCollisionPoint = + [&]() -> std::optional> { + for (const auto & predicted_path : resampled_predicted_paths) { + const auto collision_point = createCollisionPointForOutsideStopObstacle( + odometry, traj_points, traj_polys, obstacle, predicted_path, max_lat_margin_for_stop); + if (collision_point) { + return collision_point; + } + } + return std::nullopt; + }; + + const auto collision_point = getCollisionPoint(); + + if (collision_point) { + return StopObstacle{ + obstacle.uuid, + obstacle.stamp, + obstacle.classification, + obstacle.pose, + obstacle.shape, + obstacle.longitudinal_velocity, + obstacle.approach_velocity, + collision_point->first, + collision_point->second}; + } + return std::nullopt; + } + + // Obstacle inside the trajectory + if (!isInsideStopObstacle(obstacle.classification.label)) { + return std::nullopt; } // calculate collision points with trajectory with lateral stop margin @@ -1531,10 +1739,55 @@ std::optional ObstacleCruisePlannerNode::createStopObstacleForPred return std::nullopt; } - const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); - return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification, - obstacle.pose, obstacle.shape, tangent_vel, - normal_vel, collision_point->first, collision_point->second}; + // check transient obstacle or not + const double abs_ego_offset = + min_behavior_stop_margin_ + (is_driving_forward_ + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m)); + + const double time_to_reach_stop_point = + calcTimeToReachCollisionPoint(odometry, collision_point->first, traj_points, abs_ego_offset); + const bool is_transient_obstacle = [&]() { + if (time_to_reach_stop_point <= p.collision_time_margin) { + return false; + } + // get the predicted position of the obstacle when ego reaches the collision point + const auto resampled_predicted_paths = resampleHighestConfidencePredictedPaths( + obstacle.predicted_paths, p.prediction_resampling_time_interval, + p.prediction_resampling_time_horizon, 1); + if (resampled_predicted_paths.empty() || resampled_predicted_paths.front().path.empty()) { + return false; + } + const auto future_obj_pose = autoware::object_recognition_utils::calcInterpolatedPose( + resampled_predicted_paths.front(), time_to_reach_stop_point - p.collision_time_margin); + + Obstacle tmp_future_obs = obstacle; + tmp_future_obs.pose = + future_obj_pose ? future_obj_pose.value() : resampled_predicted_paths.front().path.back(); + const auto future_collision_point = polygon_utils::getCollisionPoint( + traj_points, traj_polys_with_lat_margin, tmp_future_obs, is_driving_forward_, vehicle_info_); + + return !future_collision_point; + }(); + + if (is_transient_obstacle) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Stop] Ignore inside obstacle (%s) since it is transient obstacle.", object_id.c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + return std::nullopt; + } + + return StopObstacle{ + obstacle.uuid, + obstacle.stamp, + obstacle.classification, + obstacle.pose, + obstacle.shape, + obstacle.longitudinal_velocity, + obstacle.approach_velocity, + collision_point->first, + collision_point->second}; } std::optional ObstacleCruisePlannerNode::createStopObstacleForPointCloud( @@ -1693,10 +1946,16 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl } } - const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); - return SlowDownObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification, - obstacle.pose, tangent_vel, normal_vel, - precise_lat_dist, front_collision_point, back_collision_point}; + return SlowDownObstacle{ + obstacle.uuid, + obstacle.stamp, + obstacle.classification, + obstacle.pose, + obstacle.longitudinal_velocity, + obstacle.approach_velocity, + precise_lat_dist, + front_collision_point, + back_collision_point}; } std::optional ObstacleCruisePlannerNode::createSlowDownObstacleForPointCloud( @@ -1780,29 +2039,28 @@ double ObstacleCruisePlannerNode::calcCollisionTimeMargin( const Odometry & odometry, const std::vector & collision_points, const std::vector & traj_points, const bool is_driving_forward) const { - const auto & ego_pose = odometry.pose.pose; - const double ego_vel = odometry.twist.twist.linear.x; - - const double time_to_reach_collision_point = [&]() { - const double abs_ego_offset = is_driving_forward - ? std::abs(vehicle_info_.max_longitudinal_offset_m) - : std::abs(vehicle_info_.min_longitudinal_offset_m); - const double dist_from_ego_to_obstacle = - std::abs(autoware::motion_utils::calcSignedArcLength( - traj_points, ego_pose.position, collision_points.front().point)) - - abs_ego_offset; - return dist_from_ego_to_obstacle / std::max(1e-6, std::abs(ego_vel)); - }(); + const auto & p = behavior_determination_param_; + const double abs_ego_offset = + min_behavior_stop_margin_ + (is_driving_forward + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m)); + const double time_to_reach_stop_point = calcTimeToReachCollisionPoint( + odometry, collision_points.front().point, traj_points, abs_ego_offset); + + const double time_to_leave_collision_point = + time_to_reach_stop_point + + abs_ego_offset / + std::max(p.min_velocity_to_reach_collision_point, odometry.twist.twist.linear.x); const double time_to_start_cross = (rclcpp::Time(collision_points.front().stamp) - now()).seconds(); const double time_to_end_cross = (rclcpp::Time(collision_points.back().stamp) - now()).seconds(); - if (time_to_reach_collision_point < time_to_start_cross) { // Ego goes first. - return time_to_start_cross - time_to_reach_collision_point; + if (time_to_leave_collision_point < time_to_start_cross) { // Ego goes first. + return time_to_start_cross - time_to_reach_stop_point; } - if (time_to_end_cross < time_to_reach_collision_point) { // Obstacle goes first. - return time_to_reach_collision_point - time_to_end_cross; + if (time_to_end_cross < time_to_reach_stop_point) { // Obstacle goes first. + return time_to_reach_stop_point - time_to_end_cross; } return 0.0; // Ego and obstacle will collide. } @@ -1982,6 +2240,19 @@ void ObstacleCruisePlannerNode::publishCalculationTime(const double calculation_ calculation_time_msg.data = calculation_time; debug_calculation_time_pub_->publish(calculation_time_msg); } + +double ObstacleCruisePlannerNode::calcTimeToReachCollisionPoint( + const Odometry & odometry, const geometry_msgs::msg::Point & collision_point, + const std::vector & traj_points, const double abs_ego_offset) const +{ + const auto & p = behavior_determination_param_; + const double dist_from_ego_to_obstacle = + std::abs(autoware::motion_utils::calcSignedArcLength( + traj_points, odometry.pose.pose.position, collision_point)) - + abs_ego_offset; + return dist_from_ego_to_obstacle / + std::max(p.min_velocity_to_reach_collision_point, std::abs(odometry.twist.twist.linear.x)); +} } // namespace autoware::motion_planning #include diff --git a/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp b/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp index 8c3abe58f5d59..639feec856a7e 100644 --- a/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp @@ -125,6 +125,37 @@ std::optional> getCollisionPoint( *max_collision_length); } +std::optional> getCollisionPoint( + const std::vector & traj_points, const size_t collision_idx, + const std::vector & collision_points, const bool is_driving_forward, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) +{ + std::pair> collision_info; + collision_info.first = collision_idx; + collision_info.second = collision_points; + + const double x_diff_to_bumper = is_driving_forward ? vehicle_info.max_longitudinal_offset_m + : vehicle_info.min_longitudinal_offset_m; + const auto bumper_pose = autoware::universe_utils::calcOffsetPose( + traj_points.at(collision_info.first).pose, x_diff_to_bumper, 0.0, 0.0); + + std::optional max_collision_length = std::nullopt; + std::optional max_collision_point = std::nullopt; + for (const auto & poly_vertex : collision_info.second) { + const double dist_from_bumper = + std::abs(autoware::universe_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x); + + if (!max_collision_length.has_value() || dist_from_bumper > *max_collision_length) { + max_collision_length = dist_from_bumper; + max_collision_point = poly_vertex.point; + } + } + return std::make_pair( + *max_collision_point, + autoware::motion_utils::calcSignedArcLength(traj_points, 0, collision_info.first) - + *max_collision_length); +} + // NOTE: max_lat_dist is used for efficient calculation to suppress boost::geometry's polygon // calculation. std::vector getCollisionPoints( diff --git a/planning/autoware_obstacle_cruise_planner/src/utils.cpp b/planning/autoware_obstacle_cruise_planner/src/utils.cpp index 8162d8036d9dc..bc52e800c8fc5 100644 --- a/planning/autoware_obstacle_cruise_planner/src/utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/utils.cpp @@ -14,8 +14,8 @@ #include "autoware/obstacle_cruise_planner/utils.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" namespace obstacle_cruise_utils { @@ -30,7 +30,8 @@ std::optional getCurrentObjectPoseFromPredictedPath( return std::nullopt; } - const auto pose = object_recognition_utils::calcInterpolatedPose(predicted_path, rel_time); + const auto pose = + autoware::object_recognition_utils::calcInterpolatedPose(predicted_path, rel_time); if (!pose) { return std::nullopt; } diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp index 3d9192c93d5b1..9a397e255da13 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp @@ -17,6 +17,7 @@ #include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/interpolation/spline_interpolation_points_2d.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/state_equation_generator.hpp" #include "autoware/path_optimizer/type_alias.hpp" @@ -24,7 +25,6 @@ #include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "gtest/gtest.h" -#include "osqp_interface/osqp_interface.hpp" #include #include @@ -228,7 +228,7 @@ class MPTOptimizer MPTParam mpt_param_; StateEquationGenerator state_equation_generator_; - std::unique_ptr osqp_solver_ptr_; + std::unique_ptr osqp_solver_ptr_; const double osqp_epsilon_ = 1.0e-3; diff --git a/planning/autoware_path_optimizer/package.xml b/planning/autoware_path_optimizer/package.xml index 1a7869b6a87fd..cdd7123d21ab8 100644 --- a/planning/autoware_path_optimizer/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -16,13 +16,13 @@ autoware_interpolation autoware_motion_utils + autoware_osqp_interface autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils autoware_vehicle_info_utils geometry_msgs nav_msgs - osqp_interface rclcpp rclcpp_components std_msgs diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index fdffc8a926c24..df07f3f98957a 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -414,7 +414,7 @@ MPTOptimizer::MPTOptimizer( StateEquationGenerator(vehicle_info_.wheel_base_m, mpt_param_.max_steer_rad, time_keeper_); // osqp solver - osqp_solver_ptr_ = std::make_unique(osqp_epsilon_); + osqp_solver_ptr_ = std::make_unique(osqp_epsilon_); // publisher debug_fixed_traj_pub_ = node->create_publisher("~/debug/mpt_fixed_traj", 1); @@ -477,8 +477,13 @@ std::vector MPTOptimizer::optimizeTrajectory(const PlannerData const auto get_prev_optimized_traj_points = [&]() { if (prev_optimized_traj_points_ptr_) { + RCLCPP_WARN(logger_, "return the previous optimized_trajectory as exceptional behavior."); return *prev_optimized_traj_points_ptr_; } + RCLCPP_WARN( + logger_, + "Try to return the previous optimized_trajectory as exceptional behavior, " + "but this failure also. Then return path_smoother output."); return traj_points; }; @@ -505,8 +510,7 @@ std::vector MPTOptimizer::optimizeTrajectory(const PlannerData // 6. optimize steer angles const auto optimized_variables = calcOptimizedSteerAngles(ref_points, obj_mat, const_mat); if (!optimized_variables) { - RCLCPP_INFO_EXPRESSION( - logger_, enable_debug_info_, "return std::nullopt since could not solve qp"); + RCLCPP_WARN(logger_, "return std::nullopt since could not solve qp"); return get_prev_optimized_traj_points(); } @@ -1326,8 +1330,8 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( // NOTE: The following takes 1 [ms] Eigen::MatrixXd A = Eigen::MatrixXd::Zero(A_rows, N_v); - Eigen::VectorXd lb = Eigen::VectorXd::Constant(A_rows, -autoware::common::osqp::INF); - Eigen::VectorXd ub = Eigen::VectorXd::Constant(A_rows, autoware::common::osqp::INF); + Eigen::VectorXd lb = Eigen::VectorXd::Constant(A_rows, -autoware::osqp_interface::INF); + Eigen::VectorXd ub = Eigen::VectorXd::Constant(A_rows, autoware::osqp_interface::INF); size_t A_rows_end = 0; // 1. State equation @@ -1498,9 +1502,9 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( // initialize or update solver according to warm start time_keeper_->start_track("initOsqp"); - const autoware::common::osqp::CSC_Matrix P_csc = - autoware::common::osqp::calCSCMatrixTrapezoidal(H); - const autoware::common::osqp::CSC_Matrix A_csc = autoware::common::osqp::calCSCMatrix(A); + const autoware::osqp_interface::CSC_Matrix P_csc = + autoware::osqp_interface::calCSCMatrixTrapezoidal(H); + const autoware::osqp_interface::CSC_Matrix A_csc = autoware::osqp_interface::calCSCMatrix(A); if ( prev_solution_status_ == 1 && mpt_param_.enable_warm_start && prev_mat_n_ == H.rows() && prev_mat_m_ == A.rows()) { @@ -1511,7 +1515,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); } else { RCLCPP_INFO_EXPRESSION(logger_, enable_debug_info_, "no warm start"); - osqp_solver_ptr_ = std::make_unique( + osqp_solver_ptr_ = std::make_unique( P_csc, A_csc, f, lower_bound, upper_bound, osqp_epsilon_); } prev_mat_n_ = H.rows(); diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band.hpp index 1d0441c66c481..2d6c1d1b2d002 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band.hpp @@ -15,9 +15,9 @@ #ifndef AUTOWARE__PATH_SMOOTHER__ELASTIC_BAND_HPP_ #define AUTOWARE__PATH_SMOOTHER__ELASTIC_BAND_HPP_ +#include "autoware/osqp_interface/osqp_interface.hpp" #include "autoware/path_smoother/common_structs.hpp" #include "autoware/path_smoother/type_alias.hpp" -#include "osqp_interface/osqp_interface.hpp" #include @@ -109,7 +109,7 @@ class EBPathSmoother rclcpp::Publisher::SharedPtr debug_eb_traj_pub_; rclcpp::Publisher::SharedPtr debug_eb_fixed_traj_pub_; - std::unique_ptr osqp_solver_ptr_; + std::unique_ptr osqp_solver_ptr_; std::shared_ptr> prev_eb_traj_points_ptr_{nullptr}; std::vector insertFixedPoint( diff --git a/planning/autoware_path_smoother/package.xml b/planning/autoware_path_smoother/package.xml index b9b79bb6ceaf6..582553c2a94b2 100644 --- a/planning/autoware_path_smoother/package.xml +++ b/planning/autoware_path_smoother/package.xml @@ -16,12 +16,12 @@ autoware_interpolation autoware_motion_utils + autoware_osqp_interface autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils geometry_msgs nav_msgs - osqp_interface rclcpp rclcpp_components std_msgs diff --git a/planning/autoware_path_smoother/src/elastic_band.cpp b/planning/autoware_path_smoother/src/elastic_band.cpp index 4222e0fe98438..0bec4e46c8b42 100644 --- a/planning/autoware_path_smoother/src/elastic_band.cpp +++ b/planning/autoware_path_smoother/src/elastic_band.cpp @@ -381,7 +381,7 @@ void EBPathSmoother::updateConstraint( osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); osqp_solver_ptr_->updateEpsRel(p.qp_param.eps_rel); } else { - osqp_solver_ptr_ = std::make_unique( + osqp_solver_ptr_ = std::make_unique( P, A, q, lower_bound, upper_bound, p.qp_param.eps_abs); osqp_solver_ptr_->updateEpsRel(p.qp_param.eps_rel); osqp_solver_ptr_->updateEpsAbs(p.qp_param.eps_abs); 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 487c643fe5673..244876ee2cb22 100644 --- a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp +++ b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -37,6 +38,7 @@ #endif #include #include +#include #include #include @@ -95,6 +97,7 @@ 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_; // polling subscribers universe_utils::InterProcessPollingSubscriber< @@ -130,6 +133,9 @@ class ScenarioSelectorNode : public rclcpp::Node static constexpr double lane_stopping_timeout_s = 5.0; static constexpr double empty_parking_trajectory_timeout_s = 3.0; + + // processing time + autoware::universe_utils::StopWatch stop_watch; }; } // namespace autoware::scenario_selector #endif // AUTOWARE__SCENARIO_SELECTOR__NODE_HPP_ diff --git a/planning/autoware_scenario_selector/src/node.cpp b/planning/autoware_scenario_selector/src/node.cpp index 1f4547d04c8fc..eb3303766205f 100644 --- a/planning/autoware_scenario_selector/src/node.cpp +++ b/planning/autoware_scenario_selector/src/node.cpp @@ -330,6 +330,9 @@ bool ScenarioSelectorNode::isDataReady() void ScenarioSelectorNode::updateData() { + { + stop_watch.tic(); + } { auto msg = sub_parking_state_->takeData(); is_parking_completed_ = msg ? msg->data : is_parking_completed_; @@ -370,6 +373,12 @@ void ScenarioSelectorNode::onTimer() } pub_scenario_->publish(scenario); + + // Publish ProcessingTime + tier4_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); } void ScenarioSelectorNode::onLaneDrivingTrajectory( @@ -466,6 +475,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); } } // namespace autoware::scenario_selector diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 2abae4af2ca67..4f1399533ed46 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -23,6 +23,7 @@ autoware_map_projection_loader autoware_mission_planner autoware_motion_utils + autoware_osqp_interface autoware_path_optimizer autoware_path_smoother autoware_perception_msgs @@ -33,7 +34,6 @@ geometry_msgs global_parameter_loader map_loader - osqp_interface rclcpp rclcpp_components tier4_map_msgs diff --git a/planning/autoware_surround_obstacle_checker/CMakeLists.txt b/planning/autoware_surround_obstacle_checker/CMakeLists.txt index 1b13843de511c..bd6af24577eac 100644 --- a/planning/autoware_surround_obstacle_checker/CMakeLists.txt +++ b/planning/autoware_surround_obstacle_checker/CMakeLists.txt @@ -8,6 +8,14 @@ find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED COMPONENTS common) +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wno-error=deprecated-declarations) +endif() + include_directories( SYSTEM ${EIGEN3_INCLUDE_DIR} @@ -32,6 +40,15 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE ${PROJECT_NAME}_node ) +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_surround_obstacle_checker.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) +endif() + ament_auto_package( INSTALL_TO_SHARE config diff --git a/planning/autoware_surround_obstacle_checker/package.xml b/planning/autoware_surround_obstacle_checker/package.xml index 8362962f3dadb..f1d0d3e9fb76d 100644 --- a/planning/autoware_surround_obstacle_checker/package.xml +++ b/planning/autoware_surround_obstacle_checker/package.xml @@ -18,6 +18,7 @@ autoware_motion_utils autoware_perception_msgs autoware_planning_msgs + autoware_planning_test_manager autoware_universe_utils autoware_vehicle_info_utils diagnostic_msgs @@ -36,8 +37,10 @@ tier4_planning_msgs visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common + autoware_test_utils ament_cmake diff --git a/planning/autoware_surround_obstacle_checker/src/node.cpp b/planning/autoware_surround_obstacle_checker/src/node.cpp index d841e152cd596..da0dd0ec09a26 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -14,8 +14,10 @@ #include "node.hpp" +#include #include #include +#include #include #include @@ -81,70 +83,6 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( no_start_reason_diag.values.push_back(no_start_reason_diag_kv); return no_start_reason_diag; } - -geometry_msgs::msg::Point32 createPoint32(const double x, const double y, const double z) -{ - geometry_msgs::msg::Point32 p; - p.x = x; - p.y = y; - p.z = z; - return p; -} - -Polygon2d createObjPolygon( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Polygon & footprint) -{ - geometry_msgs::msg::Polygon transformed_polygon{}; - geometry_msgs::msg::TransformStamped geometry_tf{}; - geometry_tf.transform = pose2transform(pose); - tf2::doTransform(footprint, transformed_polygon, geometry_tf); - - Polygon2d object_polygon; - for (const auto & p : transformed_polygon.points) { - object_polygon.outer().push_back(Point2d(p.x, p.y)); - } - - bg::correct(object_polygon); - - return object_polygon; -} - -Polygon2d createObjPolygon( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size) -{ - const double & length_m = size.x / 2.0; - const double & width_m = size.y / 2.0; - - geometry_msgs::msg::Polygon polygon{}; - - polygon.points.push_back(createPoint32(length_m, -width_m, 0.0)); - polygon.points.push_back(createPoint32(length_m, width_m, 0.0)); - polygon.points.push_back(createPoint32(-length_m, width_m, 0.0)); - polygon.points.push_back(createPoint32(-length_m, -width_m, 0.0)); - - return createObjPolygon(pose, polygon); -} - -Polygon2d createSelfPolygon( - const VehicleInfo & vehicle_info, const double front_margin, const double side_margin, - const double rear_margin) -{ - const double & front_m = vehicle_info.max_longitudinal_offset_m + front_margin; - const double & width_left_m = vehicle_info.max_lateral_offset_m + side_margin; - const double & width_right_m = vehicle_info.min_lateral_offset_m - side_margin; - const double & rear_m = vehicle_info.min_longitudinal_offset_m - rear_margin; - - Polygon2d ego_polygon; - - ego_polygon.outer().push_back(Point2d(front_m, width_left_m)); - ego_polygon.outer().push_back(Point2d(front_m, width_right_m)); - ego_polygon.outer().push_back(Point2d(rear_m, width_right_m)); - ego_polygon.outer().push_back(Point2d(rear_m, width_left_m)); - - bg::correct(ego_polygon); - - return ego_polygon; -} } // namespace SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptions & node_options) @@ -172,6 +110,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); using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( @@ -214,6 +154,9 @@ bool SurroundObstacleCheckerNode::getUseDynamicObject() const void SurroundObstacleCheckerNode::onTimer() { + autoware::universe_utils::StopWatch stop_watch; + stop_watch.tic(); + odometry_ptr_ = sub_odometry_.takeData(); pointcloud_ptr_ = sub_pointcloud_.takeData(); object_ptr_ = sub_dynamic_objects_.takeData(); @@ -256,7 +199,11 @@ void SurroundObstacleCheckerNode::onTimer() const auto is_obstacle_found = !nearest_obstacle ? false : nearest_obstacle.value().first < epsilon; - if (!isStopRequired(is_obstacle_found, is_vehicle_stopped)) { + bool is_stop_required = false; + std::tie(is_stop_required, last_obstacle_found_time_) = isStopRequired( + is_obstacle_found, is_vehicle_stopped, state_, last_obstacle_found_time_, + param.state_clear_time); + if (!is_stop_required) { break; } @@ -281,7 +228,11 @@ void SurroundObstacleCheckerNode::onTimer() : nearest_obstacle.value().first < param.surround_check_hysteresis_distance; - if (isStopRequired(is_obstacle_found, is_vehicle_stopped)) { + bool is_stop_required = false; + std::tie(is_stop_required, last_obstacle_found_time_) = isStopRequired( + is_obstacle_found, is_vehicle_stopped, state_, last_obstacle_found_time_, + param.state_clear_time); + if (is_stop_required) { break; } @@ -311,6 +262,11 @@ void SurroundObstacleCheckerNode::onTimer() no_start_reason_diag = makeStopReasonDiag("obstacle", odometry_ptr_->pose.pose); } + tier4_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); + pub_stop_reason_->publish(no_start_reason_diag); debug_ptr_->publish(); } @@ -365,7 +321,11 @@ std::optional SurroundObstacleCheckerNode::getNearestObstacleByPointCl const double front_margin = pointcloud_param.surround_check_front_distance; const double side_margin = pointcloud_param.surround_check_side_distance; const double back_margin = pointcloud_param.surround_check_back_distance; - const auto ego_polygon = createSelfPolygon(vehicle_info_, front_margin, side_margin, back_margin); + const double base_to_front = vehicle_info_.max_longitudinal_offset_m + front_margin; + const double base_to_rear = vehicle_info_.rear_overhang_m + back_margin; + const double width = vehicle_info_.vehicle_width_m + side_margin * 2; + const auto ego_polygon = autoware::universe_utils::toFootprint( + odometry_ptr_->pose.pose, base_to_front, base_to_rear, width); geometry_msgs::msg::Point nearest_point; double minimum_distance = std::numeric_limits::max(); @@ -392,18 +352,8 @@ std::optional SurroundObstacleCheckerNode::getNearestObstacleByDynamic { if (!object_ptr_ || !getUseDynamicObject()) return std::nullopt; - const auto transform_stamped = - getTransform(object_ptr_->header.frame_id, "base_link", object_ptr_->header.stamp, 0.5); - - if (!transform_stamped) { - return std::nullopt; - } - const auto param = param_listener_->get_params(); - tf2::Transform tf_src2target; - tf2::fromMsg(transform_stamped.value().transform, tf_src2target); - // TODO(murooka) check computation cost geometry_msgs::msg::Point nearest_point; double minimum_distance = std::numeric_limits::max(); @@ -420,19 +370,13 @@ std::optional SurroundObstacleCheckerNode::getNearestObstacleByDynamic const double front_margin = object_param.surround_check_front_distance; const double side_margin = object_param.surround_check_side_distance; const double back_margin = object_param.surround_check_back_distance; - const auto ego_polygon = - createSelfPolygon(vehicle_info_, front_margin, side_margin, back_margin); + const double base_to_front = vehicle_info_.max_longitudinal_offset_m + front_margin; + const double base_to_rear = vehicle_info_.rear_overhang_m + back_margin; + const double width = vehicle_info_.vehicle_width_m + side_margin * 2; + const auto ego_polygon = autoware::universe_utils::toFootprint( + odometry_ptr_->pose.pose, base_to_front, base_to_rear, width); - tf2::Transform tf_src2object; - tf2::fromMsg(object_pose, tf_src2object); - - geometry_msgs::msg::Pose transformed_object_pose; - tf2::toMsg(tf_src2target.inverse() * tf_src2object, transformed_object_pose); - - const auto object_polygon = - object.shape.type == Shape::POLYGON - ? createObjPolygon(transformed_object_pose, object.shape.footprint) - : createObjPolygon(transformed_object_pose, object.shape.dimensions); + const auto object_polygon = autoware::universe_utils::toPolygon2d(object); const auto distance_to_object = bg::distance(ego_polygon, object_polygon); @@ -465,34 +409,32 @@ std::optional SurroundObstacleCheckerNode: return transform_stamped; } -bool SurroundObstacleCheckerNode::isStopRequired( - const bool is_obstacle_found, const bool is_vehicle_stopped) +auto SurroundObstacleCheckerNode::isStopRequired( + const bool is_obstacle_found, const bool is_vehicle_stopped, const State & state, + const std::optional & last_obstacle_found_time, + const double time_threshold) const -> std::pair> { if (!is_vehicle_stopped) { - return false; + return std::make_pair(false, std::nullopt); } if (is_obstacle_found) { - last_obstacle_found_time_ = std::make_shared(this->now()); - return true; + return std::make_pair(true, this->now()); } - if (state_ != State::STOP) { - return false; + if (state != State::STOP) { + return std::make_pair(false, std::nullopt); } - const auto param = param_listener_->get_params(); - // Keep stop state - if (last_obstacle_found_time_) { - const auto elapsed_time = this->now() - *last_obstacle_found_time_; - if (elapsed_time.seconds() <= param.state_clear_time) { - return true; + if (last_obstacle_found_time.has_value()) { + const auto elapsed_time = this->now() - last_obstacle_found_time.value(); + if (elapsed_time.seconds() <= time_threshold) { + return std::make_pair(true, last_obstacle_found_time.value()); } } - last_obstacle_found_time_ = {}; - return false; + return std::make_pair(false, std::nullopt); } } // namespace autoware::surround_obstacle_checker diff --git a/planning/autoware_surround_obstacle_checker/src/node.hpp b/planning/autoware_surround_obstacle_checker/src/node.hpp index 2e40eac3dfdfc..480a937a4a909 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.hpp +++ b/planning/autoware_surround_obstacle_checker/src/node.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -86,7 +87,10 @@ class SurroundObstacleCheckerNode : public rclcpp::Node const std::string & source, const std::string & target, const rclcpp::Time & stamp, double duration_sec) const; - bool isStopRequired(const bool is_obstacle_found, const bool is_vehicle_stopped); + auto isStopRequired( + const bool is_obstacle_found, const bool is_vehicle_stopped, const State & state, + const std::optional & last_obstacle_found_time, + const double time_threshold) const -> std::pair>; // ros mutable tf2_ros::Buffer tf_buffer_{get_clock()}; @@ -103,6 +107,7 @@ class SurroundObstacleCheckerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_stop_reason_; rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; rclcpp::Publisher::SharedPtr pub_velocity_limit_; + rclcpp::Publisher::SharedPtr pub_processing_time_; // parameter callback result OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -124,11 +129,14 @@ class SurroundObstacleCheckerNode : public rclcpp::Node // State Machine State state_ = State::PASS; - std::shared_ptr last_obstacle_found_time_; + std::optional last_obstacle_found_time_; std::unique_ptr logger_configure_; std::unordered_map label_map_; + +public: + friend class SurroundObstacleCheckerNodeTest; }; } // namespace autoware::surround_obstacle_checker diff --git a/planning/autoware_surround_obstacle_checker/test/test_surround_obstacle_checker.cpp b/planning/autoware_surround_obstacle_checker/test/test_surround_obstacle_checker.cpp new file mode 100644 index 0000000000000..c5fbb7958208d --- /dev/null +++ b/planning/autoware_surround_obstacle_checker/test/test_surround_obstacle_checker.cpp @@ -0,0 +1,129 @@ +// 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 "../src/node.hpp" + +#include +#include +#include + +#include + +namespace autoware::surround_obstacle_checker +{ +auto generateTestTargetNode() -> std::shared_ptr +{ + rclcpp::init(0, nullptr); + + auto node_options = rclcpp::NodeOptions{}; + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); + + 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", + ament_index_cpp::get_package_share_directory("autoware_surround_obstacle_checker") + + "/config/surround_obstacle_checker.param.yaml"}); + + return std::make_shared(node_options); +} + +class SurroundObstacleCheckerNodeTest : public ::testing::Test +{ +public: + SurroundObstacleCheckerNodeTest() : node_{generateTestTargetNode()} {} + + auto isStopRequired( + const bool is_obstacle_found, const bool is_vehicle_stopped, const State & state, + const std::optional & last_obstacle_found_time, + const double time_threshold) const -> std::pair> + { + return node_->isStopRequired( + is_obstacle_found, is_vehicle_stopped, state, last_obstacle_found_time, time_threshold); + } + +private: + std::shared_ptr node_; +}; + +TEST_F(SurroundObstacleCheckerNodeTest, isStopRequired) +{ + const auto LAST_STOP_TIME = rclcpp::Clock{RCL_ROS_TIME}.now(); + + using namespace std::literals::chrono_literals; + rclcpp::sleep_for(500ms); + + { + constexpr double THRESHOLD = 1.0; + const auto [is_stop, stop_time] = + isStopRequired(false, false, State::STOP, LAST_STOP_TIME, THRESHOLD); + EXPECT_FALSE(is_stop); + EXPECT_EQ(stop_time, std::nullopt); + } + + { + constexpr double THRESHOLD = 1.0; + const auto [is_stop, stop_time] = + isStopRequired(false, true, State::PASS, LAST_STOP_TIME, THRESHOLD); + EXPECT_FALSE(is_stop); + EXPECT_EQ(stop_time, std::nullopt); + } + + { + constexpr double THRESHOLD = 1.0; + const auto [is_stop, stop_time] = + isStopRequired(true, true, State::STOP, LAST_STOP_TIME, THRESHOLD); + + ASSERT_TRUE(stop_time.has_value()); + + const auto time_diff = rclcpp::Clock{RCL_ROS_TIME}.now() - stop_time.value(); + + EXPECT_TRUE(is_stop); + EXPECT_NEAR(time_diff.seconds(), 0.0, 1e-3); + } + + { + constexpr double THRESHOLD = 1.0; + const auto [is_stop, stop_time] = + isStopRequired(false, true, State::STOP, LAST_STOP_TIME, THRESHOLD); + + ASSERT_TRUE(stop_time.has_value()); + + const auto time_diff = rclcpp::Clock{RCL_ROS_TIME}.now() - stop_time.value(); + + EXPECT_TRUE(is_stop); + EXPECT_NEAR(time_diff.seconds(), 0.5, 1e-3); + } + + { + constexpr double THRESHOLD = 0.25; + const auto [is_stop, stop_time] = + isStopRequired(false, true, State::STOP, LAST_STOP_TIME, THRESHOLD); + EXPECT_FALSE(is_stop); + EXPECT_EQ(stop_time, std::nullopt); + } + + { + constexpr double THRESHOLD = 1.0; + const auto [is_stop, stop_time] = + isStopRequired(false, true, State::STOP, std::nullopt, THRESHOLD); + EXPECT_FALSE(is_stop); + EXPECT_EQ(stop_time, std::nullopt); + } + + rclcpp::shutdown(); +} +} // namespace autoware::surround_obstacle_checker 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 d6033b66d5d6f..3f1c313e052da 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -17,6 +17,7 @@ #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" +#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/logger_level_configure.hpp" @@ -31,7 +32,6 @@ #include "autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "autoware/velocity_smoother/trajectory_utils.hpp" -#include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/transform_listener.h" diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index 8f6bbc2236eb6..fda8e8f31df95 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -16,10 +16,10 @@ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" -#include "osqp_interface/osqp_interface.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -57,7 +57,7 @@ class L2PseudoJerkSmoother : public SmootherBase private: Param smoother_param_; - autoware::common::osqp::OSQPInterface qp_solver_; + autoware::osqp_interface::OSQPInterface qp_solver_; rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("l2_pseudo_jerk_smoother")}; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index e27a4db10e748..119cb4d358de2 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -16,10 +16,10 @@ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" -#include "osqp_interface/osqp_interface.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -57,7 +57,7 @@ class LinfPseudoJerkSmoother : public SmootherBase private: Param smoother_param_; - autoware::common::osqp::OSQPInterface qp_solver_; + autoware::osqp_interface::OSQPInterface qp_solver_; rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("linf_pseudo_jerk_smoother")}; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index 65684e414e90d..222b189057820 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -23,13 +23,13 @@ autoware_interpolation autoware_motion_utils + autoware_osqp_interface autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils autoware_vehicle_info_utils geometry_msgs nav_msgs - osqp_interface qp_interface rclcpp tf2 diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml index aa3a207ea5be9..f5703d896d58f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml @@ -11,6 +11,8 @@ Tomoya Kimura Shumpei Wakabayashi Tomohito Ando + Maxime CLEMENT + Alqudah Mohammad Apache License 2.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml index 9308aab7642f5..858bc78de09eb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml @@ -20,6 +20,7 @@ autoware_behavior_path_planner_common autoware_lanelet2_extension autoware_motion_utils + autoware_object_recognition_utils autoware_perception_msgs autoware_planning_msgs autoware_signal_processing @@ -27,7 +28,6 @@ autoware_vehicle_msgs geometry_msgs lanelet2_core - object_recognition_utils pluginlib rclcpp tf2 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 3fa3f04435e87..852a3fe0feedf 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 @@ -16,9 +16,9 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/signal_processing/lowpass_filter_1d.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" #include #include @@ -796,7 +796,7 @@ void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObje return is_object_left; } const auto future_obj_pose = - object_recognition_utils::calcInterpolatedPose(obj_path, time_to_collision); + autoware::object_recognition_utils::calcInterpolatedPose(obj_path, time_to_collision); const size_t future_obj_idx = autoware::motion_utils::findNearestIndex(input_path.points, future_obj_pose->position); @@ -1001,7 +1001,7 @@ DynamicObstacleAvoidanceModule::calcCollisionSection( const auto future_ego_pose = ego_path.at(i); const auto future_obj_pose = - object_recognition_utils::calcInterpolatedPose(obj_path, elapsed_time); + autoware::object_recognition_utils::calcInterpolatedPose(obj_path, elapsed_time); if (future_obj_pose) { const double dist_ego_to_obj = diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml index 41db5ade58a74..464951bc81008 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml @@ -11,6 +11,8 @@ Tomoya Kimura Shumpei Wakabayashi Tomohito Ando + Maxime CLEMENT + Alqudah Mohammad Apache License 2.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp index 67aa41a5af7e5..1092047e65030 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp @@ -35,7 +35,7 @@ class PathDecisionState }; DecisionKind state{DecisionKind::NOT_DECIDED}; - rclcpp::Time stamp{}; + std::optional deciding_start_time{std::nullopt}; bool is_stable_safe{false}; std::optional safe_start_time{std::nullopt}; }; @@ -43,7 +43,7 @@ class PathDecisionState class PathDecisionStateController { public: - PathDecisionStateController() = default; + explicit PathDecisionStateController(rclcpp::Logger logger) : logger_(logger) {} /** * @brief update current state and save old current state to prev state @@ -62,11 +62,10 @@ class PathDecisionStateController PathDecisionState get_current_state() const { return current_state_; } - PathDecisionState get_prev_state() const { return prev_state_; } - private: + rclcpp::Logger logger_; + PathDecisionState current_state_{}; - PathDecisionState prev_state_{}; /** * @brief update current state and save old current state to prev state 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 d029b2f5953a7..e8079c4e27720 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 @@ -15,6 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ +#include "autoware/behavior_path_goal_planner_module/decision_state.hpp" #include "autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp" #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" @@ -110,15 +111,27 @@ struct PullOverContextData PullOverContextData() = delete; explicit PullOverContextData( const bool is_stable_safe_path, const PredictedObjects & static_objects, - const PredictedObjects & dynamic_objects) + const PredictedObjects & dynamic_objects, std::optional && pull_over_path_opt, + const std::vector & pull_over_path_candidates, + const PathDecisionState & prev_state) : is_stable_safe_path(is_stable_safe_path), static_target_objects(static_objects), - dynamic_target_objects(dynamic_objects) + dynamic_target_objects(dynamic_objects), + pull_over_path_opt(pull_over_path_opt), + pull_over_path_candidates(pull_over_path_candidates), + prev_state_for_debug(prev_state), + last_path_idx_increment_time(std::nullopt) { } const bool is_stable_safe_path; const PredictedObjects static_target_objects; const PredictedObjects dynamic_target_objects; + // TODO(soblin): due to deceleratePath(), this cannot be const member(velocity is modified by it) + std::optional pull_over_path_opt; + const std::vector pull_over_path_candidates; + // TODO(soblin): goal_candidate_from_thread, modifed_goal_pose_from_thread, closest_start_pose + const PathDecisionState prev_state_for_debug; + std::optional last_path_idx_increment_time; }; class GoalPlannerModule : public SceneModuleInterface @@ -314,7 +327,7 @@ class GoalPlannerModule : public SceneModuleInterface // context_data_ is initialized in updateData(), used in plan() and refreshed in postProcess() std::optional context_data_{std::nullopt}; // path_decision_controller is updated in updateData(), and used in plan() - PathDecisionStateController path_decision_controller_{}; + PathDecisionStateController path_decision_controller_{getLogger()}; std::unique_ptr last_approval_data_{nullptr}; // approximate distance from the start point to the end point of pull_over. @@ -354,21 +367,25 @@ class GoalPlannerModule : public SceneModuleInterface void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const; void decelerateBeforeSearchStart( const Pose & search_start_offset_pose, PathWithLaneId & path) const; - PathWithLaneId generateStopPath() const; + PathWithLaneId generateStopPath(const PullOverContextData & context_data) const; PathWithLaneId generateFeasibleStopPath(const PathWithLaneId & path) const; - void keepStoppedWithCurrentPath(PathWithLaneId & path) const; + void keepStoppedWithCurrentPath( + const PullOverContextData & ctx_data, PathWithLaneId & path) const; double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; // status bool isStopped(); bool isStopped( std::deque & odometry_buffer, const double time); - bool hasFinishedCurrentPath(); - bool isOnModifiedGoal(const Pose & current_pose, const GoalPlannerParameters & parameters) const; + bool hasFinishedCurrentPath(const PullOverContextData & ctx_data); + bool isOnModifiedGoal( + const Pose & current_pose, const std::optional & modified_goal_opt, + const GoalPlannerParameters & parameters) const; double calcModuleRequestLength() const; bool needPathUpdate( const Pose & current_pose, const double path_update_duration, + const std::optional & modified_goal_opt, const GoalPlannerParameters & parameters) const; bool isStuck( const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, @@ -399,23 +416,25 @@ class GoalPlannerModule : public SceneModuleInterface BehaviorModuleOutput planPullOver(const PullOverContextData & context_data); BehaviorModuleOutput planPullOverAsOutput(const PullOverContextData & context_data); BehaviorModuleOutput planPullOverAsCandidate(const PullOverContextData & context_data); - std::optional> selectPullOverPath( + std::optional selectPullOverPath( const PullOverContextData & context_data, const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const; // lanes and drivable area std::vector generateDrivableLanes() const; - void setDrivableAreaInfo(BehaviorModuleOutput & output) const; + void setDrivableAreaInfo( + const PullOverContextData & context_data, BehaviorModuleOutput & output) const; // output setter void setOutput(const PullOverContextData & context_data, BehaviorModuleOutput & output); - void setModifiedGoal(BehaviorModuleOutput & output) const; - void setTurnSignalInfo(BehaviorModuleOutput & output); + void setModifiedGoal( + const PullOverContextData & context_data, BehaviorModuleOutput & output) const; + void setTurnSignalInfo(const PullOverContextData & context_data, BehaviorModuleOutput & output); // new turn signal - TurnSignalInfo calcTurnSignalInfo(); + TurnSignalInfo calcTurnSignalInfo(const PullOverContextData & context_data); std::optional ignore_signal_{std::nullopt}; bool hasPreviousModulePathShapeChanged(const BehaviorModuleOutput & previous_module_output) const; @@ -431,10 +450,12 @@ class GoalPlannerModule : public SceneModuleInterface // steering factor void updateSteeringFactor( - const std::array & pose, const std::array distance, const uint16_t type); + const PullOverContextData & context_data, const std::array & pose, + const std::array distance, const uint16_t type); // rtc - std::pair calcDistanceToPathChange() const; + std::pair calcDistanceToPathChange( + const PullOverContextData & context_data) const; // safety check void initializeSafetyCheckParameters(); @@ -450,7 +471,7 @@ class GoalPlannerModule : public SceneModuleInterface */ bool isSafePath( const std::shared_ptr planner_data, const bool found_pull_over_path, - const std::optional & pull_over_path_opt, + const std::optional & pull_over_path_opt, const PathDecisionState & prev_data, const GoalPlannerParameters & parameters, const std::shared_ptr & ego_predicted_path_params, const std::shared_ptr & objects_filtering_params, 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 34743ae5fbf5f..ee370fd96b9a0 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 @@ -37,8 +37,9 @@ class FreespacePullOver : public PullOverPlannerBase PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } std::optional plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override; + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_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 2dfcfb3dc6e9e..89181b258fbea 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 @@ -43,8 +43,9 @@ class GeometricPullOver : public PullOverPlannerBase Pose getCl() const { return planner_.getCl(); } std::optional plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override; + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_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 d952f8ddbd22d..e061a0203602f 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 @@ -15,6 +15,7 @@ #pragma once #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" #include "autoware/behavior_path_planner_common/data_manager.hpp" #include @@ -41,27 +42,29 @@ struct PullOverPath { public: static std::optional create( - const PullOverPlannerType & type, const size_t goal_id, const size_t id, + const PullOverPlannerType & type, const size_t id, const std::vector & partial_paths, const Pose & start_pose, - const Pose & end_pose, + const GoalCandidate & modified_goal_pose, const std::vector> & pairs_terminal_velocity_and_accel); PullOverPath(const PullOverPath & other); + PullOverPath & operator=(const PullOverPath & other) = default; PullOverPlannerType type() const { return type_; } - size_t goal_id() const { return goal_id_; } + size_t goal_id() const { return modified_goal_pose_.id; } size_t id() const { return id_; } Pose start_pose() const { return start_pose_; } - Pose end_pose() const { return end_pose_; } + Pose modified_goal_pose() const { return modified_goal_pose_.goal_pose; } + const GoalCandidate & modified_goal() const { return modified_goal_pose_; } std::vector & partial_paths() { return partial_paths_; } const std::vector & partial_paths() const { return partial_paths_; } // TODO(soblin): use reference to avoid copy once thread-safe design is finished - PathWithLaneId full_path() const { return full_path_; } + const PathWithLaneId & full_path() const { return full_path_; } PathWithLaneId parking_path() const { return parking_path_; } - std::vector full_path_curvatures() { return full_path_curvatures_; } - std::vector parking_path_curvatures() const { return parking_path_curvatures_; } + const std::vector & full_path_curvatures() const { return full_path_curvatures_; } + const std::vector & parking_path_curvatures() const { return parking_path_curvatures_; } double full_path_max_curvature() const { return full_path_max_curvature_; } double parking_path_max_curvature() const { return parking_path_max_curvature_; } size_t path_idx() const { return path_idx_; } @@ -85,19 +88,18 @@ struct PullOverPath private: PullOverPath( - const PullOverPlannerType & type, const size_t goal_id, const size_t id, - const Pose & start_pose, const Pose & end_pose, - const std::vector & partial_paths, const PathWithLaneId & full_path, - const PathWithLaneId & parking_path, const std::vector & full_path_curvatures, + const PullOverPlannerType & type, const size_t id, const Pose & start_pose, + const GoalCandidate & modified_goal_pose, const std::vector & partial_paths, + const PathWithLaneId & full_path, const PathWithLaneId & parking_path, + const std::vector & full_path_curvatures, const std::vector & parking_path_curvatures, const double full_path_max_curvature, const double parking_path_max_curvature, const std::vector> & pairs_terminal_velocity_and_accel); PullOverPlannerType type_; - size_t goal_id_; + GoalCandidate modified_goal_pose_; size_t id_; Pose start_pose_; - Pose end_pose_; std::vector partial_paths_; PathWithLaneId full_path_; @@ -125,8 +127,9 @@ class PullOverPlannerBase virtual PullOverPlannerType getPlannerType() const = 0; virtual std::optional plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) = 0; + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_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 9baceb4430dec..bfb0173874784 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 @@ -36,8 +36,9 @@ class ShiftPullOver : public PullOverPlannerBase const LaneDepartureChecker & lane_departure_checker); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::SHIFT; }; std::optional plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override; + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output) override; protected: PathWithLaneId generateReferencePath( @@ -46,10 +47,10 @@ class ShiftPullOver : public PullOverPlannerBase std::optional cropPrevModulePath( const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const; std::optional generatePullOverPath( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, + 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 lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, - const double lateral_jerk) const; + const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const; static double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double after_shifted_arc_length, const double dr); static std::vector splineTwoPoints( 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 3bfaa90b845fb..3b526bf3a8f24 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 @@ -63,11 +63,7 @@ class ThreadSafeData return false; } - if (pull_over_path_->incrementPathIndex()) { - last_path_idx_increment_time_ = clock_->now(); - return true; - } - return false; + return pull_over_path_->incrementPathIndex(); } void set_pull_over_path(const PullOverPath & path) @@ -105,52 +101,41 @@ class ThreadSafeData return true; } - std::optional getPullOverPlannerType() const - { - const std::lock_guard lock(mutex_); - if (!pull_over_path_) { - return std::nullopt; - } - return pull_over_path_->type(); - }; - void reset() { const std::lock_guard lock(mutex_); pull_over_path_ = nullptr; pull_over_path_candidates_.clear(); goal_candidates_.clear(); - modified_goal_pose_ = std::nullopt; last_path_update_time_ = std::nullopt; - last_path_idx_increment_time_ = std::nullopt; closest_start_pose_ = std::nullopt; last_previous_module_output_ = std::nullopt; prev_data_ = PathDecisionState{}; } + // main --> lane/freespace + DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, static_target_objects) + DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, dynamic_target_objects) + // main --> lane + DEFINE_SETTER_GETTER_WITH_MUTEX(PathDecisionState, prev_data) + DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, last_previous_module_output) + + // lane --> main + DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, closest_start_pose) + DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector, pull_over_path_candidates) + + // main <--> lane/freespace DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) - DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, lane_parking_pull_over_path) DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_update_time) - DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_idx_increment_time) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector, pull_over_path_candidates) DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, modified_goal_pose) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, closest_start_pose) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, last_previous_module_output) DEFINE_SETTER_GETTER_WITH_MUTEX( utils::path_safety_checker::CollisionCheckDebugMap, collision_check) - DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, static_target_objects) - DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, dynamic_target_objects) - DEFINE_SETTER_GETTER_WITH_MUTEX(PathDecisionState, prev_data) private: void set_pull_over_path_no_lock(const PullOverPath & path) { pull_over_path_ = std::make_shared(path); - if (path.type() != PullOverPlannerType::FREESPACE) { - lane_parking_pull_over_path_ = std::make_shared(path); - } last_path_update_time_ = clock_->now(); } @@ -158,9 +143,6 @@ class ThreadSafeData void set_pull_over_path_no_lock(const std::shared_ptr & path) { pull_over_path_ = path; - if (path->type() != PullOverPlannerType::FREESPACE) { - lane_parking_pull_over_path_ = path; - } last_path_update_time_ = clock_->now(); } @@ -168,7 +150,6 @@ class ThreadSafeData void set_no_lock(const std::vector & arg) { pull_over_path_candidates_ = arg; } void set_no_lock(const std::shared_ptr & arg) { set_pull_over_path_no_lock(arg); } void set_no_lock(const PullOverPath & arg) { set_pull_over_path_no_lock(arg); } - void set_no_lock(const GoalCandidate & arg) { modified_goal_pose_ = arg; } void set_no_lock(const BehaviorModuleOutput & arg) { last_previous_module_output_ = arg; } void set_no_lock(const utils::path_safety_checker::CollisionCheckDebugMap & arg) { @@ -176,12 +157,9 @@ class ThreadSafeData } std::shared_ptr pull_over_path_{nullptr}; - std::shared_ptr lane_parking_pull_over_path_{nullptr}; std::vector pull_over_path_candidates_; GoalCandidates goal_candidates_{}; - std::optional modified_goal_pose_; std::optional last_path_update_time_; - std::optional last_path_idx_increment_time_; std::optional closest_start_pose_{}; std::optional last_previous_module_output_{}; utils::path_safety_checker::CollisionCheckDebugMap collision_check_{}; 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 147fe9f79dba0..88349f20a0540 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 @@ -17,6 +17,7 @@ #include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" #include "autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp" #include "autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" #include "autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp" #include "autoware/behavior_path_goal_planner_module/util.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" @@ -259,7 +260,17 @@ void GoalPlannerModule::onTimer() // check if new pull over path candidates are needed to be generated const auto current_state = thread_safe_data_.get_prev_data().state; const bool need_update = std::invoke([&]() { - if (isOnModifiedGoal(local_planner_data->self_odometry->pose.pose, parameters)) { + const bool found_pull_over_path = thread_safe_data_.foundPullOverPath(); + const std::optional pull_over_path_opt = + found_pull_over_path + ? std::make_optional(*thread_safe_data_.get_pull_over_path()) + : std::nullopt; + const std::optional modified_goal_opt = + pull_over_path_opt + ? 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)) { return false; } if (hasDeviatedFromCurrentPreviousModulePath(local_planner_data, previous_module_output)) { @@ -303,8 +314,7 @@ void GoalPlannerModule::onTimer() const std::shared_ptr & planner, const GoalCandidate & goal_candidate) { const auto pull_over_path = planner->plan( - goal_candidate.id, path_candidates.size(), local_planner_data, previous_module_output, - goal_candidate.goal_pose); + goal_candidate, path_candidates.size(), local_planner_data, previous_module_output); if (pull_over_path) { // calculate absolute maximum curvature of parking path(start pose to end pose) for path // priority @@ -409,7 +419,16 @@ void GoalPlannerModule::onFreespaceParkingTimer() return; } - if (isOnModifiedGoal(local_planner_data->self_odometry->pose.pose, parameters)) { + const bool found_pull_over_path = thread_safe_data_.foundPullOverPath(); + const std::optional pull_over_path_opt = + found_pull_over_path ? std::make_optional(*thread_safe_data_.get_pull_over_path()) + : std::nullopt; + const std::optional modified_goal_opt = + pull_over_path_opt + ? 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)) { return; } @@ -422,7 +441,8 @@ void GoalPlannerModule::onFreespaceParkingTimer() local_planner_data, occupancy_grid_map, parameters) && is_new_costmap && needPathUpdate( - local_planner_data->self_odometry->pose.pose, path_update_duration, parameters)) { + local_planner_data->self_odometry->pose.pose, path_update_duration, modified_goal_opt, + parameters)) { planFreespacePath(local_planner_data, goal_searcher, occupancy_grid_map); } } @@ -540,21 +560,34 @@ void GoalPlannerModule::updateData() path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); const bool found_pull_over_path = thread_safe_data_.foundPullOverPath(); - const std::optional pull_over_path = + std::optional pull_over_path_recv = found_pull_over_path ? std::make_optional(*thread_safe_data_.get_pull_over_path()) : std::nullopt; + const auto modified_goal_pose = [&]() -> std::optional { + if (!pull_over_path_recv) { + return std::nullopt; + } + const auto & pull_over_path = pull_over_path_recv.value(); + return pull_over_path.modified_goal(); + }(); + + // save "old" state + const auto prev_decision_state = path_decision_controller_.get_current_state(); const bool is_current_safe = isSafePath( - planner_data_, found_pull_over_path, pull_over_path, *parameters_, ego_predicted_path_params_, - objects_filtering_params_, safety_check_params_); + planner_data_, found_pull_over_path, pull_over_path_recv, prev_decision_state, *parameters_, + ego_predicted_path_params_, objects_filtering_params_, safety_check_params_); + // update to latest state path_decision_controller_.transit_state( found_pull_over_path, clock_->now(), static_target_objects, dynamic_target_objects, - thread_safe_data_.get_modified_goal_pose(), planner_data_, occupancy_grid_map_, is_current_safe, - *parameters_, goal_searcher_, isActivated(), pull_over_path, debug_data_.ego_polygons_expanded); + modified_goal_pose, planner_data_, occupancy_grid_map_, is_current_safe, *parameters_, + goal_searcher_, isActivated(), pull_over_path_recv, debug_data_.ego_polygons_expanded); context_data_.emplace( path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, - dynamic_target_objects); + dynamic_target_objects, std::move(pull_over_path_recv), + thread_safe_data_.get_pull_over_path_candidates(), prev_decision_state); + auto & ctx_data = context_data_.value(); // update goal searcher and generate goal candidates if (thread_safe_data_.get_goal_candidates().empty()) { @@ -569,8 +602,10 @@ void GoalPlannerModule::updateData() return; } - if (hasFinishedCurrentPath()) { - thread_safe_data_.incrementPathIndex(); + if (hasFinishedCurrentPath(ctx_data)) { + if (thread_safe_data_.incrementPathIndex()) { + ctx_data.last_path_idx_increment_time = clock_->now(); + } } if (!last_approval_data_) { @@ -800,9 +835,8 @@ bool GoalPlannerModule::planFreespacePath( continue; } const auto freespace_path = freespace_planner_->plan( - goal_candidate.id, 0, planner_data, - BehaviorModuleOutput{}, // NOTE: not used so passing {} is OK - goal_candidate.goal_pose); + goal_candidate, 0, planner_data, BehaviorModuleOutput{} // NOTE: not used so passing {} is OK + ); if (!freespace_path) { continue; } @@ -810,7 +844,6 @@ bool GoalPlannerModule::planFreespacePath( { const std::lock_guard lock(mutex_); thread_safe_data_.set_pull_over_path(*freespace_path); - thread_safe_data_.set_modified_goal_pose(goal_candidate); debug_data_.freespace_planner.is_planning = false; } @@ -829,13 +862,16 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte return false; } - const auto lane_parking_path = thread_safe_data_.get_lane_parking_pull_over_path(); - if (!lane_parking_path) { + if (!context_data.pull_over_path_opt) { + return false; + } + if (context_data.pull_over_path_opt.value().type() == PullOverPlannerType::FREESPACE) { return false; } + const auto & lane_parking_path = context_data.pull_over_path_opt.value(); - const PathWithLaneId path = lane_parking_path->full_path(); - const std::vector curvatures = lane_parking_path->full_path_curvatures(); + const auto & path = lane_parking_path.full_path(); + const auto & curvatures = lane_parking_path.full_path_curvatures(); if ( parameters_->use_object_recognition && goal_planner_utils::checkObjectsCollision( @@ -891,22 +927,20 @@ BehaviorModuleOutput GoalPlannerModule::plan() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (!context_data_) { - RCLCPP_ERROR(getLogger(), " [pull_over] plan() is called without valid context_data"); - } - const auto context_data = context_data_.has_value() - ? context_data_.value() - : PullOverContextData(true, PredictedObjects{}, PredictedObjects{}); - if (utils::isAllowedGoalModification(planner_data_->route_handler)) { - return planPullOver(context_data); + if (!context_data_) { + RCLCPP_ERROR(getLogger(), " [pull_over] plan() is called without valid context_data"); + } else { + const auto & context_data = context_data_.value(); + return planPullOver(context_data); + } } fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); return fixed_goal_planner_->plan(planner_data_); } -std::optional> GoalPlannerModule::selectPullOverPath( +std::optional GoalPlannerModule::selectPullOverPath( const PullOverContextData & context_data, const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const @@ -1003,7 +1037,7 @@ std::optional> GoalPlannerModule::selectP // Create a map of PullOverPath pointer to largest collision check margin std::map path_id_to_rough_margin_map; - const auto target_objects = thread_safe_data_.get_static_target_objects(); + const auto & target_objects = context_data.static_target_objects; for (const size_t i : sorted_path_indices) { const auto & path = pull_over_path_candidates[i]; const double distance = utils::path_safety_checker::calculateRoughDistanceToObjects( @@ -1131,7 +1165,7 @@ std::optional> GoalPlannerModule::selectP checkOccupancyGridCollision(parking_path, occupancy_grid_map_)) { continue; } - return std::make_pair(path, goal_candidates.at(goal_id_to_index.at(path.goal_id()))); + return path; } return {}; } @@ -1157,23 +1191,23 @@ void GoalPlannerModule::setOutput( output.reference_path = getPreviousModuleOutput().reference_path; - if (!thread_safe_data_.foundPullOverPath()) { + if (!context_data.pull_over_path_opt) { // situation : not safe against static objects use stop_path - output.path = generateStopPath(); + output.path = generateStopPath(context_data); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); - setDrivableAreaInfo(output); + setDrivableAreaInfo(context_data, output); return; } + const auto & pull_over_path = context_data.pull_over_path_opt.value(); if ( parameters_->safety_check_params.enable_safety_check && !context_data.is_stable_safe_path && isActivated()) { // situation : not safe against dynamic objects after approval // insert stop point in current path if ego is able to stop with acceleration and jerk // constraints - output.path = - generateFeasibleStopPath(thread_safe_data_.get_pull_over_path()->getCurrentPath()); + output.path = generateFeasibleStopPath(pull_over_path.getCurrentPath()); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not safe against dynamic objects, generate stop path"); debug_stop_pose_with_info_.set(std::string("feasible stop after approval")); @@ -1182,29 +1216,31 @@ void GoalPlannerModule::setOutput( // before approval) don't stop // keep stop if not enough time passed, // because it takes time for the trajectory to be reflected - auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); - keepStoppedWithCurrentPath(current_path); + auto current_path = pull_over_path.getCurrentPath(); + keepStoppedWithCurrentPath(context_data, current_path); output.path = current_path; } - setModifiedGoal(output); - setDrivableAreaInfo(output); + setModifiedGoal(context_data, output); + setDrivableAreaInfo(context_data, output); // set hazard and turn signal if ( path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::DECIDED && isActivated()) { - setTurnSignalInfo(output); + setTurnSignalInfo(context_data, output); } } -void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const +void GoalPlannerModule::setDrivableAreaInfo( + const PullOverContextData & context_data, BehaviorModuleOutput & output) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); - if (planner_type_opt && planner_type_opt.value() == PullOverPlannerType::FREESPACE) { + if ( + context_data.pull_over_path_opt && + context_data.pull_over_path_opt.value().type() == PullOverPlannerType::FREESPACE) { const double drivable_area_margin = planner_data_->parameters.vehicle_width; output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; @@ -1219,13 +1255,14 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const } } -void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const +void GoalPlannerModule::setModifiedGoal( + const PullOverContextData & context_data, BehaviorModuleOutput & output) const { const auto & route_handler = planner_data_->route_handler; - if (thread_safe_data_.foundPullOverPath()) { + if (context_data.pull_over_path_opt) { PoseWithUuidStamped modified_goal{}; modified_goal.uuid = route_handler->getRouteUuid(); - modified_goal.pose = thread_safe_data_.get_modified_goal_pose()->goal_pose; + modified_goal.pose = context_data.pull_over_path_opt.value().modified_goal_pose(); modified_goal.header = route_handler->getRouteHeader(); output.modified_goal = modified_goal; } else { @@ -1233,10 +1270,11 @@ void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const } } -void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) +void GoalPlannerModule::setTurnSignalInfo( + const PullOverContextData & context_data, BehaviorModuleOutput & output) { const auto original_signal = getPreviousModuleOutput().turn_signal_info; - const auto new_signal = calcTurnSignalInfo(); + const auto new_signal = calcTurnSignalInfo(context_data); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, original_signal, new_signal, @@ -1245,10 +1283,11 @@ void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) } void GoalPlannerModule::updateSteeringFactor( - const std::array & pose, const std::array distance, const uint16_t type) + const PullOverContextData & context_data, const std::array & pose, + const std::array distance, const uint16_t type) { - const uint16_t steering_factor_direction = std::invoke([this]() { - const auto turn_signal = calcTurnSignalInfo(); + const uint16_t steering_factor_direction = std::invoke([&]() { + const auto turn_signal = calcTurnSignalInfo(context_data); if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return SteeringFactor::LEFT; } else if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { @@ -1268,6 +1307,7 @@ void GoalPlannerModule::decideVelocity() const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; // partial_paths + // TODO(soblin): only update velocity on main thread side, use that on main thread side auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths().front(); const auto vel = static_cast(std::max(current_vel, parameters_->pull_over_minimum_velocity)); @@ -1295,14 +1335,14 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate( universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // if pull over path candidates generation is not finished, use previous module output - if (thread_safe_data_.get_pull_over_path_candidates().empty()) { + if (context_data.pull_over_path_candidates.empty()) { return getPreviousModuleOutput(); } BehaviorModuleOutput output{}; const BehaviorModuleOutput pull_over_output = planPullOverAsOutput(context_data); output.modified_goal = pull_over_output.modified_goal; - output.path = generateStopPath(); + output.path = generateStopPath(context_data); output.reference_path = getPreviousModuleOutput().reference_path; const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( @@ -1313,7 +1353,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate( output.drivable_area_info = utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - if (!thread_safe_data_.foundPullOverPath()) { + if (!context_data.pull_over_path_opt) { return output; } @@ -1331,19 +1371,30 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( start = std::chrono::system_clock::now(); // if pull over path candidates generation is not finished, use previous module output - if (thread_safe_data_.get_pull_over_path_candidates().empty()) { + if (context_data.pull_over_path_candidates.empty()) { return getPreviousModuleOutput(); } - const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); + auto context_data_with_velocity = context_data; + /** + NOTE(soblin): this path originates from the previously selected(by main thread) pull_over_path + which was originally generated by either road_parking or freespace thread + */ + auto & pull_over_path_with_velocity_opt = context_data_with_velocity.pull_over_path_opt; const bool is_freespace = - planner_type_opt && planner_type_opt.value() == PullOverPlannerType::FREESPACE; + pull_over_path_with_velocity_opt && + pull_over_path_with_velocity_opt.value().type() == PullOverPlannerType::FREESPACE; + const std::optional modified_goal_opt = + pull_over_path_with_velocity_opt + ? std::make_optional(pull_over_path_with_velocity_opt.value().modified_goal()) + : std::nullopt; if ( path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::NOT_DECIDED && !is_freespace && needPathUpdate( - planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, *parameters_)) { + planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, modified_goal_opt, + *parameters_)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates // and set it to thread_safe_data_ @@ -1354,22 +1405,37 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( // update goal candidates auto goal_candidates = thread_safe_data_.get_goal_candidates(); goal_searcher_->update( - goal_candidates, occupancy_grid_map_, planner_data_, - thread_safe_data_.get_static_target_objects()); + goal_candidates, occupancy_grid_map_, planner_data_, context_data.static_target_objects); // Select a path that is as safe as possible and has a high priority. - const auto pull_over_path_candidates = thread_safe_data_.get_pull_over_path_candidates(); - const auto path_and_goal_opt = + const auto & pull_over_path_candidates = context_data.pull_over_path_candidates; + auto path_and_goal_opt = selectPullOverPath(context_data, pull_over_path_candidates, goal_candidates); // update thread_safe_data_ if (path_and_goal_opt) { - auto [pull_over_path, modified_goal] = *path_and_goal_opt; - deceleratePath(pull_over_path); - thread_safe_data_.set(goal_candidates, pull_over_path, modified_goal); + const auto & pull_over_path = path_and_goal_opt.value(); + /** TODO(soblin): since thread_safe_data::pull_over_path was used as a global variable, old + * code was setting deceleration to thread_safe_data::pull_over_path and setOutput() accessed + * to the velocity profile in thread_safe_data::pull_over_path, which is a very bad usage of + * member variable + * + * set this selected pull_over_path to ThreadSafeData, but actually RoadParking thread does + * not use pull_over_path, but only FreespaceParking thread use this selected pull_over_path. + * As the next action item, only set this selected pull_over_path to only + * FreespaceThreadSafeData. + */ + thread_safe_data_.set(goal_candidates, pull_over_path); + if (pull_over_path_with_velocity_opt) { + auto & pull_over_path_with_velocity = pull_over_path_with_velocity_opt.value(); + // copy the path for later setOutput() + pull_over_path_with_velocity = pull_over_path; + // modify the velocity for latest setOutput() + deceleratePath(pull_over_path_with_velocity); + } RCLCPP_DEBUG( getLogger(), "selected pull over path: path_id: %ld, goal_id: %ld", pull_over_path.id(), - modified_goal.id); + pull_over_path.modified_goal().id); } else { thread_safe_data_.set(goal_candidates); } @@ -1377,27 +1443,27 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( // set output and status BehaviorModuleOutput output{}; - setOutput(context_data, output); + setOutput(context_data_with_velocity, output); // return to lane parking if it is possible - if (is_freespace && canReturnToLaneParking(context_data)) { - thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path()); + if (is_freespace && canReturnToLaneParking(context_data_with_velocity)) { + // TODO(soblin): return from freespace to lane is disabled temporarily, because if + // context_data_with_velocity contained freespace path, since lane_parking_pull_over_path is + // deleted, freespace path is set again + if (context_data_with_velocity.pull_over_path_opt) { + thread_safe_data_.set_pull_over_path(context_data_with_velocity.pull_over_path_opt.value()); + } } // For debug - setDebugData(context_data); - - if (parameters_->print_debug_info) { - // For evaluations - printParkingPositionError(); - } + setDebugData(context_data_with_velocity); - if (!thread_safe_data_.foundPullOverPath()) { + if (!pull_over_path_with_velocity_opt) { return output; } path_candidate_ = - std::make_shared(thread_safe_data_.get_pull_over_path()->full_path()); + std::make_shared(pull_over_path_with_velocity_opt.value().full_path()); return output; } @@ -1409,63 +1475,65 @@ void GoalPlannerModule::postProcess() if (!context_data_) { RCLCPP_ERROR(getLogger(), " [pull_over] postProcess() is called without valid context_data"); } - const auto context_data = context_data_.has_value() - ? context_data_.value() - : PullOverContextData(true, PredictedObjects{}, PredictedObjects{}); + const auto context_data_dummy = + PullOverContextData(true, PredictedObjects{}, PredictedObjects{}, std::nullopt, {}, {}); + const auto & context_data = + context_data_.has_value() ? context_data_.value() : context_data_dummy; const bool has_decided_path = path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::DECIDED; - context_data_ = std::nullopt; - - if (!thread_safe_data_.foundPullOverPath()) { + if (!context_data.pull_over_path_opt) { + context_data_ = std::nullopt; return; } + const auto & pull_over_path = context_data.pull_over_path_opt.value(); - const auto distance_to_path_change = calcDistanceToPathChange(); + const auto distance_to_path_change = calcDistanceToPathChange(context_data); if (has_decided_path) { updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); } updateSteeringFactor( - {thread_safe_data_.get_pull_over_path()->start_pose(), - thread_safe_data_.get_modified_goal_pose()->goal_pose}, + 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); - setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->full_path()); + setStopReason(StopReason::GOAL_PLANNER, pull_over_path.full_path()); + + context_data_ = std::nullopt; } BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (!context_data_) { - RCLCPP_ERROR( - getLogger(), " [pull_over] planWaitingApproval() is called without valid context_data"); - } - const auto context_data = context_data_.has_value() - ? context_data_.value() - : PullOverContextData(true, PredictedObjects{}, PredictedObjects{}); - if (utils::isAllowedGoalModification(planner_data_->route_handler)) { - return planPullOverAsCandidate(context_data); + if (!context_data_) { + RCLCPP_ERROR( + getLogger(), " [pull_over] planWaitingApproval() is called without valid context_data"); + } else { + const auto & context_data = context_data_.value(); + return planPullOverAsCandidate(context_data); + } } fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); return fixed_goal_planner_->plan(planner_data_); } -std::pair GoalPlannerModule::calcDistanceToPathChange() const +std::pair GoalPlannerModule::calcDistanceToPathChange( + const PullOverContextData & context_data) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (!thread_safe_data_.foundPullOverPath()) { + if (!context_data.pull_over_path_opt) { return {std::numeric_limits::max(), std::numeric_limits::max()}; } + const auto & pull_over_path = context_data.pull_over_path_opt.value(); - const auto full_path = thread_safe_data_.get_pull_over_path()->full_path(); + const auto & full_path = context_data.pull_over_path_opt.value().full_path(); const auto ego_segment_idx = autoware::motion_utils::findNearestSegmentIndex( full_path.points, planner_data_->self_odometry->pose.pose, std::numeric_limits::max(), @@ -1475,15 +1543,15 @@ std::pair GoalPlannerModule::calcDistanceToPathChange() const } const size_t start_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( - full_path.points, thread_safe_data_.get_pull_over_path()->start_pose().position); + full_path.points, pull_over_path.start_pose().position); const double dist_to_parking_start_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, - thread_safe_data_.get_pull_over_path()->start_pose().position, start_pose_segment_idx); + pull_over_path.start_pose().position, start_pose_segment_idx); const size_t goal_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( - full_path.points, thread_safe_data_.get_modified_goal_pose()->goal_pose.position); + full_path.points, pull_over_path.modified_goal_pose().position); const double dist_to_parking_finish_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, - thread_safe_data_.get_modified_goal_pose()->goal_pose.position, goal_pose_segment_idx); + pull_over_path.modified_goal_pose().position, goal_pose_segment_idx); return {dist_to_parking_start_pose, dist_to_parking_finish_pose}; } @@ -1493,7 +1561,7 @@ void GoalPlannerModule::setParameters(const std::shared_ptr std::optional> { - if (thread_safe_data_.foundPullOverPath()) { + if (context_data.pull_over_path_opt) { return std::make_pair( - thread_safe_data_.get_pull_over_path()->start_pose(), "stop at selected start pose"); + context_data.pull_over_path_opt.value().start_pose(), "stop at selected start pose"); } if (thread_safe_data_.get_closest_start_pose()) { return std::make_pair( @@ -1666,8 +1734,16 @@ bool GoalPlannerModule::isStuck( const std::shared_ptr occupancy_grid_map, const GoalPlannerParameters & parameters) { + const bool found_pull_over_path = thread_safe_data_.foundPullOverPath(); + const std::optional pull_over_path_opt = + found_pull_over_path ? std::make_optional(*thread_safe_data_.get_pull_over_path()) + : std::nullopt; + const std::optional modified_goal_opt = + pull_over_path_opt + ? std::make_optional(pull_over_path_opt.value().modified_goal()) + : std::nullopt; const std::lock_guard lock(mutex_); - if (isOnModifiedGoal(planner_data->self_odometry->pose.pose, parameters)) { + if (isOnModifiedGoal(planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) { return false; } @@ -1676,19 +1752,13 @@ bool GoalPlannerModule::isStuck( return false; } - // not found safe path - if (!thread_safe_data_.foundPullOverPath()) { + if (!found_pull_over_path) { return true; } - // any path has never been found - if (!thread_safe_data_.get_pull_over_path()) { - return false; - } - - const auto pull_over_path = thread_safe_data_.get_pull_over_path(); + const auto & pull_over_path = pull_over_path_opt.value(); if (parameters.use_object_recognition) { - const auto path = pull_over_path->getCurrentPath(); + const auto & path = pull_over_path.getCurrentPath(); const auto curvatures = autoware::motion_utils::calcCurvature(path.points); if (goal_planner_utils::checkObjectsCollision( path, curvatures, static_target_objects, dynamic_target_objects, planner_data->parameters, @@ -1710,7 +1780,7 @@ bool GoalPlannerModule::isStuck( return false; } -bool GoalPlannerModule::hasFinishedCurrentPath() +bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1744,34 +1814,43 @@ bool GoalPlannerModule::hasFinishedCurrentPath() } // check if self pose is near the end of current path - const auto current_path_end = - thread_safe_data_.get_pull_over_path()->getCurrentPath().points.back(); + if (!ctx_data.pull_over_path_opt) { + return false; + } + const auto & current_path_end = + ctx_data.pull_over_path_opt.value().getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; return autoware::universe_utils::calcDistance2d(current_path_end, self_pose) < parameters_->th_arrived_distance; } bool GoalPlannerModule::isOnModifiedGoal( - const Pose & current_pose, const GoalPlannerParameters & parameters) const + const Pose & current_pose, const std::optional & modified_goal_opt, + const GoalPlannerParameters & parameters) const { - if (!thread_safe_data_.get_modified_goal_pose()) { + if (!modified_goal_opt) { return false; } - return calcDistance2d(current_pose, thread_safe_data_.get_modified_goal_pose()->goal_pose) < + return calcDistance2d(current_pose, modified_goal_opt.value().goal_pose) < parameters.th_arrived_distance; } -TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() +TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo(const PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto path = thread_safe_data_.get_pull_over_path()->full_path(); + if (!context_data.pull_over_path_opt) { + return {}; + } + const auto & pull_over_path = context_data.pull_over_path_opt.value(); + + const auto & path = pull_over_path.full_path(); if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info; const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto & start_pose = thread_safe_data_.get_pull_over_path()->start_pose(); - const auto & end_pose = thread_safe_data_.get_pull_over_path()->end_pose(); + const auto & start_pose = pull_over_path.start_pose(); + const auto & end_pose = pull_over_path.modified_goal_pose(); const auto shift_start_idx = autoware::motion_utils::findNearestIndex(path.points, start_pose.position); @@ -1785,7 +1864,7 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() return ignore_signal_.value() == id; }; - const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) { + const auto update_ignore_signal = [](const lanelet::Id & id, const bool is_ignore) { return is_ignore ? std::make_optional(id) : std::nullopt; }; @@ -1815,13 +1894,11 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() constexpr bool is_lane_change = false; constexpr bool is_pull_over = true; const bool override_ego_stopped_check = std::invoke([&]() { - const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); - if (planner_type_opt && planner_type_opt.value() == PullOverPlannerType::SHIFT) { + if (pull_over_path.type() == PullOverPlannerType::SHIFT) { return false; } constexpr double distance_threshold = 1.0; - const auto stop_point = - thread_safe_data_.get_pull_over_path()->partial_paths().front().points.back(); + const auto stop_point = pull_over_path.partial_paths().front().points.back(); const double distance_from_ego_to_stop_point = std::abs(autoware::motion_utils::calcSignedArcLength( path.points, stop_point.point.pose.position, current_pose.position)); @@ -1885,15 +1962,16 @@ bool GoalPlannerModule::hasEnoughDistance( return true; } -void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) const +void GoalPlannerModule::keepStoppedWithCurrentPath( + const PullOverContextData & ctx_data, PathWithLaneId & path) const { + const auto last_path_idx_increment_time = ctx_data.last_path_idx_increment_time; constexpr double keep_stop_time = 2.0; - if (!thread_safe_data_.get_last_path_idx_increment_time()) { + if (!last_path_idx_increment_time) { return; } - const auto time_diff = - (clock_->now() - *thread_safe_data_.get_last_path_idx_increment_time()).seconds(); + const auto time_diff = (clock_->now() - last_path_idx_increment_time.value()).seconds(); if (time_diff > keep_stop_time) { return; } @@ -2091,7 +2169,7 @@ bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) { const lanelet::ConstLanelets lanes = utils::transformToLanelets(generateDrivableLanes()); const Pose & start_pose = pull_over_path.start_pose(); - const Pose & end_pose = pull_over_path.end_pose(); + const Pose & end_pose = pull_over_path.modified_goal_pose(); return isCrossingPossible(start_pose, end_pose, lanes); } @@ -2125,7 +2203,7 @@ static std::vector filterOb const auto & target_object_types = params->object_types_to_check; PredictedObjects filtered_objects = utils::path_safety_checker::filterObjectsByVelocity( - *objects, ignore_object_velocity_threshold, false); + *objects, ignore_object_velocity_threshold, true); utils::path_safety_checker::filterObjectsByClass(filtered_objects, target_object_types); @@ -2153,7 +2231,8 @@ static std::vector filterOb bool GoalPlannerModule::isSafePath( const std::shared_ptr planner_data, const bool found_pull_over_path, - const std::optional & pull_over_path_opt, const GoalPlannerParameters & parameters, + const std::optional & pull_over_path_opt, const PathDecisionState & prev_data, + const GoalPlannerParameters & parameters, const std::shared_ptr & ego_predicted_path_params, const std::shared_ptr & objects_filtering_params, const std::shared_ptr & safety_check_params) const @@ -2240,7 +2319,6 @@ bool GoalPlannerModule::isSafePath( const auto filtered_objects = filterObjectsByWithinPolicy( dynamic_object, {merged_expanded_pull_over_lanes}, objects_filtering_params); - const auto prev_data = thread_safe_data_.get_prev_data(); const double hysteresis_factor = prev_data.is_stable_safe ? 1.0 : parameters.hysteresis_factor_expand_rate; @@ -2345,27 +2423,19 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) add(createPathMarkerArray( getPreviousModuleOutput().path, "previous_module_path", 0, 1.0, 0.0, 0.0)); - const auto last_previous_module_output = thread_safe_data_.get_last_previous_module_output(); - if (last_previous_module_output.has_value()) { - add(createPathMarkerArray( - last_previous_module_output.value().path, "last_previous_module_path", 0, 0.0, 1.0, 1.0)); - } - // Visualize path and related pose - if (thread_safe_data_.foundPullOverPath()) { - add(createPoseMarkerArray( - thread_safe_data_.get_pull_over_path()->start_pose(), "pull_over_start_pose", 0, 0.3, 0.3, - 0.9)); + if (context_data.pull_over_path_opt) { + const auto & pull_over_path = context_data.pull_over_path_opt.value(); + add( + createPoseMarkerArray(pull_over_path.start_pose(), "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( - thread_safe_data_.get_pull_over_path()->end_pose(), "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); - add(createPathMarkerArray( - thread_safe_data_.get_pull_over_path()->full_path(), "full_path", 0, 0.0, 0.5, 0.9)); - add(createPathMarkerArray( - thread_safe_data_.get_pull_over_path()->getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); + pull_over_path.modified_goal_pose(), "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); + add(createPathMarkerArray(pull_over_path.full_path(), "full_path", 0, 0.0, 0.5, 0.9)); + add(createPathMarkerArray(pull_over_path.getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); // visualize each partial path - for (size_t i = 0; i < thread_safe_data_.get_pull_over_path()->partial_paths().size(); ++i) { - const auto & partial_path = thread_safe_data_.get_pull_over_path()->partial_paths().at(i); + for (size_t i = 0; i < context_data.pull_over_path_opt.value().partial_paths().size(); ++i) { + const auto & partial_path = context_data.pull_over_path_opt.value().partial_paths().at(i); add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } @@ -2398,7 +2468,7 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) } // Visualize debug poses - const auto & debug_poses = thread_safe_data_.get_pull_over_path()->debug_poses; + const auto & debug_poses = pull_over_path.debug_poses; for (size_t i = 0; i < debug_poses.size(); ++i) { add(createPoseMarkerArray( debug_poses.at(i), "debug_pose_" + std::to_string(i), 0, 0.3, 0.3, 0.3)); @@ -2436,7 +2506,7 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) // visualize safety status maker { - const auto prev_data = thread_safe_data_.get_prev_data(); + const auto & prev_data = context_data.prev_state_for_debug; visualization_msgs::msg::MarkerArray marker_array{}; const auto color = prev_data.is_stable_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) : createMarkerColor(1.0, 0.0, 0.0, 0.99); @@ -2460,28 +2530,30 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) // Visualize planner type text { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = thread_safe_data_.foundPullOverPath() - ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = context_data.pull_over_path_opt ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); - marker.pose = thread_safe_data_.get_modified_goal_pose() - ? thread_safe_data_.get_modified_goal_pose()->goal_pose + marker.pose = context_data.pull_over_path_opt + ? context_data.pull_over_path_opt.value().modified_goal_pose() : planner_data_->self_odometry->pose.pose; - const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); - if (planner_type_opt) { - marker.text = magic_enum::enum_name(planner_type_opt.value()); - marker.text += - " " + std::to_string(thread_safe_data_.get_pull_over_path()->path_idx()) + "/" + - std::to_string(thread_safe_data_.get_pull_over_path()->partial_paths().size() - 1); + if (context_data.pull_over_path_opt) { + const auto & pull_over_path = context_data.pull_over_path_opt.value(); + marker.text = magic_enum::enum_name(pull_over_path.type()); + marker.text += " " + std::to_string(pull_over_path.path_idx()) + "/" + + std::to_string(pull_over_path.partial_paths().size() - 1); } + /* + TODO(soblin): disable until thread safe design is done if (isStuck( context_data.static_target_objects, context_data.dynamic_target_objects, planner_data_, occupancy_grid_map_, *parameters_)) { marker.text += " stuck"; - } else if (isStopped()) { + } + */ + if (isStopped()) { marker.text += " stopped"; } @@ -2510,30 +2582,12 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) } } -void GoalPlannerModule::printParkingPositionError() const -{ - const auto current_pose = planner_data_->self_odometry->pose.pose; - const double real_shoulder_to_map_shoulder = 0.0; - - const Pose goal_to_ego = - inverseTransformPose(current_pose, thread_safe_data_.get_modified_goal_pose()->goal_pose); - const double dx = goal_to_ego.position.x; - const double dy = goal_to_ego.position.y; - const double distance_from_real_shoulder = - real_shoulder_to_map_shoulder + parameters_->margin_from_boundary - dy; - RCLCPP_INFO( - getLogger(), "current pose to goal, dx:%f dy:%f dyaw:%f from_real_shoulder:%f", dx, dy, - autoware::universe_utils::rad2deg( - tf2::getYaw(current_pose.orientation) - - tf2::getYaw(thread_safe_data_.get_modified_goal_pose()->goal_pose.orientation)), - distance_from_real_shoulder); -} - bool GoalPlannerModule::needPathUpdate( const Pose & current_pose, const double path_update_duration, + const std::optional & modified_goal_opt, const GoalPlannerParameters & parameters) const { - return !isOnModifiedGoal(current_pose, parameters) && + return !isOnModifiedGoal(current_pose, modified_goal_opt, parameters) && hasEnoughTimePassedSincePathUpdate(path_update_duration); } @@ -2607,7 +2661,6 @@ void PathDecisionStateController::transit_state( found_pull_over_path, now, static_target_objects, dynamic_target_objects, modified_goal_opt, planner_data, occupancy_grid_map, is_current_safe, parameters, goal_searcher, is_activated, pull_over_path, ego_polygons_expanded); - prev_state_ = current_state_; current_state_ = next_state; } @@ -2622,10 +2675,7 @@ PathDecisionState PathDecisionStateController::get_next_state( const std::optional & pull_over_path_opt, std::vector & ego_polygons_expanded) const { - auto next_state = prev_state_; - - // update timestamp - next_state.stamp = now; + auto next_state = current_state_; // update safety if (!parameters.safety_check_params.enable_safety_check) { @@ -2648,7 +2698,6 @@ PathDecisionState PathDecisionStateController::get_next_state( // Once this function returns true, it will continue to return true thereafter if (next_state.state == PathDecisionState::DecisionKind::DECIDED) { - next_state.state = PathDecisionState::DecisionKind::DECIDED; return next_state; } @@ -2663,12 +2712,16 @@ PathDecisionState PathDecisionStateController::get_next_state( // If it is dangerous against dynamic objects before approval, do not determine the path. // This eliminates a unsafe path to be approved if (enable_safety_check && !next_state.is_stable_safe && !is_activated) { + RCLCPP_DEBUG( + logger_, + "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " + "approval"); next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; return next_state; } const auto & current_path = pull_over_path.getCurrentPath(); - if (prev_state_.state == PathDecisionState::DecisionKind::DECIDING) { + if (current_state_.state == PathDecisionState::DecisionKind::DECIDING) { const double hysteresis_factor = 0.9; // check goal pose collision @@ -2676,13 +2729,15 @@ PathDecisionState PathDecisionStateController::get_next_state( modified_goal_opt && !goal_searcher->isSafeGoalWithMarginScaleFactor( modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, planner_data, static_target_objects)) { + RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe"); next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + next_state.deciding_start_time = std::nullopt; return next_state; } // check current parking path collision const auto & parking_path = pull_over_path.parking_path(); - const std::vector parking_path_curvatures = pull_over_path.parking_path_curvatures(); + const auto & parking_path_curvatures = pull_over_path.parking_path_curvatures(); const double margin = parameters.object_recognition_collision_check_hard_margins.back() * hysteresis_factor; if (goal_planner_utils::checkObjectsCollision( @@ -2691,24 +2746,36 @@ PathDecisionState PathDecisionStateController::get_next_state( /*extract_static_objects=*/false, parameters.maximum_deceleration, parameters.object_recognition_collision_check_max_extra_stopping_margin, ego_polygons_expanded, true)) { + RCLCPP_DEBUG( + logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + next_state.deciding_start_time = std::nullopt; return next_state; } if (enable_safety_check && !next_state.is_stable_safe) { + RCLCPP_DEBUG( + logger_, + "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects"); next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; return next_state; } // if enough time has passed since deciding status starts, transition to DECIDED constexpr double check_collision_duration = 1.0; - const double elapsed_time_from_deciding = (now - prev_state_.stamp).seconds(); + const double elapsed_time_from_deciding = + (now - current_state_.deciding_start_time.value()).seconds(); if (elapsed_time_from_deciding > check_collision_duration) { + RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->DECIDED. has enough safe time passed"); next_state.state = PathDecisionState::DecisionKind::DECIDED; + next_state.deciding_start_time = std::nullopt; return next_state; } // if enough time has NOT passed since deciding status starts, keep DECIDING + RCLCPP_DEBUG( + logger_, "[DecidingPathStatus]: keep DECIDING. elapsed_time_from_deciding: %f", + elapsed_time_from_deciding); return next_state; } @@ -2730,10 +2797,15 @@ PathDecisionState PathDecisionStateController::get_next_state( // if object recognition for path collision check is enabled, transition to DECIDING to check // collision for a certain period of time. Otherwise, transition to DECIDED directly. if (parameters.use_object_recognition) { - next_state.state = PathDecisionState::DecisionKind::DECIDED; + RCLCPP_DEBUG( + logger_, + "[DecidingPathStatus]: NOT_DECIDED->DECIDING. start checking collision for certain " + "period of time"); + next_state.state = PathDecisionState::DecisionKind::DECIDING; + next_state.deciding_start_time = now; return next_state; } - return {PathDecisionState::DecisionKind::DECIDED, now}; + return {PathDecisionState::DecisionKind::DECIDED, std::nullopt}; } } // namespace autoware::behavior_path_planner 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 dbdac08c8778c..452090571ac45 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 @@ -57,8 +57,9 @@ FreespacePullOver::FreespacePullOver( } std::optional FreespacePullOver::plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) { const Pose & current_pose = planner_data->self_odometry->pose.pose; @@ -67,6 +68,7 @@ std::optional FreespacePullOver::plan( // offset goal pose to make straight path near goal for improving parking precision // todo: support straight path when using back constexpr double straight_distance = 1.0; + const auto & goal_pose = modified_goal_pose.goal_pose; const Pose end_pose = use_back_ ? goal_pose : autoware::universe_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); @@ -146,7 +148,7 @@ std::optional FreespacePullOver::plan( } auto pull_over_path_opt = PullOverPath::create( - getPlannerType(), goal_id, id, partial_paths, current_pose, goal_pose, + getPlannerType(), id, partial_paths, current_pose, modified_goal_pose, pairs_terminal_velocity_and_accel); if (!pull_over_path_opt) { return {}; 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 6c4aee5b96abf..09be040019338 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 @@ -38,11 +38,13 @@ GeometricPullOver::GeometricPullOver( } std::optional GeometricPullOver::plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) { const auto & route_handler = planner_data->route_handler; + const auto & goal_pose = modified_goal_pose.goal_pose; // prepare road nad shoulder lanes const auto road_lanes = utils::getExtendedCurrentLanes( planner_data, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, @@ -73,8 +75,8 @@ std::optional GeometricPullOver::plan( if (lane_departure_checker_.checkPathWillLeaveLane(lanes, arc_path)) return {}; auto pull_over_path_opt = PullOverPath::create( - getPlannerType(), goal_id, id, planner_.getPaths(), planner_.getStartPose(), - planner_.getArcEndPose(), planner_.getPairsTerminalVelocityAndAccel()); + getPlannerType(), id, planner_.getPaths(), planner_.getStartPose(), modified_goal_pose, + planner_.getPairsTerminalVelocityAndAccel()); if (!pull_over_path_opt) { return {}; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp index f6535e7adb8f8..63610f5ac31f7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp @@ -18,8 +18,9 @@ namespace autoware::behavior_path_planner { std::optional PullOverPath::create( - const PullOverPlannerType & type, const size_t goal_id, const size_t id, - const std::vector & partial_paths, const Pose & start_pose, const Pose & end_pose, + const PullOverPlannerType & type, const size_t id, + const std::vector & partial_paths, const Pose & start_pose, + const GoalCandidate & modified_goal_pose, const std::vector> & pairs_terminal_velocity_and_accel) { if (partial_paths.empty()) { @@ -73,20 +74,19 @@ std::optional PullOverPath::create( double parking_path_max_curvature{}; std::tie(full_path_curvatures, full_path_max_curvature) = calculateCurvaturesAndMax(full_path); std::tie(parking_path_curvatures, parking_path_max_curvature) = - calculateCurvaturesAndMax(full_path); + calculateCurvaturesAndMax(parking_path); return PullOverPath( - type, goal_id, id, start_pose, end_pose, partial_paths, full_path, parking_path, + type, id, start_pose, modified_goal_pose, partial_paths, full_path, parking_path, full_path_curvatures, parking_path_curvatures, full_path_max_curvature, parking_path_max_curvature, pairs_terminal_velocity_and_accel); } PullOverPath::PullOverPath(const PullOverPath & other) : type_(other.type_), - goal_id_(other.goal_id_), + modified_goal_pose_(other.modified_goal_pose_), id_(other.id_), start_pose_(other.start_pose_), - end_pose_(other.end_pose_), partial_paths_(other.partial_paths_), full_path_(other.full_path_), parking_path_(other.parking_path_), @@ -100,18 +100,17 @@ PullOverPath::PullOverPath(const PullOverPath & other) } PullOverPath::PullOverPath( - const PullOverPlannerType & type, const size_t goal_id, const size_t id, const Pose & start_pose, - const Pose & end_pose, const std::vector & partial_paths, + const PullOverPlannerType & type, const size_t id, const Pose & start_pose, + const GoalCandidate & modified_goal_pose, const std::vector & partial_paths, const PathWithLaneId & full_path, const PathWithLaneId & parking_path, const std::vector & full_path_curvatures, const std::vector & parking_path_curvatures, const double full_path_max_curvature, const double parking_path_max_curvature, const std::vector> & pairs_terminal_velocity_and_accel) : type_(type), - goal_id_(goal_id), + modified_goal_pose_(modified_goal_pose), id_(id), start_pose_(start_pose), - end_pose_(end_pose), partial_paths_(partial_paths), full_path_(full_path), parking_path_(parking_path), 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 645d74b6385da..f70dbbd9c1e50 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 @@ -18,6 +18,7 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include #include #include @@ -35,8 +36,9 @@ ShiftPullOver::ShiftPullOver( { } std::optional ShiftPullOver::plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output) { const auto & route_handler = planner_data->route_handler; const double min_jerk = parameters_.minimum_lateral_jerk; @@ -59,7 +61,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( - goal_id, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, goal_pose, + modified_goal_pose, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, lateral_jerk); if (!pull_over_path) continue; return *pull_over_path; @@ -127,14 +129,16 @@ std::optional ShiftPullOver::cropPrevModulePath( } std::optional ShiftPullOver::generatePullOverPath( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, + 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 lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, - const double lateral_jerk) const + const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const { const double pull_over_velocity = parameters_.pull_over_velocity; const double after_shift_straight_distance = parameters_.after_shift_straight_distance; + const auto & goal_pose = goal_candidate.goal_pose; + // shift end pose is longitudinal offset from goal pose to improve parking angle accuracy const Pose shift_end_pose = autoware::universe_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); @@ -178,7 +182,7 @@ std::optional ShiftPullOver::generatePullOverPath( .y; // calculate shift start pose on road lane - const double pull_over_distance = PathShifter::calcLongitudinalDistFromJerk( + const double pull_over_distance = autoware::motion_utils::calc_longitudinal_dist_from_jerk( shift_end_road_to_target_distance, lateral_jerk, pull_over_velocity); const double before_shifted_pull_over_distance = calcBeforeShiftedArcLength( processed_prev_module_path.value(), pull_over_distance, shift_end_road_to_target_distance); @@ -256,8 +260,8 @@ std::optional ShiftPullOver::generatePullOverPath( // set pull over path auto pull_over_path_opt = PullOverPath::create( - getPlannerType(), goal_id, id, {shifted_path.path}, path_shifter.getShiftLines().front().start, - path_shifter.getShiftLines().front().end, {std::make_pair(pull_over_velocity, 0)}); + getPlannerType(), id, {shifted_path.path}, path_shifter.getShiftLines().front().start, + goal_candidate, {std::make_pair(pull_over_velocity, 0)}); if (!pull_over_path_opt) { return {}; 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 18a797976161c..a52d842b5dcb9 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 @@ -195,7 +195,7 @@ if (max_lane_change_length > ego's distance to the end of the current lanes.) t stop endif -if (isVehicleStuck(current_lanes)) then (yes) +if (ego is stuck in the current lanes) then (yes) :Return **sampled acceleration values**; stop else (no) @@ -540,14 +540,65 @@ The following figure illustrates when the lane is blocked in multiple lane chang ![multiple-lane-changes](./images/lane_change-when_cannot_change_lanes.png) -#### Stopping position when an object exists ahead +### Stopping behavior -When an obstacle is in front of the ego vehicle, stop with keeping a distance for lane change. -The position to be stopped depends on the situation, such as when the lane change is blocked by the target lane obstacle, or when the lane change is not needed immediately.The following shows the division in that case. +The stopping behavior of the ego vehicle is determined based on various factors, such as the number of lane changes required, the presence of obstacles, and the position of blocking objects in relation to the lane change plan. The objective is to choose a suitable stopping point that allows for a safe and effective lane change while adapting to different traffic scenarios. -##### When the ego vehicle is near the end of the lane change +The following flowchart and subsections explain the conditions for deciding where to insert a stop point when an obstacle is ahead. -Regardless of the presence or absence of objects in the lane change target lane, stop by keeping the distance necessary for lane change to the object ahead. +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +title Inserting Stop Point + +start +if (number of lane changes is zero?) then (YES) +stop +else (NO) +endif + +if (do we want to insert stop point in current lanes?) then (NO) +#LightPink:Insert stop point at next lane change terminal start.; +stop +else (YES) +endif + +if (Is there leading object in the current lanes that blocks ego's path?) then (NO) +#LightPink:Insert stop point at terminal stop.; +stop +else (YES) +endif + +if (Blocking object's position is after target lane's start position?) then (NO) +#LightPink:Insert stop at the target lane's start position.; +stop +else (YES) +endif + +if (Blocking object's position is before terminal stop position?) then (NO) +#LightPink:Insert stop at the terminal stop position; +stop +else (YES) +endif + +if (Are there target lane objects between the ego and the blocking object?) then (NO) +#LightGreen:Insert stop behind the blocking object; +stop +else (YES) +#LightPink:Insert stop at terminal stop; +stop +@enduml +``` + +#### Ego vehicle's stopping position when an object exists ahead + +When the ego vehicle encounters an obstacle ahead, it stops while maintaining a safe distance to prepare for a possible lane change. The exact stopping position depends on factors like whether the target lane is clear or if the lane change needs to be delayed. The following explains how different stopping scenarios are handled: + +##### When the near the end of the lane change + +Whether the target lane has obstacles or is clear, the ego vehicle stops while keeping a safe distance from the obstacle ahead, ensuring there is enough room for the lane change. ![stop_at_terminal_no_block](./images/lane_change-stop_at_terminal_no_block.drawio.svg) @@ -555,20 +606,40 @@ Regardless of the presence or absence of objects in the lane change target lane, ##### When the ego vehicle is not near the end of the lane change -If there are NO objects in the lane change section of the target lane, stop by keeping the distance necessary for lane change to the object ahead. +The ego vehicle stops while maintaining a safe distance from the obstacle ahead, ensuring there is enough space for a lane change. + +![stop_not_at_terminal_no_blocking_object](./images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg) + +#### Ego vehicle's stopping position when an object exists in the lane changing section + +If there are objects within the lane change section of the target lane, the ego vehicle stops closer to the obstacle ahead, without maintaining the usual distance for a lane change. + +##### When near the end of the lane change + +Regardless of whether there are obstacles in the target lane, the ego vehicle stops while keeping a safe distance from the obstacle ahead, allowing for the lane change. + +![stop_at_terminal_no_block](./images/lane_change-stop_at_terminal_no_block.drawio.svg) + +![stop_at_terminal](./images/lane_change-stop_at_terminal.drawio.svg) + +##### When not near the end of the lane change + +If there are no obstacles in the lane change section of the target lane, the ego vehicle stops while keeping a safe distance from the obstacle ahead to accommodate the lane change. ![stop_not_at_terminal_no_blocking_object](./images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg) -If there are objects in the lane change section of the target lane, stop WITHOUT keeping the distance necessary for lane change to the object ahead. +If there are obstacles within the lane change section of the target lane, the ego vehicle stops closer to the obstacle ahead, without keeping the usual distance needed for a lane change. ![stop_not_at_terminal](./images/lane_change-stop_not_at_terminal.drawio.svg) -##### When the target lane is far away +#### When the target lane is far away -When the target lane for lane change is far away and not next to the current lane, do not keep the distance necessary for lane change to the object ahead. +If the target lane for the lane change is far away and not next to the current lane, the ego vehicle stops closer to the obstacle ahead, as maintaining the usual distance for a lane change is not necessary. ![stop_far_from_target_lane](./images/lane_change-stop_far_from_target_lane.drawio.svg) +![stop_not_at_terminal](./images/lane_change-stop_not_at_terminal.drawio.svg) + ### Lane Change When Stuck The ego vehicle is considered stuck if it is stopped and meets any of the following conditions: 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 f76d776150875..3b414235a9f54 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 @@ -70,7 +70,9 @@ class NormalLaneChange : public LaneChangeBase void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; - void insertStopPoint(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; + void insert_stop_point(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; + + void insert_stop_point_on_current_lanes(PathWithLaneId & path); PathWithLaneId getReferencePath() const override; @@ -89,10 +91,6 @@ class NormalLaneChange : public LaneChangeBase bool isRequiredStop(const bool is_trailing_object) override; - bool isNearEndOfCurrentLanes( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const double threshold) const override; - bool hasFinishedLaneChange() const override; bool isAbleToReturnCurrentLane() const override; @@ -116,8 +114,7 @@ class NormalLaneChange : public LaneChangeBase TurnSignalInfo get_current_turn_signal_info() const final; protected: - lanelet::ConstLanelets getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, Direction direction) const override; + lanelet::ConstLanelets get_lane_change_lanes(const lanelet::ConstLanelets & current_lanes) const; int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; @@ -127,9 +124,7 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; - std::vector calcPrepareDuration( - const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) const; + std::vector calc_prepare_durations() const; lane_change::TargetObjects getTargetObjects( const FilteredByLanesExtendedObjects & predicted_objects, @@ -142,30 +137,28 @@ class NormalLaneChange : public LaneChangeBase FilteredByLanesObjects filterObjectsByLanelets( const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const; - PathWithLaneId getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double backward_path_length, - const double prepare_length) const override; + bool get_prepare_segment( + PathWithLaneId & prepare_segment, const double prepare_length) const override; PathWithLaneId getTargetSegment( const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, const double lane_changing_length, const double lane_changing_velocity, const double buffer_for_next_lane_change) const; - bool hasEnoughLength( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const; + 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; bool get_lane_change_paths(LaneChangePaths & candidate_paths) const; LaneChangePath get_candidate_path( const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, - const Pose & lc_start_pose, const double target_lane_length, const double shift_length, - const double next_lc_buffer, const bool is_goal_in_route) const; + const Pose & lc_start_pose, const double shift_length) const; bool check_candidate_path_safety( - const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, - const double lane_change_buffer, const bool is_stuck) const; + const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const; std::optional calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, @@ -189,15 +182,9 @@ class NormalLaneChange : public LaneChangeBase const std::vector & ego_predicted_path, const RSSparams & selected_rss_param, CollisionCheckDebugMap & debug_data) const; - //! @brief Check if the ego vehicle is in stuck by a stationary obstacle. - //! @param obstacle_check_distance Distance to check ahead for any objects that might be - //! obstructing ego path. It makes sense to use values like the maximum lane change distance. - bool isVehicleStuck( - const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const; - double get_max_velocity_for_safety_check() const; - bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const; + bool is_ego_stuck() const; /** * @brief Checks if the given pose is a valid starting point for a lane change. @@ -224,7 +211,7 @@ class NormalLaneChange : public LaneChangeBase std::pair calcCurrentMinMaxAcceleration() const; - void setStopPose(const Pose & stop_pose); + void set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path); void updateStopTime(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index 72a40caca1d6a..aaf9df7144d74 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -112,10 +112,6 @@ class LaneChangeBase virtual PathSafetyStatus evaluateApprovedPathWithUnsafeHysteresis( PathSafetyStatus approve_path_safety_status) = 0; - virtual bool isNearEndOfCurrentLanes( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const double threshold) const = 0; - virtual bool isStoppedAtRedTrafficLight() const = 0; virtual bool calcAbortPath() = 0; @@ -131,7 +127,7 @@ class LaneChangeBase virtual void updateSpecialData() {} - virtual void insertStopPoint( + virtual void insert_stop_point( [[maybe_unused]] const lanelet::ConstLanelets & lanelets, [[maybe_unused]] PathWithLaneId & path) { @@ -238,17 +234,13 @@ class LaneChangeBase protected: virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; - virtual PathWithLaneId getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double backward_path_length, - const double prepare_length) const = 0; + virtual bool get_prepare_segment( + PathWithLaneId & prepare_segment, const double prepare_length) const = 0; virtual bool isValidPath(const PathWithLaneId & path) const = 0; virtual bool isAbleToStopSafely() const = 0; - virtual lanelet::ConstLanelets getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, Direction direction) const = 0; - virtual TurnSignalInfo get_terminal_turn_signal_info() const = 0; TurnSignalInfo get_turn_signal(const Pose & start, const Pose & end) const diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index fc5e78e44b77f..e4f70f54a8437 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -338,7 +338,13 @@ struct TransientData double max_prepare_length{ std::numeric_limits::max()}; // maximum prepare length, starting from ego's base link + double target_lane_length{std::numeric_limits::min()}; + + 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 + bool is_ego_near_current_terminal_start{false}; + bool is_ego_stuck{false}; }; using RouteHandlerPtr = std::shared_ptr; 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 363fa970f54c4..729eaa5ca8268 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 @@ -51,6 +51,7 @@ using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LanesPolygon; +using behavior_path_planner::lane_change::ModuleType; using behavior_path_planner::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -58,6 +59,8 @@ using geometry_msgs::msg::Twist; using path_safety_checker::CollisionCheckDebugMap; using tier4_planning_msgs::msg::PathWithLaneId; +bool is_mandatory_lane_change(const ModuleType lc_type); + double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); @@ -79,9 +82,7 @@ std::vector replaceWithSortedIds( const std::vector & original_lane_ids, const std::vector> & sorted_lane_ids); -std::vector> getSortedLaneIds( - const RouteHandler & route_handler, const Pose & current_pose, - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); +std::vector> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr); lanelet::ConstLanelets getTargetNeighborLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, @@ -104,12 +105,9 @@ ShiftLine get_lane_changing_shift_line( const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, const PathWithLaneId & reference_path, const double shift_length); -PathWithLaneId getReferencePathFromTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double target_lane_length, - const double lane_changing_length, const double forward_path_length, - const double resample_interval, const bool is_goal_in_route, - const double next_lane_change_buffer); +PathWithLaneId get_reference_path_from_target_Lane( + const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, + const double lane_changing_length, const double resample_interval); std::vector generateDrivableLanes( const std::vector & original_drivable_lanes, const RouteHandler & route_handler, @@ -123,9 +121,9 @@ double getLateralShift(const LaneChangePath & path); CandidateOutput assignToCandidate( const LaneChangePath & lane_change_path, const Point & ego_position); -std::optional getLaneChangeTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const LaneChangeModuleType type, const Direction & direction); + +std::optional get_lane_change_target_lane( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes); std::vector convertToPredictedPath( const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose, @@ -145,8 +143,7 @@ bool isParkedObject( bool passed_parked_objects( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, const double minimum_lane_change_length, - CollisionCheckDebugMap & object_debug); + const std::vector & objects, CollisionCheckDebugMap & object_debug); std::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, @@ -269,6 +266,48 @@ ExtendedPredictedObjects transform_to_extended_objects( double get_distance_to_next_regulatory_element( const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk = false, const bool ignore_intersection = false); + +/** + * @brief Calculates the minimum distance to a stationary object in the current lanes. + * + * This function determines the closest distance from the ego vehicle to a stationary object + * in the current lanes. It checks if the object is within the stopping criteria based on its + * velocity and calculates the distance while accounting for the object's size. Only objects + * positioned after the specified distance to the target lane's start are considered. + * + * @param common_data_ptr Pointer to the common data structure containing parameters for lane + * change. + * @param filtered_objects A collection of objects filtered by lanes, including those in the current + * lane. + * @param dist_to_target_lane_start The distance to the start of the target lane from the ego + * vehicle. + * @param path The current path of the ego vehicle, containing path points and lane information. + * @return The minimum distance to a stationary object in the current lanes. If no valid object is + * found, returns the maximum possible double value. + */ +double get_min_dist_to_current_lanes_obj( + const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, + const double dist_to_target_lane_start, const PathWithLaneId & path); + +/** + * @brief Checks if there is an object in the target lane that influences the decision to insert a + * stop point. + * + * This function determines whether any objects exist in the target lane that would affect + * the decision to place a stop point behind a blocking object in the current lane. + * + * @param common_data_ptr Pointer to the common data structure containing parameters for the lane + * change. + * @param filtered_objects A collection of objects filtered by lanes, including those in the target + * lane. + * @param stop_arc_length The arc length at which the ego vehicle is expected to stop. + * @param path The current path of the ego vehicle, containing path points and lane information. + * @return true if there is an object in the target lane that influences the stop point decision; + * otherwise, false. + */ +bool has_blocking_target_object( + const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, + const double stop_arc_length, const PathWithLaneId & path); } // 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/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 0b647acd44268..8fb1459f62c98 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 @@ -76,8 +76,8 @@ void LaneChangeInterface::updateData() universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); - module_type_->update_transient_data(); module_type_->update_filtered_objects(); + module_type_->update_transient_data(); module_type_->updateSpecialData(); if (isWaitingApproval() || module_type_->isAbortState()) { @@ -154,7 +154,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() BehaviorModuleOutput out = getPreviousModuleOutput(); - module_type_->insertStopPoint(module_type_->get_current_lanes(), out.path); + module_type_->insert_stop_point(module_type_->get_current_lanes(), out.path); out.turn_signal_info = module_type_->get_current_turn_signal_info(); const auto & lane_change_debug = module_type_->getDebugData(); 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 a91aaba86bb1a..5961435085346 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 @@ -23,6 +23,8 @@ #include "autoware/behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include +#include #include #include #include @@ -69,7 +71,7 @@ void NormalLaneChange::update_lanes(const bool is_approved) return; } - const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); + const auto target_lanes = get_lane_change_lanes(current_lanes); if (target_lanes.empty()) { return; } @@ -129,9 +131,21 @@ void NormalLaneChange::update_transient_data() transient_data.max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); + transient_data.target_lane_length = + lanelet::utils::getLaneletLength2d(common_data_ptr_->lanes_ptr->target); + + transient_data.current_lanes_ego_arc = lanelet::utils::getArcCoordinates( + common_data_ptr_->lanes_ptr->current, common_data_ptr_->get_ego_pose()); + + transient_data.target_lanes_ego_arc = lanelet::utils::getArcCoordinates( + common_data_ptr_->lanes_ptr->target, common_data_ptr_->get_ego_pose()); + transient_data.is_ego_near_current_terminal_start = transient_data.dist_to_terminal_start < transient_data.max_prepare_length; + updateStopTime(); + transient_data.is_ego_stuck = is_ego_stuck(); + RCLCPP_DEBUG( logger_, "acc - min: %.4f, max: %.4f", transient_data.acc.min, transient_data.acc.max); RCLCPP_DEBUG( @@ -159,15 +173,13 @@ void NormalLaneChange::update_filtered_objects() void NormalLaneChange::updateLaneChangeStatus() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - updateStopTime(); const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); // Update status status_.is_valid_path = found_valid_path; status_.is_safe = found_safe_path; - const auto arclength_start = lanelet::utils::getArcCoordinates(get_target_lanes(), getEgoPose()); - status_.start_distance = arclength_start.length; + status_.start_distance = common_data_ptr_->transient_data.target_lanes_ego_arc.length; status_.lane_change_path.path.header = getRouteHeader(); } @@ -233,10 +245,7 @@ bool NormalLaneChange::is_near_regulatory_element() const { if (!common_data_ptr_ || !common_data_ptr_->is_data_available()) return false; - const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - const auto dist_to_terminal_start = common_data_ptr_->transient_data.dist_to_terminal_start; - - if (dist_to_terminal_start <= max_prepare_length) return false; + if (common_data_ptr_->transient_data.is_ego_near_current_terminal_start) return false; const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; @@ -244,8 +253,9 @@ bool NormalLaneChange::is_near_regulatory_element() const RCLCPP_DEBUG(logger_, "Stop time is over threshold. Ignore crosswalk and intersection checks."); } - return max_prepare_length > utils::lane_change::get_distance_to_next_regulatory_element( - common_data_ptr_, only_tl, only_tl); + return common_data_ptr_->transient_data.max_prepare_length > + utils::lane_change::get_distance_to_next_regulatory_element( + common_data_ptr_, only_tl, only_tl); } bool NormalLaneChange::isStoppedAtRedTrafficLight() const @@ -337,7 +347,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const } const auto terminal_path = - calcTerminalLaneChangePath(current_lanes, getLaneChangeLanes(current_lanes, direction_)); + calcTerminalLaneChangePath(current_lanes, get_lane_change_lanes(current_lanes)); if (!terminal_path) { RCLCPP_DEBUG(logger_, "Terminal path not found. Returning previous module's path as output."); return prev_module_output_; @@ -362,14 +372,14 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!status_.is_valid_path) { RCLCPP_DEBUG(logger_, "No valid path found. Returning previous module's path as output."); - insertStopPoint(get_current_lanes(), prev_module_output_.path); + insert_stop_point(get_current_lanes(), prev_module_output_.path); return prev_module_output_; } auto output = prev_module_output_; if (isAbortState() && abort_path_) { output.path = abort_path_->path; - insertStopPoint(get_current_lanes(), output.path); + insert_stop_point(get_current_lanes(), output.path); } else { output.path = status_.lane_change_path.path; @@ -387,10 +397,9 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() output.path.points, output.path.points.front().point.pose.position, getEgoPosition()); const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); - const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, output.path); - setStopPose(stop_point.point.pose); + set_stop_pose(stop_dist + current_dist, output.path); } else { - insertStopPoint(get_target_lanes(), output.path); + insert_stop_point(get_target_lanes(), output.path); } } @@ -424,7 +433,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) c current_drivable_area_info, prev_module_output_.drivable_area_info); } -void NormalLaneChange::insertStopPoint( +void NormalLaneChange::insert_stop_point( const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -438,131 +447,109 @@ void NormalLaneChange::insertStopPoint( return; } - const auto [_, lanes_dist_buffer] = - calculation::calc_lc_length_and_dist_buffer(common_data_ptr_, lanelets); + const auto & current_lanes = get_current_lanes(); + const auto is_current_lane = lanelets.front().id() == current_lanes.front().id() && + lanelets.back().id() == current_lanes.back().id(); + + // if input is not current lane, we can just insert the points at terminal. + if (!is_current_lane) { + const auto arc_length_to_stop_pose = motion_utils::calcArcLength(path.points) - + common_data_ptr_->transient_data.next_dist_buffer.min; + set_stop_pose(arc_length_to_stop_pose, path); + return; + } - const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { - return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); + insert_stop_point_on_current_lanes(path); +} + +void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) +{ + 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); }; - // If lanelets.back() is in goal route section, get distance to goal. - // Otherwise, get distance to end of lane. - double distance_to_terminal = 0.0; - if (route_handler->isInGoalRouteSection(lanelets.back())) { - const auto goal = route_handler->getGoalPose(); - distance_to_terminal = getDistanceAlongLanelet(goal); - } else { - distance_to_terminal = utils::getDistanceToEndOfLane(path.points.front().point.pose, lanelets); - } + 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 double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const auto target_objects = filterObjects(); - double stopping_distance = distance_to_terminal - lanes_dist_buffer.min - stop_point_buffer; + 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 & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); - if (utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), planner_data_->parameters)) { - const double distance_to_last_fit_width = - utils::lane_change::calculation::calc_dist_to_last_fit_width( - lanelets, path.points.front().point.pose, planner_data_->parameters); - stopping_distance = std::min(stopping_distance, distance_to_last_fit_width); - } + 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); - const auto & lc_start_point = status_.lane_change_path.info.lane_changing_start; + const auto distance_to_last_fit_width = std::invoke([&]() -> double { + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); + 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); + } + return std::numeric_limits::max(); + }); - if (!is_valid_start_point(common_data_ptr_, lc_start_point)) { - const auto stop_point = utils::insertStopPoint(stopping_distance, path); - setStopPose(stop_point.point.pose); + const auto dist_to_terminal_stop = std::min(dist_to_terminal_start, distance_to_last_fit_width); + if (filtered_objects_.current_lane.empty()) { + set_stop_pose(dist_to_terminal_stop, path); return; } - // calculate minimum distance from path front to the stationary object on the ego lane. - const auto distance_to_ego_lane_obj = [&]() -> double { - double distance_to_obj = distance_to_terminal; - const double distance_to_ego = getDistanceAlongLanelet(getEgoPose()); - - for (const auto & object : target_objects.current_lane) { - // check if stationary - const auto obj_v = std::abs(object.initial_twist.linear.x); - if (obj_v > lane_change_parameters_->stopped_object_velocity_threshold) { - continue; - } + 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); + }); - // calculate distance from path front to the stationary object polygon on the ego lane. - const auto polygon = - autoware::universe_utils::toPolygon2d(object.initial_pose, object.shape).outer(); - for (const auto & polygon_p : polygon) { - const auto p_fp = autoware::universe_utils::toMsg(polygon_p.to_3d()); - const auto lateral_fp = autoware::motion_utils::calcLateralOffset(path.points, p_fp); + const auto arc_length_to_current_obj = utils::lane_change::get_min_dist_to_current_lanes_obj( + common_data_ptr_, filtered_objects_, dist_to_target_lane_start, path); - // ignore if the point is around the ego path - if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) { - continue; - } + // margin with leading vehicle + // consider rss distance when the LC need to avoid obstacles + const auto rss_dist = calcRssDistance( + 0.0, lc_param_ptr->minimum_lane_changing_velocity, lc_param_ptr->rss_params_for_parked); - const double current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp); + const auto stop_margin = transient_data.lane_changing_length.min + + lc_param_ptr->backward_length_buffer_for_blocking_object + rss_dist + + bpp_param_ptr->base_link2front; + const auto stop_arc_length_behind_obj = arc_length_to_current_obj - stop_margin; - // ignore backward object - if (current_distance_to_obj < distance_to_ego) { - continue; - } - distance_to_obj = std::min(distance_to_obj, current_distance_to_obj); - } - } - return distance_to_obj; - }(); - - // Need to stop before blocking obstacle - if (distance_to_ego_lane_obj < distance_to_terminal) { - // consider rss distance when the LC need to avoid obstacles - const auto rss_dist = calcRssDistance( - 0.0, lane_change_parameters_->minimum_lane_changing_velocity, - lane_change_parameters_->rss_params); - - const auto stopping_distance_for_obj = - distance_to_ego_lane_obj - lanes_dist_buffer.min - - lane_change_parameters_->backward_length_buffer_for_blocking_object - rss_dist - - getCommonParam().base_link2front; - - // If the target lane in the lane change section is blocked by a stationary obstacle, there - // is no reason for stopping with a lane change margin. Instead, stop right behind the - // obstacle. - // ---------------------------------------------------------- - // [obj]> - // ---------------------------------------------------------- - // [ego]> | <--- lane change margin ---> [obj]> - // ---------------------------------------------------------- - const bool has_blocking_target_lane_obj = std::any_of( - target_objects.target_lane_leading.begin(), target_objects.target_lane_leading.end(), - [&](const auto & o) { - const auto v = std::abs(o.initial_twist.linear.x); - if (v > lane_change_parameters_->stopped_object_velocity_threshold) { - return false; - } + if (stop_arc_length_behind_obj < dist_to_target_lane_start) { + set_stop_pose(dist_to_target_lane_start, path); + return; + } - // target_objects includes objects out of target lanes, so filter them out - if (!boost::geometry::intersects( - autoware::universe_utils::toPolygon2d(o.initial_pose, o.shape).outer(), - lanelet::utils::combineLaneletsShape(get_target_lanes()) - .polygon2d() - .basicPolygon())) { - return false; - } + if (stop_arc_length_behind_obj > dist_to_terminal_stop) { + set_stop_pose(dist_to_terminal_stop, path); + return; + } - const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose); - return stopping_distance_for_obj < distance_to_target_lane_obj && - distance_to_target_lane_obj < distance_to_ego_lane_obj; - }); + // If the target lane in the lane change section is blocked by a stationary obstacle, there + // is no reason for stopping with a lane change margin. Instead, stop right behind the + // obstacle. + // ---------------------------------------------------------- + // [obj]> + // ---------------------------------------------------------- + // [ego]> | <--- stop margin ---> [obj]> + // ---------------------------------------------------------- + const auto has_blocking_target_lane_obj = utils::lane_change::has_blocking_target_object( + common_data_ptr_, filtered_objects_, stop_arc_length_behind_obj, path); - if (!has_blocking_target_lane_obj) { - stopping_distance = stopping_distance_for_obj; - } + if (has_blocking_target_lane_obj || stop_arc_length_behind_obj <= 0.0) { + set_stop_pose(dist_to_terminal_stop, path); + return; } - if (stopping_distance > 0.0) { - const auto stop_point = utils::insertStopPoint(stopping_distance, path); - setStopPose(stop_point.point.pose); - } + set_stop_pose(stop_arc_length_behind_obj, path); } PathWithLaneId NormalLaneChange::getReferencePath() const @@ -593,12 +580,13 @@ std::optional NormalLaneChange::extendPath() } auto & target_lanes = common_data_ptr_->lanes_ptr->target; - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); - const auto dist_in_target = lanelet::utils::getArcCoordinates(target_lanes, getEgoPose()); + const auto & transient_data = common_data_ptr_->transient_data; const auto forward_path_length = getCommonParam().forward_path_length; - if ((target_lane_length - dist_in_target.length) > forward_path_length) { + if ( + (transient_data.target_lane_length - transient_data.target_lanes_ego_arc.length) > + forward_path_length) { return std::nullopt; } @@ -627,15 +615,7 @@ std::optional NormalLaneChange::extendPath() return getRouteHandler()->getGoalPose(); } - Pose back_pose; - const auto back_point = - lanelet::utils::conversion::toGeomMsgPt(next_lane.centerline2d().back()); - const double front_yaw = lanelet::utils::getLaneletAngle(next_lane, back_point); - back_pose.position = back_point; - tf2::Quaternion tf_quat; - tf_quat.setRPY(0, 0, front_yaw); - back_pose.orientation = tf2::toMsg(tf_quat); - return back_pose; + return utils::to_geom_msg_pose(next_lane.centerline2d().back(), next_lane); }); const auto dist_to_target_pose = @@ -646,6 +626,7 @@ std::optional NormalLaneChange::extendPath() return getRouteHandler()->getCenterLinePath( target_lanes, dist_to_end_of_path, dist_to_target_pose); } + void NormalLaneChange::resetParameters() { is_abort_path_approved_ = false; @@ -662,11 +643,10 @@ void NormalLaneChange::resetParameters() TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto & pose = getEgoPose(); const auto & current_lanes = get_current_lanes(); const auto & shift_line = status_.lane_change_path.info.shift_line; const auto & shift_path = status_.lane_change_path.shifted_path; - const auto current_shift_length = lanelet::utils::getArcCoordinates(current_lanes, pose).distance; + const auto current_shift_length = common_data_ptr_->transient_data.current_lanes_ego_arc.distance; constexpr bool is_driving_forward = true; // The getBehaviorTurnSignalInfo method expects the shifted line to be generated off of the ego's // current lane, lane change is different, so we set this flag to false. @@ -680,8 +660,8 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const return new_signal; } -lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, Direction direction) const +lanelet::ConstLanelets NormalLaneChange::get_lane_change_lanes( + const lanelet::ConstLanelets & current_lanes) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (current_lanes.empty()) { @@ -690,69 +670,27 @@ lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( // Get lane change lanes const auto & route_handler = getRouteHandler(); - const auto lane_change_lane = utils::lane_change::getLaneChangeTargetLane( - *getRouteHandler(), current_lanes, type_, direction); + const auto lane_change_lane = + utils::lane_change::get_lane_change_target_lane(common_data_ptr_, current_lanes); if (!lane_change_lane) { return {}; } - const auto front_pose = std::invoke([&lane_change_lane]() { - const auto & p = lane_change_lane->centerline().front(); - const auto front_point = lanelet::utils::conversion::toGeomMsgPt(p); - const auto front_yaw = lanelet::utils::getLaneletAngle(*lane_change_lane, front_point); - geometry_msgs::msg::Pose front_pose; - front_pose.position = front_point; - tf2::Quaternion quat; - quat.setRPY(0, 0, front_yaw); - front_pose.orientation = tf2::toMsg(quat); - return front_pose; - }); - const auto forward_length = std::invoke([&]() { + const auto front_pose = + utils::to_geom_msg_pose(lane_change_lane->centerline().front(), *lane_change_lane); const auto signed_distance = utils::getSignedDistance(front_pose, getEgoPose(), current_lanes); const auto forward_path_length = planner_data_->parameters.forward_path_length; - if (signed_distance <= 0.0) { - return forward_path_length; - } - - return signed_distance + forward_path_length; + return forward_path_length + std::max(signed_distance, 0.0); }); + const auto backward_length = lane_change_parameters_->backward_lane_length; return route_handler->getLaneletSequence( lane_change_lane.value(), getEgoPose(), backward_length, forward_length); } -bool NormalLaneChange::isNearEndOfCurrentLanes( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const double threshold) const -{ - if (current_lanes.empty()) { - return false; - } - - const auto & route_handler = getRouteHandler(); - const auto & current_pose = getEgoPose(); - - // TODO(Azu) fully change to transient data - const auto distance_to_lane_change_end = std::invoke([&]() { - auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); - - if (!target_lanes.empty() && route_handler->isInGoalRouteSection(target_lanes.back())) { - distance_to_end = std::min( - distance_to_end, - utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)); - } - - return std::max(0.0, distance_to_end) - - common_data_ptr_->transient_data.current_dist_buffer.min; - }); - - lane_change_debug_.distance_to_end_of_current_lane = distance_to_lane_change_end; - return distance_to_lane_change_end < threshold; -} - bool NormalLaneChange::hasFinishedLaneChange() const { const auto & current_pose = getEgoPose(); @@ -789,7 +727,7 @@ bool NormalLaneChange::hasFinishedLaneChange() const return false; } - const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); + const auto & arc_length = common_data_ptr_->transient_data.target_lanes_ego_arc; const auto reach_target_lane = std::abs(arc_length.distance) < lane_change_parameters_->finish_judge_lateral_threshold; @@ -956,6 +894,7 @@ std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() cons std::vector NormalLaneChange::sampleLongitudinalAccValues( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { + // TODO(Azu): sampler should work even when we're not approaching terminal if (prev_module_output_.path.points.empty()) { return {}; } @@ -979,7 +918,7 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( const auto current_max_dist_buffer = calculation::calc_maximum_lane_change_length(common_data_ptr_, current_lanes.back(), max_acc); - if (current_max_dist_buffer > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + if (current_max_dist_buffer > common_data_ptr_->transient_data.dist_to_terminal_end) { RCLCPP_DEBUG( logger_, "No enough distance to the end of lane. Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); @@ -988,7 +927,7 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( } // If the ego is in stuck, sampling all possible accelerations to find avoiding path. - if (isVehicleStuck(current_lanes)) { + if (common_data_ptr_->transient_data.is_ego_stuck) { auto clock = rclcpp::Clock(RCL_ROS_TIME); RCLCPP_INFO_THROTTLE( logger_, clock, 1000, "Vehicle is in stuck. Sample all possible acc: [%f ~ %f]", min_acc, @@ -1016,52 +955,72 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num); } -std::vector NormalLaneChange::calcPrepareDuration( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +std::vector NormalLaneChange::calc_prepare_durations() const { - const auto base_link2front = planner_data_->parameters.base_link2front; - const auto threshold = - lane_change_parameters_->min_length_for_turn_signal_activation + base_link2front; + const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; + const auto threshold = common_data_ptr_->bpp_param_ptr->base_link2front + + lc_param_ptr->min_length_for_turn_signal_activation; + const auto max_prepare_duration = lc_param_ptr->lane_change_prepare_duration; + + // TODO(Azu) this check seems to cause scenario failures. + if (common_data_ptr_->transient_data.dist_to_terminal_start >= threshold) { + return {max_prepare_duration}; + } std::vector prepare_durations; constexpr double step = 0.5; - for (double duration = lane_change_parameters_->lane_change_prepare_duration; duration >= 0.0; - duration -= step) { + for (double duration = max_prepare_duration; duration >= 0.0; duration -= step) { prepare_durations.push_back(duration); - if (!isNearEndOfCurrentLanes(current_lanes, target_lanes, threshold)) { - break; - } } return prepare_durations; } -PathWithLaneId NormalLaneChange::getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double backward_path_length, - const double prepare_length) const +bool NormalLaneChange::get_prepare_segment( + PathWithLaneId & prepare_segment, const double prepare_length) const { - if (current_lanes.empty()) { - return PathWithLaneId(); + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto backward_path_length = common_data_ptr_->bpp_param_ptr->backward_path_length; + + if (current_lanes.empty() || target_lanes.empty()) { + return false; } - auto prepare_segment = prev_module_output_.path; + prepare_segment = prev_module_output_.path; const size_t current_seg_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( prepare_segment.points, getEgoPose(), 3.0, 1.0); utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); - return prepare_segment; + if (prepare_segment.points.empty()) return false; + + const auto & lc_start_pose = prepare_segment.points.back().point.pose; + + // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is + // larger than distance to target lane start + if (!is_valid_start_point(common_data_ptr_, lc_start_pose)) return false; + + // lane changing start is at the end of prepare segment + const auto target_length_from_lane_change_start_pose = + utils::getArcLengthToTargetLanelet(current_lanes, target_lanes.front(), lc_start_pose); + + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > std::numeric_limits::epsilon()) { + throw std::logic_error("lane change start is behind target lanelet!"); + } + + return true; } lane_change::TargetObjects NormalLaneChange::getTargetObjects( const FilteredByLanesExtendedObjects & filtered_objects, - const lanelet::ConstLanelets & current_lanes) const + [[maybe_unused]] const lanelet::ConstLanelets & current_lanes) const { ExtendedPredictedObjects leading_objects = filtered_objects.target_lane_leading; - const auto is_stuck = isVehicleStuck(current_lanes); const auto chk_obj_in_curr_lanes = lane_change_parameters_->check_objects_on_current_lanes; - if (chk_obj_in_curr_lanes || is_stuck) { + if (chk_obj_in_curr_lanes || common_data_ptr_->transient_data.is_ego_stuck) { leading_objects.insert( leading_objects.end(), filtered_objects.current_lane.begin(), filtered_objects.current_lane.end()); @@ -1106,8 +1065,7 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const return {}; } - const auto path = - route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + const auto & path = common_data_ptr_->current_lanes_path; auto filtered_by_lanes_objects = filterObjectsByLanelets(objects, path); @@ -1130,6 +1088,7 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const }); } + // TODO(Azu): We have to think about how to remove is_within_vel_th without breaking AW behavior utils::path_safety_checker::filterObjects( filtered_by_lanes_objects.current_lane, [&](const PredictedObject & object) { const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object); @@ -1333,110 +1292,89 @@ PathWithLaneId NormalLaneChange::getTargetSegment( return target_segment; } -bool NormalLaneChange::hasEnoughLength( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, [[maybe_unused]] const Direction direction) const +std::vector NormalLaneChange::get_prepare_metrics() const { - if (target_lanes.empty()) { - return false; - } + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto current_velocity = getEgoVelocity(); - const auto current_pose = getEgoPose(); - const auto & route_handler = getRouteHandler(); - const auto overall_graphs_ptr = route_handler->getOverallGraphPtr(); - const auto minimum_lane_change_length_to_preferred_lane = - common_data_ptr_->transient_data.next_dist_buffer.min; + // get sampling acceleration values + const auto longitudinal_acc_sampling_values = + sampleLongitudinalAccValues(current_lanes, target_lanes); - const double lane_change_length = path.info.length.sum(); - if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { - return false; - } + const auto prepare_durations = calc_prepare_durations(); - const auto goal_pose = route_handler->getGoalPose(); - if ( - route_handler->isInGoalRouteSection(current_lanes.back()) && - lane_change_length + minimum_lane_change_length_to_preferred_lane > - utils::getSignedDistance(current_pose, goal_pose, current_lanes)) { - return false; - } + RCLCPP_DEBUG( + logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", + prepare_durations.size(), longitudinal_acc_sampling_values.size()); - // return if there are no target lanes - if ( - lane_change_length + minimum_lane_change_length_to_preferred_lane > - utils::getDistanceToEndOfLane(current_pose, target_lanes)) { - return false; - } + const auto dist_to_target_start = + calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); + return calculation::calc_prepare_phase_metrics( + common_data_ptr_, prepare_durations, longitudinal_acc_sampling_values, current_velocity, + dist_to_target_start, common_data_ptr_->transient_data.dist_to_terminal_start); +} - return true; +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 auto & route_handler = getRouteHandler(); + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + 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( + common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, + prep_segment.points.back().point.pose); + + const auto max_lane_changing_length = std::invoke([&]() { + double max_length = + transient_data.is_ego_near_current_terminal_start + ? transient_data.dist_to_terminal_end - prep_metric.length + : std::min(transient_data.dist_to_terminal_end, dist_to_reg_element) - prep_metric.length; + auto target_lane_buffer = lane_change_parameters_->lane_change_finish_judge_buffer + + transient_data.next_dist_buffer.min; + if (std::abs(route_handler->getNumLaneToPreferredLane(target_lanes.back(), direction_)) > 0) { + target_lane_buffer += lane_change_parameters_->backward_length_buffer_for_end_of_lane; + } + max_length = std::min(max_length, dist_lc_start_to_end_of_lanes - target_lane_buffer); + return max_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, + prep_metric.sampled_lon_accel, max_lane_changing_length); } bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const { lane_change_debug_.collision_check_objects.clear(); - const auto & current_lanes = get_current_lanes(); - const auto & target_lanes = get_target_lanes(); - const auto is_stuck = isVehicleStuck(current_lanes); - - const auto is_empty = [&](const auto & data, const auto & s) { - if (!data.empty()) return false; - RCLCPP_WARN(logger_, "%s is empty. Not expected.", s); - return true; - }; + if (!common_data_ptr_->is_lanes_available()) { + RCLCPP_WARN(logger_, "lanes are not available. Not expected."); + return false; + } - const auto target_lane_neighbors_polygon_2d = - common_data_ptr_->lanes_polygon_ptr->target_neighbor; - if ( - is_empty(current_lanes, "current_lanes") || is_empty(target_lanes, "target_lanes") || - is_empty(target_lane_neighbors_polygon_2d, "target_lane_neighbors_polygon_2d")) { + if (common_data_ptr_->lanes_polygon_ptr->target_neighbor.empty()) { + RCLCPP_WARN(logger_, "target_lane_neighbors_polygon_2d is empty. Not expected."); return false; } - const auto & route_handler = *getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; + const auto & current_lanes = get_current_lanes(); + const auto & target_lanes = get_target_lanes(); - // get velocity const auto current_velocity = getEgoVelocity(); - - // get sampling acceleration values - const auto longitudinal_acc_sampling_values = - sampleLongitudinalAccValues(current_lanes, target_lanes); - - const auto is_goal_in_route = common_data_ptr_->lanes_ptr->target_lane_in_goal_section; - - const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; - const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; - - const auto dist_to_target_start = - calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); - const auto dist_to_terminal_end = common_data_ptr_->transient_data.dist_to_terminal_end; - const auto dist_to_terminal_start = common_data_ptr_->transient_data.dist_to_terminal_start; - - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); - - const auto sorted_lane_ids = - utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); - + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); - const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); + const auto prepare_phase_metrics = get_prepare_metrics(); candidate_paths.reserve( - longitudinal_acc_sampling_values.size() * lane_change_parameters_->lateral_acc_sampling_num * - prepare_durations.size()); - - RCLCPP_DEBUG( - logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", - prepare_durations.size(), longitudinal_acc_sampling_values.size()); + prepare_phase_metrics.size() * lane_change_parameters_->lateral_acc_sampling_num); const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; const auto dist_to_next_regulatory_element = utils::lane_change::get_distance_to_next_regulatory_element(common_data_ptr_, only_tl, only_tl); - const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - - const auto prepare_phase_metrics = calculation::calc_prepare_phase_metrics( - common_data_ptr_, prepare_durations, longitudinal_acc_sampling_values, current_velocity, - dist_to_target_start, dist_to_terminal_start); auto check_length_diff = [&](const double prep_length, const double lc_length, const bool check_lc) { @@ -1452,10 +1390,10 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) }; for (const auto & prep_metric : prepare_phase_metrics) { - const auto debug_print = [&](const auto & s) { + const auto debug_print = [&](const std::string & s) { RCLCPP_DEBUG( - logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s, prep_metric.duration, - prep_metric.actual_lon_accel, prep_metric.length); + logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s.c_str(), + prep_metric.duration, prep_metric.actual_lon_accel, prep_metric.length); }; if (!check_length_diff(prep_metric.length, 0.0, false)) { @@ -1463,59 +1401,26 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) continue; } - auto prepare_segment = - getPrepareSegment(current_lanes, common_parameters.backward_path_length, prep_metric.length); - - if (prepare_segment.points.empty()) { - debug_print("prepare segment is empty...? Unexpected."); - continue; - } - - // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is - // larger than distance to target lane start - if (!is_valid_start_point(common_data_ptr_, prepare_segment.points.back().point.pose)) { - debug_print( - "Reject: lane changing start point is not within the preferred lanes or its neighbors"); - continue; - } - - // lane changing start is at the end of prepare segment - const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( - current_lanes, target_lanes.front(), lane_changing_start_pose); - - // Check if the lane changing start point is not on the lanes next to target lanes, - if (target_length_from_lane_change_start_pose > 0.0) { - debug_print("lane change start is behind target lanelet!"); + PathWithLaneId prepare_segment; + try { + if (!get_prepare_segment(prepare_segment, prep_metric.length)) { + debug_print("Reject: failed to get valid prepare segment!"); + continue; + } + } catch (const std::exception & e) { + debug_print(e.what()); break; } debug_print("Prepare path satisfy constraints"); + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; + 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, lane_changing_start_pose); - - const auto max_lane_changing_length = std::invoke([&]() { - double max_length = - dist_to_terminal_start > max_prepare_length - ? std::min(dist_to_terminal_end, dist_to_next_regulatory_element) - prep_metric.length - : dist_to_terminal_end - prep_metric.length; - auto target_lane_buffer = - lane_change_parameters_->lane_change_finish_judge_buffer + next_min_dist_buffer; - if (std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction_)) > 0) { - target_lane_buffer += lane_change_parameters_->backward_length_buffer_for_end_of_lane; - } - max_length = std::min(max_length, dist_lc_start_to_end_of_lanes - target_lane_buffer); - return max_length; - }); - - const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; - const auto lane_changing_metrics = calculation::calc_shift_phase_metrics( - common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, - prep_metric.sampled_lon_accel, max_lane_changing_length); + const auto lane_changing_metrics = get_lane_changing_metrics( + prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element); utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prep_metric.velocity); @@ -1535,7 +1440,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) try { candidate_path = get_candidate_path( prep_metric, lc_metric, prepare_segment, sorted_lane_ids, lane_changing_start_pose, - target_lane_length, shift_length, next_min_dist_buffer, is_goal_in_route); + shift_length); } catch (const std::exception & e) { debug_print_lat(std::string("Reject: ") + e.what()); continue; @@ -1543,20 +1448,16 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) candidate_paths.push_back(candidate_path); - bool is_safe = false; try { - is_safe = check_candidate_path_safety( - candidate_path, target_objects, current_min_dist_buffer, is_stuck); + if (check_candidate_path_safety(candidate_path, target_objects)) { + debug_print_lat("ACCEPT!!!: it is valid and safe!"); + return true; + } } catch (const std::exception & e) { debug_print_lat(std::string("Reject: ") + e.what()); return false; } - if (is_safe) { - debug_print_lat("ACCEPT!!!: it is valid and safe!"); - return true; - } - debug_print_lat("Reject: sampled path is not safe."); } } @@ -1568,19 +1469,15 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) LaneChangePath NormalLaneChange::get_candidate_path( const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, - const Pose & lc_start_pose, const double target_lane_length, const double shift_length, - const double next_lc_buffer, const bool is_goal_in_route) const + const Pose & lc_start_pose, const double shift_length) const { const auto & route_handler = *getRouteHandler(); - const auto & current_lanes = common_data_ptr_->lanes_ptr->current; const auto & target_lanes = common_data_ptr_->lanes_ptr->target; - const auto & forward_path_length = planner_data_->parameters.forward_path_length; const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval(lc_metrics.length, prep_metrics.velocity); - const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( - route_handler, target_lanes, lc_start_pose, target_lane_length, lc_metrics.length, - forward_path_length, resample_interval, is_goal_in_route, next_lc_buffer); + const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( + common_data_ptr_, lc_start_pose, lc_metrics.length, resample_interval); if (target_lane_reference_path.points.empty()) { throw std::logic_error("target_lane_reference_path is empty!"); @@ -1605,7 +1502,10 @@ LaneChangePath NormalLaneChange::get_candidate_path( throw std::logic_error("failed to generate candidate path!"); } - if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction_)) { + if ( + candidate_path.value().info.length.sum() + + common_data_ptr_->transient_data.next_dist_buffer.min > + common_data_ptr_->transient_data.dist_to_terminal_end) { throw std::logic_error("invalid candidate path length!"); } @@ -1613,13 +1513,13 @@ LaneChangePath NormalLaneChange::get_candidate_path( } bool NormalLaneChange::check_candidate_path_safety( - const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, - const double lane_change_buffer, const bool is_stuck) const + const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const { + const auto is_stuck = common_data_ptr_->transient_data.is_ego_stuck; if ( !is_stuck && !utils::lane_change::passed_parked_objects( common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading, - lane_change_buffer, lane_change_debug_.collision_check_objects)) { + lane_change_debug_.collision_check_objects)) { throw std::logic_error( "Ego is not stuck and parked vehicle exists in the target lane. Skip lane change."); } @@ -1669,9 +1569,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( } const auto & route_handler = *getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; - const auto forward_path_length = common_parameters.forward_path_length; const auto minimum_lane_changing_velocity = lane_change_parameters_->minimum_lane_changing_velocity; @@ -1680,10 +1578,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); - - const auto sorted_lane_ids = - utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); // lane changing start getEgoPose() is at the end of prepare segment const auto current_lane_terminal_point = @@ -1719,9 +1614,10 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto [min_lateral_acc, max_lateral_acc] = lane_change_parameters_->lane_change_lat_acc_map.find(minimum_lane_changing_velocity); - const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( + const auto lane_changing_time = autoware::motion_utils::calc_shift_time_from_jerk( shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc); + const auto target_lane_length = common_data_ptr_->transient_data.target_lane_length; const auto target_segment = getTargetSegment( target_lanes, lane_changing_start_pose.value(), target_lane_length, current_min_dist_buffer, minimum_lane_changing_velocity, next_min_dist_buffer); @@ -1752,10 +1648,8 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( current_min_dist_buffer, minimum_lane_changing_velocity); - const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( - route_handler, target_lanes, lane_changing_start_pose.value(), target_lane_length, - current_min_dist_buffer, forward_path_length, resample_interval, is_goal_in_route, - next_min_dist_buffer); + const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( + common_data_ptr_, lane_changing_start_pose.value(), current_min_dist_buffer, resample_interval); if (target_lane_reference_path.points.empty()) { RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); @@ -1796,11 +1690,8 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const CollisionCheckDebugMap debug_data; - const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; - const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects_.target_lane_leading, current_min_dist_buffer, - debug_data); + common_data_ptr_, path, filtered_objects_.target_lane_leading, debug_data); if (!has_passed_parked_objects) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); @@ -1892,10 +1783,9 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const bool NormalLaneChange::isRequiredStop(const bool is_trailing_object) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane; if ( - isNearEndOfCurrentLanes(get_current_lanes(), get_target_lanes(), threshold) && - isAbleToStopSafely() && is_trailing_object) { + common_data_ptr_->transient_data.is_ego_near_current_terminal_start && isAbleToStopSafely() && + is_trailing_object) { current_lane_change_state_ = LaneChangeStates::Stop; return true; } @@ -1988,9 +1878,8 @@ bool NormalLaneChange::calcAbortPath() PathShifter path_shifter; path_shifter.setPath(lane_changing_path); path_shifter.addShiftLine(shift_line); - const auto lateral_jerk = - autoware::behavior_path_planner::PathShifter::calcJerkFromLatLonDistance( - shift_line.end_shift_length, abort_start_dist, current_velocity); + const auto lateral_jerk = autoware::motion_utils::calc_jerk_from_lat_lon_distance( + shift_line.end_shift_length, abort_start_dist, current_velocity); path_shifter.setVelocity(current_velocity); const auto lateral_acc_range = lane_change_parameters_->lane_change_lat_acc_map.find(current_velocity); @@ -2196,63 +2085,6 @@ bool NormalLaneChange::is_collided( return !is_collided; } -// Check if the ego vehicle is in stuck by a stationary obstacle or by the terminal of current lanes -bool NormalLaneChange::isVehicleStuck( - const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const -{ - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - // Ego is still moving, not in stuck - if (std::abs(getEgoVelocity()) > lane_change_parameters_->stop_velocity_threshold) { - RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); - return false; - } - - // Ego is just stopped, not sure it is in stuck yet. - if (getStopTime() < lane_change_parameters_->stop_time_threshold) { - RCLCPP_DEBUG(logger_, "Ego is just stopped, counting for stuck judge... (%f)", getStopTime()); - return false; - } - - // Check if any stationary object exist in obstacle_check_distance - using lanelet::utils::getArcCoordinates; - const auto base_distance = getArcCoordinates(current_lanes, getEgoPose()).length; - - for (const auto & object : lane_change_debug_.filtered_objects.current_lane) { - const auto & p = object.initial_pose; // TODO(Horibe): consider footprint point - - // Note: it needs chattering prevention. - if (std::abs(object.initial_twist.linear.x) > 0.3) { // check if stationary - continue; - } - - const auto ego_to_obj_dist = getArcCoordinates(current_lanes, p).length - base_distance; - if (0 < ego_to_obj_dist && ego_to_obj_dist < obstacle_check_distance) { - RCLCPP_DEBUG(logger_, "Stationary object is in front of ego."); - return true; // Stationary object is in front of ego. - } - } - - // Check if Ego is in terminal of current lanes - const auto & route_handler = getRouteHandler(); - const double distance_to_terminal = - route_handler->isInGoalRouteSection(current_lanes.back()) - ? utils::getSignedDistance(getEgoPose(), route_handler->getGoalPose(), current_lanes) - : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); - const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; - const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const double terminal_judge_buffer = current_min_dist_buffer + stop_point_buffer + 1.0; - if (distance_to_terminal < terminal_judge_buffer) { - return true; - } - - // No stationary objects found in obstacle_check_distance and Ego is not in terminal of current - RCLCPP_DEBUG( - logger_, - "No stationary objects found in obstacle_check_distance and Ego is not in " - "terminal of current lanes"); - return false; -} - double NormalLaneChange::get_max_velocity_for_safety_check() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -2265,35 +2097,56 @@ double NormalLaneChange::get_max_velocity_for_safety_check() const return getCommonParam().max_vel; } -bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const +bool NormalLaneChange::is_ego_stuck() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (current_lanes.empty()) { - lane_change_debug_.is_stuck = false; - return false; // can not check + const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; + + if (std::abs(common_data_ptr_->get_ego_speed()) > lc_param_ptr->stop_velocity_threshold) { + RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); + return false; } - const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); - const auto current_max_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.max; - const auto rss_dist = calcRssDistance( - 0.0, lane_change_parameters_->minimum_lane_changing_velocity, - lane_change_parameters_->rss_params); + // Ego is just stopped, not sure it is in stuck yet. + if (getStopTime() < lc_param_ptr->stop_time_threshold) { + RCLCPP_DEBUG(logger_, "Ego is just stopped, counting for stuck judge... (%f)", getStopTime()); + return false; + } + + // Check if any stationary object exist in obstacle_check_distance + const auto & current_lanes_path = common_data_ptr_->current_lanes_path; + const auto & ego_pose = common_data_ptr_->get_ego_pose(); + const auto rss_dist = + calcRssDistance(0.0, lc_param_ptr->minimum_lane_changing_velocity, lc_param_ptr->rss_params); // It is difficult to define the detection range. If it is too short, the stuck will not be // determined, even though you are stuck by an obstacle. If it is too long, // the ego will be judged to be stuck by a distant vehicle, even though the ego is only // stopped at a traffic light. Essentially, the calculation should be based on the information of // the stop reason, but this is outside the scope of one module. I keep it as a TODO. - constexpr double DETECTION_DISTANCE_MARGIN = 10.0; - const auto detection_distance = current_max_dist_buffer + rss_dist + - getCommonParam().base_link2front + DETECTION_DISTANCE_MARGIN; - RCLCPP_DEBUG( - logger_, "current_max_dist_buffer: %f, max_acc: %f", current_max_dist_buffer, max_acc); + constexpr auto detection_distance_margin = 10.0; + const auto obstacle_check_distance = common_data_ptr_->transient_data.lane_changing_length.max + + rss_dist + common_data_ptr_->bpp_param_ptr->base_link2front + + detection_distance_margin; + const auto has_object_blocking = std::any_of( + filtered_objects_.current_lane.begin(), filtered_objects_.current_lane.end(), + [&](const auto & object) { + // Note: it needs chattering prevention. + if ( + std::abs(object.initial_twist.linear.x) > + lc_param_ptr->stopped_object_velocity_threshold) { // check if stationary + return false; + } - auto is_vehicle_stuck = isVehicleStuck(current_lanes, detection_distance); + const auto ego_to_obj_dist = + calcSignedArcLength( + current_lanes_path.points, ego_pose.position, object.initial_pose.position) - + obstacle_check_distance; + return ego_to_obj_dist < 0.0; + }); - lane_change_debug_.is_stuck = is_vehicle_stuck; - return is_vehicle_stuck; + lane_change_debug_.is_stuck = has_object_blocking; + return has_object_blocking; } bool NormalLaneChange::is_valid_start_point( @@ -2308,9 +2161,10 @@ bool NormalLaneChange::is_valid_start_point( boost::geometry::covered_by(lc_start_point, target_lane_poly); } -void NormalLaneChange::setStopPose(const Pose & stop_pose) +void NormalLaneChange::set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path) { - lane_change_stop_pose_ = stop_pose; + const auto stop_point = utils::insertStopPoint(arc_length_to_stop_pose, path); + lane_change_stop_pose_ = stop_point.point.pose; } void NormalLaneChange::updateStopTime() diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 1c4aede51074a..762cada4e59d2 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 @@ -15,6 +15,7 @@ #include #include +#include #include @@ -163,7 +164,7 @@ double calc_maximum_lane_change_length( const auto [min_lat_acc, max_lat_acc] = lane_change_parameters.lane_change_lat_acc_map.find(vel); const auto t_lane_changing = - PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + autoware::motion_utils::calc_shift_time_from_jerk(shift_interval, lat_jerk, max_lat_acc); const auto lane_changing_length = vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing; @@ -208,7 +209,8 @@ std::vector calc_all_min_lc_lengths( min_lc_lengths.reserve(shift_intervals.size()); const auto min_lc_length = [&](const auto shift_interval) { - const auto t = PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + const auto t = + autoware::motion_utils::calc_shift_time_from_jerk(shift_interval, lat_jerk, max_lat_acc); return min_vel * t; }; @@ -248,7 +250,7 @@ std::vector calc_all_max_lc_lengths( // lane changing section const auto [min_lat_acc, max_lat_acc] = lc_param_ptr->lane_change_lat_acc_map.find(vel); const auto t_lane_changing = - PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + autoware::motion_utils::calc_shift_time_from_jerk(shift_interval, lat_jerk, max_lat_acc); const auto lane_changing_length = vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing; @@ -410,7 +412,7 @@ std::vector calc_shift_phase_metrics( for (double lat_acc = min_lateral_acc; lat_acc < max_lateral_acc + eps; lat_acc += lateral_acc_resolution) { - const auto lane_changing_duration = PathShifter::calcShiftTimeFromJerk( + const auto lane_changing_duration = autoware::motion_utils::calc_shift_time_from_jerk( shift_length, common_data_ptr->lc_param_ptr->lane_changing_lateral_jerk, lat_acc); const double lane_changing_accel = calc_lane_changing_acceleration( 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 8794d79a4d10f..82a832d30c0b5 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,8 +24,8 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" #include #include @@ -78,6 +78,12 @@ rclcpp::Logger get_logger() return logger; } +bool is_mandatory_lane_change(const ModuleType lc_type) +{ + return lc_type == LaneChangeModuleType::NORMAL || + lc_type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE; +} + double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity) { @@ -160,14 +166,13 @@ lanelet::ConstLanelets getTargetNeighborLanes( lanelet::ConstLanelets neighbor_lanes; for (const auto & current_lane : current_lanes) { + const auto mandatory_lane_change = is_mandatory_lane_change(type); if (route_handler.getNumLaneToPreferredLane(current_lane) != 0) { - if ( - type == LaneChangeModuleType::NORMAL || - type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { + if (mandatory_lane_change) { neighbor_lanes.push_back(current_lane); } } else { - if (type != LaneChangeModuleType::NORMAL) { + if (!mandatory_lane_change) { neighbor_lanes.push_back(current_lane); } } @@ -322,12 +327,17 @@ std::optional construct_candidate_path( return std::optional{candidate_path}; } -PathWithLaneId getReferencePathFromTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double target_lane_length, - const double lane_changing_length, const double forward_path_length, - const double resample_interval, const bool is_goal_in_route, const double next_lane_change_buffer) +PathWithLaneId get_reference_path_from_target_Lane( + const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, + const double lane_changing_length, const double resample_interval) { + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto target_lane_length = common_data_ptr->transient_data.target_lane_length; + const auto is_goal_in_route = common_data_ptr->lanes_ptr->target_lane_in_goal_section; + const auto next_lc_buffer = common_data_ptr->transient_data.next_dist_buffer.min; + const auto forward_path_length = common_data_ptr->bpp_param_ptr->forward_path_length; + const ArcCoordinates lane_change_start_arc_position = lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); @@ -337,10 +347,10 @@ PathWithLaneId getReferencePathFromTargetLane( if (is_goal_in_route) { const double s_goal = lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length - - next_lane_change_buffer; + next_lc_buffer; return std::min(dist_from_lc_start, s_goal); } - return std::min(dist_from_lc_start, target_lane_length - next_lane_change_buffer); + return std::min(dist_from_lc_start, target_lane_length - next_lc_buffer); }); constexpr double epsilon = 1e-4; @@ -548,10 +558,13 @@ double getLateralShift(const LaneChangePath & path) return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); } -std::vector> getSortedLaneIds( - const RouteHandler & route_handler, const Pose & current_pose, - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) +std::vector> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr) { + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & current_pose = common_data_ptr->get_ego_pose(); + const auto rough_shift_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose).distance; @@ -625,17 +638,16 @@ CandidateOutput assignToCandidate( return candidate_output; } -std::optional getLaneChangeTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const LaneChangeModuleType type, const Direction & direction) +std::optional get_lane_change_target_lane( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes) { - if ( - type == LaneChangeModuleType::NORMAL || - type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { - return route_handler.getLaneChangeTarget(current_lanes, direction); + const auto direction = common_data_ptr->direction; + const auto route_handler_ptr = common_data_ptr->route_handler_ptr; + if (is_mandatory_lane_change(common_data_ptr->lc_type)) { + return route_handler_ptr->getLaneChangeTarget(current_lanes, direction); } - return route_handler.getLaneChangeTargetExceptPreferredLane(current_lanes, direction); + return route_handler_ptr->getLaneChangeTargetExceptPreferredLane(current_lanes, direction); } std::vector convertToPredictedPath( @@ -817,8 +829,7 @@ bool isParkedObject( bool passed_parked_objects( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, const double minimum_lane_change_length, - CollisionCheckDebugMap & object_debug) + const std::vector & objects, CollisionCheckDebugMap & object_debug) { const auto route_handler = *common_data_ptr->route_handler_ptr; const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; @@ -868,7 +879,7 @@ bool passed_parked_objects( }); // If there are still enough length after the target object, we delay the lane change - if (min_dist_to_end_of_current_lane <= minimum_lane_change_length) { + if (min_dist_to_end_of_current_lane <= common_data_ptr->transient_data.current_dist_buffer.min) { return true; } @@ -979,7 +990,7 @@ ExtendedPredictedObject transform( if (t < prepare_duration && obj_vel_norm < velocity_threshold) { continue; } - const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); + const auto obj_pose = autoware::object_recognition_utils::calcInterpolatedPose(path, t); if (obj_pose) { const auto obj_polygon = autoware::universe_utils::toPolygon2d(*obj_pose, object.shape); extended_object.predicted_paths.at(i).path.emplace_back( @@ -1239,4 +1250,69 @@ double get_distance_to_next_regulatory_element( return distance; } + +double get_min_dist_to_current_lanes_obj( + const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, + const double dist_to_target_lane_start, const PathWithLaneId & path) +{ + const auto & path_points = path.points; + auto min_dist_to_obj = std::numeric_limits::max(); + for (const auto & object : filtered_objects.current_lane) { + // check if stationary + const auto obj_v = std::abs(object.initial_twist.linear.x); + if (obj_v > common_data_ptr->lc_param_ptr->stop_velocity_threshold) { + continue; + } + + // provide "estimation" based on size of object + const auto dist_to_obj = + motion_utils::calcSignedArcLength( + path_points, path_points.front().point.pose.position, object.initial_pose.position) - + (object.shape.dimensions.x / 2); + + if (dist_to_obj < dist_to_target_lane_start) { + 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); + } + } + return min_dist_to_obj; +} + +bool has_blocking_target_object( + const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, + const double stop_arc_length, const PathWithLaneId & path) +{ + return std::any_of( + filtered_objects.target_lane_leading.begin(), filtered_objects.target_lane_leading.end(), + [&](const auto & object) { + const auto v = std::abs(object.initial_twist.linear.x); + if (v > common_data_ptr->lc_param_ptr->stop_velocity_threshold) { + return false; + } + + // filtered_objects includes objects out of target lanes, so filter them out + if (boost::geometry::disjoint( + object.initial_polygon, common_data_ptr->lanes_polygon_ptr->target.value())) { + return false; + } + + const auto arc_length_to_target_lane_obj = motion_utils::calcSignedArcLength( + path.points, path.points.front().point.pose.position, object.initial_pose.position); + const auto width_margin = object.shape.dimensions.x / 2; + return (arc_length_to_target_lane_obj - width_margin) >= stop_arc_length; + }); +} } // namespace autoware::behavior_path_planner::utils::lane_change 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 6196a4f02cff9..1c3509bc22482 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml @@ -44,6 +44,7 @@ autoware_lane_departure_checker autoware_lanelet2_extension autoware_motion_utils + autoware_object_recognition_utils autoware_path_sampler autoware_perception_msgs autoware_planning_msgs @@ -57,7 +58,6 @@ libboost-dev libopencv-dev magic_enum - object_recognition_utils pluginlib rclcpp rclcpp_components diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_utils.cpp index da8657c82c58e..36e2914571c1f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_utils.cpp @@ -73,7 +73,7 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, setGoal) path.points.at(4).lane_ids.push_back(5); PathWithLaneId path_with_goal; - autoware::behavior_path_planner::utils::setGoal( + autoware::behavior_path_planner::utils::set_goal( 3.5, M_PI * 0.5, path, path.points.back().point.pose, 5, &path_with_goal); // Check if skipped lane ids by smooth skip connection are filled in output path. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt index 4a2891e116cc8..d3f76ed904244 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt @@ -34,21 +34,47 @@ target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC if(BUILD_TESTING) ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_utilities - test/test_drivable_area_expansion.cpp + test/test_utils.cpp ) target_link_libraries(test_${PROJECT_NAME}_utilities + ${PROJECT_NAME} + ) + + ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_drivable_area_expansion + test/test_drivable_area_expansion.cpp + test/test_footprints.cpp + ) + + target_link_libraries(test_${PROJECT_NAME}_drivable_area_expansion + ${PROJECT_NAME} + ) + + ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_occupancy_grid_based_collision_detector + test/test_occupancy_grid_based_collision_detector.cpp + ) + + target_link_libraries(test_${PROJECT_NAME}_occupancy_grid_based_collision_detector ${PROJECT_NAME} ) ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_safety_check test/test_safety_check.cpp + test/test_objects_filtering.cpp ) target_link_libraries(test_${PROJECT_NAME}_safety_check ${PROJECT_NAME} ) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_parking_departure + test/test_parking_departure_utils.cpp + ) + + target_link_libraries(test_${PROJECT_NAME}_parking_departure + ${PROJECT_NAME} + ) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_turn_signal test/test_turn_signal.cpp ) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp index c0f953578a213..aea3818053171 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp @@ -36,7 +36,7 @@ Polygon2d translate_polygon(const Polygon2d & polygon, const double x, const dou /// @param[in] pose the origin pose of the footprint /// @param[in] base_footprint the base axis-aligned footprint /// @return footprint polygon -Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint); +Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d & base_footprint); /// @brief create footprints of the predicted paths of an object /// @param [in] objects objects from which to create polygons diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp index fa04ff04c8e44..434eb79189eaf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp @@ -25,7 +25,13 @@ namespace autoware::behavior_path_planner { -int discretizeAngle(const double theta, const int theta_size); +/** + * @brief Discretizes a given angle into an index within a specified range. + * @param theta The angle in radians to discretize. + * @param theta_size The number of discrete bins or angular intervals. + * @return The discretized angle as an integer index. + */ +int discretize_angle(const double theta, const int theta_size); struct IndexXYT { @@ -40,19 +46,36 @@ struct IndexXY int y; }; +/** + * @brief Converts a given local pose into a 3D grid index (x, y, theta). + * @param costmap The occupancy grid used for indexing. + * @param pose_local The local pose. + * @param theta_size The number of discrete angular intervals for yaw. + * @return IndexXYT The grid index representing the position and discretized orientation. + */ IndexXYT pose2index( const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local, const int theta_size); +/** + * @brief Converts a grid index (x, y, theta) back into a pose in the local frame. + * @param costmap The occupancy grid used for indexing. + * @param index The grid index containing x, y, and theta. + * @param theta_size The number of discrete angular intervals for yaw. + * @return geometry_msgs::msg::Pose The corresponding local pose. + */ geometry_msgs::msg::Pose index2pose( const nav_msgs::msg::OccupancyGrid & costmap, const IndexXYT & index, const int theta_size); +/** + * @brief Transforms a global pose into a local pose relative to the costmap's origin. + * @param costmap The occupancy grid that defines the local frame. + * @param pose_global The global pose to transform. + * @return geometry_msgs::msg::Pose The transformed pose in the local frame. + */ geometry_msgs::msg::Pose global2local( const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_global); -geometry_msgs::msg::Pose local2global( - const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local); - struct VehicleShape { double length; // X [m] @@ -76,12 +99,6 @@ struct PlannerWaypoint bool is_back = false; }; -struct PlannerWaypoints -{ - std_msgs::msg::Header header; - std::vector waypoints; -}; - class OccupancyGridBasedCollisionDetector { public: @@ -92,21 +109,40 @@ class OccupancyGridBasedCollisionDetector default; OccupancyGridBasedCollisionDetector & operator=(OccupancyGridBasedCollisionDetector &&) = delete; void setParam(const OccupancyGridMapParam & param) { param_ = param; }; - OccupancyGridMapParam getParam() const { return param_; }; + [[nodiscard]] OccupancyGridMapParam getParam() const { return param_; }; void setMap(const nav_msgs::msg::OccupancyGrid & costmap); - nav_msgs::msg::OccupancyGrid getMap() const { return costmap_; }; - void setVehicleShape(const VehicleShape & vehicle_shape) { param_.vehicle_shape = vehicle_shape; } - bool hasObstacleOnPath( - const geometry_msgs::msg::PoseArray & path, const bool check_out_of_range) const; - bool hasObstacleOnPath( + [[nodiscard]] nav_msgs::msg::OccupancyGrid getMap() const { return costmap_; }; + + /** + * @brief Detects if a collision occurs with given path. + * @param path The path to check collision defined in a global frame + * @param check_out_of_range A boolean flag indicating whether out-of-range conditions is + * collision + * @return true if a collision is detected, false if no collision is detected. + */ + [[nodiscard]] bool hasObstacleOnPath( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool check_out_of_range) const; - const PlannerWaypoints & getWaypoints() const { return waypoints_; } - bool detectCollision(const IndexXYT & base_index, const bool check_out_of_range) const; + + /** + * @brief Detects if a collision occurs at the specified base index in the occupancy grid map. + * @param base_index The 3D index (x, y, theta) of the base position in the occupancy grid. + * @param check_out_of_range A boolean flag indicating whether out-of-range conditions is + * collision + * @return true if a collision is detected, false if no collision is detected. + */ + [[nodiscard]] bool detectCollision( + const IndexXYT & base_index, const bool check_out_of_range) const; virtual ~OccupancyGridBasedCollisionDetector() = default; protected: - void computeCollisionIndexes(int theta_index, std::vector & indexes); - inline bool isOutOfRange(const IndexXYT & index) const + /** + * @brief Computes the 2D grid indexes where collision might occur for a given theta index. + * @param theta_index The discretized orientation index for yaw. + * @param indexes_2d The output vector of 2D grid indexes where collisions could happen. + */ + void compute_collision_indexes(int theta_index, std::vector & indexes); + + [[nodiscard]] inline bool is_out_of_range(const IndexXYT & index) const { if (index.x < 0 || static_cast(costmap_.info.width) <= index.x) { return true; @@ -116,7 +152,7 @@ class OccupancyGridBasedCollisionDetector } return false; } - inline bool isObs(const IndexXYT & index) const + [[nodiscard]] inline bool is_obs(const IndexXYT & index) const { // NOTE: Accessing by .at() instead makes 1.2 times slower here. // Also, boundary check is already done in isOutOfRange before calling this function. @@ -134,15 +170,7 @@ class OccupancyGridBasedCollisionDetector // is_obstacle's table std::vector> is_obstacle_table_; - - // pose in costmap frame - geometry_msgs::msg::Pose start_pose_; - geometry_msgs::msg::Pose goal_pose_; - - // result path - PlannerWaypoints waypoints_; }; - } // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index f73f989174b54..d9873975dabe1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -34,14 +34,44 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker::filter using autoware_perception_msgs::msg::PredictedObject; using tier4_planning_msgs::msg::PathPointWithLaneId; +/** + * @brief Filters object based on velocity. + * + * @param object The predicted object to filter. + * @param velocity_threshold Lower bound + * @param max_velocity Upper bound + * @return Returns true when the object is within a certain velocity range. + */ bool velocity_filter( const PredictedObject & object, double velocity_threshold, double max_velocity); +/** + * @brief Filters object based on position. + * + * @param object The predicted object to filter. + * @param path_points Ego path + * @param current_pose Ego current pose + * @param forward_distance Upper bound for relative distance + * @param backward_distance Lower bound for relative distance + * @return Returns true when the object is within a certain distance. + */ bool position_filter( - PredictedObject & object, const std::vector & path_points, + const PredictedObject & object, const std::vector & path_points, const geometry_msgs::msg::Point & current_pose, const double forward_distance, const double backward_distance); +/** + * @brief Filters object by searching within a radius. + * + * @param object The predicted object to filter. + * @param reference_point Reference point + * @param search_radius Search radius + * @return Returns true when the object is within radius. + */ +bool is_within_circle( + const geometry_msgs::msg::Point & object_pos, const geometry_msgs::msg::Point & reference_point, + const double search_radius); + } // namespace autoware::behavior_path_planner::utils::path_safety_checker::filter namespace autoware::behavior_path_planner::utils::path_safety_checker @@ -113,7 +143,7 @@ PredictedObjects filterObjects( */ PredictedObjects filterObjectsByVelocity( const PredictedObjects & objects, const double velocity_threshold, - const bool remove_above_threshold = true); + const bool remove_above_threshold = false); /** * @brief Helper function to filter objects based on their velocity. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp index 8164ef659b7b1..4364bb1744a7e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp @@ -108,11 +108,6 @@ class PathShifter */ std::vector getShiftLines() const { return shift_lines_; } - /** - * @brief Get shift points size. - */ - size_t getShiftLinesSize() const { return shift_lines_.size(); } - /** * @brief Get base offset. */ @@ -138,36 +133,10 @@ class PathShifter */ void removeBehindShiftLineAndSetBaseOffset(const size_t nearest_idx); - //////////////////////////////////////// - // Utility Functions - //////////////////////////////////////// - - static double calcFeasibleVelocityFromJerk( - const double lateral, const double jerk, const double longitudinal_distance); - - static double calcLateralDistFromJerk( - const double longitudinal, const double jerk, const double velocity); - - static double calcLongitudinalDistFromJerk( - const double lateral, const double jerk, const double velocity); - - static double calcShiftTimeFromJerk(const double lateral, const double jerk, const double acc); - - static double calcJerkFromLatLonDistance( - const double lateral, const double longitudinal, const double velocity); - - double getTotalShiftLength() const; - double getLastShiftLength() const; std::optional getLastShiftLine() const; - /** - * @brief Calculate the theoretical lateral jerk by spline shifting for current shift_lines_. - * @return Jerk array. The size is same as the shift points. - */ - std::vector calcLateralJerk() const; - private: // The reference path along which the shift will be performed. PathWithLaneId reference_path_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp index 7ac9993ee8b01..d6d37c3f581e4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp @@ -104,6 +104,33 @@ FrenetPoint convertToFrenetPoint( return frenet_point; } +/** + * @brief Converts a Lanelet point to a ROS Pose message. + * + * This function converts a point from a Lanelet map to a ROS geometry_msgs::msg::Pose. + * It sets the position from the point and calculates the orientation (yaw) based on the target + * lane. + * + * @tparam LaneletPointType The type of the input point. + * + * @param[in] src_point The point to convert. + * @param[in] target_lane The lanelet used to determine the orientation. + * + * @return A Pose message with the position and orientation of the point. + */ +template +Pose to_geom_msg_pose(const LaneletPointType & src_point, const lanelet::ConstLanelet & target_lane) +{ + const auto point = lanelet::utils::conversion::toGeomMsgPt(src_point); + const auto yaw = lanelet::utils::getLaneletAngle(target_lane, point); + geometry_msgs::msg::Pose pose; + pose.position = point; + tf2::Quaternion quat; + quat.setRPY(0, 0, yaw); + pose.orientation = tf2::toMsg(quat); + return pose; +} + // distance (arclength) calculation double l2Norm(const Vector3 vector); @@ -161,7 +188,7 @@ double calcLateralDistanceFromEgoToObject( * @brief calculate longitudinal distance from ego pose to object * @return distance from ego pose to object */ -double calcLongitudinalDistanceFromEgoToObject( +double calc_longitudinal_distance_from_ego_to_object( const Pose & ego_pose, const double base_link2front, const double base_link2rear, const PredictedObject & dynamic_object); @@ -195,7 +222,7 @@ std::optional getLeftLanelet( * @param [in] goal_lane_id [unused] * @param [in] output_ptr output path with modified points for the goal */ -bool setGoal( +bool set_goal( const double search_radius_range, const double search_rad_range, const PathWithLaneId & input, const Pose & goal, const int64_t goal_lane_id, PathWithLaneId * output_ptr); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml index b1d3a39379d15..d98e832e23e00 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml @@ -48,6 +48,7 @@ autoware_lane_departure_checker autoware_lanelet2_extension autoware_motion_utils + autoware_object_recognition_utils autoware_objects_of_interest_marker_interface autoware_perception_msgs autoware_planning_msgs @@ -57,15 +58,16 @@ autoware_vehicle_info_utils geometry_msgs magic_enum - object_recognition_utils rclcpp tf2 tier4_planning_msgs traffic_light_utils visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common + autoware_test_utils ament_cmake diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp index a25a33c1668fa..10356150fc689 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp @@ -33,7 +33,7 @@ Polygon2d translate_polygon(const Polygon2d & polygon, const double x, const dou return translated_polygon; } -Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint) +Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d & base_footprint) { const auto angle = tf2::getYaw(pose.orientation); return translate_polygon( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index e4f1919b08dbc..e65c67065bc54 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -25,7 +25,7 @@ using autoware::universe_utils::createQuaternionFromYaw; using autoware::universe_utils::normalizeRadian; using autoware::universe_utils::transformPose; -int discretizeAngle(const double theta, const int theta_size) +int discretize_angle(const double theta, const int theta_size) { const double one_angle_range = 2.0 * M_PI / theta_size; return static_cast(std::rint(normalizeRadian(theta, 0.0) / one_angle_range)) % theta_size; @@ -37,7 +37,7 @@ IndexXYT pose2index( { const int index_x = pose_local.position.x / costmap.info.resolution; const int index_y = pose_local.position.y / costmap.info.resolution; - const int index_theta = discretizeAngle(tf2::getYaw(pose_local.orientation), theta_size); + const int index_theta = discretize_angle(tf2::getYaw(pose_local.orientation), theta_size); return {index_x, index_y, index_theta}; } @@ -68,18 +68,6 @@ geometry_msgs::msg::Pose global2local( return transformPose(pose_global, transform); } -geometry_msgs::msg::Pose local2global( - const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local) -{ - tf2::Transform tf_origin; - tf2::convert(costmap.info.origin, tf_origin); - - geometry_msgs::msg::TransformStamped transform; - transform.transform = tf2::toMsg(tf_origin); - - return transformPose(pose_local, transform); -} - void OccupancyGridBasedCollisionDetector::setMap(const nav_msgs::msg::OccupancyGrid & costmap) { costmap_ = costmap; @@ -105,7 +93,7 @@ void OccupancyGridBasedCollisionDetector::setMap(const nav_msgs::msg::OccupancyG coll_indexes_table_.clear(); for (int i = 0; i < param_.theta_size; i++) { std::vector indexes_2d; - computeCollisionIndexes(i, indexes_2d); + compute_collision_indexes(i, indexes_2d); coll_indexes_table_.push_back(indexes_2d); } } @@ -118,7 +106,7 @@ static IndexXY position2index( return {index_x, index_y}; } -void OccupancyGridBasedCollisionDetector::computeCollisionIndexes( +void OccupancyGridBasedCollisionDetector::compute_collision_indexes( int theta_index, std::vector & indexes_2d) { IndexXYT base_index{0, 0, theta_index}; @@ -134,7 +122,7 @@ void OccupancyGridBasedCollisionDetector::computeCollisionIndexes( const auto base_theta = tf2::getYaw(base_pose.orientation); // Convert each point to index and check if the node is Obstacle - const auto addIndex2d = [&](const double x, const double y) { + const auto add_index2d = [&](const double x, const double y) { // Calculate offset in rotated frame const double offset_x = std::cos(base_theta) * x - std::sin(base_theta) * y; const double offset_y = std::sin(base_theta) * x + std::cos(base_theta) * y; @@ -149,14 +137,14 @@ void OccupancyGridBasedCollisionDetector::computeCollisionIndexes( for (double x = back; x <= front; x += costmap_.info.resolution / 2) { for (double y = right; y <= left; y += costmap_.info.resolution / 2) { - addIndex2d(x, y); + add_index2d(x, y); } - addIndex2d(x, left); + add_index2d(x, left); } for (double y = right; y <= left; y += costmap_.info.resolution / 2) { - addIndex2d(front, y); + add_index2d(front, y); } - addIndex2d(front, left); + add_index2d(front, left); } bool OccupancyGridBasedCollisionDetector::detectCollision( @@ -175,27 +163,12 @@ bool OccupancyGridBasedCollisionDetector::detectCollision( coll_index.x += base_index.x; coll_index.y += base_index.y; if (check_out_of_range) { - if (isOutOfRange(coll_index) || isObs(coll_index)) return true; + if (is_out_of_range(coll_index) || is_obs(coll_index)) return true; } else { - if (isOutOfRange(coll_index)) return false; - if (isObs(coll_index)) return true; - } - } - return false; -} - -bool OccupancyGridBasedCollisionDetector::hasObstacleOnPath( - const geometry_msgs::msg::PoseArray & path, const bool check_out_of_range) const -{ - for (const auto & pose : path.poses) { - const auto pose_local = global2local(costmap_, pose); - const auto index = pose2index(costmap_, pose_local, param_.theta_size); - - if (detectCollision(index, check_out_of_range)) { - return true; + if (is_out_of_range(coll_index)) return false; + if (is_obs(coll_index)) return true; } } - return false; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 25b307ab2cc4d..80936b655cc48 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -15,7 +15,7 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include #include @@ -39,6 +39,8 @@ bool position_filter( const geometry_msgs::msg::Point & current_pose, const double forward_distance, const double backward_distance) { + if (path_points.empty()) return false; + const auto dist_ego_to_obj = autoware::motion_utils::calcSignedArcLength( path_points, current_pose, object.kinematics.initial_pose_with_covariance.pose.position); @@ -118,7 +120,7 @@ PredictedObjects filterObjects( const ObjectTypesToCheck & target_object_types = params->object_types_to_check; PredictedObjects filtered_objects = - filterObjectsByVelocity(*objects, ignore_object_velocity_threshold, false); + filterObjectsByVelocity(*objects, ignore_object_velocity_threshold, true); filterObjectsByClass(filtered_objects, target_object_types); @@ -136,7 +138,7 @@ PredictedObjects filterObjectsByVelocity( const PredictedObjects & objects, const double velocity_threshold, const bool remove_above_threshold) { - if (remove_above_threshold) { + if (!remove_above_threshold) { return filterObjectsByVelocity(objects, -velocity_threshold, velocity_threshold); } return filterObjectsByVelocity(objects, velocity_threshold, std::numeric_limits::max()); @@ -336,7 +338,7 @@ ExtendedPredictedObject transform( // Create path based on time horizon and resolution for (double t = 0.0; t < safety_check_time_horizon + 1e-3; t += safety_check_time_resolution) { - const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); + const auto obj_pose = autoware::object_recognition_utils::calcInterpolatedPose(path, t); if (obj_pose) { const auto obj_polygon = autoware::universe_utils::toPolygon2d(*obj_pose, object.shape); extended_object.predicted_paths[i].path.emplace_back( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp index 69c538fa6cb54..22baffe80fdf6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp @@ -395,27 +395,6 @@ std::pair, std::vector> PathShifter::calcBaseLengths return {base_lon, base_lat}; } -std::vector PathShifter::calcLateralJerk() const -{ - const auto arclength_arr = utils::calcPathArcLengthArray(reference_path_); - - constexpr double epsilon = 1.0e-8; // to avoid 0 division - - std::vector lateral_jerk{}; - - // TODO(Watanabe) write docs. - double current_shift = base_offset_; - for (const auto & shift_line : shift_lines_) { - const double delta_shift = shift_line.end_shift_length - current_shift; - const auto shifting_arclength = std::max( - arclength_arr.at(shift_line.end_idx) - arclength_arr.at(shift_line.start_idx), epsilon); - lateral_jerk.push_back((delta_shift / 2.0) / std::pow(shifting_arclength / 4.0, 3.0)); - current_shift = shift_line.end_shift_length; - } - - return lateral_jerk; -} - void PathShifter::updateShiftLinesIndices(ShiftLineArray & shift_lines) const { if (reference_path_.points.empty()) { @@ -542,86 +521,6 @@ void PathShifter::shiftBaseLength(ShiftedPath * path, double offset) } } -double PathShifter::calcShiftTimeFromJerk(const double lateral, const double jerk, const double acc) -{ - const double j = std::abs(jerk); - const double a = std::abs(acc); - const double l = std::abs(lateral); - if (j < 1.0e-8 || a < 1.0e-8) { - return 1.0e10; // TODO(Horibe) maybe invalid arg? - } - - // time with constant jerk - double tj = a / j; - - // time with constant acceleration (zero jerk) - double ta = (std::sqrt(a * a + 4.0 * j * j * l / a) - 3.0 * a) / (2.0 * j); - - if (ta < 0.0) { - // it will not hit the acceleration limit this time - tj = std::pow(l / (2.0 * j), 1.0 / 3.0); - ta = 0.0; - } - - const double t_total = 4.0 * tj + 2.0 * ta; - return t_total; -} - -double PathShifter::calcFeasibleVelocityFromJerk( - const double lateral, const double jerk, const double longitudinal_distance) -{ - const double j = std::abs(jerk); - const double l = std::abs(lateral); - const double d = std::abs(longitudinal_distance); - if (j < 1.0e-8) { - return 1.0e10; // TODO(Horibe) maybe invalid arg? - } - return d / (4.0 * std::pow(0.5 * l / j, 1.0 / 3.0)); -} - -double PathShifter::calcLateralDistFromJerk( - const double longitudinal, const double jerk, const double velocity) -{ - const double j = std::abs(jerk); - const double d = std::abs(longitudinal); - const double v = std::abs(velocity); - if (j < 1.0e-8) { - return 1.0e10; // TODO(Horibe) maybe invalid arg? - } - return 2.0 * std::pow(d / (4.0 * v), 3.0) * j; -} - -double PathShifter::calcLongitudinalDistFromJerk( - const double lateral, const double jerk, const double velocity) -{ - const double j = std::abs(jerk); - const double l = std::abs(lateral); - const double v = std::abs(velocity); - if (j < 1.0e-8) { - return 1.0e10; // TODO(Horibe) maybe invalid arg? - } - return 4.0 * std::pow(0.5 * l / j, 1.0 / 3.0) * v; -} - -double PathShifter::calcJerkFromLatLonDistance( - const double lateral, const double longitudinal, const double velocity) -{ - constexpr double ep = 1.0e-3; - const double lat = std::abs(lateral); - const double lon = std::max(std::abs(longitudinal), ep); - const double v = std::abs(velocity); - return 0.5 * lat * std::pow(4.0 * v / lon, 3); -} - -double PathShifter::getTotalShiftLength() const -{ - double sum = base_offset_; - for (const auto & l : shift_lines_) { - sum += l.end_shift_length; - } - return sum; -} - double PathShifter::getLastShiftLength() const { if (shift_lines_.empty()) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index a48e36e3599ac..57641306cfa2b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -15,7 +15,6 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" #include #include @@ -156,7 +155,7 @@ double calcLateralDistanceFromEgoToObject( return min_distance; } -double calcLongitudinalDistanceFromEgoToObject( +double calc_longitudinal_distance_from_ego_to_object( const Pose & ego_pose, const double base_link2front, const double base_link2rear, const PredictedObject & dynamic_object) { @@ -196,18 +195,12 @@ double calcLongitudinalDistanceFromEgoToObjects( double min_distance = std::numeric_limits::max(); for (const auto & object : dynamic_objects.objects) { min_distance = std::min( - min_distance, - calcLongitudinalDistanceFromEgoToObject(ego_pose, base_link2front, base_link2rear, object)); + min_distance, calc_longitudinal_distance_from_ego_to_object( + ego_pose, base_link2front, base_link2rear, object)); } return min_distance; } -template -bool exists(std::vector vec, T element) -{ - return std::find(vec.begin(), vec.end(), element) != vec.end(); -} - std::optional findIndexOutOfGoalSearchRange( const std::vector & points, const Pose & goal, const int64_t goal_lane_id, const double max_dist = std::numeric_limits::max()) @@ -253,7 +246,7 @@ std::optional findIndexOutOfGoalSearchRange( } // goal does not have z -bool setGoal( +bool set_goal( const double search_radius_range, [[maybe_unused]] const double search_rad_range, const PathWithLaneId & input, const Pose & goal, const int64_t goal_lane_id, PathWithLaneId * output_ptr) @@ -383,7 +376,7 @@ PathWithLaneId refinePathForGoal( filtered_path.points.back().point.longitudinal_velocity_mps = 0.0; } - if (setGoal( + if (set_goal( search_radius_range, search_rad_range, filtered_path, goal, goal_lane_id, &path_with_goal)) { return path_with_goal; @@ -896,25 +889,8 @@ double getArcLengthToTargetLanelet( const auto target_center_line = target_lane.centerline().basicLineString(); - Pose front_pose, back_pose; - - { - const auto front_point = lanelet::utils::conversion::toGeomMsgPt(target_center_line.front()); - const double front_yaw = lanelet::utils::getLaneletAngle(target_lane, front_point); - front_pose.position = front_point; - tf2::Quaternion tf_quat; - tf_quat.setRPY(0, 0, front_yaw); - front_pose.orientation = tf2::toMsg(tf_quat); - } - - { - const auto back_point = lanelet::utils::conversion::toGeomMsgPt(target_center_line.back()); - const double back_yaw = lanelet::utils::getLaneletAngle(target_lane, back_point); - back_pose.position = back_point; - tf2::Quaternion tf_quat; - tf_quat.setRPY(0, 0, back_yaw); - back_pose.orientation = tf2::toMsg(tf_quat); - } + const auto front_pose = to_geom_msg_pose(target_center_line.front(), target_lane); + const auto back_pose = to_geom_msg_pose(target_center_line.back(), target_lane); const auto arc_front = lanelet::utils::getArcCoordinates(lanelet_sequence, front_pose); const auto arc_back = lanelet::utils::getArcCoordinates(lanelet_sequence, back_pose); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_footprints.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_footprints.cpp new file mode 100644 index 0000000000000..debb8e0de6cce --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_footprints.cpp @@ -0,0 +1,134 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp" + +#include + +#include + +using autoware::test_utils::createPose; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; + +constexpr auto epsilon = 1e-6; + +TEST(FootprintTest, translate_polygon) +{ + using autoware::behavior_path_planner::drivable_area_expansion::translate_polygon; + + Polygon2d polygon; + polygon.outer() = { + Point2d{0.0, 0.0}, Point2d{1.0, 0.0}, Point2d{1.0, 1.0}, Point2d{0.0, 1.0}, Point2d{0.0, 0.0}}; + + Polygon2d translated_polygon = translate_polygon(polygon, 1.0, 2.0); + + ASSERT_EQ(translated_polygon.outer().size(), 5); + EXPECT_NEAR(translated_polygon.outer().at(0).x(), 1.0, epsilon); + EXPECT_NEAR(translated_polygon.outer().at(0).y(), 2.0, epsilon); + EXPECT_NEAR(translated_polygon.outer().at(1).x(), 2.0, epsilon); + EXPECT_NEAR(translated_polygon.outer().at(1).y(), 2.0, epsilon); + EXPECT_NEAR(translated_polygon.outer().at(2).x(), 2.0, epsilon); + EXPECT_NEAR(translated_polygon.outer().at(2).y(), 3.0, epsilon); + EXPECT_NEAR(translated_polygon.outer().at(3).x(), 1.0, epsilon); + EXPECT_NEAR(translated_polygon.outer().at(3).y(), 3.0, epsilon); + EXPECT_NEAR(translated_polygon.outer().at(4).x(), 1.0, epsilon); + EXPECT_NEAR(translated_polygon.outer().at(4).y(), 2.0, epsilon); +} + +TEST(FootprintTest, create_footprint) +{ + using autoware::behavior_path_planner::drivable_area_expansion::create_footprint; + + Polygon2d base_footprint; + base_footprint.outer() = { + Point2d{1.0, 1.0}, Point2d{2.0, 1.0}, Point2d{2.0, 2.0}, Point2d{1.0, 2.0}, Point2d{1.0, 1.0}}; + + // Condition: without rotation + auto pose = createPose(1.0, 2.0, 0.0, 0.0, 0.0, 0.0); + auto footprint = create_footprint(pose, base_footprint); + ASSERT_EQ(footprint.outer().size(), 5); + EXPECT_NEAR(footprint.outer().at(0).x(), 2.0, epsilon); + EXPECT_NEAR(footprint.outer().at(0).y(), 3.0, epsilon); + EXPECT_NEAR(footprint.outer().at(1).x(), 3.0, epsilon); + EXPECT_NEAR(footprint.outer().at(1).y(), 3.0, epsilon); + EXPECT_NEAR(footprint.outer().at(2).x(), 3.0, epsilon); + EXPECT_NEAR(footprint.outer().at(2).y(), 4.0, epsilon); + EXPECT_NEAR(footprint.outer().at(3).x(), 2.0, epsilon); + EXPECT_NEAR(footprint.outer().at(3).y(), 4.0, epsilon); + EXPECT_NEAR(footprint.outer().at(4).x(), 2.0, epsilon); + EXPECT_NEAR(footprint.outer().at(4).y(), 3.0, epsilon); + + // Condition: with rotation + pose = createPose(1.0, 2.0, 0.0, 0.0, 0.0, M_PI_2); + footprint = create_footprint(pose, base_footprint); + ASSERT_EQ(footprint.outer().size(), 5); + EXPECT_NEAR(footprint.outer().at(0).x(), 0.0, epsilon); + EXPECT_NEAR(footprint.outer().at(0).y(), 3.0, epsilon); + EXPECT_NEAR(footprint.outer().at(1).x(), 0.0, epsilon); + EXPECT_NEAR(footprint.outer().at(1).y(), 4.0, epsilon); + EXPECT_NEAR(footprint.outer().at(2).x(), -1.0, epsilon); + EXPECT_NEAR(footprint.outer().at(2).y(), 4.0, epsilon); + EXPECT_NEAR(footprint.outer().at(3).x(), -1.0, epsilon); + EXPECT_NEAR(footprint.outer().at(3).y(), 3.0, epsilon); + EXPECT_NEAR(footprint.outer().at(4).x(), 0.0, epsilon); + EXPECT_NEAR(footprint.outer().at(4).y(), 3.0, epsilon); +} + +TEST(FootprintTest, create_object_footprints) +{ + using autoware::behavior_path_planner::drivable_area_expansion::create_object_footprints; + + autoware_perception_msgs::msg::PredictedObjects objects; + autoware_perception_msgs::msg::PredictedObject object; + object.shape.dimensions.x = 4.0; + object.shape.dimensions.y = 2.0; + + // Add a predicted path + autoware_perception_msgs::msg::PredictedPath path; + auto pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + path.path.push_back(pose); + object.kinematics.predicted_paths.push_back(path); + + objects.objects.push_back(object); + + autoware::behavior_path_planner::drivable_area_expansion::DrivableAreaExpansionParameters params; + params.avoid_dynamic_objects = false; + params.dynamic_objects_extra_front_offset = 0.5; + params.dynamic_objects_extra_rear_offset = 0.5; + params.dynamic_objects_extra_left_offset = 0.5; + params.dynamic_objects_extra_right_offset = 0.5; + + // Condition: doesn't avoid dynamic objects + auto footprints = create_object_footprints(objects, params); + EXPECT_TRUE(footprints.empty()); + + // Condition: single object and single point path + params.avoid_dynamic_objects = true; + footprints = create_object_footprints(objects, params); + + ASSERT_EQ(footprints.size(), 1); + ASSERT_EQ(footprints.front().outer().size(), 5); + + EXPECT_NEAR(footprints.front().outer().at(0).x(), 2.5, epsilon); + EXPECT_NEAR(footprints.front().outer().at(0).y(), 1.5, epsilon); + EXPECT_NEAR(footprints.front().outer().at(1).x(), 2.5, epsilon); + EXPECT_NEAR(footprints.front().outer().at(1).y(), -1.5, epsilon); + EXPECT_NEAR(footprints.front().outer().at(2).x(), -2.5, epsilon); + EXPECT_NEAR(footprints.front().outer().at(2).y(), -1.5, epsilon); + EXPECT_NEAR(footprints.front().outer().at(3).x(), -2.5, epsilon); + EXPECT_NEAR(footprints.front().outer().at(3).y(), 1.5, epsilon); + EXPECT_NEAR(footprints.front().outer().at(4).x(), 2.5, epsilon); + EXPECT_NEAR(footprints.front().outer().at(4).y(), 1.5, epsilon); +} diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp new file mode 100644 index 0000000000000..a2058db51c9ea --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp @@ -0,0 +1,382 @@ +// Copyright 2024 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include +#include + +#include +#include + +using PredictedObject = autoware_perception_msgs::msg::PredictedObject; +using PredictedObjects = autoware_perception_msgs::msg::PredictedObjects; +using ObjectClassification = autoware_perception_msgs::msg::ObjectClassification; +using PoseWithCovariance = geometry_msgs::msg::PoseWithCovariance; +using TwistWithCovariance = geometry_msgs::msg::TwistWithCovariance; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_planning_msgs::msg::Trajectory; +using tier4_planning_msgs::msg::PathPointWithLaneId; + +using autoware::test_utils::createPose; +using autoware::test_utils::generateTrajectory; +using autoware::universe_utils::createPoint; + +constexpr double epsilon = 1e-6; + +std::vector trajectory_to_path_with_lane_id(const Trajectory & trajectory) +{ + std::vector path_with_lane_id; + PathPointWithLaneId path_point_with_lane_id; + for (const auto & point : trajectory.points) { + path_point_with_lane_id.point.pose = point.pose; + path_point_with_lane_id.point.lateral_velocity_mps = point.lateral_velocity_mps; + path_point_with_lane_id.point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; + path_point_with_lane_id.point.heading_rate_rps = point.heading_rate_rps; + path_with_lane_id.push_back(path_point_with_lane_id); + } + return path_with_lane_id; +} + +PredictedPath trajectory_to_predicted_path(const Trajectory & trajectory) +{ + PredictedPath path; + geometry_msgs::msg::Pose pose; + for (const auto & point : trajectory.points) { + pose = point.pose; + path.path.push_back(pose); + } + return path; +} + +TEST(BehaviorPathPlanningObjectsFiltering, velocity_filter) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::filter::velocity_filter; + + PredictedObject predicted_obj; + predicted_obj.kinematics.initial_twist_with_covariance.twist.linear.x = 4.0; + predicted_obj.kinematics.initial_twist_with_covariance.twist.linear.y = 3.0; + + EXPECT_TRUE(velocity_filter(predicted_obj, 4.0, 10.0)); + EXPECT_FALSE(velocity_filter(predicted_obj, 6.0, 10.0)); + EXPECT_FALSE(velocity_filter(predicted_obj, 2.0, 4.9)); + EXPECT_FALSE(velocity_filter(predicted_obj, 6.0, 2.0)); +} + +TEST(BehaviorPathPlanningObjectsFiltering, position_filter) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::filter::position_filter; + + auto current_pos = createPoint(0.0, 0.0, 0.0); + PredictedObject object; + object.kinematics.initial_pose_with_covariance.pose = createPose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0); + auto straight_trajectory = generateTrajectory(20, 1.0); + double forward_distance = 20.0; + double backward_distance = 1.0; + + std::vector empty_path; + auto straight_path = trajectory_to_path_with_lane_id(generateTrajectory(20, 1.0)); + auto curved_path = + trajectory_to_path_with_lane_id(generateTrajectory(20, 1.0, 0.0, 0.0, 0.01)); + + EXPECT_TRUE( + position_filter(object, straight_path, current_pos, forward_distance, -backward_distance)); + EXPECT_FALSE( + position_filter(object, empty_path, current_pos, forward_distance, -backward_distance)); + EXPECT_TRUE( + position_filter(object, curved_path, current_pos, forward_distance, -backward_distance)); + + forward_distance = 2.0; + EXPECT_FALSE( + position_filter(object, straight_path, current_pos, forward_distance, -backward_distance)); + EXPECT_FALSE( + position_filter(object, curved_path, current_pos, forward_distance, -backward_distance)); +} + +TEST(BehaviorPathPlanningObjectsFiltering, is_within_circle) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::filter::is_within_circle; + + auto object_pos = createPoint(0.0, 0.0, 0.0); + auto ref_point = createPoint(0.0, 0.0, 0.0); + double search_radius = 1.0; + + EXPECT_TRUE(is_within_circle(object_pos, ref_point, search_radius)); + + object_pos.x = 2.0; + object_pos.x = 2.0; + + EXPECT_FALSE(is_within_circle(object_pos, ref_point, search_radius)); + + ref_point.x = 2.5; + ref_point.y = 2.4; + object_pos.x = 3.0; + object_pos.y = 3.0; + + EXPECT_TRUE(is_within_circle(object_pos, ref_point, search_radius)); +} + +TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByVelocity) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::filterObjectsByVelocity; + + PredictedObjects objects; + PredictedObject slow_obj; + PredictedObject fast_obj; + double vel_thr = 5.0; + + slow_obj.object_id = autoware::universe_utils::generateUUID(); + slow_obj.kinematics.initial_twist_with_covariance.twist.linear.x = 2.0; + slow_obj.kinematics.initial_twist_with_covariance.twist.linear.y = 0.0; + objects.objects.push_back(slow_obj); + + fast_obj.object_id = autoware::universe_utils::generateUUID(); + fast_obj.kinematics.initial_twist_with_covariance.twist.linear.x = 10.0; + fast_obj.kinematics.initial_twist_with_covariance.twist.linear.y = 0.0; + objects.objects.push_back(fast_obj); + + auto filtered_obj = filterObjectsByVelocity(objects, vel_thr, false); + ASSERT_FALSE(filtered_obj.objects.empty()); + EXPECT_EQ(filtered_obj.objects.front().object_id, slow_obj.object_id); + + filtered_obj = filterObjectsByVelocity(objects, vel_thr, true); + ASSERT_FALSE(filtered_obj.objects.empty()); + EXPECT_EQ(filtered_obj.objects.front().object_id, fast_obj.object_id); + + vel_thr = 0.0; + filtered_obj = filterObjectsByVelocity(objects, vel_thr, false); + EXPECT_TRUE(filtered_obj.objects.empty()); + + filtered_obj = filterObjectsByVelocity(objects, vel_thr, true); + EXPECT_EQ(filtered_obj.objects.size(), 2); +} + +TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByPosition) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::filterObjectsByPosition; + using autoware::behavior_path_planner::utils::path_safety_checker::filterObjectsWithinRadius; + + auto current_pos = createPoint(0.0, 0.0, 0.0); + auto straight_path = trajectory_to_path_with_lane_id(generateTrajectory(20, 1.0)); + double forward_distance = 10.0; + double backward_distance = 1.0; + double search_radius = 10.0; + + PredictedObjects objects; + PredictedObject far_obj; + PredictedObject near_obj; + + near_obj.object_id = autoware::universe_utils::generateUUID(); + near_obj.kinematics.initial_pose_with_covariance.pose = createPose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0); + objects.objects.push_back(near_obj); + auto target_uuid = near_obj.object_id; + + far_obj.object_id = autoware::universe_utils::generateUUID(); + far_obj.kinematics.initial_pose_with_covariance.pose = createPose(50.0, 0.0, 0.0, 0.0, 0.0, 0.0); + objects.objects.push_back(far_obj); + + filterObjectsByPosition(objects, straight_path, current_pos, forward_distance, backward_distance); + + ASSERT_FALSE(objects.objects.empty()); + EXPECT_EQ(objects.objects.front().object_id, target_uuid); + + objects.objects.clear(); + objects.objects.push_back(far_obj); + objects.objects.push_back(near_obj); + + filterObjectsWithinRadius(objects, current_pos, search_radius); + ASSERT_FALSE(objects.objects.empty()); + EXPECT_EQ(objects.objects.front().object_id, target_uuid); +} + +TEST(BehaviorPathPlanningObjectsFiltering, createPredictedPath) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::createPredictedPath; + + std::shared_ptr< + autoware::behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams> + param = std::make_shared< + autoware::behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams>(); + + param->min_velocity = -1.0; + param->acceleration = 0.1; + param->max_velocity = 5.0; + param->time_horizon_for_front_object = 2.0; + param->time_horizon_for_rear_object = 1.0; + param->time_resolution = 0.5; + param->delay_until_departure = 0.1; + + auto vehicle_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + + std::vector empty_path; + auto predicted_path = createPredictedPath(param, empty_path, vehicle_pose, 0.0, 0, false, false); + EXPECT_TRUE(predicted_path.empty()); + + size_t i = 0; + auto straight_path = trajectory_to_path_with_lane_id(generateTrajectory(20, 1.0)); + predicted_path = createPredictedPath(param, straight_path, vehicle_pose, 0.0, 0, true, false); + + ASSERT_EQ(predicted_path.size(), 4); + for (const auto & point : predicted_path) { + auto time = + std::max(0.0, param->time_resolution * static_cast(i) - param->delay_until_departure); + EXPECT_NEAR(point.time, i * param->time_resolution, epsilon); + EXPECT_NEAR(point.pose.position.x, std::pow(time, 2.0) * param->acceleration * 0.5, epsilon); + EXPECT_NEAR(point.velocity, time * param->acceleration, epsilon); + i++; + } + + i = 0; + predicted_path = createPredictedPath(param, straight_path, vehicle_pose, 0.0, 0, false, false); + + ASSERT_EQ(predicted_path.size(), 2); + for (const auto & point : predicted_path) { + auto time = + std::max(0.0, param->time_resolution * static_cast(i) - param->delay_until_departure); + EXPECT_NEAR(point.time, i * param->time_resolution, epsilon); + EXPECT_NEAR(point.pose.position.x, std::pow(time, 2.0) * param->acceleration * 0.5, epsilon); + EXPECT_NEAR(point.velocity, time * param->acceleration, epsilon); + i++; + } +} + +TEST(BehaviorPathPlanningObjectsFiltering, transform) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::transform; + auto velocity = autoware::universe_utils::createVector3(2.0, 0.0, 0.0); + auto angular = autoware::universe_utils::createVector3(0.0, 0.0, 0.0); + + PredictedObject obj; + obj.object_id = autoware::universe_utils::generateUUID(); + obj.kinematics.initial_pose_with_covariance.pose = createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0); + obj.kinematics.initial_twist_with_covariance.twist = + autoware::universe_utils::createTwist(velocity, angular); + autoware_perception_msgs::msg::PredictedPath predicted_path; + auto straight_path = trajectory_to_predicted_path(generateTrajectory(5, 1.0)); + straight_path.confidence = 0.6; + straight_path.time_step.sec = 1.0; + obj.kinematics.predicted_paths.push_back(straight_path); + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.dimensions.x = 2.0; + obj.shape.dimensions.y = 1.0; + obj.shape.dimensions.z = 1.0; + + double safety_check_time_horizon = 2.0; + double safety_check_time_resolution = 0.5; + + auto extended_obj = transform(obj, safety_check_time_horizon, safety_check_time_resolution); + EXPECT_NEAR( + extended_obj.predicted_paths.front().confidence, + obj.kinematics.predicted_paths.front().confidence, epsilon); + ASSERT_FALSE(extended_obj.predicted_paths.empty()); + EXPECT_EQ(extended_obj.predicted_paths.front().path.size(), 5); + + size_t i = 0; + for (const auto & point : extended_obj.predicted_paths.front().path) { + EXPECT_NEAR(point.pose.position.x, i * 0.5, epsilon); + EXPECT_NEAR(point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(point.velocity, velocity.x, epsilon); + ASSERT_EQ(point.poly.outer().size(), 5); + + EXPECT_NEAR( + point.poly.outer().at(0).x(), point.pose.position.x + 0.5 * obj.shape.dimensions.x, epsilon); + EXPECT_NEAR( + point.poly.outer().at(0).y(), point.pose.position.y + 0.5 * obj.shape.dimensions.y, epsilon); + + EXPECT_NEAR( + point.poly.outer().at(1).x(), point.pose.position.x + 0.5 * obj.shape.dimensions.x, epsilon); + EXPECT_NEAR( + point.poly.outer().at(1).y(), point.pose.position.y - 0.5 * obj.shape.dimensions.y, epsilon); + + EXPECT_NEAR( + point.poly.outer().at(2).x(), point.pose.position.x - 0.5 * obj.shape.dimensions.x, epsilon); + EXPECT_NEAR( + point.poly.outer().at(2).y(), point.pose.position.y - 0.5 * obj.shape.dimensions.y, epsilon); + + EXPECT_NEAR( + point.poly.outer().at(3).x(), point.pose.position.x - 0.5 * obj.shape.dimensions.x, epsilon); + EXPECT_NEAR( + point.poly.outer().at(3).y(), point.pose.position.y + 0.5 * obj.shape.dimensions.y, epsilon); + i++; + } +} + +TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByClass) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::filterObjectsByClass; + + PredictedObjects objects; + PredictedObject car; + PredictedObject truck; + PredictedObject pedestrian; + ObjectClassification classification; + autoware::behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck types_to_check; + + car.object_id = autoware::universe_utils::generateUUID(); + classification.label = ObjectClassification::Type::CAR; + classification.probability = 1.0; + car.classification.push_back(classification); + objects.objects.push_back(car); + + truck.object_id = autoware::universe_utils::generateUUID(); + classification.label = ObjectClassification::Type::TRUCK; + classification.probability = 1.0; + truck.classification.push_back(classification); + objects.objects.push_back(truck); + + pedestrian.object_id = autoware::universe_utils::generateUUID(); + classification.label = ObjectClassification::Type::PEDESTRIAN; + classification.probability = 1.0; + pedestrian.classification.push_back(classification); + objects.objects.push_back(pedestrian); + + filterObjectsByClass(objects, types_to_check); + EXPECT_EQ(objects.objects.size(), 3); + + types_to_check.check_pedestrian = false; + filterObjectsByClass(objects, types_to_check); + EXPECT_EQ(objects.objects.size(), 2); + + types_to_check.check_car = false; + types_to_check.check_truck = false; + filterObjectsByClass(objects, types_to_check); + EXPECT_TRUE(objects.objects.empty()); +} + +TEST(BehaviorPathPlanningObjectsFiltering, isTargetObjectType) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::isTargetObjectType; + + autoware::behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck types_to_check; + PredictedObject obj; + ObjectClassification classification; + classification.label = ObjectClassification::Type::CAR; + classification.probability = 0.6; + obj.classification.push_back(classification); + classification.label = ObjectClassification::Type::TRUCK; + classification.probability = 0.4; + obj.classification.push_back(classification); + + EXPECT_TRUE(isTargetObjectType(obj, types_to_check)); + types_to_check.check_car = false; + EXPECT_FALSE(isTargetObjectType(obj, types_to_check)); + + obj.classification.front().probability = 0.4; + obj.classification.at(1).probability = 0.6; + EXPECT_TRUE(isTargetObjectType(obj, types_to_check)); + + obj.classification.at(1).label = ObjectClassification::Type::PEDESTRIAN; + EXPECT_TRUE(isTargetObjectType(obj, types_to_check)); +} diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_occupancy_grid_based_collision_detector.cpp new file mode 100644 index 0000000000000..b212cce551252 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_occupancy_grid_based_collision_detector.cpp @@ -0,0 +1,149 @@ +// Copyright 2024 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" + +#include + +#include + +using autoware::behavior_path_planner::OccupancyGridBasedCollisionDetector; +using autoware::behavior_path_planner::OccupancyGridMapParam; +using autoware::test_utils::createPose; + +class OccupancyGridBasedCollisionDetectorTest : public ::testing::Test +{ +protected: + OccupancyGridBasedCollisionDetector detector_; + nav_msgs::msg::OccupancyGrid costmap_; + OccupancyGridMapParam param_; + + void SetUp() override + { + costmap_.info.width = 40; + costmap_.info.height = 40; + costmap_.info.resolution = 0.25; + costmap_.data = std::vector(1600, 0); + costmap_.data[28 * costmap_.info.width + 28] = 100; + costmap_.info.origin = createPose(1.0, 1.0, 0.0, 0.0, 0.0, 0.0); + + param_.theta_size = 8; + param_.obstacle_threshold = 1; + param_.vehicle_shape.base2back = 0.0; + param_.vehicle_shape.length = 4.0; + param_.vehicle_shape.width = 2.0; + detector_.setParam(param_); + } +}; + +TEST_F(OccupancyGridBasedCollisionDetectorTest, discretize_angle) +{ + using autoware::behavior_path_planner::discretize_angle; + + int theta_size = 8; + + EXPECT_EQ(discretize_angle(0.0, theta_size), 0); + EXPECT_EQ(discretize_angle(M_PI * 0.25, theta_size), 1); + EXPECT_EQ(discretize_angle(2.5 * M_PI, theta_size), 2); +} + +TEST_F(OccupancyGridBasedCollisionDetectorTest, pose2index) +{ + using autoware::behavior_path_planner::pose2index; + + auto pose_local = createPose(2.0, 3.0, 0.0, 0.0, 0.0, 0.0); + + int theta_size = 8; + auto index = pose2index(costmap_, pose_local, theta_size); + + EXPECT_EQ(index.x, 8); + EXPECT_EQ(index.y, 12); + EXPECT_EQ(index.theta, 0); +} + +TEST_F(OccupancyGridBasedCollisionDetectorTest, index2pose) +{ + using autoware::behavior_path_planner::index2pose; + + autoware::behavior_path_planner::IndexXYT index{4, 6, 2}; + + int theta_size = 8; + auto pose_local = index2pose(costmap_, index, theta_size); + + EXPECT_DOUBLE_EQ(pose_local.position.x, 1.0); + EXPECT_DOUBLE_EQ(pose_local.position.y, 1.5); + EXPECT_DOUBLE_EQ(tf2::getYaw(pose_local.orientation), M_PI * 0.5); +} + +TEST_F(OccupancyGridBasedCollisionDetectorTest, global2local) +{ + using autoware::behavior_path_planner::global2local; + + auto pose_global = createPose(2.0, 2.0, 0.0, 0.0, 0.0, 0.0); + auto pose_local = global2local(costmap_, pose_global); + + EXPECT_DOUBLE_EQ(pose_local.position.x, 1.0); + EXPECT_DOUBLE_EQ(pose_local.position.y, 1.0); +} + +TEST_F(OccupancyGridBasedCollisionDetectorTest, detectCollision) +{ + using autoware::behavior_path_planner::IndexXYT; + + // Condition: map not set + IndexXYT base_index{0, 0, 0}; + base_index.x = 24; + base_index.y = 24; + EXPECT_FALSE(detector_.detectCollision(base_index, true)); + + // Condition: with object + detector_.setMap(costmap_); + EXPECT_TRUE(detector_.detectCollision(base_index, true)); + + // Condition: position without obstacle + base_index.x = 4; + base_index.y = 4; + EXPECT_FALSE(detector_.detectCollision(base_index, true)); + + // Condition: position out of range + base_index.x = -100; + base_index.y = -100; + EXPECT_TRUE(detector_.detectCollision(base_index, true)); + EXPECT_FALSE(detector_.detectCollision(base_index, false)); +} + +TEST_F(OccupancyGridBasedCollisionDetectorTest, hasObstacleOnPath) +{ + tier4_planning_msgs::msg::PathWithLaneId path; + detector_.setMap(costmap_); + + // Condition: empty path + EXPECT_FALSE(detector_.hasObstacleOnPath(path, true)); + + // Condition: no obstacle on path + size_t path_length = 10; + path.points.reserve(path_length); + for (size_t i = 0; i < path_length; i++) { + tier4_planning_msgs::msg::PathPointWithLaneId path_point; + path_point.point.pose = createPose(static_cast(i), 0.0, 0.0, 0.0, 0.0, 0.0); + path.points.push_back(path_point); + } + EXPECT_FALSE(detector_.hasObstacleOnPath(path, false)); + + // Condition: obstacle on path + for (size_t i = 0; i < path_length; i++) { + path.points.at(i).point.pose.position.y = 8.0; + } + EXPECT_TRUE(detector_.hasObstacleOnPath(path, false)); +} diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp new file mode 100644 index 0000000000000..4cb1b8ef14d5a --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp @@ -0,0 +1,315 @@ +// Copyright 2024 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include +#include + +#include +#include +#include + +#include + +constexpr double epsilon = 1e-6; + +using autoware::behavior_path_planner::PlannerData; +using autoware_planning_msgs::msg::Trajectory; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; + +using autoware::test_utils::generateTrajectory; + +PathWithLaneId trajectory_to_path_with_lane_id(const Trajectory & trajectory) +{ + PathWithLaneId path_with_lane_id; + PathPointWithLaneId path_point_with_lane_id; + for (const auto & point : trajectory.points) { + path_point_with_lane_id.point.pose = point.pose; + path_point_with_lane_id.point.lateral_velocity_mps = point.lateral_velocity_mps; + path_point_with_lane_id.point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; + path_point_with_lane_id.point.heading_rate_rps = point.heading_rate_rps; + path_with_lane_id.points.push_back(path_point_with_lane_id); + } + return path_with_lane_id; +} + +TEST(BehaviorPathPlanningParkingDepartureUtil, calcFeasibleDecelDistance) +{ + using autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance; + + auto data = std::make_shared(); + double velocity = 2.0; + double acceleration = 1.0; + double acceleration_limit = 2.0; + double jerk_limit = 1.0; + double target_velocity = 3.0; + + auto odometry = std::make_shared(); + odometry->pose.pose = autoware::test_utils::createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + odometry->twist.twist.linear.x = velocity; + auto accel_with_covariance = + std::make_shared(); + accel_with_covariance->accel.accel.linear.x = acceleration; + data->self_odometry = odometry; + data->self_acceleration = accel_with_covariance; + auto planner_data = std::static_pointer_cast(data); + + // condition: current velocity is slower than target velocity + auto distance = + calcFeasibleDecelDistance(planner_data, acceleration_limit, jerk_limit, target_velocity); + ASSERT_TRUE(distance.has_value()); + EXPECT_DOUBLE_EQ(distance.value(), 0.0); + + // condition: calculates deceleration distance + velocity = 5.0; + odometry->twist.twist.linear.x = velocity; + data->self_odometry = odometry; + planner_data = std::static_pointer_cast(data); + distance = + calcFeasibleDecelDistance(planner_data, acceleration_limit, jerk_limit, target_velocity); + ASSERT_TRUE(distance.has_value()); + EXPECT_NEAR(distance.value(), 18.7730133, epsilon); + + // condition: not valid condition + velocity = 0.3; + target_velocity = 0.0; + acceleration = -1.5; + jerk_limit = 0.25; + acceleration_limit = 2.0; + odometry->twist.twist.linear.x = velocity; + accel_with_covariance->accel.accel.linear.x = acceleration; + data->self_odometry = odometry; + data->self_acceleration = accel_with_covariance; + planner_data = std::static_pointer_cast(data); + distance = + calcFeasibleDecelDistance(planner_data, acceleration_limit, jerk_limit, target_velocity); + ASSERT_FALSE(distance.has_value()); +} + +TEST(BehaviorPathPlanningParkingDepartureUtil, modifyVelocityByDirection) +{ + using autoware::behavior_path_planner::utils::parking_departure::modifyVelocityByDirection; + + const double velocity = 3.0; + const double target_velocity = 2.0; + const double acceleration = 2.0; + + std::vector paths; + auto short_path = trajectory_to_path_with_lane_id(generateTrajectory(1, 1.0)); + auto long_path = + trajectory_to_path_with_lane_id(generateTrajectory(10, 1.0, -velocity)); + auto reverse_path = + trajectory_to_path_with_lane_id(generateTrajectory(10, -1.0, velocity, -M_PI)); + + paths.push_back(short_path); + paths.push_back(long_path); + paths.push_back(reverse_path); + + std::vector> terminal_vel_acc_pairs; + terminal_vel_acc_pairs.emplace_back(0.5, 0.5); + terminal_vel_acc_pairs.emplace_back(1.0, 1.0); + terminal_vel_acc_pairs.emplace_back(1.5, 1.5); + + modifyVelocityByDirection(paths, terminal_vel_acc_pairs, target_velocity, acceleration); + + // condition: number of point less than 2 + EXPECT_DOUBLE_EQ(terminal_vel_acc_pairs.at(0).first, 0.0); + EXPECT_DOUBLE_EQ(terminal_vel_acc_pairs.at(0).second, 0.0); + + // condition: forward driving + EXPECT_DOUBLE_EQ(terminal_vel_acc_pairs.at(1).first, target_velocity); + EXPECT_DOUBLE_EQ(terminal_vel_acc_pairs.at(1).second, acceleration); + for (const auto & point : paths.at(1).points) { + if (point == paths.at(1).points.back()) + EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, 0.0); + else + EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, velocity); + } + + // condition: reverse driving + EXPECT_DOUBLE_EQ(terminal_vel_acc_pairs.at(2).first, -target_velocity); + EXPECT_DOUBLE_EQ(terminal_vel_acc_pairs.at(2).second, -acceleration); + for (const auto & point : paths.at(2).points) { + if (point == paths.at(2).points.back()) + EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, 0.0); + else + EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, -velocity); + } +} + +TEST(BehaviorPathPlanningParkingDepartureUtil, updatePathProperty) +{ + using autoware::behavior_path_planner::utils::parking_departure::EgoPredictedPathParams; + using autoware::behavior_path_planner::utils::parking_departure::updatePathProperty; + + auto params = std::make_shared(); + params->min_acceleration = 1.0; + params->acceleration = 1.5; + params->max_velocity = 0.0; + auto pair_terminal_velocity_and_accel = std::make_pair(3.0, 2.0); + + updatePathProperty(params, pair_terminal_velocity_and_accel); + EXPECT_DOUBLE_EQ(params->max_velocity, 3.0); + EXPECT_DOUBLE_EQ(params->acceleration, 2.0); +} + +TEST(BehaviorPathPlanningParkingDepartureUtil, initializeCollisionCheckDebugMap) +{ + using autoware::behavior_path_planner::utils::parking_departure::initializeCollisionCheckDebugMap; + + autoware::behavior_path_planner::CollisionCheckDebugMap debug_map; + auto uuid1 = autoware::universe_utils::toBoostUUID(autoware::universe_utils::generateUUID()); + autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug debug_info; + debug_map[uuid1] = debug_info; + + initializeCollisionCheckDebugMap(debug_map); + ASSERT_TRUE(debug_map.empty()); +} + +TEST(BehaviorPathPlanningParkingDepartureUtil, getPairsTerminalVelocityAndAccel) +{ + using autoware::behavior_path_planner::utils::parking_departure::getPairsTerminalVelocityAndAccel; + + std::vector> pairs_terminal_velocity_and_accel; + pairs_terminal_velocity_and_accel.emplace_back(2.0, 1.0); + pairs_terminal_velocity_and_accel.emplace_back(0.05, -1.0); + + // condition: current path idx exceeds pairs size + auto pair = getPairsTerminalVelocityAndAccel(pairs_terminal_velocity_and_accel, 2); + EXPECT_DOUBLE_EQ(pair.first, 0.0); + EXPECT_DOUBLE_EQ(pair.first, 0.0); + + // condition: get current idx pair + pair = getPairsTerminalVelocityAndAccel(pairs_terminal_velocity_and_accel, 1); + EXPECT_DOUBLE_EQ(pair.first, 0.05); + EXPECT_DOUBLE_EQ(pair.second, -1.0); +} + +TEST(BehaviorPathPlanningParkingDepartureUtil, generateFeasibleStopPath) +{ + using autoware::behavior_path_planner::utils::parking_departure::generateFeasibleStopPath; + + auto data = std::make_shared(); + double velocity = 0.3; + double acceleration = -1.5; + double maximum_jerk = 0.25; + double maximum_deceleration = 2.0; + + auto odometry = std::make_shared(); + odometry->pose.pose = autoware::test_utils::createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + odometry->twist.twist.linear.x = velocity; + auto accel_with_covariance = + std::make_shared(); + accel_with_covariance->accel.accel.linear.x = acceleration; + data->self_odometry = odometry; + data->self_acceleration = accel_with_covariance; + auto planner_data = std::static_pointer_cast(data); + + std::optional stop_pose; + + // condition: empty path + PathWithLaneId path; + auto stop_path = + generateFeasibleStopPath(path, planner_data, stop_pose, maximum_deceleration, maximum_jerk); + EXPECT_FALSE(stop_path.has_value()); + + // condition: not valid condition for stop distance + path = trajectory_to_path_with_lane_id(generateTrajectory(10, 1.0, velocity)); + stop_path = + generateFeasibleStopPath(path, planner_data, stop_pose, maximum_deceleration, maximum_jerk); + EXPECT_FALSE(stop_path.has_value()); + + // condition: not valid condition for stop index + velocity = 5.0; + acceleration = 1.0; + odometry->twist.twist.linear.x = velocity; + accel_with_covariance->accel.accel.linear.x = acceleration; + data->self_odometry = odometry; + data->self_acceleration = accel_with_covariance; + planner_data = std::static_pointer_cast(data); + path = trajectory_to_path_with_lane_id(generateTrajectory(10, 1.0, velocity)); + stop_path = + generateFeasibleStopPath(path, planner_data, stop_pose, maximum_deceleration, maximum_jerk); + EXPECT_FALSE(stop_path.has_value()); + + // condition: valid condition + maximum_jerk = 5.0; + maximum_deceleration = -3.0; + stop_path = + generateFeasibleStopPath(path, planner_data, stop_pose, maximum_deceleration, maximum_jerk); + size_t i = 0; + ASSERT_TRUE(stop_path.has_value()); + for (const auto & point : stop_path->points) { + if (i < 7) + EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, velocity); + else + EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, 0.0); + i++; + } +} + +TEST(BehaviorPathPlanningParkingDepartureUtil, calcEndArcLength) +{ + using autoware::behavior_path_planner::utils::parking_departure::calcEndArcLength; + + lanelet::LineString3d left_bound; + lanelet::LineString3d right_bound; + + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1}); + lanelet::Lanelet lanelet{lanelet::InvalId, left_bound, right_bound}; + + lanelet::LineString3d centerline; + centerline.push_back(lanelet::Point3d{lanelet::InvalId, -1, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 0, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 1, 0}); + lanelet.setCenterline(centerline); + + lanelet::ConstLanelets road_lanes = {lanelet}; + + double s_start = 0.2; + double forward_path_length = 0.1; + auto goal_pose = autoware::test_utils::createPose(5.0, 5.0, 0.0, 0.0, 0.0, 0.0); + + // condition: goal pose not in lanelets + auto end_arc = calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + EXPECT_DOUBLE_EQ(end_arc.first, s_start + forward_path_length); + EXPECT_FALSE(end_arc.second); + + // condition: goal pose behind start + goal_pose.position.x = -0.9; + goal_pose.position.y = 0.0; + end_arc = calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + EXPECT_DOUBLE_EQ(end_arc.first, s_start + forward_path_length); + EXPECT_FALSE(end_arc.second); + + // condition: goal pose beyond start + goal_pose.position.x = 0.0; + end_arc = calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + EXPECT_DOUBLE_EQ(end_arc.first, s_start + forward_path_length); + EXPECT_FALSE(end_arc.second); + + // condition: path end is goal + goal_pose.position.x = -0.75; + end_arc = calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + EXPECT_DOUBLE_EQ(end_arc.first, 0.25); + EXPECT_TRUE(end_arc.second); +} diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp new file mode 100644 index 0000000000000..51d5fb7055a4f --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp @@ -0,0 +1,233 @@ +// Copyright 2024 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_path_planner_common/utils/utils.hpp" + +#include + +#include + +using autoware::universe_utils::Point2d; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::Trajectory; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; +using ObjectClassification = autoware_perception_msgs::msg::ObjectClassification; + +using autoware::test_utils::createPose; +using autoware::test_utils::generateTrajectory; + +PathWithLaneId trajectory_to_path_with_lane_id(const Trajectory & trajectory) +{ + PathWithLaneId path_with_lane_id; + PathPointWithLaneId path_point_with_lane_id; + for (const auto & point : trajectory.points) { + path_point_with_lane_id.point.pose = point.pose; + path_point_with_lane_id.point.lateral_velocity_mps = point.lateral_velocity_mps; + path_point_with_lane_id.point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; + path_point_with_lane_id.point.heading_rate_rps = point.heading_rate_rps; + path_with_lane_id.points.push_back(path_point_with_lane_id); + } + return path_with_lane_id; +} + +TEST(BehaviorPathPlanningUtilTest, l2Norm) +{ + using autoware::behavior_path_planner::utils::l2Norm; + + geometry_msgs::msg::Vector3 vector = autoware::universe_utils::createVector3(0.0, 0.0, 0.0); + auto norm = l2Norm(vector); + EXPECT_DOUBLE_EQ(norm, 0.0); + + vector = autoware::universe_utils::createVector3(1.0, 2.0, 2.0); + norm = l2Norm(vector); + EXPECT_DOUBLE_EQ(norm, 3.0); +} + +TEST(BehaviorPathPlanningUtilTest, checkCollisionBetweenPathFootprintsAndObjects) +{ + using autoware::behavior_path_planner::utils::checkCollisionBetweenPathFootprintsAndObjects; + + autoware::universe_utils::LinearRing2d base_footprint = { + Point2d{1.0, 1.0}, Point2d{1.0, -1.0}, Point2d{-1.0, -1.0}, Point2d{-1.0, 1.0}, + Point2d{1.0, -1.0}}; + double margin = 0.2; + PredictedObjects objs; + PredictedObject obj; + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.dimensions.x = 2.0; + obj.shape.dimensions.y = 2.0; + obj.kinematics.initial_pose_with_covariance.pose = createPose(9.0, 1.0, 0.0, 0.0, 0.0, 0.0); + objs.objects.push_back(obj); + + PathWithLaneId ego_path; + + // Condition: no path + EXPECT_FALSE( + checkCollisionBetweenPathFootprintsAndObjects(base_footprint, ego_path, objs, margin)); + + // Condition: object in front of path + ego_path = trajectory_to_path_with_lane_id(generateTrajectory(5, 1.0)); + EXPECT_FALSE( + checkCollisionBetweenPathFootprintsAndObjects(base_footprint, ego_path, objs, margin)); + + // Condition: object overlapping path + ego_path = trajectory_to_path_with_lane_id(generateTrajectory(10, 1.0)); + EXPECT_TRUE( + checkCollisionBetweenPathFootprintsAndObjects(base_footprint, ego_path, objs, margin)); +} + +TEST(BehaviorPathPlanningUtilTest, checkCollisionBetweenFootprintAndObjects) +{ + using autoware::behavior_path_planner::utils::checkCollisionBetweenFootprintAndObjects; + + auto ego_pose = createPose(1.0, 1.0, 0.0, 0.0, 0.0, 0.0); + autoware::universe_utils::LinearRing2d base_footprint = { + Point2d{1.0, 1.0}, Point2d{1.0, -1.0}, Point2d{-1.0, -1.0}, Point2d{-1.0, 1.0}, + Point2d{1.0, -1.0}}; + double margin = 0.2; + PredictedObjects objs; + + // Condition: no object + EXPECT_FALSE(checkCollisionBetweenFootprintAndObjects(base_footprint, ego_pose, objs, margin)); + + // Condition: no collision + PredictedObject obj; + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.dimensions.x = 2.0; + obj.shape.dimensions.y = 2.0; + obj.kinematics.initial_pose_with_covariance.pose = createPose(9.0, 9.0, 0.0, 0.0, 0.0, 0.0); + objs.objects.push_back(obj); + EXPECT_FALSE(checkCollisionBetweenFootprintAndObjects(base_footprint, ego_pose, objs, margin)); + + // Condition: collision + obj.kinematics.initial_pose_with_covariance.pose.position.x = 1.0; + obj.kinematics.initial_pose_with_covariance.pose.position.y = 1.0; + objs.objects.push_back(obj); + EXPECT_TRUE(checkCollisionBetweenFootprintAndObjects(base_footprint, ego_pose, objs, margin)); +} + +TEST(BehaviorPathPlanningUtilTest, calcLateralDistanceFromEgoToObject) +{ + using autoware::behavior_path_planner::utils::calcLateralDistanceFromEgoToObject; + + auto ego_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + double vehicle_width = 2.0; + + PredictedObject obj; + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.dimensions.x = 2.0; + obj.shape.dimensions.y = 2.0; + + // Condition: overlapping + obj.kinematics.initial_pose_with_covariance.pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + EXPECT_DOUBLE_EQ(calcLateralDistanceFromEgoToObject(ego_pose, vehicle_width, obj), 0.0); + + // Condition: object on left + obj.kinematics.initial_pose_with_covariance.pose.position.y = 5.0; + EXPECT_DOUBLE_EQ(calcLateralDistanceFromEgoToObject(ego_pose, vehicle_width, obj), 3.0); + + // Condition: object on right + obj.kinematics.initial_pose_with_covariance.pose.position.y = -5.0; + EXPECT_DOUBLE_EQ(calcLateralDistanceFromEgoToObject(ego_pose, vehicle_width, obj), 3.0); +} + +TEST(BehaviorPathPlanningUtilTest, calc_longitudinal_distance_from_ego_to_object) +{ + using autoware::behavior_path_planner::utils::calc_longitudinal_distance_from_ego_to_object; + + auto ego_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + PredictedObject obj; + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.dimensions.x = 2.0; + obj.shape.dimensions.y = 1.0; + + // Condition: overlapping + double base_link2front = 0.0; + double base_link2rear = 0.0; + obj.kinematics.initial_pose_with_covariance.pose = createPose(1.0, 0.0, 0.0, 0.0, 0.0, 0.0); + EXPECT_DOUBLE_EQ( + calc_longitudinal_distance_from_ego_to_object(ego_pose, base_link2front, base_link2rear, obj), + 0.0); + + // Condition: object in front + base_link2front = 1.0; + base_link2rear = -1.0; + obj.kinematics.initial_pose_with_covariance.pose.position.x = 4.0; + obj.kinematics.initial_pose_with_covariance.pose.position.y = 2.0; + EXPECT_DOUBLE_EQ( + calc_longitudinal_distance_from_ego_to_object(ego_pose, base_link2front, base_link2rear, obj), + 2.0); + + // Condition: object in rear + obj.kinematics.initial_pose_with_covariance.pose.position.x = -4.0; + EXPECT_DOUBLE_EQ( + calc_longitudinal_distance_from_ego_to_object(ego_pose, base_link2front, base_link2rear, obj), + 2.0); +} + +TEST(BehaviorPathPlanningUtilTest, calcLongitudinalDistanceFromEgoToObjects) +{ + using autoware::behavior_path_planner::utils::calcLongitudinalDistanceFromEgoToObjects; + + auto ego_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + double base_link2front = 1.0; + double base_link2rear = -1.0; + + PredictedObjects objs; + + // Condition: none object + EXPECT_DOUBLE_EQ( + calcLongitudinalDistanceFromEgoToObjects(ego_pose, base_link2front, base_link2rear, objs), + std::numeric_limits::max()); + + // Condition: both object in front + PredictedObject near_obj; + near_obj.kinematics.initial_pose_with_covariance.pose = createPose(5.0, 2.0, 0.0, 0.0, 0.0, 0.0); + near_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + near_obj.shape.dimensions.x = 2.0; + near_obj.shape.dimensions.y = 1.0; + + PredictedObject far_obj; + far_obj.kinematics.initial_pose_with_covariance.pose = createPose(25.0, 2.0, 0.0, 0.0, 0.0, 0.0); + far_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + far_obj.shape.dimensions.x = 5.0; + far_obj.shape.dimensions.y = 1.0; + + objs.objects.push_back(near_obj); + objs.objects.push_back(far_obj); + EXPECT_DOUBLE_EQ( + calcLongitudinalDistanceFromEgoToObjects(ego_pose, base_link2front, base_link2rear, objs), 3.0); +} + +TEST(BehaviorPathPlanningUtilTest, getHighestProbLabel) +{ + using autoware::behavior_path_planner::utils::getHighestProbLabel; + + PredictedObject obj; + ObjectClassification classification; + + // Condition: no classification + EXPECT_EQ(getHighestProbLabel(obj.classification), ObjectClassification::Type::UNKNOWN); + + // Condition: with 2 label + obj.classification.emplace_back(autoware_perception_msgs::build() + .label(ObjectClassification::CAR) + .probability(0.4)); + obj.classification.emplace_back(autoware_perception_msgs::build() + .label(ObjectClassification::TRUCK) + .probability(0.6)); + EXPECT_EQ(getHighestProbLabel(obj.classification), ObjectClassification::Type::TRUCK); +} 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 d831f65b23117..e1f6d0bb6af9c 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 @@ -20,6 +20,7 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/behavior_path_side_shift_module/utils.hpp" +#include #include #include @@ -346,7 +347,7 @@ ShiftLine SideShiftModule::calcShiftLine() const const double dist_to_end = [&]() { const double shift_length = requested_lateral_offset_ - getClosestShiftLength(); - const double jerk_shifting_distance = path_shifter_.calcLongitudinalDistFromJerk( + const double jerk_shifting_distance = autoware::motion_utils::calc_longitudinal_dist_from_jerk( shift_length, p->shifting_lateral_jerk, std::max(ego_speed, p->min_shifting_speed)); const double shifting_distance = std::max(jerk_shifting_distance, p->min_shifting_distance); const double dist_to_end = dist_to_start + shifting_distance; 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 3f750a7f1a87b..2d94ceb85de28 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 @@ -22,6 +22,7 @@ #include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include #include #include @@ -306,7 +307,7 @@ std::vector ShiftPullOut::calcPullOutPaths( path_shifter.setPath(road_lane_reference_path); const double shift_time = - PathShifter::calcShiftTimeFromJerk(shift_length, lateral_jerk, lateral_acc); + autoware::motion_utils::calc_shift_time_from_jerk(shift_length, lateral_jerk, lateral_acc); const double longitudinal_acc = std::clamp(road_velocity / shift_time, 0.0, /* max acc */ 1.0); const auto pull_out_distance = calcPullOutLongitudinalDistance( longitudinal_acc, shift_time, shift_length, maximum_curvature, 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 9df0791ed9375..f44bf204bd2d7 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 @@ -329,7 +329,7 @@ bool StartPlannerModule::noMovingObjectsAround() const utils::path_safety_checker::filterObjectsByClass( dynamic_objects, parameters_->surround_moving_obstacles_type_to_check); const auto filtered_objects = utils::path_safety_checker::filterObjectsByVelocity( - dynamic_objects, parameters_->th_moving_obstacle_velocity, false); + dynamic_objects, parameters_->th_moving_obstacle_velocity, true); if (!filtered_objects.objects.empty()) { DEBUG_PRINT("Moving objects exist in the safety check area"); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp index 0f3536eeb4e7c..b59dc9e91e23c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp @@ -20,6 +20,8 @@ #include "autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp" #include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp" +#include + #include #include #include @@ -118,26 +120,27 @@ class AvoidanceHelper const auto nominal_speed = std::max(getEgoSpeed(), p->nominal_avoidance_speed); const auto nominal_jerk = p->lateral_min_jerk_map.at(getConstraintsMapIndex(nominal_speed, p->velocity_map)); - return PathShifter::calcLongitudinalDistFromJerk(shift_length, nominal_jerk, nominal_speed); + return autoware::motion_utils::calc_longitudinal_dist_from_jerk( + shift_length, nominal_jerk, nominal_speed); } double getMinAvoidanceDistance(const double shift_length) const { const auto & p = parameters_; - return PathShifter::calcLongitudinalDistFromJerk( + return autoware::motion_utils::calc_longitudinal_dist_from_jerk( shift_length, p->lateral_max_jerk_map.front(), p->velocity_map.front()); } double getMaxAvoidanceDistance(const double shift_length) const { - const auto distance_from_jerk = PathShifter::calcLongitudinalDistFromJerk( + const auto distance_from_jerk = autoware::motion_utils::calc_longitudinal_dist_from_jerk( shift_length, getLateralMinJerkLimit(), getAvoidanceEgoSpeed()); return std::max(getNominalAvoidanceDistance(shift_length), distance_from_jerk); } double getSharpAvoidanceDistance(const double shift_length) const { - return PathShifter::calcLongitudinalDistFromJerk( + return autoware::motion_utils::calc_longitudinal_dist_from_jerk( shift_length, getLateralMaxJerkLimit(), getAvoidanceEgoSpeed()); } @@ -227,8 +230,8 @@ class AvoidanceHelper const auto max_shift_length = std::max( std::abs(parameters_->max_right_shift_length), std::abs(parameters_->max_left_shift_length)); - const auto dynamic_distance = - PathShifter::calcLongitudinalDistFromJerk(max_shift_length, getLateralMinJerkLimit(), speed); + const auto dynamic_distance = autoware::motion_utils::calc_longitudinal_dist_from_jerk( + max_shift_length, getLateralMinJerkLimit(), speed); return std::clamp( 1.5 * dynamic_distance + getNominalPrepareDistance(), @@ -312,7 +315,7 @@ class AvoidanceHelper { const auto JERK_BUFFER = 0.1; // [m/sss] return std::all_of(shift_lines.begin(), shift_lines.end(), [&](const auto & line) { - return PathShifter::calcJerkFromLatLonDistance( + return autoware::motion_utils::calc_jerk_from_lat_lon_distance( line.getRelativeLength(), line.getRelativeLongitudinal(), getAvoidanceEgoSpeed()) < getLateralMaxJerkLimit() + JERK_BUFFER; }); 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 4d13d4164d263..2c5950bfeae0c 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 @@ -1316,7 +1316,7 @@ void StaticObstacleAvoidanceModule::addNewShiftLines( const double road_velocity = avoid_data_.reference_path.points.at(front_new_shift_line.start_idx) .point.longitudinal_velocity_mps; - const double shift_time = PathShifter::calcShiftTimeFromJerk( + const double shift_time = autoware::motion_utils::calc_shift_time_from_jerk( front_new_shift_line.getRelativeLength(), helper_->getLateralMaxJerkLimit(), helper_->getLateralMaxAccelLimit()); const double longitudinal_acc = @@ -1673,7 +1673,7 @@ void StaticObstacleAvoidanceModule::insertReturnDeadLine( getEgoPosition(), to_stop_line - parameters_->stop_buffer, 0.0, shifted_path.path, stop_pose_); // insert slow down speed. - const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( + const double current_target_velocity = autoware::motion_utils::calc_feasible_velocity_from_jerk( shift_length, helper_->getLateralMinJerkLimit(), to_stop_line); if (current_target_velocity < getEgoSpeed()) { RCLCPP_DEBUG(getLogger(), "current velocity exceeds target slow down speed."); @@ -1692,7 +1692,7 @@ void StaticObstacleAvoidanceModule::insertReturnDeadLine( } // target speed with nominal jerk limits. - const double v_target = PathShifter::calcFeasibleVelocityFromJerk( + const double v_target = autoware::motion_utils::calc_feasible_velocity_from_jerk( shift_length, helper_->getLateralMinJerkLimit(), shift_longitudinal_distance); const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; const double v_insert = @@ -1880,7 +1880,7 @@ void StaticObstacleAvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_ const auto lower_speed = object.value().avoid_required ? 0.0 : parameters_->min_slow_down_speed; // insert slow down speed. - const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( + const double current_target_velocity = autoware::motion_utils::calc_feasible_velocity_from_jerk( shift_length, helper_->getLateralMinJerkLimit(), distance_to_object); if (current_target_velocity < getEgoSpeed() + parameters_->buf_slow_down_speed) { utils::static_obstacle_avoidance::insertDecelPoint( @@ -1901,7 +1901,7 @@ void StaticObstacleAvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_ } // target speed with nominal jerk limits. - const double v_target = PathShifter::calcFeasibleVelocityFromJerk( + const double v_target = autoware::motion_utils::calc_feasible_velocity_from_jerk( shift_length, helper_->getLateralMinJerkLimit(), shift_longitudinal_distance); const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; const double v_insert = std::max(v_target - parameters_->buf_slow_down_speed, lower_speed); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index 10d003fcad90f..917736b1680db 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -179,7 +179,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } // calculate lateral jerk. - const auto required_jerk = PathShifter::calcJerkFromLatLonDistance( + const auto required_jerk = autoware::motion_utils::calc_jerk_from_lat_lon_distance( avoiding_shift, avoidance_distance, helper_->getAvoidanceEgoSpeed()); // relax lateral jerk limit. avoidable. @@ -201,7 +201,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } // output avoidance path under lateral jerk constraints. - const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk( + const auto feasible_relative_shift_length = autoware::motion_utils::calc_lateral_dist_from_jerk( avoidance_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed()); if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) { 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 f56552485b5fc..4ec3e43ee91ca 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 @@ -353,13 +353,16 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( if (collision_state != CollisionState::YIELD) { continue; } + if ( isVehicleType(object.classification) && ego_crosswalk_passage_direction && collision_point.crosswalk_passage_direction) { - const double direction_diff = autoware::universe_utils::normalizeRadian( + double direction_diff = std::abs(std::fmod( collision_point.crosswalk_passage_direction.value() - - ego_crosswalk_passage_direction.value()); - if (std::fabs(direction_diff) < planner_param_.vehicle_object_cross_angle_threshold) { + ego_crosswalk_passage_direction.value(), + M_PI_2)); + direction_diff = std::min(direction_diff, M_PI_2 - direction_diff); + if (direction_diff < planner_param_.vehicle_object_cross_angle_threshold) { continue; } } @@ -670,8 +673,8 @@ std::pair CrosswalkModule::clampAttentionRangeByNeighborCrosswal std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( const PathWithLaneId & path) const { - auto findIntersectPoint = [&](const lanelet::ConstLineString3d line) - -> std::optional> { + auto findIntersectPoint = + [&](const lanelet::ConstLineString3d line) -> std::optional { const auto line_start = autoware::universe_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); const auto line_end = @@ -682,29 +685,19 @@ std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( if (const auto intersect = autoware::universe_utils::intersect(line_start, line_end, start, end); intersect.has_value()) { - return std::make_optional(std::make_pair(i, intersect.value())); + return intersect; } } return std::nullopt; }; - const auto intersect_pt1 = findIntersectPoint(crosswalk_.leftBound()); - const auto intersect_pt2 = findIntersectPoint(crosswalk_.rightBound()); + const auto pt1 = findIntersectPoint(crosswalk_.leftBound()); + const auto pt2 = findIntersectPoint(crosswalk_.rightBound()); - if (!intersect_pt1 || !intersect_pt2) { + if (!pt1 || !pt2) { return std::nullopt; } - const auto idx1 = intersect_pt1.value().first; - const auto idx2 = intersect_pt2.value().first; - - const auto min_idx = std::min(idx1, idx2); - const auto dist1 = calcSignedArcLength(path.points, min_idx, intersect_pt1.value().second); - const auto dist2 = calcSignedArcLength(path.points, min_idx, intersect_pt2.value().second); - - const auto & front = dist1 > dist2 ? intersect_pt2.value().second : intersect_pt1.value().second; - const auto & back = dist1 > dist2 ? intersect_pt1.value().second : intersect_pt2.value().second; - - return std::atan2(back.y - front.y, back.x - front.x); + return std::atan2(pt2->y - pt1->y, pt2->x - pt1->x); } std::optional CrosswalkModule::findObjectPassageDirectionAlongVehicleLane( 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 f4646b1b9eb6e..b710924410549 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 @@ -6,9 +6,16 @@ autoware_package() pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED - src/debug.cpp - src/manager.cpp - src/scene_no_stopping_area.cpp + DIRECTORY src ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_utils.cpp + ) + 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_no_stopping_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml index 21664f7596d60..18a35ea12d5f6 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 @@ -8,6 +8,7 @@ Kosuke Takeuchi Tomoya Kimura Shumpei Wakabayashi + Shumpei Wakabayashi Apache License 2.0 @@ -15,7 +16,6 @@ ament_cmake_auto autoware_cmake - eigen3_cmake_module autoware_behavior_velocity_planner_common autoware_interpolation @@ -23,20 +23,16 @@ autoware_motion_utils autoware_perception_msgs autoware_planning_msgs - autoware_route_handler autoware_universe_utils autoware_vehicle_info_utils - eigen geometry_msgs libboost-dev pluginlib rclcpp - tf2 - tf2_eigen - tf2_geometry_msgs tier4_planning_msgs visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp index 09f1908b107d5..b9509e308d1fc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp @@ -13,17 +13,13 @@ // limitations under the License. #include "scene_no_stopping_area.hpp" +#include "utils.hpp" #include #include #include #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif #include #include @@ -33,13 +29,13 @@ namespace autoware::behavior_velocity_planner namespace { const double marker_lifetime = 0.2; -using DebugData = NoStoppingAreaModule::DebugData; +using DebugData = no_stopping_area::DebugData; using autoware::universe_utils::appendMarkerArray; using autoware::universe_utils::createDefaultMarker; using autoware::universe_utils::createMarkerColor; using autoware::universe_utils::createMarkerScale; -lanelet::BasicPoint3d getCentroidPoint(const lanelet::BasicPolygon3d & poly) +lanelet::BasicPoint3d get_centroid_point(const lanelet::BasicPolygon3d & poly) { lanelet::BasicPoint3d p_sum{0.0, 0.0, 0.0}; for (const auto & p : poly) { @@ -48,7 +44,7 @@ lanelet::BasicPoint3d getCentroidPoint(const lanelet::BasicPolygon3d & poly) return p_sum / poly.size(); } -geometry_msgs::msg::Point toMsg(const lanelet::BasicPoint3d & point) +geometry_msgs::msg::Point to_msg(const lanelet::BasicPoint3d & point) { geometry_msgs::msg::Point msg; msg.x = point.x(); @@ -57,7 +53,7 @@ geometry_msgs::msg::Point toMsg(const lanelet::BasicPoint3d & point) return msg; } -visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( +visualization_msgs::msg::MarkerArray create_lanelet_info_marker_array( const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const rclcpp::Time & now) { visualization_msgs::msg::MarkerArray msg; @@ -65,7 +61,7 @@ visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( // ID { auto marker = createDefaultMarker( - "map", now, "no_stopping_area_id", no_stopping_area_reg_elem.id(), + "map", now, "no_stopping_area_id", static_cast(no_stopping_area_reg_elem.id()), visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); @@ -73,7 +69,7 @@ visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( for (const auto & detection_area : no_stopping_area_reg_elem.noStoppingAreas()) { const auto poly = detection_area.basicPolygon(); - marker.pose.position = toMsg(poly.front()); + marker.pose.position = to_msg(poly.front()); marker.pose.position.z += 2.0; marker.text = std::to_string(no_stopping_area_reg_elem.id()); @@ -84,7 +80,7 @@ visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( // Polygon { auto marker = createDefaultMarker( - "map", now, "no_stopping_area_polygon", no_stopping_area_reg_elem.id(), + "map", now, "no_stopping_area_polygon", static_cast(no_stopping_area_reg_elem.id()), visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(0.1, 0.1, 1.0, 0.500)); marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); @@ -99,8 +95,8 @@ visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( const auto & p_front = poly.at(idx_front); const auto & p_back = poly.at(idx_back); - marker.points.push_back(toMsg(p_front)); - marker.points.push_back(toMsg(p_back)); + marker.points.push_back(to_msg(p_front)); + marker.points.push_back(to_msg(p_back)); } } msg.markers.push_back(marker); @@ -112,16 +108,17 @@ visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( const auto stop_line_center_point = (stop_line.value().front().basicPoint() + stop_line.value().back().basicPoint()) / 2; auto marker = createDefaultMarker( - "map", now, "no_stopping_area_correspondence", no_stopping_area_reg_elem.id(), + "map", now, "no_stopping_area_correspondence", + static_cast(no_stopping_area_reg_elem.id()), visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(0.1, 0.1, 1.0, 0.500)); marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); for (const auto & detection_area : no_stopping_area_reg_elem.noStoppingAreas()) { const auto poly = detection_area.basicPolygon(); - const auto centroid_point = getCentroidPoint(poly); + const auto centroid_point = get_centroid_point(poly); for (size_t i = 0; i < poly.size(); ++i) { - marker.points.push_back(toMsg(centroid_point)); - marker.points.push_back(toMsg(stop_line_center_point)); + marker.points.push_back(to_msg(centroid_point)); + marker.points.push_back(to_msg(stop_line_center_point)); } } msg.markers.push_back(marker); @@ -137,7 +134,7 @@ visualization_msgs::msg::MarkerArray NoStoppingAreaModule::createDebugMarkerArra const rclcpp::Time now = clock_->now(); appendMarkerArray( - createLaneletInfoMarkerArray(no_stopping_area_reg_elem_, now), &debug_marker_array, now); + create_lanelet_info_marker_array(no_stopping_area_reg_elem_, now), &debug_marker_array, now); if (!debug_data_.stuck_points.empty()) { appendMarkerArray( 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 4631195ec47fc..9d66aa6672d36 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 @@ -16,17 +16,12 @@ #include #include -#include - -#include #include #include #include #include -#include #include -#include 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 1abd36d585d51..7009e94612580 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 @@ -37,7 +37,7 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC const char * getModuleName() override { return "no_stopping_area"; } private: - NoStoppingAreaModule::PlannerParam planner_param_; + NoStoppingAreaModule::PlannerParam planner_param_{}; void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; 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 ac58c62036e26..031f2976da0a7 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 @@ -14,26 +14,22 @@ #include "scene_no_stopping_area.hpp" -#include -#include +#include "utils.hpp" + +#include #include #include #include #include #include +#include +#include #include +#include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif -#include -#include -#include -#include +#include #include namespace autoware::behavior_velocity_planner @@ -43,7 +39,7 @@ namespace bg = boost::geometry; 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 PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id), @@ -56,55 +52,6 @@ NoStoppingAreaModule::NoStoppingAreaModule( state_machine_.setMarginTime(planner_param_.state_clear_time); } -boost::optional NoStoppingAreaModule::getStopLineGeometry2d( - const tier4_planning_msgs::msg::PathWithLaneId & path, const double stop_line_margin) const -{ - // get stop line from map - { - const auto & stop_line = no_stopping_area_reg_elem_.stopLine(); - if (stop_line) { - return planning_utils::extendLine( - stop_line.value()[0], stop_line.value()[1], planner_data_->stop_line_extend_length); - } - } - // auto gen stop line - { - LineString2d stop_line; - /** - * @brief auto gen no stopping area stop line from area polygon if stop line is not set - * --------------- - * ------col-------------|--> ego path - * | Area | - * --------------- - **/ - - for (const auto & no_stopping_area : no_stopping_area_reg_elem_.noStoppingAreas()) { - const auto & area_poly = lanelet::utils::to2D(no_stopping_area).basicPolygon(); - lanelet::BasicLineString2d path_line; - for (size_t i = 0; i < path.points.size() - 1; ++i) { - const auto p0 = path.points.at(i).point.pose.position; - const auto p1 = path.points.at(i + 1).point.pose.position; - const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}}; - std::vector collision_points; - bg::intersection(area_poly, line, collision_points); - if (!collision_points.empty()) { - const double yaw = autoware::universe_utils::calcAzimuthAngle(p0, p1); - const double w = planner_data_->vehicle_info_.vehicle_width_m; - const double l = stop_line_margin; - stop_line.emplace_back( - -l * std::cos(yaw) + collision_points.front().x() + w * std::cos(yaw + M_PI_2), - collision_points.front().y() + w * std::sin(yaw + M_PI_2)); - stop_line.emplace_back( - -l * std::cos(yaw) + collision_points.front().x() + w * std::cos(yaw - M_PI_2), - collision_points.front().y() + w * std::sin(yaw - M_PI_2)); - return stop_line; - } - } - } - } - return {}; -} - bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { // Store original path @@ -115,12 +62,16 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason return true; } // Reset data - debug_data_ = DebugData(); + debug_data_ = no_stopping_area::DebugData(); debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; *stop_reason = planning_utils::initializeStopReason(StopReason::NO_STOPPING_AREA); + const no_stopping_area::EgoData ego_data(*planner_data_); + // Get stop line geometry - const auto stop_line = getStopLineGeometry2d(original_path, planner_param_.stop_line_margin); + const auto stop_line = no_stopping_area::get_stop_line_geometry2d( + original_path, no_stopping_area_reg_elem_, planner_param_.stop_line_margin, + planner_data_->stop_line_extend_length, planner_data_->vehicle_info_.vehicle_width_m); if (!stop_line) { setSafe(true); return true; @@ -146,14 +97,16 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason const double margin = planner_param_.stop_line_margin; const double ego_space_in_front_of_stuck_vehicle = margin + vi.vehicle_length_m + planner_param_.stuck_vehicle_front_margin; - const Polygon2d stuck_vehicle_detect_area = generateEgoNoStoppingAreaLanePolygon( - *path, current_pose->pose, ego_space_in_front_of_stuck_vehicle, - planner_param_.detection_area_length); + const Polygon2d stuck_vehicle_detect_area = + no_stopping_area::generate_ego_no_stopping_area_lane_polygon( + *path, current_pose->pose, no_stopping_area_reg_elem_, ego_space_in_front_of_stuck_vehicle, + planner_param_.detection_area_length, planner_param_.path_expand_width, logger_, *clock_); const double ego_space_in_front_of_stop_line = margin + planner_param_.stop_margin + vi.rear_overhang_m; - const Polygon2d stop_line_detect_area = generateEgoNoStoppingAreaLanePolygon( - *path, current_pose->pose, ego_space_in_front_of_stop_line, - planner_param_.detection_area_length); + const Polygon2d stop_line_detect_area = + no_stopping_area::generate_ego_no_stopping_area_lane_polygon( + *path, current_pose->pose, no_stopping_area_reg_elem_, ego_space_in_front_of_stop_line, + planner_param_.detection_area_length, planner_param_.path_expand_width, logger_, *clock_); if (stuck_vehicle_detect_area.outer().empty() && stop_line_detect_area.outer().empty()) { setSafe(true); return true; @@ -162,26 +115,28 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason debug_data_.stop_line_detect_area = toGeomPoly(stop_line_detect_area); // Find stuck vehicle in no stopping area const bool is_entry_prohibited_by_stuck_vehicle = - checkStuckVehiclesInNoStoppingArea(stuck_vehicle_detect_area, predicted_obj_arr_ptr); + check_stuck_vehicles_in_no_stopping_area(stuck_vehicle_detect_area, predicted_obj_arr_ptr); // Find stop line in no stopping area const bool is_entry_prohibited_by_stop_line = - checkStopLinesInNoStoppingArea(*path, stop_line_detect_area); + no_stopping_area::check_stop_lines_in_no_stopping_area( + *path, stop_line_detect_area, debug_data_); const bool is_entry_prohibited = is_entry_prohibited_by_stuck_vehicle || is_entry_prohibited_by_stop_line; - if (!isStoppable(current_pose->pose, stop_point->second)) { + if (!no_stopping_area::is_stoppable( + pass_judge_, current_pose->pose, stop_point->second, ego_data, logger_, *clock_)) { state_machine_.setState(StateMachine::State::GO); setSafe(true); return false; - } else { - state_machine_.setStateWithMarginTime( - is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO, - logger_.get_child("state_machine"), *clock_); } + state_machine_.setStateWithMarginTime( + is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO, + logger_.get_child("state_machine"), *clock_); + setSafe(state_machine_.getState() != StateMachine::State::STOP); if (!isActivated()) { // ----------------stop reason and stop point-------------------------- - insertStopPoint(*path, *stop_point); + no_stopping_area::insert_stop_point(*path, *stop_point); // For virtual wall debug_data_.stop_poses.push_back(stop_pose); @@ -208,19 +163,19 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason } } else if (state_machine_.getState() == StateMachine::State::GO) { // reset pass judge if current state is go - is_stoppable_ = true; - pass_judged_ = false; + pass_judge_.is_stoppable = true; + pass_judge_.pass_judged = false; } return true; } -bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( +bool NoStoppingAreaModule::check_stuck_vehicles_in_no_stopping_area( const Polygon2d & poly, const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr & predicted_obj_arr_ptr) { // stuck points by predicted objects for (const auto & object : predicted_obj_arr_ptr->objects) { - if (!isTargetStuckVehicleType(object)) { + if (!no_stopping_area::is_vehicle_type(object)) { continue; // not target vehicle type } const auto obj_v = std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x); @@ -248,182 +203,4 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( } return false; } -bool NoStoppingAreaModule::checkStopLinesInNoStoppingArea( - const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly) -{ - const double stop_vel = std::numeric_limits::min(); - - // if the detected stop point is near goal, it's ignored. - static constexpr double close_to_goal_distance = 1.0; - - // stuck points by stop line - for (size_t i = 0; i < path.points.size() - 1; ++i) { - const auto p0 = path.points.at(i).point.pose.position; - const auto p1 = path.points.at(i + 1).point.pose.position; - const auto v0 = path.points.at(i).point.longitudinal_velocity_mps; - const auto v1 = path.points.at(i + 1).point.longitudinal_velocity_mps; - if (v0 > stop_vel && v1 > stop_vel) { - continue; - } - // judge if stop point p0 is near goal, by its distance to the path end. - const double dist_to_path_end = - autoware::motion_utils::calcSignedArcLength(path.points, i, path.points.size() - 1); - if (dist_to_path_end < close_to_goal_distance) { - // exit with false, cause position is near goal. - return false; - } - - const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}}; - std::vector collision_points; - bg::intersection(poly, line, collision_points); - if (!collision_points.empty()) { - geometry_msgs::msg::Point point; - point.x = collision_points.front().x(); - point.y = collision_points.front().y(); - point.z = 0.0; - debug_data_.stuck_points.emplace_back(point); - return true; - } - } - return false; -} - -Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, - const double margin, const double extra_dist) const -{ - Polygon2d ego_area; // open polygon - double dist_from_start_sum = 0.0; - const double interpolation_interval = 0.5; - bool is_in_area = false; - tier4_planning_msgs::msg::PathWithLaneId interpolated_path; - if (!splineInterpolate(path, interpolation_interval, interpolated_path, logger_)) { - return ego_area; - } - auto & pp = interpolated_path.points; - /* calc closest index */ - const auto closest_idx_opt = - autoware::motion_utils::findNearestIndex(interpolated_path.points, ego_pose, 3.0, M_PI_4); - if (!closest_idx_opt) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger_, *clock_, 1000 /* ms */, "autoware::motion_utils::findNearestIndex fail"); - return ego_area; - } - const size_t closest_idx = closest_idx_opt.value(); - - const int num_ignore_nearest = 1; // Do not consider nearest lane polygon - size_t ego_area_start_idx = closest_idx + num_ignore_nearest; - // return if area size is not intentional - if (no_stopping_area_reg_elem_.noStoppingAreas().size() != 1) { - return ego_area; - } - const auto no_stopping_area = no_stopping_area_reg_elem_.noStoppingAreas().front(); - for (size_t i = closest_idx + num_ignore_nearest; i < pp.size() - 1; ++i) { - dist_from_start_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); - const auto & p = pp.at(i).point.pose.position; - if (bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { - is_in_area = true; - break; - } - if (dist_from_start_sum > extra_dist) { - return ego_area; - } - ++ego_area_start_idx; - } - if (ego_area_start_idx > num_ignore_nearest) { - ego_area_start_idx--; - } - if (!is_in_area) { - return ego_area; - } - double dist_from_area_sum = 0.0; - // decide end idx with extract distance - size_t ego_area_end_idx = ego_area_start_idx; - for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) { - dist_from_start_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); - const auto & p = pp.at(i).point.pose.position; - if (!bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { - dist_from_area_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); - } - if (dist_from_start_sum > extra_dist || dist_from_area_sum > margin) { - break; - } - ++ego_area_end_idx; - } - - const auto width = planner_param_.path_expand_width; - ego_area = planning_utils::generatePathPolygon( - interpolated_path, ego_area_start_idx, ego_area_end_idx, width); - return ego_area; -} - -bool NoStoppingAreaModule::isTargetStuckVehicleType( - const autoware_perception_msgs::msg::PredictedObject & object) const -{ - if ( - object.classification.front().label == - autoware_perception_msgs::msg::ObjectClassification::CAR || - object.classification.front().label == - autoware_perception_msgs::msg::ObjectClassification::BUS || - object.classification.front().label == - autoware_perception_msgs::msg::ObjectClassification::TRUCK || - object.classification.front().label == - autoware_perception_msgs::msg::ObjectClassification::TRAILER || - object.classification.front().label == - autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { - return true; - } - return false; -} - -bool NoStoppingAreaModule::isStoppable( - const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const -{ - // get vehicle info and compute pass_judge_line_distance - const auto current_velocity = planner_data_->current_velocity->twist.linear.x; - const auto current_acceleration = planner_data_->current_acceleration->accel.accel.linear.x; - const double max_acc = planner_data_->max_stop_acceleration_threshold; - const double max_jerk = planner_data_->max_stop_jerk_threshold; - const double delay_response_time = planner_data_->delay_response_time; - const double stoppable_distance = planning_utils::calcJudgeLineDistWithJerkLimit( - current_velocity, current_acceleration, max_acc, max_jerk, delay_response_time); - const double signed_arc_length = - arc_lane_utils::calcSignedDistance(self_pose, line_pose.position); - const bool distance_stoppable = stoppable_distance < signed_arc_length; - const bool slow_velocity = planner_data_->current_velocity->twist.linear.x < 2.0; - // ego vehicle is high speed and can't stop before stop line -> GO - const bool not_stoppable = !distance_stoppable && !slow_velocity; - // stoppable or not is judged only once - RCLCPP_DEBUG( - logger_, "stoppable_dist: %lf signed_arc_length: %lf", stoppable_distance, signed_arc_length); - if (!distance_stoppable && !pass_judged_) { - pass_judged_ = true; - // can't stop using maximum brake consider jerk limit - if (not_stoppable) { - // pass through - is_stoppable_ = false; - RCLCPP_WARN_THROTTLE( - logger_, *clock_, 1000, "[NoStoppingArea] can't stop in front of no stopping area"); - } else { - is_stoppable_ = true; - } - } - return is_stoppable_; -} - -void NoStoppingAreaModule::insertStopPoint( - tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point) -{ - size_t insert_idx = static_cast(stop_point.first + 1); - const auto stop_pose = stop_point.second; - - // To PathPointWithLaneId - tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; - stop_point_with_lane_id = path.points.at(insert_idx); - stop_point_with_lane_id.point.pose = stop_pose; - stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; - - // Insert stop point or replace with zero velocity - planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); -} } // namespace autoware::behavior_velocity_planner 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 2c553ac457ef0..886c8f04dc702 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 @@ -15,48 +15,25 @@ #ifndef SCENE_NO_STOPPING_AREA_HPP_ #define SCENE_NO_STOPPING_AREA_HPP_ -#define EIGEN_MPL2_ONLY +#include "utils.hpp" -#include #include #include #include #include #include -#include #include #include #include -#include - -#include -#include - #include -#include -#include namespace autoware::behavior_velocity_planner { -using PathIndexWithPose = std::pair; // front index, pose -using PathIndexWithPoint2d = std::pair; // front index, point2d -using PathIndexWithOffset = std::pair; // front index, offset - class NoStoppingAreaModule : public SceneModuleInterface { public: - struct DebugData - { - double base_link2front; - std::vector stop_poses; - geometry_msgs::msg::Pose first_stop_pose; - std::vector stuck_points; - geometry_msgs::msg::Polygon stuck_vehicle_detect_area; - geometry_msgs::msg::Polygon stop_line_detect_area; - }; - struct PlannerParam { /** @@ -76,11 +53,10 @@ class NoStoppingAreaModule : public SceneModuleInterface double path_expand_width; //! [m] path width to calculate the edge line for both side }; -public: 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 PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; @@ -91,77 +67,19 @@ class NoStoppingAreaModule : public SceneModuleInterface private: const int64_t lane_id_; - mutable bool pass_judged_ = false; - mutable bool is_stoppable_ = true; + no_stopping_area::PassJudge pass_judge_; StateMachine state_machine_; //! for state - /** - * @brief check if the object has a target type for stuck check - * @param object target object - * @return true if the object has a target type - */ - bool isTargetStuckVehicleType( - const autoware_perception_msgs::msg::PredictedObject & object) const; - /** * @brief Check if there is a stopped vehicle in stuck vehicle detect area. * @param poly ego focusing area polygon * @param objects_ptr target objects * @return true if exists */ - bool checkStuckVehiclesInNoStoppingArea( + bool check_stuck_vehicles_in_no_stopping_area( const Polygon2d & poly, const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr & predicted_obj_arr_ptr); - /** - * @brief Check if there is a stop line in "stop line detect area". - * @param path ego-car lane - * @param poly ego focusing area polygon - * @return true if exists - */ - bool checkStopLinesInNoStoppingArea( - const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly); - - /** - * @brief Calculate the polygon of the path from the ego-car position to the end of the - * no stopping lanelet (+ extra distance). - * @param path ego-car lane - * @param ego_pose ego-car pose - * @param margin margin from the end point of the ego-no stopping area lane - * @param extra_dist extra distance from the end point of the no stopping area lanelet - * @return generated polygon - */ - Polygon2d generateEgoNoStoppingAreaLanePolygon( - const tier4_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Pose & ego_pose, const double margin, const double extra_dist) const; - - /** - * @brief Calculate the polygon of the path from the ego-car position to the end of the - * no stopping lanelet (+ extra distance). - * @param path ego-car lane - * @param stop_line_margin stop line margin from the stopping area lane - * @return generated stop line - */ - boost::optional getStopLineGeometry2d( - const tier4_planning_msgs::msg::PathWithLaneId & path, const double stop_line_margin) const; - - /** - * @brief Calculate if it's possible for ego-vehicle to stop before area consider jerk limit - * @param self_pose ego-car pose - * @param line_pose stop line pose on the lane - * @return is stoppable in front of no stopping area - */ - bool isStoppable( - const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const; - - /** - * @brief insert stop point on ego path - * @param path original path - * @param stop_point stop line point on the lane - */ - void insertStopPoint( - tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point); - // Key Feature const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem_; std::shared_ptr last_obstacle_found_time_; @@ -170,7 +88,7 @@ class NoStoppingAreaModule : public SceneModuleInterface PlannerParam planner_param_; // Debug - DebugData debug_data_; + no_stopping_area::DebugData debug_data_; }; } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp new file mode 100644 index 0000000000000..5159233c341ed --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp @@ -0,0 +1,262 @@ +// 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 "utils.hpp" + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include + +namespace autoware::behavior_velocity_planner::no_stopping_area +{ + +bool is_vehicle_type(const autoware_perception_msgs::msg::PredictedObject & object) +{ + // TODO(anyone): should we switch to using functions from the common object_recognition_utils ? + return !object.classification.empty() && + (object.classification.front().label == + autoware_perception_msgs::msg::ObjectClassification::CAR || + object.classification.front().label == + autoware_perception_msgs::msg::ObjectClassification::BUS || + object.classification.front().label == + autoware_perception_msgs::msg::ObjectClassification::TRUCK || + object.classification.front().label == + autoware_perception_msgs::msg::ObjectClassification::TRAILER || + object.classification.front().label == + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE); +} + +void insert_stop_point( + tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point) +{ + auto insert_idx = stop_point.first + 1UL; + const auto & stop_pose = stop_point.second; + + // To PathPointWithLaneId + tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + stop_point_with_lane_id = path.points.at(insert_idx); + stop_point_with_lane_id.point.pose = stop_pose; + stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; + + // Insert stop point or replace with zero velocity + planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); +} + +std::optional generate_stop_line( + const tier4_planning_msgs::msg::PathWithLaneId & path, + const lanelet::ConstPolygons3d & no_stopping_areas, const double ego_width, + const double stop_line_margin) +{ + LineString2d stop_line; + for (const auto & no_stopping_area : no_stopping_areas) { + const auto & area_poly = lanelet::utils::to2D(no_stopping_area).basicPolygon(); + for (size_t i = 0; i < path.points.size() - 1; ++i) { + const auto p0 = path.points.at(i).point.pose.position; + const auto p1 = path.points.at(i + 1).point.pose.position; + const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}}; + std::vector collision_points; + boost::geometry::intersection(area_poly, line, collision_points); + if (!collision_points.empty()) { + const double yaw = autoware::universe_utils::calcAzimuthAngle(p0, p1); + const double w = ego_width; + const double l = stop_line_margin; + stop_line.emplace_back( + -l * std::cos(yaw) + collision_points.front().x() + w * std::cos(yaw + M_PI_2), + collision_points.front().y() + w * std::sin(yaw + M_PI_2)); + stop_line.emplace_back( + -l * std::cos(yaw) + collision_points.front().x() + w * std::cos(yaw - M_PI_2), + collision_points.front().y() + w * std::sin(yaw - M_PI_2)); + return stop_line; + } + } + } + return {}; +} + +bool is_stoppable( + PassJudge & pass_judge, const geometry_msgs::msg::Pose & self_pose, + const geometry_msgs::msg::Pose & line_pose, const EgoData & ego_data, + const rclcpp::Logger & logger, rclcpp::Clock & clock) +{ + // compute pass_judge_line_distance + const double stoppable_distance = planning_utils::calcJudgeLineDistWithJerkLimit( + ego_data.current_velocity, ego_data.current_acceleration, ego_data.max_stop_acc, + ego_data.max_stop_jerk, ego_data.delay_response_time); + const double signed_arc_length = + arc_lane_utils::calcSignedDistance(self_pose, line_pose.position); + const bool distance_stoppable = stoppable_distance < signed_arc_length; + const bool slow_velocity = ego_data.current_velocity < 2.0; + // ego vehicle is high speed and can't stop before stop line -> GO + const bool not_stoppable = !distance_stoppable && !slow_velocity; + // stoppable or not is judged only once + RCLCPP_DEBUG( + logger, "stoppable_dist: %lf signed_arc_length: %lf", stoppable_distance, signed_arc_length); + if (!distance_stoppable && !pass_judge.pass_judged) { + pass_judge.pass_judged = true; + // can't stop using maximum brake consider jerk limit + if (not_stoppable) { // TODO(someone): this can be replaced by !slow_velocity, is this correct? + // pass through + pass_judge.is_stoppable = false; + RCLCPP_WARN_THROTTLE( + logger, clock, 1000, "[NoStoppingArea] can't stop in front of no stopping area"); + } else { + pass_judge.is_stoppable = true; + } + } + return pass_judge.is_stoppable; +} + +Polygon2d generate_ego_no_stopping_area_lane_polygon( + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, + const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const double margin, + const double max_polygon_length, const double path_expand_width, const rclcpp::Logger & logger, + rclcpp::Clock & clock) +{ + Polygon2d ego_area; // open polygon + double dist_from_start_sum = 0.0; + constexpr double interpolation_interval = 0.5; + bool is_in_area = false; + tier4_planning_msgs::msg::PathWithLaneId interpolated_path; + if (!splineInterpolate(path, interpolation_interval, interpolated_path, logger)) { + return ego_area; + } + const auto & pp = interpolated_path.points; + /* calc closest index */ + const auto closest_idx_opt = autoware::motion_utils::findNearestIndex(pp, ego_pose, 3.0, M_PI_4); + if (!closest_idx_opt) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger, clock, 1000 /* ms */, "autoware::motion_utils::findNearestIndex fail"); + return ego_area; + } + const size_t closest_idx = closest_idx_opt.value(); + + const int num_ignore_nearest = 1; // Do not consider nearest lane polygon + size_t ego_area_start_idx = closest_idx + num_ignore_nearest; + // return if area size is not intentional + if (no_stopping_area_reg_elem.noStoppingAreas().size() != 1) { + return ego_area; + } + const auto no_stopping_area = no_stopping_area_reg_elem.noStoppingAreas().front(); + for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) { + dist_from_start_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + const auto & p = pp.at(i).point.pose.position; + // TODO(someone): within will skip points on the edge of polygons so some points can be skipped + // depending on the interpolation + if (bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { + is_in_area = true; + break; + } + if (dist_from_start_sum > max_polygon_length) { + return ego_area; + } + ++ego_area_start_idx; + } + if (ego_area_start_idx > num_ignore_nearest) { + // TODO(someone): check if this is a bug + // this -1 causes pp[ego_area_start_idx] to be outside the no_stopping_area + // it causes "dist_from_area" to count the first point in the next loop + // moreover it causes the "dist_from_start_sum" to count the same point twice + ego_area_start_idx--; + } + if (!is_in_area) { + return ego_area; + } + double dist_from_area_sum = 0.0; + // decide end idx with extract distance + size_t ego_area_end_idx = ego_area_start_idx; + for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) { + dist_from_start_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + const auto & p = pp.at(i).point.pose.position; + // TODO(someone): within will skip points on the edge of polygons so some points can be skipped + // depending on the interpolation + if (!bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { + dist_from_area_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + } + if (dist_from_start_sum > max_polygon_length || dist_from_area_sum > margin) { + break; + } + ++ego_area_end_idx; + } + + ego_area = planning_utils::generatePathPolygon( + interpolated_path, ego_area_start_idx, ego_area_end_idx, path_expand_width); + return ego_area; +} + +bool check_stop_lines_in_no_stopping_area( + const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly, + no_stopping_area::DebugData & debug_data) +{ + const double stop_vel = std::numeric_limits::min(); + + // if the detected stop point is near goal, it's ignored. + static constexpr double close_to_goal_distance = 1.0; + + // stuck points by stop line + for (size_t i = 0; i + 1 < path.points.size(); ++i) { + const auto p0 = path.points.at(i).point.pose.position; + const auto p1 = path.points.at(i + 1).point.pose.position; + const auto v0 = path.points.at(i).point.longitudinal_velocity_mps; + const auto v1 = path.points.at(i + 1).point.longitudinal_velocity_mps; + if (v0 > stop_vel && v1 > stop_vel) { + continue; + } + // judge if stop point p0 is near goal, by its distance to the path end. + const double dist_to_path_end = + autoware::motion_utils::calcSignedArcLength(path.points, i, path.points.size() - 1); + if (dist_to_path_end < close_to_goal_distance) { + // exit with false, cause position is near goal. + return false; + } + + const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}}; + std::vector collision_points; + bg::intersection(poly, line, collision_points); + if (!collision_points.empty()) { + geometry_msgs::msg::Point point; + point.x = collision_points.front().x(); + point.y = collision_points.front().y(); + point.z = 0.0; + debug_data.stuck_points.emplace_back(point); + return true; + } + } + return false; +} + +std::optional get_stop_line_geometry2d( + const tier4_planning_msgs::msg::PathWithLaneId & path, + const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, + const double stop_line_margin, const double stop_line_extend_length, const double vehicle_width) +{ + const auto & stop_line = no_stopping_area_reg_elem.stopLine(); + if (stop_line && stop_line->size() >= 2) { + // get stop line from map + return planning_utils::extendLine( + stop_line.value()[0], stop_line.value()[1], stop_line_extend_length); + } + return generate_stop_line( + path, no_stopping_area_reg_elem.noStoppingAreas(), vehicle_width, stop_line_margin); +} + +} // namespace autoware::behavior_velocity_planner::no_stopping_area diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp new file mode 100644 index 0000000000000..73b835b4701dc --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp @@ -0,0 +1,165 @@ +// 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 UTILS_HPP_ +#define UTILS_HPP_ + +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner::no_stopping_area +{ +using PathIndexWithPose = std::pair; // front index, pose +using PathIndexWithPoint2d = std::pair; // front index, point2d +using PathIndexWithOffset = std::pair; // front index, offset + +struct PassJudge +{ + bool pass_judged = false; + bool is_stoppable = true; +}; + +struct DebugData +{ + double base_link2front; + std::vector stop_poses; + geometry_msgs::msg::Pose first_stop_pose; + std::vector stuck_points; + geometry_msgs::msg::Polygon stuck_vehicle_detect_area; + geometry_msgs::msg::Polygon stop_line_detect_area; +}; + +// intermediate data about the ego vehicle taken from the PlannerData +struct EgoData +{ + EgoData() = default; + explicit EgoData(const PlannerData & planner_data) + { + current_velocity = planner_data.current_velocity->twist.linear.x; + current_acceleration = planner_data.current_acceleration->accel.accel.linear.x; + max_stop_acc = planner_data.max_stop_acceleration_threshold; + max_stop_jerk = planner_data.max_stop_jerk_threshold; + delay_response_time = planner_data.delay_response_time; + } + + double current_velocity{}; + double current_acceleration{}; + double max_stop_acc{}; + double max_stop_jerk{}; + double delay_response_time{}; +}; + +/** + * @brief check if the object is a vehicle (car, bus, truck, trailer, motorcycle) + * @param object input object + * @return true if the object is a vehicle + */ +bool is_vehicle_type(const autoware_perception_msgs::msg::PredictedObject & object); + +/** + * @brief insert stop point on ego path + * @param path original path + * @param stop_point stop line point on the lane + */ +void insert_stop_point( + tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point); + +/** + * @brief generate stop line from no stopping area polygons + * ________________ + * ------|--|--------------|--> ego path + * stop | | Area | + * line | L______________/ + * @param path input path + * @param no_stopping_areas no stopping area polygons + * @param ego_width [m] width of ego + * @param stop_line_margin [m] margin to keep between the stop line and the no stopping areas + **/ +std::optional generate_stop_line( + const tier4_planning_msgs::msg::PathWithLaneId & path, + const lanelet::ConstPolygons3d & no_stopping_areas, const double ego_width, + const double stop_line_margin); + +/** + * @brief Calculate if it's possible for ego-vehicle to stop before area consider jerk limit + * @param [inout] pass_judge the pass judge decision to update + * @param self_pose ego-car pose + * @param line_pose stop line pose on the lane + * @param ego_data planner data with ego pose, velocity, etc + * @param logger ros logger + * @param clock ros clock + * @return is stoppable in front of no stopping area + */ +bool is_stoppable( + PassJudge & pass_judge, const geometry_msgs::msg::Pose & self_pose, + const geometry_msgs::msg::Pose & line_pose, const EgoData & ego_data, + const rclcpp::Logger & logger, rclcpp::Clock & clock); + +/** + * @brief Calculate the polygon of the path from the ego-car position to the end of the + * no stopping lanelet (+ extra distance). + * @param path ego-car lane + * @param ego_pose ego-car pose + * @param margin margin from the end point of the ego-no stopping area lane + * @param max_polygon_length maximum length of the polygon + * @return generated polygon + */ +Polygon2d generate_ego_no_stopping_area_lane_polygon( + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, + const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const double margin, + const double max_polygon_length, const double path_expand_width, const rclcpp::Logger & logger, + rclcpp::Clock & clock); + +/** + * @brief Check if there is a stop line in "stop line detect area". + * @param path ego-car lane + * @param poly ego focusing area polygon + * @param [out] debug_data structure to store the stuck points for debugging + * @return true if exists + */ +bool check_stop_lines_in_no_stopping_area( + const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly, + DebugData & debug_data); + +/** + * @brief Calculate the stop line of a no stopping area + * @details use the stop line of the regulatory element if it exists, otherwise generate it + * @param path ego path + * @param no_stopping_area_reg_elem no_stopping_area regulatory element + * @param stop_line_margin [m] margin between the stop line and the start of the no stopping area + * @param stop_line_extend_length [m] extra length to add on each side of the stop line (only added + * to the stop line of the regulatory element) + * @param vehicle_width [m] width of the ego vehicle + * @return generated stop line + */ +std::optional get_stop_line_geometry2d( + const tier4_planning_msgs::msg::PathWithLaneId & path, + const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, + const double stop_line_margin, const double stop_line_extend_length, const double vehicle_width); + +} // namespace autoware::behavior_velocity_planner::no_stopping_area + +#endif // UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp new file mode 100644 index 0000000000000..cc5bbd2903a52 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp @@ -0,0 +1,438 @@ +// 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 "../src/utils.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include + +template +bool point_in_polygon(const Point & p, const Polygon & poly) +{ + return std::find_if(poly.outer().begin(), poly.outer().end(), [&](const auto & o) { + return p.x() == o.x() && p.y() == o.y(); + }) != poly.outer().end(); +} + +tier4_planning_msgs::msg::PathWithLaneId generate_straight_path( + const size_t nb_points, const float velocity = 0.0, const double resolution = 1.0) +{ + tier4_planning_msgs::msg::PathWithLaneId path; + for (auto x = 0UL; x < nb_points; ++x) { + tier4_planning_msgs::msg::PathPointWithLaneId p; + p.point.pose.position.x = resolution * static_cast(x); + p.point.pose.position.y = 0.0; + p.point.longitudinal_velocity_mps = velocity; + path.points.push_back(p); + } + return path; +} + +TEST(NoStoppingAreaTest, isTargetStuckVehicleType) +{ + using autoware::behavior_velocity_planner::no_stopping_area::is_vehicle_type; + autoware_perception_msgs::msg::PredictedObject object; + EXPECT_NO_FATAL_FAILURE(is_vehicle_type(object)); + autoware_perception_msgs::msg::ObjectClassification classification; + for (const auto label : { + autoware_perception_msgs::msg::ObjectClassification::CAR, + autoware_perception_msgs::msg::ObjectClassification::BUS, + autoware_perception_msgs::msg::ObjectClassification::TRUCK, + autoware_perception_msgs::msg::ObjectClassification::TRAILER, + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + }) { + classification.label = label; + object.classification = {classification}; + EXPECT_TRUE(is_vehicle_type(object)); + } +} + +TEST(NoStoppingAreaTest, insertStopPoint) +{ + using autoware::behavior_velocity_planner::no_stopping_area::insert_stop_point; + constexpr auto nb_points = 10; + constexpr auto default_velocity = 5.0; + const tier4_planning_msgs::msg::PathWithLaneId base_path = + generate_straight_path(nb_points, default_velocity); + autoware::behavior_velocity_planner::no_stopping_area::PathIndexWithPose stop_point; + // stop exactly at a point, expect no new points but a 0 velocity at the stop point and after + auto path = base_path; + stop_point.first = 4UL; + stop_point.second = path.points[stop_point.first].point.pose; + insert_stop_point(path, stop_point); + ASSERT_EQ(path.points.size(), nb_points); + for (auto i = 0UL; i < stop_point.first; ++i) { + EXPECT_EQ(path.points[i].point.longitudinal_velocity_mps, default_velocity); + } + for (auto i = stop_point.first; i < nb_points; ++i) { + EXPECT_EQ(path.points[i].point.longitudinal_velocity_mps, 0.0); + } + + // stop between points + path = base_path; + stop_point.first = 3UL; + stop_point.second.position.x = 3.5; + insert_stop_point(path, stop_point); + ASSERT_EQ(path.points.size(), nb_points + 1); + EXPECT_EQ(path.points[stop_point.first + 1].point.pose.position.x, stop_point.second.position.x); + for (auto i = 0UL; i <= stop_point.first; ++i) { + EXPECT_EQ(path.points[i].point.longitudinal_velocity_mps, default_velocity); + } + for (auto i = stop_point.first + 1; i < nb_points + 1; ++i) { + EXPECT_EQ(path.points[i].point.longitudinal_velocity_mps, 0.0); + } + + // stop at the last point: exception // TODO(anyone): is this okay ? + path = base_path; + stop_point.first = path.points.size() - 1; + stop_point.second = path.points.back().point.pose; + EXPECT_THROW(insert_stop_point(path, stop_point), std::out_of_range); +} + +TEST(NoStoppingAreaTest, generateStopLine) +{ + using autoware::behavior_velocity_planner::no_stopping_area::generate_stop_line; + constexpr auto nb_points = 10; + const tier4_planning_msgs::msg::PathWithLaneId path = generate_straight_path(nb_points); + lanelet::ConstPolygons3d no_stopping_areas; + double ego_width = 0.0; + double stop_line_margin = 0.0; + // no areas, expect no stop line + auto stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin); + EXPECT_FALSE(stop_line.has_value()); + + // area outside of the path + lanelet::Polygon3d poly; + poly.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -2.0)); + poly.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -0.5)); + poly.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, -0.5)); + poly.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, -2.0)); + no_stopping_areas.push_back(poly); + stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin); + EXPECT_FALSE(stop_line.has_value()); + // area on the path, 0 margin and 0 width + poly.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, -1.0)); + poly.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, 1.0)); + poly.push_back(lanelet::Point3d(lanelet::InvalId, 6.0, 1.0)); + poly.push_back(lanelet::Point3d(lanelet::InvalId, 6.0, -1.0)); + no_stopping_areas.push_back(poly); + stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin); + EXPECT_TRUE(stop_line.has_value()); + ASSERT_EQ(stop_line->size(), 2UL); + EXPECT_EQ(stop_line->front().x(), 5.0); + EXPECT_EQ(stop_line->front().y(), 0.0); + EXPECT_EQ(stop_line->back().x(), 5.0); + EXPECT_EQ(stop_line->back().y(), 0.0); + // set a margin + stop_line_margin = 1.0; + stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin); + EXPECT_TRUE(stop_line.has_value()); + ASSERT_EQ(stop_line->size(), 2UL); + EXPECT_EQ(stop_line->front().x(), 4.0); + EXPECT_EQ(stop_line->front().y(), 0.0); + EXPECT_EQ(stop_line->back().x(), 4.0); + EXPECT_EQ(stop_line->back().y(), 0.0); + // set a width + ego_width = 2.0; + stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin); + EXPECT_TRUE(stop_line.has_value()); + ASSERT_EQ(stop_line->size(), 2UL); + // TODO(anyone): if we stop at this stop line ego overlaps the 1st area, is this okay ? + EXPECT_EQ(stop_line->front().x(), 4.0); + EXPECT_EQ(stop_line->front().y(), 2.0); + EXPECT_EQ(stop_line->back().x(), 4.0); + EXPECT_EQ(stop_line->back().y(), -2.0); +} + +TEST(NoStoppingAreaTest, isStoppable) +{ + using autoware::behavior_velocity_planner::no_stopping_area::is_stoppable; + autoware::behavior_velocity_planner::no_stopping_area::PassJudge pass_judge; + geometry_msgs::msg::Pose self_pose; + geometry_msgs::msg::Pose line_pose; + rclcpp::Clock clock; + auto logger = rclcpp::get_logger("test_logger"); + logger.set_level(rclcpp::Logger::Level::Debug); + autoware::behavior_velocity_planner::no_stopping_area::EgoData ego_data; + ego_data.current_velocity = 1.0; + ego_data.current_acceleration = 0.0; + ego_data.delay_response_time = 0.0; + ego_data.max_stop_acc = -1.0; + ego_data.max_stop_jerk = -1.0; + // try to stop 1m ahead + self_pose.position.x = 0.0; + self_pose.position.y = 0.0; + line_pose.position.x = 1.0; + line_pose.position.y = 0.0; + + // enough distance to stop and slow velocity + EXPECT_TRUE(is_stoppable(pass_judge, self_pose, line_pose, ego_data, logger, clock)); + EXPECT_TRUE(pass_judge.is_stoppable); + EXPECT_FALSE(pass_judge.pass_judged); + + // unstoppable and fast velocity + ego_data.current_velocity = 10.0; + EXPECT_FALSE(is_stoppable(pass_judge, self_pose, line_pose, ego_data, logger, clock)); + EXPECT_FALSE(pass_judge.is_stoppable); + EXPECT_TRUE(pass_judge.pass_judged); + + // unstoppable and slow velocity, but since we already judged, it is not updated + ego_data.current_velocity = 1.9; + EXPECT_FALSE(is_stoppable(pass_judge, self_pose, line_pose, ego_data, logger, clock)); + EXPECT_FALSE(pass_judge.is_stoppable); + EXPECT_TRUE(pass_judge.pass_judged); + + // reset and result is updated + // TODO(someone): is this the correct behavior ? distance is unstoppable but result is stoppable + pass_judge.pass_judged = false; + EXPECT_TRUE(is_stoppable(pass_judge, self_pose, line_pose, ego_data, logger, clock)); + EXPECT_TRUE(pass_judge.is_stoppable); + EXPECT_TRUE(pass_judge.pass_judged); +} + +TEST(NoStoppingAreaTest, generateEgoNoStoppingAreaLanePolygon) +{ + using autoware::behavior_velocity_planner::no_stopping_area:: + generate_ego_no_stopping_area_lane_polygon; + using autoware::universe_utils::Point2d; + geometry_msgs::msg::Pose ego_pose; // ego at (0,0) + ego_pose.position.x = 0.0; + ego_pose.position.y = 0.0; + const tier4_planning_msgs::msg::PathWithLaneId path = + generate_straight_path(8); // ego path at x = 0, 1,..., 7 and y=0 + double margin = 1.0; // desired margin added to the polygon after the end of the area + double max_polygon_length = 10.0; // maximum length of the generated polygon + double path_expand_width = 2.0; // width of the generated polygon + auto logger = rclcpp::get_logger("test_logger"); + logger.set_level(rclcpp::Logger::Level::Debug); + rclcpp::Clock clock; + + lanelet::Polygon3d no_stopping_area; + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -1.0)); + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, 1.0)); + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, 1.0)); + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, -1.0)); + const auto no_stopping_area_reg_elem = + lanelet::autoware::NoStoppingArea::make(lanelet::InvalId, {}, {no_stopping_area}, {}); + // basic case + { + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + path, ego_pose, *no_stopping_area_reg_elem, margin, max_polygon_length, path_expand_width, + logger, clock); + autoware::universe_utils::Polygon2d simplified_poly; + EXPECT_FALSE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + // simplify the polygon to make it easier to check + boost::geometry::simplify(lane_poly, simplified_poly, 1.0); + EXPECT_EQ(simplified_poly.outer().size(), 5UL); // closed polygon so 1st point == last point + // polygon over the path/no_stop_area overlap and with the desired width + // TODO(someone): we expect x=6 (end of area [5] + margin [1]) but current is 5.5 because a + // point is counted twice + // EXPECT_TRUE(point_in_polygon(Point2d(6.0, 2.0), simplified_poly)); + // EXPECT_TRUE(point_in_polygon(Point2d(6.0, -2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(3.0, 2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(3.0, -2.0), simplified_poly)); + } + + // big margin -> get a polygon until the end of the path + { + const double big_margin = 10.0; + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + path, ego_pose, *no_stopping_area_reg_elem, big_margin, max_polygon_length, path_expand_width, + logger, clock); + autoware::universe_utils::Polygon2d simplified_poly; + EXPECT_FALSE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + // simplify the polygon to make it easier to check + boost::geometry::simplify(lane_poly, simplified_poly, 1.0); + EXPECT_EQ(simplified_poly.outer().size(), 5UL); // closed polygon so 1st point == last point + // polygon over the path/no_stop_area overlap and with the desired width + EXPECT_TRUE(point_in_polygon(Point2d(7.0, 2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(7.0, -2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(3.0, 2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(3.0, -2.0), simplified_poly)); + } + // small max polygon length + { + const double small_polygon_length = 5.0; + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + path, ego_pose, *no_stopping_area_reg_elem, margin, small_polygon_length, path_expand_width, + logger, clock); + autoware::universe_utils::Polygon2d simplified_poly; + EXPECT_FALSE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + // simplify the polygon to make it easier to check + boost::geometry::simplify(lane_poly, simplified_poly, 1.0); + EXPECT_EQ(simplified_poly.outer().size(), 5UL); // closed polygon so 1st point == last point + // polygon over the path/no_stop_area overlap and with the desired width + // TODO(someone): the length calculation is currently buggy + // ego at x=0, no_stopping_area starts at x=3 so we expect a lane polygon of length = 2.0, but + // some point is counted twice so the length is only 1.5 (interpolation interval is 0.5) + // EXPECT_TRUE(point_in_polygon(Point2d(4.0, 2.0), simplified_poly)); + // EXPECT_TRUE(point_in_polygon(Point2d(4.0, -2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(3.0, 2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(3.0, -2.0), simplified_poly)); + } + + // cases where the polygon returned is empty + // path is empty + { + tier4_planning_msgs::msg::PathWithLaneId empty_path; + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + empty_path, ego_pose, *no_stopping_area_reg_elem, margin, max_polygon_length, + path_expand_width, logger, clock); + EXPECT_TRUE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + } + // path points do not enter the no stopping area + // ego is far from the path + { + auto far_ego_pose = ego_pose; + far_ego_pose.position.y = 10.0; + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + path, far_ego_pose, *no_stopping_area_reg_elem, margin, max_polygon_length, path_expand_width, + logger, clock); + EXPECT_TRUE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + } + // no stopping area starts after the minimum length + { + const double short_max_polygon_length = 2.0; + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + path, ego_pose, *no_stopping_area_reg_elem, margin, short_max_polygon_length, + path_expand_width, logger, clock); + EXPECT_TRUE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + } + // path crosses the no stopping area but the current interpolation (0.5) is not enough to detect + // it + // TODO(someone): should this be fixed ? + { + lanelet::Polygon3d small_no_stopping_area; + small_no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.1, -1.0)); + small_no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.1, 1.0)); + small_no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.4, 1.0)); + small_no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.4, -1.0)); + const auto small_no_stopping_area_reg_elem = + lanelet::autoware::NoStoppingArea::make(lanelet::InvalId, {}, {small_no_stopping_area}, {}); + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + path, ego_pose, *small_no_stopping_area_reg_elem, margin, max_polygon_length, + path_expand_width, logger, clock); + EXPECT_TRUE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + } +} + +TEST(NoStoppingAreaTest, checkStopLinesInNoStoppingArea) +{ + using autoware::behavior_velocity_planner::no_stopping_area::check_stop_lines_in_no_stopping_area; + tier4_planning_msgs::msg::PathWithLaneId path; + autoware::universe_utils::Polygon2d poly; + autoware::behavior_velocity_planner::no_stopping_area::DebugData debug_data; + + // empty inputs + EXPECT_FALSE(check_stop_lines_in_no_stopping_area(path, poly, debug_data)); + + constexpr auto nb_points = 10; + constexpr auto non_stopped_velocity = 5.0; + const tier4_planning_msgs::msg::PathWithLaneId non_stopping_path = + generate_straight_path(nb_points, non_stopped_velocity); + path = non_stopping_path; + // set x=4 and x=5 to be stopping points + path.points[4].point.longitudinal_velocity_mps = 0.0; + path.points[5].point.longitudinal_velocity_mps = 0.0; + // empty polygon + EXPECT_FALSE(check_stop_lines_in_no_stopping_area(path, poly, debug_data)); + // non stopping path + poly.outer().emplace_back(3.0, -1.0); + poly.outer().emplace_back(3.0, 1.0); + poly.outer().emplace_back(5.0, 1.0); + poly.outer().emplace_back(5.0, -1.0); + poly.outer().push_back(poly.outer().front()); // close the polygon + EXPECT_FALSE(check_stop_lines_in_no_stopping_area(non_stopping_path, poly, debug_data)); + // stop in the area + EXPECT_TRUE(check_stop_lines_in_no_stopping_area(path, poly, debug_data)); + // if stop in the area is within 1m of the end of the path we ignore it: only for 1st stop + path.points[8].point.longitudinal_velocity_mps = 0.0; + path.points[9].point.longitudinal_velocity_mps = 0.0; + EXPECT_TRUE(check_stop_lines_in_no_stopping_area(path, poly, debug_data)); + // if stop in the area is within 1m of the end of the path we ignore it + path.points[4].point.longitudinal_velocity_mps = non_stopped_velocity; + path.points[5].point.longitudinal_velocity_mps = non_stopped_velocity; + EXPECT_FALSE(check_stop_lines_in_no_stopping_area(path, poly, debug_data)); +} + +TEST(NoStoppingAreaTest, getStopLineGeometry2d) +{ + using autoware::behavior_velocity_planner::no_stopping_area::generate_stop_line; + using autoware::behavior_velocity_planner::no_stopping_area::get_stop_line_geometry2d; + const tier4_planning_msgs::msg::PathWithLaneId path = generate_straight_path(10); + lanelet::Polygon3d no_stopping_area; + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -1.0)); + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, 1.0)); + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, 1.0)); + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, -1.0)); + double stop_line_margin = 1.0; + double stop_line_extend_length = 1.0; + double vehicle_width = 1.0; + { // get stop line of the regulatory element extended by the extend length + lanelet::LineString3d reg_elem_stop_line; + reg_elem_stop_line.push_back(lanelet::Point3d(lanelet::InvalId, 0.0, 0.0)); + reg_elem_stop_line.push_back(lanelet::Point3d(lanelet::InvalId, 1.0, 0.0)); + const auto no_stopping_area_reg_elem = lanelet::autoware::NoStoppingArea::make( + lanelet::InvalId, {}, {no_stopping_area}, reg_elem_stop_line); + const auto stop_line = get_stop_line_geometry2d( + path, *no_stopping_area_reg_elem, stop_line_margin, stop_line_extend_length, vehicle_width); + ASSERT_TRUE(stop_line.has_value()); + ASSERT_EQ(stop_line->size(), 2UL); + EXPECT_EQ(stop_line->front().x(), -1.0); + EXPECT_EQ(stop_line->front().y(), 0.0); + EXPECT_EQ(stop_line->back().x(), 2.0); + EXPECT_EQ(stop_line->back().y(), 0.0); + } + { // regulatory element has no stop line -> get the same stop line as generate_stop_line + const auto no_stopping_area_reg_elem = + lanelet::autoware::NoStoppingArea::make(lanelet::InvalId, {}, {no_stopping_area}, {}); + const auto stop_line = get_stop_line_geometry2d( + path, *no_stopping_area_reg_elem, stop_line_margin, stop_line_extend_length, vehicle_width); + const auto generated_stop_line = + generate_stop_line(path, {no_stopping_area}, vehicle_width, stop_line_margin); + ASSERT_TRUE(stop_line.has_value()); + ASSERT_TRUE(generated_stop_line.has_value()); + ASSERT_EQ(stop_line->size(), generated_stop_line->size()); + for (auto i = 0UL; i < stop_line->size(); ++i) { + EXPECT_EQ(stop_line->at(i).x(), generated_stop_line->at(i).x()); + EXPECT_EQ(stop_line->at(i).y(), generated_stop_line->at(i).y()); + } + } +} 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 29f3794516ef5..9dd645f08fdad 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 @@ -274,7 +274,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) + static bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) { bool enable_rtc = true; 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 ce6cf92fc6450..b8fc098826e7a 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 @@ -22,6 +22,7 @@ autoware_behavior_velocity_crosswalk_module autoware_behavior_velocity_planner_common autoware_motion_utils + autoware_object_recognition_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler @@ -31,7 +32,6 @@ geometry_msgs libboost-dev message_filters - object_recognition_utils pcl_conversions pluginlib rclcpp 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 0a38e8d47ea36..5a219f654561a 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 @@ -37,7 +37,7 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using object_recognition_utils::convertLabelToString; +using autoware::object_recognition_utils::convertLabelToString; RunOutModule::RunOutModule( const int64_t module_id, const std::shared_ptr & planner_data, 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 7d8513485ac52..4f326aef298be 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 @@ -15,9 +15,9 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "debug.hpp" #include "dynamic_obstacle.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include "state_machine.hpp" #include "utils.hpp" 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 db5b02916a103..6370dd5e6c21d 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 @@ -9,8 +9,18 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp src/manager.cpp src/scene.cpp + src/utils.cpp ) +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_utils.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) +endif() + ament_auto_package(INSTALL_TO_SHARE config) install(PROGRAMS 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 45a17e8e8f598..51992a6fa40d3 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 @@ -37,8 +37,10 @@ traffic_light_utils visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common + autoware_test_utils ament_cmake 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 af16cd85741e5..af5eac943ec13 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 @@ -14,6 +14,8 @@ #include "scene.hpp" +#include "utils.hpp" + #include #include #include @@ -36,131 +38,6 @@ namespace autoware::behavior_velocity_planner { -namespace bg = boost::geometry; - -namespace -{ -bool getBackwardPointFromBasePoint( - const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, - const Eigen::Vector2d & base_point, const double backward_length, Eigen::Vector2d & output_point) -{ - Eigen::Vector2d line_vec = line_point2 - line_point1; - Eigen::Vector2d backward_vec = backward_length * line_vec.normalized(); - output_point = base_point + backward_vec; - return true; -} - -std::optional findNearestCollisionPoint( - const LineString2d & line1, const LineString2d & line2, const Point2d & origin) -{ - std::vector collision_points; - bg::intersection(line1, line2, collision_points); - - if (collision_points.empty()) { - return std::nullopt; - } - - // check nearest collision point - Point2d nearest_collision_point; - double min_dist = 0.0; - - for (size_t i = 0; i < collision_points.size(); ++i) { - double dist = bg::distance(origin, collision_points.at(i)); - if (i == 0 || dist < min_dist) { - min_dist = dist; - nearest_collision_point = collision_points.at(i); - } - } - return nearest_collision_point; -} - -bool createTargetPoint( - const tier4_planning_msgs::msg::PathWithLaneId & input, const LineString2d & stop_line, - const double offset, size_t & target_point_idx, Eigen::Vector2d & target_point) -{ - if (input.points.size() < 2) { - return false; - } - for (size_t i = 0; i < input.points.size() - 1; ++i) { - Point2d path_line_begin = { - input.points.at(i).point.pose.position.x, input.points.at(i).point.pose.position.y}; - Point2d path_line_end = { - input.points.at(i + 1).point.pose.position.x, input.points.at(i + 1).point.pose.position.y}; - LineString2d path_line = {path_line_begin, path_line_end}; - - // check nearest collision point - const auto nearest_collision_point = - findNearestCollisionPoint(path_line, stop_line, path_line_begin); - if (!nearest_collision_point) { - continue; - } - - // search target point index - target_point_idx = 0; - double length_sum = 0; - - Eigen::Vector2d point1, point2; - if (0 <= offset) { - point1 << nearest_collision_point->x(), nearest_collision_point->y(); - point2 << path_line_begin.x(), path_line_begin.y(); - length_sum += (point2 - point1).norm(); - for (size_t j = i; 0 < j; --j) { - if (offset < length_sum) { - target_point_idx = j + 1; - break; - } - point1 << input.points.at(j).point.pose.position.x, - input.points.at(j).point.pose.position.y; - point2 << input.points.at(j - 1).point.pose.position.x, - input.points.at(j - 1).point.pose.position.y; - length_sum += (point2 - point1).norm(); - } - } else { - point1 << nearest_collision_point->x(), nearest_collision_point->y(); - point2 << path_line_end.x(), path_line_end.y(); - length_sum -= (point2 - point1).norm(); - for (size_t j = i + 1; j < input.points.size() - 1; ++j) { - if (length_sum < offset) { - target_point_idx = j; - break; - } - point1 << input.points.at(j).point.pose.position.x, - input.points.at(j).point.pose.position.y; - point2 << input.points.at(j + 1).point.pose.position.x, - input.points.at(j + 1).point.pose.position.y; - length_sum -= (point2 - point1).norm(); - } - } - // create target point - getBackwardPointFromBasePoint( - point2, point1, point2, std::fabs(length_sum - offset), target_point); - return true; - } - return false; -} - -bool calcStopPointAndInsertIndex( - const tier4_planning_msgs::msg::PathWithLaneId & input_path, - const lanelet::ConstLineString3d & lanelet_stop_lines, const double & offset, - const double & stop_line_extend_length, Eigen::Vector2d & stop_line_point, - size_t & stop_line_point_idx) -{ - LineString2d stop_line; - - for (size_t i = 0; i < lanelet_stop_lines.size() - 1; ++i) { - stop_line = planning_utils::extendLine( - lanelet_stop_lines[i], lanelet_stop_lines[i + 1], stop_line_extend_length); - - // Calculate stop pose and insert index, - // if there is a collision point between path and stop line - if (createTargetPoint(input_path, stop_line, offset, stop_line_point_idx, stop_line_point)) { - return true; - } - } - return false; -} -} // namespace - 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, @@ -193,12 +70,12 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * lanelet::ConstLineString3d lanelet_stop_lines = *(traffic_light_reg_elem_.stopLine()); // Calculate stop pose and insert index - Eigen::Vector2d stop_line_point{}; - size_t stop_line_point_idx{}; - if (!calcStopPointAndInsertIndex( - input_path, lanelet_stop_lines, - planner_param_.stop_margin + planner_data_->vehicle_info_.max_longitudinal_offset_m, - planner_data_->stop_line_extend_length, stop_line_point, stop_line_point_idx)) { + const auto stop_line = calcStopPointAndInsertIndex( + input_path, lanelet_stop_lines, + planner_param_.stop_margin + planner_data_->vehicle_info_.max_longitudinal_offset_m, + planner_data_->stop_line_extend_length); + + if (!stop_line.has_value()) { RCLCPP_WARN_STREAM_ONCE( logger_, "Failed to calculate stop point and insert index for regulatory element id " << traffic_light_reg_elem_.id()); @@ -209,8 +86,8 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Calculate dist to stop pose geometry_msgs::msg::Point stop_line_point_msg; - stop_line_point_msg.x = stop_line_point.x(); - stop_line_point_msg.y = stop_line_point.y(); + stop_line_point_msg.x = stop_line.value().second.x(); + stop_line_point_msg.y = stop_line.value().second.y(); const double signed_arc_length_to_stop_point = autoware::motion_utils::calcSignedArcLength( input_path.points, self_pose->pose.position, stop_line_point_msg); setDistance(signed_arc_length_to_stop_point); @@ -226,7 +103,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } - first_ref_stop_path_point_index_ = stop_line_point_idx; + first_ref_stop_path_point_index_ = stop_line.value().first; // Check if stop is coming. const bool is_stop_signal = isStopSignal(); @@ -256,7 +133,8 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Decide whether to stop or pass even if a stop signal is received. if (!isPassthrough(signed_arc_length_to_stop_point)) { - *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); + *path = + insertStopPose(input_path, stop_line.value().first, stop_line.value().second, stop_reason); is_prev_state_stop_ = true; } return true; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.cpp new file mode 100644 index 0000000000000..c9eb93abb2e2f --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.cpp @@ -0,0 +1,144 @@ +// 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 "utils.hpp" + +#include + +#include +#include + +namespace autoware::behavior_velocity_planner +{ +namespace bg = boost::geometry; + +auto getOffsetPoint(const Eigen::Vector2d & src, const Eigen::Vector2d & dst, const double length) + -> Eigen::Vector2d +{ + Eigen::Vector2d line_vec = dst - src; + Eigen::Vector2d backward_vec = length * line_vec.normalized(); + return src + backward_vec; +} + +auto findNearestCollisionPoint( + const LineString2d & line1, const LineString2d & line2, + const Point2d & origin) -> std::optional +{ + std::vector collision_points; + bg::intersection(line1, line2, collision_points); + + if (collision_points.empty()) { + return std::nullopt; + } + + // check nearest collision point + Point2d nearest_collision_point; + double min_dist = 0.0; + + for (size_t i = 0; i < collision_points.size(); ++i) { + double dist = bg::distance(origin, collision_points.at(i)); + if (i == 0 || dist < min_dist) { + min_dist = dist; + nearest_collision_point = collision_points.at(i); + } + } + return nearest_collision_point; +} + +auto createTargetPoint( + const tier4_planning_msgs::msg::PathWithLaneId & input, const LineString2d & stop_line, + const double offset) -> std::optional> +{ + if (input.points.size() < 2) { + return std::nullopt; + } + size_t target_point_idx{}; + for (size_t i = 0; i < input.points.size() - 1; ++i) { + Point2d path_line_begin = { + input.points.at(i).point.pose.position.x, input.points.at(i).point.pose.position.y}; + Point2d path_line_end = { + input.points.at(i + 1).point.pose.position.x, input.points.at(i + 1).point.pose.position.y}; + LineString2d path_line = {path_line_begin, path_line_end}; + + // check nearest collision point + const auto nearest_collision_point = + findNearestCollisionPoint(path_line, stop_line, path_line_begin); + if (!nearest_collision_point) { + continue; + } + + // search target point index + target_point_idx = 0; + double length_sum = 0; + + Eigen::Vector2d point1, point2; + if (0 <= offset) { + point1 << nearest_collision_point->x(), nearest_collision_point->y(); + point2 << path_line_begin.x(), path_line_begin.y(); + length_sum += (point2 - point1).norm(); + for (size_t j = i; 0 < j; --j) { + if (offset < length_sum) { + target_point_idx = j + 1; + break; + } + point1 << input.points.at(j).point.pose.position.x, + input.points.at(j).point.pose.position.y; + point2 << input.points.at(j - 1).point.pose.position.x, + input.points.at(j - 1).point.pose.position.y; + length_sum += (point2 - point1).norm(); + } + } else { + point1 << nearest_collision_point->x(), nearest_collision_point->y(); + point2 << path_line_end.x(), path_line_end.y(); + length_sum -= (point2 - point1).norm(); + for (size_t j = i + 1; j < input.points.size() - 1; ++j) { + if (length_sum < offset) { + target_point_idx = j; + break; + } + point1 << input.points.at(j).point.pose.position.x, + input.points.at(j).point.pose.position.y; + point2 << input.points.at(j + 1).point.pose.position.x, + input.points.at(j + 1).point.pose.position.y; + length_sum -= (point2 - point1).norm(); + } + } + // create target point + return std::make_pair( + target_point_idx, getOffsetPoint(point2, point1, std::fabs(length_sum - offset))); + } + return std::nullopt; +} + +auto calcStopPointAndInsertIndex( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, + const lanelet::ConstLineString3d & lanelet_stop_lines, const double & offset, + const double & stop_line_extend_length) -> std::optional> +{ + LineString2d stop_line; + + for (size_t i = 0; i < lanelet_stop_lines.size() - 1; ++i) { + stop_line = planning_utils::extendLine( + lanelet_stop_lines[i], lanelet_stop_lines[i + 1], stop_line_extend_length); + + // Calculate stop pose and insert index, + // if there is a collision point between path and stop line + const auto output = createTargetPoint(input_path, stop_line, offset); + if (output.has_value()) { + return output; + } + } + return std::nullopt; +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.hpp new file mode 100644 index 0000000000000..c6655064d3ffa --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.hpp @@ -0,0 +1,82 @@ +// 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 UTILS_HPP_ +#define UTILS_HPP_ + +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include +#include +#include + +#include + +namespace autoware::behavior_velocity_planner +{ + +/** + * @brief create offset point. + * @param src point of vector. this point is a base point for offset point. + * @param dst point of vector. + * @param offset length. + * @return offset point. + */ +auto getOffsetPoint(const Eigen::Vector2d & src, const Eigen::Vector2d & dst, const double length) + -> Eigen::Vector2d; + +/** + * @brief find intersection point between a path segment and stop line and return the point. + * @param path segment. + * @param stop line. + * @param base point to find nearest intersection point. + * @return intersection point. if there is no intersection point, return std::nullopt. + */ +auto findNearestCollisionPoint( + const LineString2d & line1, const LineString2d & line2, + const Point2d & origin) -> std::optional; + +/** + * @brief find intersection point between path and stop line and return the point. + * @param input path. + * @param stop line. + * @param longitudinal offset. + * @return first: insert point index, second: insert point position. if there is no intersection + * point, return std::nullopt. + */ +auto createTargetPoint( + const tier4_planning_msgs::msg::PathWithLaneId & input, const LineString2d & stop_line, + const double offset) -> std::optional>; + +/** + * @brief find intersection point between path and stop line and return the point. + * @param input path. + * @param stop line. + * @param longitudinal offset. + * @param extend length to find intersection point between path and stop line. + * @return first: insert point index, second: insert point position. if there is no intersection + * point, return std::nullopt. + */ +auto calcStopPointAndInsertIndex( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, + const lanelet::ConstLineString3d & lanelet_stop_lines, const double & offset, + const double & stop_line_extend_length) -> std::optional>; + +} // namespace autoware::behavior_velocity_planner + +#endif // UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_utils.cpp new file mode 100644 index 0000000000000..a5d09acc103e9 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_utils.cpp @@ -0,0 +1,190 @@ +// 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 "../src/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" + +#include + +namespace autoware::behavior_velocity_planner +{ + +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; + +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternion; + +PathWithLaneId generatePath(const geometry_msgs::msg::Pose & pose) +{ + constexpr double interval_distance = 1.0; + + PathWithLaneId traj; + for (double s = 0.0; s <= 10.0 * interval_distance; s += interval_distance) { + PathPointWithLaneId p; + p.point.pose = pose; + p.point.pose.position.x += s; + traj.points.push_back(p); + } + + return traj; +} + +TEST(BehaviorTrafficLightModuleUtilsTest, getOffsetPoint) +{ + constexpr double length = 2.0; + Point2d p1 = {0.0, 0.0}; + Point2d p2 = {1.0, 1.0}; + + const auto output = getOffsetPoint(p1, p2, length); + + EXPECT_DOUBLE_EQ(output.x(), 1.41421356237309505); + EXPECT_DOUBLE_EQ(output.y(), 1.41421356237309505); +} + +TEST(BehaviorTrafficLightModuleUtilsTest, findNearestCollisionPoint) +{ + { + Point2d p = {0.5, 0.5}; + LineString2d line1 = {{-1.0, 0.0}, {1.0, 0.0}}; + LineString2d line2 = {{0.0, -1.0}, {0.0, 1.0}}; + + const auto output = findNearestCollisionPoint(line1, line2, p); + + EXPECT_TRUE(output.has_value()); + EXPECT_DOUBLE_EQ(output.value().x(), 0.0); + EXPECT_DOUBLE_EQ(output.value().y(), 0.0); + } + + { + Point2d p = {0.5, 0.5}; + LineString2d line1 = {{-1.0, -1.0}, {-1.0, 1.0}}; + LineString2d line2 = {{1.0, -1.0}, {1.0, 1.0}}; + + const auto output = findNearestCollisionPoint(line1, line2, p); + + EXPECT_FALSE(output.has_value()); + } +} + +TEST(BehaviorTrafficLightModuleUtilsTest, createTargetPoint) +{ + const auto pose = geometry_msgs::build() + .position(createPoint(0.0, 0.0, 0.0)) + .orientation(createQuaternion(0.0, 0.0, 0.0, 1.0)); + const auto path = generatePath(pose); + + { + constexpr double offset = 1.75; + LineString2d line = {{5.5, -1.0}, {5.5, 1.0}}; + const auto output = createTargetPoint(PathWithLaneId{}, line, offset); + + EXPECT_FALSE(output.has_value()); + } + + { + constexpr double offset = 1.75; + LineString2d line = {{5.5, -1.0}, {5.5, 1.0}}; + const auto output = createTargetPoint(path, line, offset); + + EXPECT_TRUE(output.has_value()); + EXPECT_EQ(output.value().first, size_t(4)); + EXPECT_DOUBLE_EQ(output.value().second.x(), 3.75); + EXPECT_DOUBLE_EQ(output.value().second.y(), 0.0); + } + + { + constexpr double offset = -1.75; + LineString2d line = {{5.5, -1.0}, {5.5, 1.0}}; + const auto output = createTargetPoint(path, line, offset); + + EXPECT_TRUE(output.has_value()); + EXPECT_EQ(output.value().first, size_t(8)); + EXPECT_DOUBLE_EQ(output.value().second.x(), 7.25); + EXPECT_DOUBLE_EQ(output.value().second.y(), 0.0); + } + + { + constexpr double offset = 1.75; + LineString2d line = {{5.5, 2.0}, {5.5, 1.0}}; + const auto output = createTargetPoint(path, line, offset); + + EXPECT_FALSE(output.has_value()); + } +} + +TEST(BehaviorTrafficLightModuleUtilsTest, calcStopPointAndInsertIndex) +{ + const auto pose = geometry_msgs::build() + .position(createPoint(0.0, 0.0, 0.0)) + .orientation(createQuaternion(0.0, 0.0, 0.0, 1.0)); + const auto path = generatePath(pose); + constexpr double offset = 1.75; + + { + lanelet::Points3d basic_line; + basic_line.emplace_back(lanelet::InvalId, 5.5, -1.0, 0.0); + basic_line.emplace_back(lanelet::InvalId, 5.5, 1.0, 0.0); + + const auto line = lanelet::LineString3d(lanelet::InvalId, basic_line); + constexpr double extend_length = 0.0; + const auto output = calcStopPointAndInsertIndex(PathWithLaneId{}, line, offset, extend_length); + + EXPECT_FALSE(output.has_value()); + } + + { + lanelet::Points3d basic_line; + basic_line.emplace_back(lanelet::InvalId, 5.5, -1.0, 0.0); + basic_line.emplace_back(lanelet::InvalId, 5.5, 1.0, 0.0); + + const auto line = lanelet::LineString3d(lanelet::InvalId, basic_line); + constexpr double extend_length = 0.0; + const auto output = calcStopPointAndInsertIndex(path, line, offset, extend_length); + + EXPECT_TRUE(output.has_value()); + EXPECT_EQ(output.value().first, size_t(4)); + EXPECT_DOUBLE_EQ(output.value().second.x(), 3.75); + EXPECT_DOUBLE_EQ(output.value().second.y(), 0.0); + } + + { + lanelet::Points3d basic_line; + basic_line.emplace_back(lanelet::InvalId, 5.5, 2.0, 0.0); + basic_line.emplace_back(lanelet::InvalId, 5.5, 1.0, 0.0); + + const auto line = lanelet::LineString3d(lanelet::InvalId, basic_line); + constexpr double extend_length = 2.0; + const auto output = calcStopPointAndInsertIndex(path, line, offset, extend_length); + + EXPECT_TRUE(output.has_value()); + EXPECT_EQ(output.value().first, size_t(4)); + EXPECT_DOUBLE_EQ(output.value().second.x(), 3.75); + EXPECT_DOUBLE_EQ(output.value().second.y(), 0.0); + } + + { + lanelet::Points3d basic_line; + basic_line.emplace_back(lanelet::InvalId, 5.5, 2.0, 0.0); + basic_line.emplace_back(lanelet::InvalId, 5.5, 1.0, 0.0); + + const auto line = lanelet::LineString3d(lanelet::InvalId, basic_line); + constexpr double extend_length = 0.0; + const auto output = calcStopPointAndInsertIndex(path, line, offset, extend_length); + + EXPECT_FALSE(output.has_value()); + } +} + +} // namespace autoware::behavior_velocity_planner diff --git a/sensing/cuda_utils/package.xml b/sensing/cuda_utils/package.xml index 4ae44469f1efa..b7690bd9e8cc8 100644 --- a/sensing/cuda_utils/package.xml +++ b/sensing/cuda_utils/package.xml @@ -5,8 +5,9 @@ cuda utility library Daisuke Nishimatsu - Daisuke Nishimatsu Manato Hirabayashi + Kenzo Lobos-Tsunekawa + Amadeusz Szymko Apache License 2.0 diff --git a/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml b/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml index 84e7d79079dae..033b20d234fd9 100644 --- a/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml +++ b/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml @@ -2,9 +2,11 @@ ros__parameters: update_rate: 10.0 processing_time_topic_name_list: + - /control/control_validator/debug/processing_time_ms - /control/trajectory_follower/controller_node_exe/lateral/debug/processing_time_ms - /control/trajectory_follower/controller_node_exe/longitudinal/debug/processing_time_ms - /control/trajectory_follower/lane_departure_checker_node/debug/processing_time_ms + - /control/vehicle_cmd_gate/debug/processing_time_ms - /perception/object_recognition/prediction/map_based_prediction/debug/processing_time_ms - /perception/object_recognition/tracking/multi_object_tracker/debug/processing_time_ms - /planning/planning_validator/debug/processing_time_ms @@ -33,5 +35,8 @@ - /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/debug/processing_time_ms - /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/processing_time_ms - /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/processing_time_ms + - /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/processing_time_ms + - /planning/scenario_planning/parking/costmap_generator/debug/processing_time_ms + - /planning/scenario_planning/scenario_selector/debug/processing_time_ms - /planning/scenario_planning/velocity_smoother/debug/processing_time_ms - /simulation/shape_estimation/debug/processing_time_ms