diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 7513e58bcfc10..5a0e8a9aa0a56 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,9 +1,9 @@ ### Automatically generated from package.xml ### common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -common/autoware_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@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_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp common/autoware_point_types/** taichi.higashide@tier4.jp common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp @@ -49,7 +49,7 @@ control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4 control/control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/external_cmd_selector/** 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/joy_controller/** 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/lane_departure_checker/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp +control/autoware_lane_departure_checker/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp control/mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@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/operation_mode_transition_manager/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp @@ -144,22 +144,31 @@ perception/traffic_light_map_based_detector/** shunsuke.miura@tier4.jp tao.zhong perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier4.jp +planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp planning/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/autoware_behavior_path_planner_common/** daniel.sanchez@tier4.jp fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/autoware_behavior_path_static_obstacle_avoidance_module/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/autoware_behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/autoware_behavior_velocity_template_module/** daniel.sanchez@tier4.jp planning/autoware_behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +planning/autoware_external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_path_optimizer/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp +planning/autoware_planning_topic_converter/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp planning/autoware_remaining_distance_time_calculator/** ahmed.ebrahim@leodrive.ai planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp +planning/autoware_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp planning/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_avoidance_module/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp -planning/behavior_path_dynamic_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp planning/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_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@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/** 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 -planning/autoware_behavior_path_planner_common/** daniel.sanchez@tier4.jp fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_sampling_planner_module/** daniel.sanchez@tier4.jp maxime.clement@tier4.jp -planning/behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_path_start_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/autoware_behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_path_start_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_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_crosswalk_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yuki.takagi@tier4.jp @@ -170,42 +179,39 @@ planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp planning/behavior_velocity_traffic_light_module/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** 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/objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp zulfaqar.azmi@tier4.jp -planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp planning/obstacle_stop_planner/** berkay@leodrive.ai bnk@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp planning/planning_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp -planning/planning_topic_converter/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp planning/route_handler/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp zulfaqar.azmi@tier4.jp planning/rtc_interface/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp -planning/sampling_based_planner/bezier_sampler/** maxime.clement@tier4.jp -planning/sampling_based_planner/frenet_planner/** maxime.clement@tier4.jp -planning/sampling_based_planner/path_sampler/** maxime.clement@tier4.jp -planning/sampling_based_planner/sampler_common/** 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 +planning/sampling_based_planner/autoware_sampler_common/** maxime.clement@tier4.jp planning/scenario_selector/** 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 -planning/surround_obstacle_checker/** satoshi.ota@tier4.jp +planning/autoware_surround_obstacle_checker/** go.sakayori@tier4.jp satoshi.ota@tier4.jp sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/image_diagnostics/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp sensing/image_transport_decompressor/** kenzo.lobos@tier4.jp yukihiro.saito@tier4.jp sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/livox/livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp -sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp kenzo.lobos@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yihsiang.fang@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp david.wong@tier4.jp kenzo.lobos@tier4.jp kyoichi.sugahara@tier4.jp melike@leodrive.ai shunsuke.miura@tier4.jp yihsiang.fang@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp @@ -240,9 +246,9 @@ system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp system/velodyne_monitor/** fumihito.ito@tier4.jp tools/reaction_analyzer/** berkay@leodrive.ai vehicle/accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp +vehicle/autoware_steer_offset_estimator/** taiki.tanaka@tier4.jp vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp -vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp vehicle/vehicle_info_util/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp ### Copied from .github/CODEOWNERS-manual ### diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index fb98d321fde88..6745976aa543a 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -74,40 +74,3 @@ jobs: - name: Show disk space after the tasks run: df -h - - clang-tidy-differential: - runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware:latest-prebuilt-cuda - steps: - - name: Check out repository - uses: actions/checkout@v4 - with: - fetch-depth: 0 - - - name: Show disk space before the tasks - run: df -h - - - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - - - name: Get modified packages - id: get-modified-packages - uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 - - - name: Get modified files - id: get-modified-files - uses: tj-actions/changed-files@v35 - with: - files: | - **/*.cpp - **/*.hpp - - - name: Run clang-tidy - if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }} - uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 - with: - rosdistro: humble - target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - target-files: ${{ steps.get-modified-files.outputs.all_changed_files }} - clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy - build-depends-repos: build_depends.repos diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index a469e7a33d190..85c531a02fca9 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -9,7 +9,7 @@ on: jobs: build-and-test: if: ${{ github.event_name != 'push' || github.ref_name == github.event.repository.default_branch }} - runs-on: ubuntu-latest + runs-on: [self-hosted, linux, X64] container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: fail-fast: false diff --git a/.github/workflows/clang-tidy-differential.yaml b/.github/workflows/clang-tidy-differential.yaml new file mode 100644 index 0000000000000..f7ec81e3394e5 --- /dev/null +++ b/.github/workflows/clang-tidy-differential.yaml @@ -0,0 +1,51 @@ +name: clang-tidy-differential + +on: + pull_request: + types: + - opened + - synchronize + - labeled + +jobs: + prevent-no-label-execution: + uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 + with: + label: tag:run-clang-tidy-differential + + clang-tidy-differential: + runs-on: ubuntu-latest + container: ghcr.io/autowarefoundation/autoware:latest-prebuilt-cuda + steps: + - name: Check out repository + uses: actions/checkout@v4 + with: + fetch-depth: 0 + + - name: Show disk space before the tasks + run: df -h + + - name: Remove exec_depend + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 + + - name: Get modified packages + id: get-modified-packages + uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 + + - name: Get modified files + id: get-modified-files + uses: tj-actions/changed-files@v35 + with: + files: | + **/*.cpp + **/*.hpp + + - name: Run clang-tidy + if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }} + uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 + with: + rosdistro: humble + target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} + target-files: ${{ steps.get-modified-files.outputs.all_changed_files }} + clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy + build-depends-repos: build_depends.repos diff --git a/.github/workflows/cppcheck-all.yaml b/.github/workflows/cppcheck-daily.yaml similarity index 97% rename from .github/workflows/cppcheck-all.yaml rename to .github/workflows/cppcheck-daily.yaml index db3bd5d259895..6a18bfb988b08 100644 --- a/.github/workflows/cppcheck-all.yaml +++ b/.github/workflows/cppcheck-daily.yaml @@ -1,13 +1,12 @@ -name: cppcheck-all +name: cppcheck-daily on: - pull_request: schedule: - cron: 0 0 * * * workflow_dispatch: jobs: - cppcheck-all: + cppcheck-daily: runs-on: ubuntu-latest steps: diff --git a/.github/workflows/openai-pr-reviewer.yaml b/.github/workflows/openai-pr-reviewer.yaml deleted file mode 100644 index 4ecbd93198276..0000000000000 --- a/.github/workflows/openai-pr-reviewer.yaml +++ /dev/null @@ -1,39 +0,0 @@ -name: Code Review By ChatGPT - -permissions: - contents: read - pull-requests: write - -on: - pull_request_target: - types: - - labeled - pull_request_review_comment: - types: [created] - -concurrency: - group: ${{ github.repository }}-${{ github.event.number || github.head_ref || - github.sha }}-${{ github.workflow }}-${{ github.event_name == - 'pull_request_review_comment' && 'pr_comment' || 'pr' }} - cancel-in-progress: ${{ github.event_name != 'pull_request_review_comment' }} - -jobs: - prevent-no-label-execution: - uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 - with: - label: tag:openai-pr-reviewer - review: - needs: prevent-no-label-execution - if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} - runs-on: ubuntu-latest - steps: - - uses: fluxninja/openai-pr-reviewer@latest - env: - GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - OPENAI_API_KEY: ${{ secrets.OPENAI_API_KEY }} - with: - debug: false - review_simple_changes: false - review_comment_lgtm: false - openai_light_model: gpt-3.5-turbo-0613 - openai_heavy_model: gpt-3.5-turbo-0613 # The default is to use GPT4, which needs to be changed to join ChatGPT Plus. diff --git a/.github/workflows/pr-agent.yaml b/.github/workflows/pr-agent.yaml index f4794b7497402..81b192a1c2095 100644 --- a/.github/workflows/pr-agent.yaml +++ b/.github/workflows/pr-agent.yaml @@ -26,6 +26,13 @@ jobs: env: OPENAI_KEY: ${{ secrets.OPENAI_KEY_PR_AGENT }} GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - github_action_config.auto_review: "false" - github_action_config.auto_describe: "false" - github_action_config.auto_improve: "false" + github_action_config.auto_review: false + github_action_config.auto_describe: false + github_action_config.auto_improve: false + # https://github.com/Codium-ai/pr-agent/blob/main/pr_agent/settings/configuration.toml + pr_description.publish_labels: false + config.model: gpt-4o + config.model_turbo: gpt-4o + config.max_model_tokens: 64000 + pr_code_suggestions.max_context_tokens: 12000 + pr_code_suggestions.commitable_code_suggestions: true diff --git a/build_depends.repos b/build_depends.repos index a8f431f97df25..bd93897d81d68 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -41,3 +41,7 @@ repositories: type: git url: https://github.com/tier4/glog.git version: v0.6.0_t4-ros + universe/external/ament_cmake: # TODO(mitsudome-r): remove when https://github.com/ament/ament_cmake/pull/448 is merged + type: git + url: https://github.com/autowarefoundation/ament_cmake.git + version: feat/faster_ament_libraries_deduplicate diff --git a/control/mpc_lateral_controller/CMakeLists.txt b/control/autoware_mpc_lateral_controller/CMakeLists.txt similarity index 96% rename from control/mpc_lateral_controller/CMakeLists.txt rename to control/autoware_mpc_lateral_controller/CMakeLists.txt index e4a3683ea1fdc..dff85d70419a1 100644 --- a/control/mpc_lateral_controller/CMakeLists.txt +++ b/control/autoware_mpc_lateral_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(mpc_lateral_controller) +project(autoware_mpc_lateral_controller) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/control/mpc_lateral_controller/README.md b/control/autoware_mpc_lateral_controller/README.md similarity index 100% rename from control/mpc_lateral_controller/README.md rename to control/autoware_mpc_lateral_controller/README.md diff --git a/control/mpc_lateral_controller/image/vehicle_error_kinematics.png b/control/autoware_mpc_lateral_controller/image/vehicle_error_kinematics.png similarity index 100% rename from control/mpc_lateral_controller/image/vehicle_error_kinematics.png rename to control/autoware_mpc_lateral_controller/image/vehicle_error_kinematics.png diff --git a/control/mpc_lateral_controller/image/vehicle_kinematics.png b/control/autoware_mpc_lateral_controller/image/vehicle_kinematics.png similarity index 100% rename from control/mpc_lateral_controller/image/vehicle_kinematics.png rename to control/autoware_mpc_lateral_controller/image/vehicle_kinematics.png diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/lowpass_filter.hpp similarity index 95% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/lowpass_filter.hpp index 227bd0594312b..73cef7cddda39 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/lowpass_filter.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ -#define MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ #include #include @@ -102,4 +102,4 @@ namespace MoveAverageFilter bool filt_vector(const int num, std::vector & u); } // namespace MoveAverageFilter } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc.hpp similarity index 97% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc.hpp index 91b803dea36fa..03abae66e4986 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__MPC_HPP_ -#define MPC_LATERAL_CONTROLLER__MPC_HPP_ - -#include "mpc_lateral_controller/lowpass_filter.hpp" -#include "mpc_lateral_controller/mpc_trajectory.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" -#include "mpc_lateral_controller/steering_predictor.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_HPP_ + +#include "autoware_mpc_lateral_controller/lowpass_filter.hpp" +#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" +#include "autoware_mpc_lateral_controller/steering_predictor.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_control_msgs/msg/lateral.hpp" @@ -525,4 +525,4 @@ class MPC }; // class MPC } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__MPC_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_lateral_controller.hpp similarity index 95% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_lateral_controller.hpp index eb1d75f9508b3..e0775e1d09def 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_lateral_controller.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ -#define MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ -#include "mpc_lateral_controller/mpc.hpp" -#include "mpc_lateral_controller/mpc_trajectory.hpp" -#include "mpc_lateral_controller/mpc_utils.hpp" -#include "mpc_lateral_controller/steering_offset/steering_offset.hpp" +#include "autoware_mpc_lateral_controller/mpc.hpp" +#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware_mpc_lateral_controller/mpc_utils.hpp" +#include "autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp" #include "rclcpp/rclcpp.hpp" #include "trajectory_follower_base/lateral_controller_base.hpp" @@ -281,4 +281,4 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_trajectory.hpp similarity index 95% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_trajectory.hpp index eb624f39ae76b..4a0ae16bc9b1c 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_trajectory.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ -#define MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -125,4 +125,4 @@ class MPCTrajectory } }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_utils.hpp similarity index 97% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_utils.hpp index 819d7fb89b8a7..e97a004438e4b 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ -#define MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" @@ -26,7 +26,7 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #endif -#include "mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -230,4 +230,4 @@ void update_param( } // namespace MPCUtils } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp similarity index 89% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp index ca30bd30e4dd1..6447437d6c274 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ -#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ #include @@ -51,4 +51,4 @@ class QPSolverInterface virtual double getObjVal() const { return 0.0; } }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ diff --git a/control/mpc_lateral_controller/include/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 similarity index 88% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp index 2611f94da324f..8c81092960097 100644 --- a/control/mpc_lateral_controller/include/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 @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ -#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ -#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" #include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" @@ -62,4 +62,4 @@ class QPSolverOSQP : public QPSolverInterface rclcpp::Logger logger_; }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp similarity index 87% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp index ef337eaaa8528..15c8137b78890 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ -#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ -#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" namespace autoware::motion::control::mpc_lateral_controller { @@ -62,4 +62,4 @@ class QPSolverEigenLeastSquareLLT : public QPSolverInterface private: }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp similarity index 84% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp index 494961ef679d2..707c1ce56a9a6 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ -#define MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ #include @@ -45,4 +45,4 @@ class SteeringOffsetEstimator double steering_offset_ = 0.0; }; -#endif // MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_predictor.hpp similarity index 92% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_predictor.hpp index 16e9b165fb1a5..0c5f04497d058 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_predictor.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ -#define MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ #include "rclcpp/rclcpp.hpp" @@ -81,4 +81,4 @@ class SteeringPredictor } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp similarity index 92% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp index 488a3c7ab8be2..20787b1f281b5 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp @@ -44,10 +44,10 @@ * Tracking", Robotics Institute, Carnegie Mellon University, February 2009. */ -#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ -#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ -#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include #include @@ -122,4 +122,4 @@ class DynamicsBicycleModel : public VehicleModelInterface double m_cr; //!< @brief rear cornering power [N/rad] }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp similarity index 90% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp index e2f66e95ab0e3..91d3ea826c7d3 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -38,10 +38,10 @@ * */ -#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include #include @@ -106,4 +106,4 @@ class KinematicsBicycleModel : public VehicleModelInterface double m_steer_tau; //!< @brief steering time constant for 1d-model [s] }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp similarity index 87% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp index b503ad8eb1d13..5731a2025d833 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp @@ -35,10 +35,10 @@ * */ -#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ -#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ // NOLINT +#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ // NOLINT -#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include #include @@ -101,4 +101,6 @@ class KinematicsBicycleModelNoDelay : public VehicleModelInterface double m_steer_lim; //!< @brief steering angle limit [rad] }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ +// clang-format off +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ // NOLINT +// clang-format on diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp similarity index 93% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp index 68806ff5a00dc..9e2d14184d5e7 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ -#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ -#include "mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" #include @@ -149,4 +149,4 @@ class VehicleModelInterface const MPCTrajectory & reference_trajectory, const double dt) const = 0; }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ diff --git a/control/mpc_lateral_controller/model_predictive_control_algorithm.md b/control/autoware_mpc_lateral_controller/model_predictive_control_algorithm.md similarity index 100% rename from control/mpc_lateral_controller/model_predictive_control_algorithm.md rename to control/autoware_mpc_lateral_controller/model_predictive_control_algorithm.md diff --git a/control/mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml similarity index 97% rename from control/mpc_lateral_controller/package.xml rename to control/autoware_mpc_lateral_controller/package.xml index 000bddc65aa1f..3e62d69f6ebb6 100644 --- a/control/mpc_lateral_controller/package.xml +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -1,7 +1,7 @@ - mpc_lateral_controller + autoware_mpc_lateral_controller 1.0.0 MPC-based lateral controller diff --git a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml similarity index 100% rename from control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml rename to control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml diff --git a/control/mpc_lateral_controller/src/lowpass_filter.cpp b/control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp similarity index 98% rename from control/mpc_lateral_controller/src/lowpass_filter.cpp rename to control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp index 8fbf3488c5a2e..e3bbe78e2428b 100644 --- a/control/mpc_lateral_controller/src/lowpass_filter.cpp +++ b/control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/lowpass_filter.hpp" +#include "autoware_mpc_lateral_controller/lowpass_filter.hpp" #include diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp similarity index 99% rename from control/mpc_lateral_controller/src/mpc.cpp rename to control/autoware_mpc_lateral_controller/src/mpc.cpp index 177c1e0854bfb..e6fa2ce3c0102 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/mpc.hpp" +#include "autoware_mpc_lateral_controller/mpc.hpp" +#include "autoware_mpc_lateral_controller/mpc_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "mpc_lateral_controller/mpc_utils.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp similarity index 98% rename from control/mpc_lateral_controller/src/mpc_lateral_controller.cpp rename to control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp index 85d33a5e9f1c0..3d66078eed1f2 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/mpc_lateral_controller.hpp" +#include "autoware_mpc_lateral_controller/mpc_lateral_controller.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "tf2/utils.h" #include "tf2_ros/create_timer_ros.h" #include "vehicle_info_util/vehicle_info_util.hpp" diff --git a/control/mpc_lateral_controller/src/mpc_trajectory.cpp b/control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp similarity index 97% rename from control/mpc_lateral_controller/src/mpc_trajectory.cpp rename to control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp index b7fed942375e8..e84e6cf79a25a 100644 --- a/control/mpc_lateral_controller/src/mpc_trajectory.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/mpc_lateral_controller/src/mpc_utils.cpp b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp similarity index 99% rename from control/mpc_lateral_controller/src/mpc_utils.cpp rename to control/autoware_mpc_lateral_controller/src/mpc_utils.cpp index 01a81d9015b47..1a0062372eb22 100644 --- a/control/mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/mpc_utils.hpp" +#include "autoware_mpc_lateral_controller/mpc_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp b/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp similarity index 97% rename from control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp rename to control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp index 7c76d67e75aa3..66981b0c5c6d4 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp +++ b/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" #include #include diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp b/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp similarity index 93% rename from control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp rename to control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp index b0357138cd646..315e29d063aad 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp +++ b/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" #include diff --git a/control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp b/control/autoware_mpc_lateral_controller/src/steering_offset/steering_offset.cpp similarity index 95% rename from control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp rename to control/autoware_mpc_lateral_controller/src/steering_offset/steering_offset.cpp index 60d4dd7901394..6425d5da5e941 100644 --- a/control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp +++ b/control/autoware_mpc_lateral_controller/src/steering_offset/steering_offset.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/steering_offset/steering_offset.hpp" +#include "autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp" #include #include diff --git a/control/mpc_lateral_controller/src/steering_predictor.cpp b/control/autoware_mpc_lateral_controller/src/steering_predictor.cpp similarity index 98% rename from control/mpc_lateral_controller/src/steering_predictor.cpp rename to control/autoware_mpc_lateral_controller/src/steering_predictor.cpp index 48d8fa8c47a97..759a36a16de62 100644 --- a/control/mpc_lateral_controller/src/steering_predictor.cpp +++ b/control/autoware_mpc_lateral_controller/src/steering_predictor.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/steering_predictor.hpp" +#include "autoware_mpc_lateral_controller/steering_predictor.hpp" namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp similarity index 98% rename from control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp rename to control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp index 2467b1f0c2311..d5272c48fcbdf 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include diff --git a/control/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 similarity index 98% rename from control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp rename to control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index cd52dd4f79314..97bcafb161cb0 100644 --- a/control/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 @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp similarity index 98% rename from control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp rename to control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp index 8f4510510aca9..d2857d8b42182 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp similarity index 94% rename from control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp rename to control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp index f54a5e325bead..e1b17d3d96278 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/mpc_lateral_controller/test/test_lowpass_filter.cpp b/control/autoware_mpc_lateral_controller/test/test_lowpass_filter.cpp similarity index 98% rename from control/mpc_lateral_controller/test/test_lowpass_filter.cpp rename to control/autoware_mpc_lateral_controller/test/test_lowpass_filter.cpp index c68513586847b..bfb695cdeee8a 100644 --- a/control/mpc_lateral_controller/test/test_lowpass_filter.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_lowpass_filter.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware_mpc_lateral_controller/lowpass_filter.hpp" #include "gtest/gtest.h" -#include "mpc_lateral_controller/lowpass_filter.hpp" #include diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp similarity index 97% rename from control/mpc_lateral_controller/test/test_mpc.cpp rename to control/autoware_mpc_lateral_controller/test/test_mpc.cpp index 7644fb28b0788..3fd4b010852c0 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware_mpc_lateral_controller/mpc.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "gtest/gtest.h" -#include "mpc_lateral_controller/mpc.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_control_msgs/msg/lateral.hpp" diff --git a/control/mpc_lateral_controller/test/test_mpc_utils.cpp b/control/autoware_mpc_lateral_controller/test/test_mpc_utils.cpp similarity index 95% rename from control/mpc_lateral_controller/test/test_mpc_utils.cpp rename to control/autoware_mpc_lateral_controller/test/test_mpc_utils.cpp index 51cc7d55e4560..b55435dffd024 100644 --- a/control/mpc_lateral_controller/test/test_mpc_utils.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_mpc_utils.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware_mpc_lateral_controller/mpc_utils.hpp" #include "gtest/gtest.h" -#include "mpc_lateral_controller/mpc_trajectory.hpp" -#include "mpc_lateral_controller/mpc_utils.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" diff --git a/control/lane_departure_checker/CMakeLists.txt b/control/lane_departure_checker/CMakeLists.txt index 874670c478240..d545e1cbb5ad1 100644 --- a/control/lane_departure_checker/CMakeLists.txt +++ b/control/lane_departure_checker/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(lane_departure_checker) +project(autoware_lane_departure_checker) find_package(autoware_cmake REQUIRED) autoware_package() @@ -10,13 +10,13 @@ include_directories( ${EIGEN3_INCLUDE_DIRS} ) -ament_auto_add_library(lane_departure_checker SHARED +ament_auto_add_library(autoware_lane_departure_checker SHARED src/lane_departure_checker_node/lane_departure_checker.cpp src/lane_departure_checker_node/lane_departure_checker_node.cpp ) -rclcpp_components_register_node(lane_departure_checker - PLUGIN "lane_departure_checker::LaneDepartureCheckerNode" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::lane_departure_checker::LaneDepartureCheckerNode" EXECUTABLE lane_departure_checker_node ) diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker.hpp similarity index 95% rename from control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp rename to control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker.hpp index 967cb65c8efa8..d90dbc9474fd8 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ -#define LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ +#ifndef AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ +#define AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ #include #include @@ -45,7 +45,7 @@ #include #include -namespace lane_departure_checker +namespace autoware::lane_departure_checker { using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::Trajectory; @@ -170,6 +170,6 @@ class LaneDepartureChecker const std::vector & vehicle_footprints, const SegmentRtree & uncrossable_segments); }; -} // namespace lane_departure_checker +} // namespace autoware::lane_departure_checker -#endif // LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ +#endif // AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker_node.hpp similarity index 93% rename from control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp rename to control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker_node.hpp index 0f0e15d0a4f62..be7126944f767 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker_node.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ -#define LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ +#ifndef AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ +#define AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ -#include "lane_departure_checker/lane_departure_checker.hpp" +#include "autoware_lane_departure_checker/lane_departure_checker.hpp" #include #include @@ -40,7 +40,7 @@ #include #include -namespace lane_departure_checker +namespace autoware::lane_departure_checker { using autoware_map_msgs::msg::LaneletMapBin; @@ -149,6 +149,6 @@ class LaneDepartureCheckerNode : public rclcpp::Node lanelet::Lanelets getRightOppositeLanelets(const lanelet::ConstLanelet & lanelet); }; -} // namespace lane_departure_checker +} // namespace autoware::lane_departure_checker -#endif // LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ +#endif // AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ diff --git a/control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp b/control/lane_departure_checker/include/autoware_lane_departure_checker/util/create_vehicle_footprint.hpp similarity index 91% rename from control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp rename to control/lane_departure_checker/include/autoware_lane_departure_checker/util/create_vehicle_footprint.hpp index edc62cd6659fd..917317361d702 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp +++ b/control/lane_departure_checker/include/autoware_lane_departure_checker/util/create_vehicle_footprint.hpp @@ -25,8 +25,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ -#define LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ +#ifndef AUTOWARE_LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ +#define AUTOWARE_LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ #include #include @@ -63,4 +63,4 @@ inline FootprintMargin calcFootprintMargin( return FootprintMargin{Cov_xy_vehicle(0, 0) * scale, Cov_xy_vehicle(1, 1) * scale}; } -#endif // LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ +#endif // AUTOWARE_LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ diff --git a/control/lane_departure_checker/launch/lane_departure_checker.launch.xml b/control/lane_departure_checker/launch/lane_departure_checker.launch.xml index 62799c1187651..7ea2e8b2de60a 100644 --- a/control/lane_departure_checker/launch/lane_departure_checker.launch.xml +++ b/control/lane_departure_checker/launch/lane_departure_checker.launch.xml @@ -4,12 +4,12 @@ - + - + diff --git a/control/lane_departure_checker/package.xml b/control/lane_departure_checker/package.xml index 73955613d21a9..466dc7351cd9e 100644 --- a/control/lane_departure_checker/package.xml +++ b/control/lane_departure_checker/package.xml @@ -1,11 +1,13 @@ - lane_departure_checker + autoware_lane_departure_checker 0.1.0 - The lane_departure_checker package + The autoware_lane_departure_checker package Kyoichi Sugahara Makoto Kurihara + Zulfaqar Azmi + Apache License 2.0 ament_cmake_auto diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index eb5d674705aaa..5e90df6773b87 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lane_departure_checker/lane_departure_checker.hpp" +#include "autoware_lane_departure_checker/lane_departure_checker.hpp" -#include "lane_departure_checker/util/create_vehicle_footprint.hpp" +#include "autoware_lane_departure_checker/util/create_vehicle_footprint.hpp" #include #include @@ -96,7 +96,7 @@ lanelet::ConstLanelets getCandidateLanelets( } // namespace -namespace lane_departure_checker +namespace autoware::lane_departure_checker { Output LaneDepartureChecker::update(const Input & input) { @@ -460,4 +460,4 @@ bool LaneDepartureChecker::willCrossBoundary( return false; } -} // namespace lane_departure_checker +} // namespace autoware::lane_departure_checker diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index c3fd1d314e371..2b919f89e6e95 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lane_departure_checker/lane_departure_checker_node.hpp" +#include "autoware_lane_departure_checker/lane_departure_checker_node.hpp" #include #include @@ -120,7 +120,7 @@ void update_param( } // namespace -namespace lane_departure_checker +namespace autoware::lane_departure_checker { LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & options) : Node("lane_departure_checker_node", options) @@ -759,7 +759,7 @@ lanelet::Lanelets LaneDepartureCheckerNode::getRightOppositeLanelets( return opposite_lanelets; } -} // namespace lane_departure_checker +} // namespace autoware::lane_departure_checker #include -RCLCPP_COMPONENTS_REGISTER_NODE(lane_departure_checker::LaneDepartureCheckerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::lane_departure_checker::LaneDepartureCheckerNode) diff --git a/control/smart_mpc_trajectory_follower/package.xml b/control/smart_mpc_trajectory_follower/package.xml index 6cdd689c8bd5a..b25a7e2ce8dd6 100644 --- a/control/smart_mpc_trajectory_follower/package.xml +++ b/control/smart_mpc_trajectory_follower/package.xml @@ -7,6 +7,9 @@ Masayuki Aino + Kosuke Takeuchi + Takamasa Horibe + Takayuki Murooka Apache License 2.0 diff --git a/control/trajectory_follower_node/README.md b/control/trajectory_follower_node/README.md index 3496284efd670..7b9e93e39fad9 100644 --- a/control/trajectory_follower_node/README.md +++ b/control/trajectory_follower_node/README.md @@ -45,7 +45,7 @@ struct LateralSyncData { } } -package mpc_lateral_controller { +package autoware_mpc_lateral_controller { class MPCLateralController { isReady(InputData) override run(InputData) override diff --git a/control/trajectory_follower_node/design/media/Controller.drawio.svg b/control/trajectory_follower_node/design/media/Controller.drawio.svg index 1152ef2b17f59..db4961922bd10 100644 --- a/control/trajectory_follower_node/design/media/Controller.drawio.svg +++ b/control/trajectory_follower_node/design/media/Controller.drawio.svg @@ -140,7 +140,7 @@
trajectory_follower/
- mpc_lateral_controller + autoware_mpc_lateral_controller
diff --git a/control/trajectory_follower_node/package.xml b/control/trajectory_follower_node/package.xml index 65446dfb3dd01..b7dd546eb4bca 100644 --- a/control/trajectory_follower_node/package.xml +++ b/control/trajectory_follower_node/package.xml @@ -21,10 +21,10 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_mpc_lateral_controller autoware_planning_msgs autoware_vehicle_msgs motion_utils - mpc_lateral_controller pid_longitudinal_controller pure_pursuit rclcpp diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index 6fe63ca07de6f..f431be6612009 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -14,7 +14,7 @@ #include "trajectory_follower_node/controller_node.hpp" -#include "mpc_lateral_controller/mpc_lateral_controller.hpp" +#include "autoware_mpc_lateral_controller/mpc_lateral_controller.hpp" #include "pid_longitudinal_controller/pid_longitudinal_controller.hpp" #include "pure_pursuit/pure_pursuit_lateral_controller.hpp" #include "tier4_autoware_utils/ros/marker_helper.hpp" diff --git a/control/trajectory_follower_node/test/test_controller_node.cpp b/control/trajectory_follower_node/test/test_controller_node.cpp index 9bdf625226134..3bf47233f1f3f 100644 --- a/control/trajectory_follower_node/test/test_controller_node.cpp +++ b/control/trajectory_follower_node/test/test_controller_node.cpp @@ -53,7 +53,7 @@ rclcpp::NodeOptions makeNodeOptions(const bool enable_keep_stopped_until_steer_c const auto longitudinal_share_dir = ament_index_cpp::get_package_share_directory("pid_longitudinal_controller"); const auto lateral_share_dir = - ament_index_cpp::get_package_share_directory("mpc_lateral_controller"); + ament_index_cpp::get_package_share_directory("autoware_mpc_lateral_controller"); rclcpp::NodeOptions node_options; node_options.append_parameter_override("lateral_controller_mode", "mpc"); node_options.append_parameter_override("longitudinal_controller_mode", "pid"); diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp index 926fbb7435f3a..eaa07f2317940 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include namespace perception_diagnostics @@ -36,7 +37,11 @@ enum class Metric { SIZE, }; +// Each metric has a different return type. (statistic or just a one value etc). +// To handle them all in the MetricsCalculator::calculate function, define MetricsMap as a variant using MetricStatMap = std::unordered_map>; +using MetricValueMap = std::unordered_map; +using MetricsMap = std::variant; struct PredictedPathDeviationMetrics { diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp index ec435b0a5e17f..a9b1388281ce8 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp @@ -96,7 +96,7 @@ class MetricsCalculator * @param [in] metric Metric enum value * @return map of string describing the requested metric and the calculated value */ - std::optional calculate(const Metric & metric) const; + std::optional calculate(const Metric & metric) const; /** * @brief set the dynamic objects used to calculate obstacle metrics @@ -143,7 +143,7 @@ class MetricsCalculator PredictedPathDeviationMetrics calcPredictedPathDeviationMetrics( const PredictedObjects & objects, const double time_horizon) const; MetricStatMap calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const; - MetricStatMap calcObjectsCountMetrics() const; + MetricValueMap calcObjectsCountMetrics() const; bool hasPassedTime(const rclcpp::Time stamp) const; bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const; diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp index daaea56178873..1bc427e667a2a 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp @@ -61,6 +61,8 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node DiagnosticStatus generateDiagnosticStatus( const std::string metric, const Stat & metric_stat) const; + DiagnosticStatus generateDiagnosticStatus( + const std::string & metric, const double metric_value) const; private: // Subscribers and publishers diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index 9c1e0667e4fef..cc455445ca9f8 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -26,7 +26,7 @@ namespace perception_diagnostics using object_recognition_utils::convertLabelToString; using tier4_autoware_utils::inverseTransformPoint; -std::optional MetricsCalculator::calculate(const Metric & metric) const +std::optional MetricsCalculator::calculate(const Metric & metric) const { // clang-format off const bool use_past_objects = metric == Metric::lateral_deviation || @@ -455,15 +455,14 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas return metric_stat_map; } -MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const +MetricValueMap MetricsCalculator::calcObjectsCountMetrics() const { - MetricStatMap metric_stat_map; + MetricValueMap metric_stat_map; // calculate the average number of objects in the detection area in all past frames const auto overall_average_count = detection_counter_.getOverallAverageCount(); for (const auto & [label, range_and_count] : overall_average_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range].add( - count); + metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } // calculate the average number of objects in the detection area in the past @@ -472,8 +471,8 @@ MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const detection_counter_.getAverageCount(parameters_->objects_count_window_seconds); for (const auto & [label, range_and_count] : average_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] - .add(count); + metric_stat_map + ["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } @@ -481,8 +480,7 @@ MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const const auto total_count = detection_counter_.getTotalCount(); for (const auto & [label, range_and_count] : total_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range].add( - count); + metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp index 0fcdd77f4d515..2933afdab3d08 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -69,16 +69,25 @@ void PerceptionOnlineEvaluatorNode::publishMetrics() // calculate metrics for (const Metric & metric : parameters_->metrics) { - const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric)); - if (!metric_stat_map.has_value()) { + const auto metric_result = metrics_calculator_.calculate(Metric(metric)); + if (!metric_result.has_value()) { continue; } - for (const auto & [metric, stat] : metric_stat_map.value()) { - if (stat.count() > 0) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, stat)); - } - } + std::visit( + [&metrics_msg, this](auto && arg) { + using T = std::decay_t; + for (const auto & [metric, value] : arg) { + if constexpr (std::is_same_v) { + if (value.count() > 0) { + metrics_msg.status.emplace_back(generateDiagnosticStatus(metric, value)); + } + } else if constexpr (std::is_same_v) { + metrics_msg.status.emplace_back(generateDiagnosticStatus(metric, value)); + } + } + }, + metric_result.value()); } // publish metrics @@ -111,6 +120,22 @@ DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( return status; } +DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( + const std::string & metric, const double value) const +{ + DiagnosticStatus status; + + status.level = status.OK; + status.name = metric; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "metric_value"; + key_value.value = std::to_string(value); + status.values.push_back(key_value); + + return status; +} + void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) { metrics_calculator_.setPredictedObjects(*objects_msg, *tf_buffer_); diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index a408821466fb9..3db487f3167ae 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -141,7 +141,19 @@ class EvalTest : public ::testing::Test [=](const DiagnosticArray::ConstSharedPtr msg) { const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric); if (it != msg->status.end()) { - metric_value_ = boost::lexical_cast(it->values[2].value); + const auto mean_it = std::find_if( + it->values.begin(), it->values.end(), + [](const auto & key_value) { return key_value.key == "mean"; }); + if (mean_it != it->values.end()) { + metric_value_ = boost::lexical_cast(mean_it->value); + } else { + const auto metric_value_it = std::find_if( + it->values.begin(), it->values.end(), + [](const auto & key_value) { return key_value.key == "metric_value"; }); + if (metric_value_it != it->values.end()) { + metric_value_ = boost::lexical_cast(metric_value_it->value); + } + } metric_updated_ = true; } }); diff --git a/launch/tier4_control_launch/control_launch.drawio.svg b/launch/tier4_control_launch/control_launch.drawio.svg index 5d13b84a0b34b..21d97ba08c0b3 100644 --- a/launch/tier4_control_launch/control_launch.drawio.svg +++ b/launch/tier4_control_launch/control_launch.drawio.svg @@ -259,17 +259,17 @@ >
- lane_departure_checker_node + autoware_lane_departure_checker_node

- package: lane_departure_checker_node + package: autoware_lane_departure_checker_node
- lane_departure_checker_node... + autoware_lane_departure_checker_node... diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 8bcd357111aa0..232de19d51d4b 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -102,8 +102,8 @@ def launch_setup(context, *args, **kwargs): # lane departure checker lane_departure_component = ComposableNode( - package="lane_departure_checker", - plugin="lane_departure_checker::LaneDepartureCheckerNode", + package="autoware_lane_departure_checker", + plugin="autoware::lane_departure_checker::LaneDepartureCheckerNode", name="lane_departure_checker_node", namespace="trajectory_follower", remappings=[ diff --git a/launch/tier4_control_launch/package.xml b/launch/tier4_control_launch/package.xml index 9900f473f6238..d761ff70063f3 100644 --- a/launch/tier4_control_launch/package.xml +++ b/launch/tier4_control_launch/package.xml @@ -12,10 +12,10 @@ autoware_cmake autoware_vehicle_cmd_gate + autoware_lane_departure_checker control_evaluator external_cmd_converter external_cmd_selector - lane_departure_checker shift_decider trajectory_follower_node diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 97bf6414189ab..1d01732454daf 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -32,7 +32,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index b093bc6295e65..779ee5c7af34a 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -101,37 +101,37 @@ diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 0612f67de4c2b..4ed38df2d3f09 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -62,7 +62,7 @@ - + @@ -80,7 +80,7 @@ - + @@ -98,7 +98,7 @@ - + @@ -232,7 +232,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml index 98315919b540a..db5a9a0093e7c 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index be85ee704ff95..f96cac4f017a6 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -19,7 +19,7 @@ - + @@ -27,7 +27,7 @@ - + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index cf96cd39043ce..e5def4c7aed0c 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -58,23 +58,23 @@ autoware_cmake autoware_behavior_velocity_planner + autoware_costmap_generator + autoware_external_velocity_limit_selector autoware_path_optimizer + autoware_planning_topic_converter + autoware_planning_validator autoware_remaining_distance_time_calculator + autoware_surround_obstacle_checker autoware_velocity_smoother behavior_path_planner - costmap_generator external_cmd_selector - external_velocity_limit_selector freespace_planner glog_component mission_planner obstacle_cruise_planner obstacle_stop_planner planning_evaluator - planning_topic_converter - planning_validator scenario_selector - surround_obstacle_checker ament_lint_auto autoware_lint_common diff --git a/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp index 49b881900b997..c5b7b7a592013 100644 --- a/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp @@ -30,14 +30,14 @@ class DiagnosticsModule public: DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name); void clear(); - void addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg); + void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg); template - void addKeyValue(const std::string & key, const T & value); - void updateLevelAndMessage(const int8_t level, const std::string & message); + void add_key_value(const std::string & key, const T & value); + void update_level_and_message(const int8_t level, const std::string & message); void publish(const rclcpp::Time & publish_time_stamp); private: - diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray( + [[nodiscard]] diagnostic_msgs::msg::DiagnosticArray create_diagnostics_array( const rclcpp::Time & publish_time_stamp) const; rclcpp::Clock::SharedPtr clock_; @@ -47,18 +47,18 @@ class DiagnosticsModule }; template -void DiagnosticsModule::addKeyValue(const std::string & key, const T & value) +void DiagnosticsModule::add_key_value(const std::string & key, const T & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; key_value.value = std::to_string(value); - addKeyValue(key_value); + add_key_value(key_value); } template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value); +void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value); template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value); +void DiagnosticsModule::add_key_value(const std::string & key, const bool & value); } // namespace autoware::gyro_odometer diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index 3b2da504f938a..f85640ab2c523 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -47,14 +47,13 @@ class GyroOdometerNode : public rclcpp::Node public: explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options); - ~GyroOdometerNode(); private: - void callbackVehicleTwist( + void callback_vehicle_twist( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr); - void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); - void concatGyroAndOdometer(); - void publishData(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw); + void callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); + void concat_gyro_and_odometer(); + void publish_data(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw); rclcpp::Subscription::SharedPtr vehicle_twist_sub_; diff --git a/localization/gyro_odometer/src/diagnostics_module.cpp b/localization/gyro_odometer/src/diagnostics_module.cpp index 9d88d8e6833ed..5d687943cef53 100644 --- a/localization/gyro_odometer/src/diagnostics_module.cpp +++ b/localization/gyro_odometer/src/diagnostics_module.cpp @@ -44,7 +44,7 @@ void DiagnosticsModule::clear() diagnostics_status_msg_.message = ""; } -void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg) +void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) { auto it = std::find_if( std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values), @@ -58,24 +58,24 @@ void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_v } template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value) +void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; key_value.value = value; - addKeyValue(key_value); + add_key_value(key_value); } template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value) +void DiagnosticsModule::add_key_value(const std::string & key, const bool & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; key_value.value = value ? "True" : "False"; - addKeyValue(key_value); + add_key_value(key_value); } -void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message) +void DiagnosticsModule::update_level_and_message(const int8_t level, const std::string & message) { if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { if (!diagnostics_status_msg_.message.empty()) { @@ -90,10 +90,10 @@ void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::str void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp) { - diagnostics_pub_->publish(createDiagnosticsArray(publish_time_stamp)); + diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp)); } -diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray( +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_array( const rclcpp::Time & publish_time_stamp) const { diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 1852d70bced71..0ec479770740f 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -31,13 +31,13 @@ namespace autoware::gyro_odometer { -std::array transformCovariance(const std::array & cov) +std::array transform_covariance(const std::array & cov) { using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]}); - std::array cov_transformed; + std::array cov_transformed = {}; cov_transformed.fill(0.); cov_transformed[COV_IDX::X_X] = max_cov; cov_transformed[COV_IDX::Y_Y] = max_cov; @@ -57,11 +57,11 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) vehicle_twist_sub_ = create_subscription( "vehicle/twist_with_covariance", rclcpp::QoS{100}, - std::bind(&GyroOdometerNode::callbackVehicleTwist, this, std::placeholders::_1)); + std::bind(&GyroOdometerNode::callback_vehicle_twist, this, std::placeholders::_1)); imu_sub_ = create_subscription( "imu", rclcpp::QoS{100}, - std::bind(&GyroOdometerNode::callbackImu, this, std::placeholders::_1)); + std::bind(&GyroOdometerNode::callback_imu, this, std::placeholders::_1)); twist_raw_pub_ = create_publisher("twist_raw", rclcpp::QoS{10}); twist_with_covariance_raw_pub_ = create_publisher( @@ -76,49 +76,46 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) // TODO(YamatoAndo) createTimer } -GyroOdometerNode::~GyroOdometerNode() -{ -} - -void GyroOdometerNode::callbackVehicleTwist( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) +void GyroOdometerNode::callback_vehicle_twist( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr) { diagnostics_->clear(); - diagnostics_->addKeyValue( - "topic_time_stamp", static_cast(vehicle_twist_ptr->header.stamp).nanoseconds()); + diagnostics_->add_key_value( + "topic_time_stamp", + static_cast(vehicle_twist_msg_ptr->header.stamp).nanoseconds()); vehicle_twist_arrived_ = true; - latest_vehicle_twist_ros_time_ = vehicle_twist_ptr->header.stamp; - vehicle_twist_queue_.push_back(*vehicle_twist_ptr); - concatGyroAndOdometer(); + latest_vehicle_twist_ros_time_ = vehicle_twist_msg_ptr->header.stamp; + vehicle_twist_queue_.push_back(*vehicle_twist_msg_ptr); + concat_gyro_and_odometer(); - diagnostics_->publish(vehicle_twist_ptr->header.stamp); + diagnostics_->publish(vehicle_twist_msg_ptr->header.stamp); } -void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) +void GyroOdometerNode::callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) { diagnostics_->clear(); - diagnostics_->addKeyValue( + diagnostics_->add_key_value( "topic_time_stamp", static_cast(imu_msg_ptr->header.stamp).nanoseconds()); imu_arrived_ = true; latest_imu_ros_time_ = imu_msg_ptr->header.stamp; gyro_queue_.push_back(*imu_msg_ptr); - concatGyroAndOdometer(); + concat_gyro_and_odometer(); diagnostics_->publish(imu_msg_ptr->header.stamp); } -void GyroOdometerNode::concatGyroAndOdometer() +void GyroOdometerNode::concat_gyro_and_odometer() { // check arrive first topic - diagnostics_->addKeyValue("is_arrived_first_vehicle_twist", vehicle_twist_arrived_); - diagnostics_->addKeyValue("is_arrived_first_imu", imu_arrived_); + diagnostics_->add_key_value("is_arrived_first_vehicle_twist", vehicle_twist_arrived_); + diagnostics_->add_key_value("is_arrived_first_imu", imu_arrived_); if (!vehicle_twist_arrived_) { std::stringstream message; message << "Twist msg is not subscribed"; RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_->updateLevelAndMessage( + diagnostics_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); vehicle_twist_queue_.clear(); @@ -129,7 +126,7 @@ void GyroOdometerNode::concatGyroAndOdometer() std::stringstream message; message << "Imu msg is not subscribed"; RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_->updateLevelAndMessage( + diagnostics_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); vehicle_twist_queue_.clear(); @@ -141,14 +138,14 @@ void GyroOdometerNode::concatGyroAndOdometer() const double vehicle_twist_dt = std::abs((this->now() - latest_vehicle_twist_ros_time_).seconds()); const double imu_dt = std::abs((this->now() - latest_imu_ros_time_).seconds()); - diagnostics_->addKeyValue("vehicle_twist_time_stamp_dt", vehicle_twist_dt); - diagnostics_->addKeyValue("imu_time_stamp_dt", imu_dt); + diagnostics_->add_key_value("vehicle_twist_time_stamp_dt", vehicle_twist_dt); + diagnostics_->add_key_value("imu_time_stamp_dt", imu_dt); if (vehicle_twist_dt > message_timeout_sec_) { const std::string message = fmt::format( "Vehicle twist msg is timeout. vehicle_twist_dt: {}[sec], tolerance {}[sec]", vehicle_twist_dt, message_timeout_sec_); RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message); - diagnostics_->updateLevelAndMessage(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); + diagnostics_->update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); vehicle_twist_queue_.clear(); gyro_queue_.clear(); @@ -158,7 +155,7 @@ void GyroOdometerNode::concatGyroAndOdometer() const std::string message = fmt::format( "Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message); - diagnostics_->updateLevelAndMessage(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); + diagnostics_->update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); vehicle_twist_queue_.clear(); gyro_queue_.clear(); @@ -166,8 +163,8 @@ void GyroOdometerNode::concatGyroAndOdometer() } // check queue size - diagnostics_->addKeyValue("vehicle_twist_queue_size", vehicle_twist_queue_.size()); - diagnostics_->addKeyValue("imu_queue_size", gyro_queue_.size()); + diagnostics_->add_key_value("vehicle_twist_queue_size", vehicle_twist_queue_.size()); + diagnostics_->add_key_value("imu_queue_size", gyro_queue_.size()); if (vehicle_twist_queue_.empty()) { // not output error and clear queue return; @@ -182,13 +179,13 @@ void GyroOdometerNode::concatGyroAndOdometer() transform_listener_->getLatestTransform(gyro_queue_.front().header.frame_id, output_frame_); const bool is_succeed_transform_imu = (tf_imu2base_ptr != nullptr); - diagnostics_->addKeyValue("is_succeed_transform_imu", is_succeed_transform_imu); + diagnostics_->add_key_value("is_succeed_transform_imu", is_succeed_transform_imu); if (!is_succeed_transform_imu) { std::stringstream message; message << "Please publish TF " << output_frame_ << " to " << gyro_queue_.front().header.frame_id; RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_->updateLevelAndMessage( + diagnostics_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); vehicle_twist_queue_.clear(); @@ -208,7 +205,7 @@ void GyroOdometerNode::concatGyroAndOdometer() gyro.header.frame_id = output_frame_; gyro.angular_velocity = transformed_angular_velocity.vector; - gyro.angular_velocity_covariance = transformCovariance(gyro.angular_velocity_covariance); + gyro.angular_velocity_covariance = transform_covariance(gyro.angular_velocity_covariance); } using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; @@ -223,8 +220,8 @@ void GyroOdometerNode::concatGyroAndOdometer() vx_mean += vehicle_twist.twist.twist.linear.x; vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0]; } - vx_mean /= vehicle_twist_queue_.size(); - vx_covariance_original /= vehicle_twist_queue_.size(); + vx_mean /= static_cast(vehicle_twist_queue_.size()); + vx_covariance_original /= static_cast(vehicle_twist_queue_.size()); for (const auto & gyro : gyro_queue_) { gyro_mean.x += gyro.angular_velocity.x; @@ -234,12 +231,12 @@ void GyroOdometerNode::concatGyroAndOdometer() gyro_covariance_original.y += gyro.angular_velocity_covariance[COV_IDX_XYZ::Y_Y]; gyro_covariance_original.z += gyro.angular_velocity_covariance[COV_IDX_XYZ::Z_Z]; } - gyro_mean.x /= gyro_queue_.size(); - gyro_mean.y /= gyro_queue_.size(); - gyro_mean.z /= gyro_queue_.size(); - gyro_covariance_original.x /= gyro_queue_.size(); - gyro_covariance_original.y /= gyro_queue_.size(); - gyro_covariance_original.z /= gyro_queue_.size(); + gyro_mean.x /= static_cast(gyro_queue_.size()); + gyro_mean.y /= static_cast(gyro_queue_.size()); + gyro_mean.z /= static_cast(gyro_queue_.size()); + gyro_covariance_original.x /= static_cast(gyro_queue_.size()); + gyro_covariance_original.y /= static_cast(gyro_queue_.size()); + gyro_covariance_original.z /= static_cast(gyro_queue_.size()); // concat geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov; @@ -257,23 +254,23 @@ void GyroOdometerNode::concatGyroAndOdometer() // From a statistical point of view, here we reduce the covariances according to the number of // observed data twist_with_cov.twist.covariance[COV_IDX_XYZRPY::X_X] = - vx_covariance_original / vehicle_twist_queue_.size(); + vx_covariance_original / static_cast(vehicle_twist_queue_.size()); twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Y_Y] = 100000.0; twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Z_Z] = 100000.0; twist_with_cov.twist.covariance[COV_IDX_XYZRPY::ROLL_ROLL] = - gyro_covariance_original.x / gyro_queue_.size(); + gyro_covariance_original.x / static_cast(gyro_queue_.size()); twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] = - gyro_covariance_original.y / gyro_queue_.size(); + gyro_covariance_original.y / static_cast(gyro_queue_.size()); twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] = - gyro_covariance_original.z / gyro_queue_.size(); + gyro_covariance_original.z / static_cast(gyro_queue_.size()); - publishData(twist_with_cov); + publish_data(twist_with_cov); vehicle_twist_queue_.clear(); gyro_queue_.clear(); } -void GyroOdometerNode::publishData( +void GyroOdometerNode::publish_data( const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw) { geometry_msgs::msg::TwistStamped twist_raw; diff --git a/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp b/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp index 54e1d320319d3..f55278998f5f0 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp @@ -17,7 +17,7 @@ using geometry_msgs::msg::TwistWithCovarianceStamped; using sensor_msgs::msg::Imu; -Imu generateSampleImu() +Imu generate_sample_imu() { Imu imu; imu.header.frame_id = "base_link"; @@ -27,7 +27,7 @@ Imu generateSampleImu() return imu; } -TwistWithCovarianceStamped generateSampleVelocity() +TwistWithCovarianceStamped generate_sample_velocity() { TwistWithCovarianceStamped twist; twist.header.frame_id = "base_link"; @@ -35,7 +35,7 @@ TwistWithCovarianceStamped generateSampleVelocity() return twist; } -rclcpp::NodeOptions getNodeOptionsWithDefaultParams() +rclcpp::NodeOptions get_node_options_with_default_params() { rclcpp::NodeOptions node_options; diff --git a/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp b/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp index 6e3aff0b841a9..9da344e410e61 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp @@ -20,11 +20,8 @@ #include #include -using geometry_msgs::msg::TwistWithCovarianceStamped; -using sensor_msgs::msg::Imu; - -Imu generateSampleImu(); -TwistWithCovarianceStamped generateSampleVelocity(); -rclcpp::NodeOptions getNodeOptionsWithDefaultParams(); +sensor_msgs::msg::Imu generate_sample_imu(); +geometry_msgs::msg::TwistWithCovarianceStamped generate_sample_velocity(); +rclcpp::NodeOptions get_node_options_with_default_params(); #endif // TEST_GYRO_ODOMETER_HELPER_HPP_ diff --git a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp index b7849ef03bfc5..fc331a638a1dd 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp @@ -50,19 +50,22 @@ class VelocityGenerator : public rclcpp::Node class GyroOdometerValidator : public rclcpp::Node { public: - GyroOdometerValidator() : Node("gyro_odometer_validator"), received_latest_twist_ptr(nullptr) - { - twist_sub = create_subscription( - "/twist_with_covariance", 1, [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { + GyroOdometerValidator() + : Node("gyro_odometer_validator"), + twist_sub(create_subscription( + "/twist_with_covariance", 1, + [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { received_latest_twist_ptr = msg; - }); + })), + received_latest_twist_ptr(nullptr) + { } rclcpp::Subscription::SharedPtr twist_sub; TwistWithCovarianceStamped::ConstSharedPtr received_latest_twist_ptr; }; -void spinSome(rclcpp::Node::SharedPtr node_ptr) +void wait_spin_some(rclcpp::Node::SharedPtr node_ptr) { for (int i = 0; i < 50; ++i) { rclcpp::spin_some(node_ptr); @@ -70,7 +73,7 @@ void spinSome(rclcpp::Node::SharedPtr node_ptr) } } -bool isTwistValid( +bool is_twist_valid( const TwistWithCovarianceStamped & twist, const TwistWithCovarianceStamped & twist_ground_truth) { if (twist.twist.twist.linear.x != twist_ground_truth.twist.twist.linear.x) { @@ -99,8 +102,8 @@ bool isTwistValid( // velocity data are provided TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) { - Imu input_imu = generateSampleImu(); - TwistWithCovarianceStamped input_velocity = generateSampleVelocity(); + Imu input_imu = generate_sample_imu(); + TwistWithCovarianceStamped input_velocity = generate_sample_velocity(); TwistWithCovarianceStamped expected_output_twist; expected_output_twist.twist.twist.linear.x = input_velocity.twist.twist.linear.x; @@ -108,8 +111,8 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) expected_output_twist.twist.twist.angular.y = input_imu.angular_velocity.y; expected_output_twist.twist.twist.angular.z = input_imu.angular_velocity.z; - auto gyro_odometer_node = - std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = std::make_shared( + get_node_options_with_default_params()); auto imu_generator = std::make_shared(); auto velocity_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); @@ -120,13 +123,13 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) velocity_generator->vehicle_velocity_pub->publish(input_velocity); // gyro_odometer receives IMU and velocity, and publishes the fused twist data. - spinSome(gyro_odometer_node); + wait_spin_some(gyro_odometer_node); // validator node receives the fused twist data and store in "received_latest_twist_ptr". - spinSome(gyro_odometer_validator_node); + wait_spin_some(gyro_odometer_validator_node); EXPECT_FALSE(gyro_odometer_validator_node->received_latest_twist_ptr == nullptr); - EXPECT_TRUE(isTwistValid( + EXPECT_TRUE(is_twist_valid( *(gyro_odometer_validator_node->received_latest_twist_ptr), expected_output_twist)); } @@ -134,20 +137,20 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) // Verify that the gyro_odometer does NOT publish any outputs when only IMU is provided TEST(GyroOdometer, TestGyroOdometerImuOnly) { - Imu input_imu = generateSampleImu(); + Imu input_imu = generate_sample_imu(); - auto gyro_odometer_node = - std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = std::make_shared( + get_node_options_with_default_params()); auto imu_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); imu_generator->imu_pub->publish(input_imu); // gyro_odometer receives IMU - spinSome(gyro_odometer_node); + wait_spin_some(gyro_odometer_node); // validator node waits for the output fused twist from gyro_odometer - spinSome(gyro_odometer_validator_node); + wait_spin_some(gyro_odometer_validator_node); EXPECT_TRUE(gyro_odometer_validator_node->received_latest_twist_ptr == nullptr); } diff --git a/localization/pose2twist/CMakeLists.txt b/localization/pose2twist/CMakeLists.txt index 2a586aa9cd049..ee63d9f43559a 100644 --- a/localization/pose2twist/CMakeLists.txt +++ b/localization/pose2twist/CMakeLists.txt @@ -14,6 +14,13 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTOR SingleThreadedExecutor ) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_angular_velocity + test/test_angular_velocity.cpp + ) +endif() + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/localization/pose2twist/README.md b/localization/pose2twist/README.md index 07d9c37b710fc..f1f7d6408fafb 100644 --- a/localization/pose2twist/README.md +++ b/localization/pose2twist/README.md @@ -5,7 +5,7 @@ This `pose2twist` calculates the velocity from the input pose history. In addition to the computed twist, this node outputs the linear-x and angular-z components as a float message to simplify debugging. The `twist.linear.x` is calculated as `sqrt(dx * dx + dy * dy + dz * dz) / dt`, and the values in the `y` and `z` fields are zero. -The `twist.angular` is calculated as `d_roll / dt`, `d_pitch / dt` and `d_yaw / dt` for each field. +The `twist.angular` is calculated as `relative_rotation_vector / dt` for each field. ## Inputs / Outputs diff --git a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp index 459a62ea5cd13..d1ff6ee5ff8b6 100644 --- a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp +++ b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp @@ -21,14 +21,23 @@ #include #include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +// Compute the relative rotation of q2 from q1 as a rotation vector +geometry_msgs::msg::Vector3 compute_relative_rotation_vector( + const tf2::Quaternion & q1, const tf2::Quaternion & q2); + class Pose2Twist : public rclcpp::Node { public: explicit Pose2Twist(const rclcpp::NodeOptions & options); - ~Pose2Twist() = default; private: - void callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr); + void callback_pose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr); rclcpp::Subscription::SharedPtr pose_sub_; diff --git a/localization/pose2twist/src/pose2twist_core.cpp b/localization/pose2twist/src/pose2twist_core.cpp index 4b98ec6c81ad4..cdde78ed7e357 100644 --- a/localization/pose2twist/src/pose2twist_core.cpp +++ b/localization/pose2twist/src/pose2twist_core.cpp @@ -14,12 +14,6 @@ #include "pose2twist/pose2twist_core.hpp" -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include #include @@ -38,35 +32,27 @@ Pose2Twist::Pose2Twist(const rclcpp::NodeOptions & options) : rclcpp::Node("pose create_publisher("angular_z", durable_qos); // Note: this callback publishes topics above pose_sub_ = create_subscription( - "pose", queue_size, std::bind(&Pose2Twist::callbackPose, this, _1)); -} - -double calcDiffForRadian(const double lhs_rad, const double rhs_rad) -{ - double diff_rad = lhs_rad - rhs_rad; - if (diff_rad > M_PI) { - diff_rad = diff_rad - 2 * M_PI; - } else if (diff_rad < -M_PI) { - diff_rad = diff_rad + 2 * M_PI; - } - return diff_rad; + "pose", queue_size, std::bind(&Pose2Twist::callback_pose, this, _1)); } -// x: roll, y: pitch, z: yaw -geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose) +tf2::Quaternion get_quaternion(const geometry_msgs::msg::PoseStamped::SharedPtr & pose_stamped_ptr) { - geometry_msgs::msg::Vector3 rpy; - tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); - tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z); - return rpy; + const auto & orientation = pose_stamped_ptr->pose.orientation; + return tf2::Quaternion{orientation.x, orientation.y, orientation.z, orientation.w}; } -geometry_msgs::msg::Vector3 getRPY(geometry_msgs::msg::PoseStamped::SharedPtr pose) +geometry_msgs::msg::Vector3 compute_relative_rotation_vector( + const tf2::Quaternion & q1, const tf2::Quaternion & q2) { - return getRPY(pose->pose); + // If we define q2 as the rotation obtained by applying dq after applying q1, + // then q2 = q1 * dq . + // Therefore, dq = q1.inverse() * q2 . + const tf2::Quaternion diff_quaternion = q1.inverse() * q2; + const tf2::Vector3 axis = diff_quaternion.getAxis() * diff_quaternion.getAngle(); + return geometry_msgs::msg::Vector3{}.set__x(axis.x()).set__y(axis.y()).set__z(axis.z()); } -geometry_msgs::msg::TwistStamped calcTwist( +geometry_msgs::msg::TwistStamped calc_twist( geometry_msgs::msg::PoseStamped::SharedPtr pose_a, geometry_msgs::msg::PoseStamped::SharedPtr pose_b) { @@ -79,18 +65,16 @@ geometry_msgs::msg::TwistStamped calcTwist( return twist; } - const auto pose_a_rpy = getRPY(pose_a); - const auto pose_b_rpy = getRPY(pose_b); + const auto pose_a_quaternion = get_quaternion(pose_a); + const auto pose_b_quaternion = get_quaternion(pose_b); geometry_msgs::msg::Vector3 diff_xyz; - geometry_msgs::msg::Vector3 diff_rpy; + const geometry_msgs::msg::Vector3 relative_rotation_vector = + compute_relative_rotation_vector(pose_a_quaternion, pose_b_quaternion); diff_xyz.x = pose_b->pose.position.x - pose_a->pose.position.x; diff_xyz.y = pose_b->pose.position.y - pose_a->pose.position.y; diff_xyz.z = pose_b->pose.position.z - pose_a->pose.position.z; - diff_rpy.x = calcDiffForRadian(pose_b_rpy.x, pose_a_rpy.x); - diff_rpy.y = calcDiffForRadian(pose_b_rpy.y, pose_a_rpy.y); - diff_rpy.z = calcDiffForRadian(pose_b_rpy.z, pose_a_rpy.z); geometry_msgs::msg::TwistStamped twist; twist.header = pose_b->header; @@ -99,34 +83,34 @@ geometry_msgs::msg::TwistStamped calcTwist( dt; twist.twist.linear.y = 0; twist.twist.linear.z = 0; - twist.twist.angular.x = diff_rpy.x / dt; - twist.twist.angular.y = diff_rpy.y / dt; - twist.twist.angular.z = diff_rpy.z / dt; + twist.twist.angular.x = relative_rotation_vector.x / dt; + twist.twist.angular.y = relative_rotation_vector.y / dt; + twist.twist.angular.z = relative_rotation_vector.z / dt; return twist; } -void Pose2Twist::callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr) +void Pose2Twist::callback_pose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr) { // TODO(YamatoAndo) check time stamp diff // TODO(YamatoAndo) check suddenly move // TODO(YamatoAndo) apply low pass filter - geometry_msgs::msg::PoseStamped::SharedPtr current_pose_msg = pose_msg_ptr; + const geometry_msgs::msg::PoseStamped::SharedPtr & current_pose_msg = pose_msg_ptr; static geometry_msgs::msg::PoseStamped::SharedPtr prev_pose_msg = current_pose_msg; - geometry_msgs::msg::TwistStamped twist_msg = calcTwist(prev_pose_msg, current_pose_msg); + geometry_msgs::msg::TwistStamped twist_msg = calc_twist(prev_pose_msg, current_pose_msg); prev_pose_msg = current_pose_msg; twist_msg.header.frame_id = "base_link"; twist_pub_->publish(twist_msg); tier4_debug_msgs::msg::Float32Stamped linear_x_msg; linear_x_msg.stamp = this->now(); - linear_x_msg.data = twist_msg.twist.linear.x; + linear_x_msg.data = static_cast(twist_msg.twist.linear.x); linear_x_pub_->publish(linear_x_msg); tier4_debug_msgs::msg::Float32Stamped angular_z_msg; angular_z_msg.stamp = this->now(); - angular_z_msg.data = twist_msg.twist.angular.z; + angular_z_msg.data = static_cast(twist_msg.twist.angular.z); angular_z_pub_->publish(angular_z_msg); } diff --git a/localization/pose2twist/test/test_angular_velocity.cpp b/localization/pose2twist/test/test_angular_velocity.cpp new file mode 100644 index 0000000000000..bf2ca0a3ba5c2 --- /dev/null +++ b/localization/pose2twist/test/test_angular_velocity.cpp @@ -0,0 +1,115 @@ +// Copyright 2022 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose2twist/pose2twist_core.hpp" + +#include + +// 1e-3 radian = 0.057 degrees +constexpr double acceptable_error = 1e-3; + +TEST(AngularVelocityFromQuaternion, CheckMultiplicationOrder) +{ + // If we define q2 as the rotation obtained by applying dq after applying q1, then q2 = q1 * dq . + // + // IT IS NOT q2 = dq * q1 . + // + // This test checks that the multiplication order is correct. + + const tf2::Vector3 target_vector(1, 2, 3); + // initial state + // Now, car is facing to the +x direction + // z + // ^ y + // | ^ + // | / + // | / + // car -> x + // + // + // + + tf2::Quaternion q1; + q1.setRPY(0., 0., M_PI / 2.); // yaw = 90 degrees + const tf2::Vector3 initially_rotated_vector = tf2::quatRotate(q1, target_vector); + // after applying q1 + // Now, car is facing to the +y direction + // z + // ^ + // | y + // | ^ + // | / + // <--car x + // + // + // + EXPECT_NEAR(initially_rotated_vector.x(), -2., acceptable_error); + EXPECT_NEAR(initially_rotated_vector.y(), 1., acceptable_error); + EXPECT_NEAR(initially_rotated_vector.z(), 3., acceptable_error); + + tf2::Quaternion dq; + dq.setRPY(0., M_PI / 2., 0.); // pitch = 90 degrees + const tf2::Vector3 finally_rotated_vector = tf2::quatRotate(q1 * dq, target_vector); + // after applying dq + // Now, car is facing to the -z direction + // z y + // ^ + // / + // / + // / + // <--car x + // | + // v + // + EXPECT_NEAR(finally_rotated_vector.x(), -2., acceptable_error); + EXPECT_NEAR(finally_rotated_vector.y(), 3., acceptable_error); + EXPECT_NEAR(finally_rotated_vector.z(), -1., acceptable_error); + + // Failure case + { + const tf2::Vector3 false_rotated_vector = tf2::quatRotate(dq * q1, target_vector); + + EXPECT_FALSE(std::abs(false_rotated_vector.x() - (-2)) < acceptable_error); + EXPECT_FALSE(std::abs(false_rotated_vector.y() - (3)) < acceptable_error); + EXPECT_FALSE(std::abs(false_rotated_vector.z() - (-1)) < acceptable_error); + } +} + +TEST(AngularVelocityFromQuaternion, CheckNumericalValidity) +{ + auto test = [](const tf2::Vector3 & expected_axis, const double expected_angle) -> void { + tf2::Quaternion expected_q; + expected_q.setRotation(expected_axis, expected_angle); + + // Create a random initial quaternion + tf2::Quaternion initial_q; + initial_q.setRPY(0.2, 0.3, 0.4); + + // Calculate the final quaternion by rotating the initial quaternion by the expected + // quaternion + const tf2::Quaternion final_q = initial_q * expected_q; + + // Calculate the relative rotation between the initial and final quaternion + const geometry_msgs::msg::Vector3 rotation_vector = + compute_relative_rotation_vector(initial_q, final_q); + + EXPECT_NEAR(rotation_vector.x, expected_axis.x() * expected_angle, acceptable_error); + EXPECT_NEAR(rotation_vector.y, expected_axis.y() * expected_angle, acceptable_error); + EXPECT_NEAR(rotation_vector.z, expected_axis.z() * expected_angle, acceptable_error); + }; + + test(tf2::Vector3(1.0, 0.0, 0.0).normalized(), 0.1); // 0.1 radian = 5.7 degrees + test(tf2::Vector3(1.0, 1.0, 0.0).normalized(), -0.2); // 0.2 radian = 11.4 degrees + test(tf2::Vector3(1.0, 2.0, 3.0).normalized(), 0.3); // 0.3 radian = 17.2 degrees +} diff --git a/localization/pose_instability_detector/README.md b/localization/pose_instability_detector/README.md index 89cf6ca3be684..4ced0fa8eb97b 100644 --- a/localization/pose_instability_detector/README.md +++ b/localization/pose_instability_detector/README.md @@ -1,20 +1,111 @@ # pose_instability_detector -The `pose_instability_detector` package includes a node designed to monitor the stability of `/localization/kinematic_state`, which is an output topic of the Extended Kalman Filter (EKF). +The `pose_instability_detector` is a node designed to monitor the stability of `/localization/kinematic_state`, which is an output topic of the Extended Kalman Filter (EKF). This node triggers periodic timer callbacks to compare two poses: -- The pose obtained by integrating the twist values from the last received message on `/localization/kinematic_state` over a duration specified by `interval_sec`. +- The pose calculated by dead reckoning starting from the pose of `/localization/kinematic_state` obtained `timer_period` seconds ago. - The latest pose from `/localization/kinematic_state`. The results of this comparison are then output to the `/diagnostics` topic. +![overview](./media/pose_instability_detector_overview.png) + +![rqt_runtime_monitor](./media/rqt_runtime_monitor.png) + If this node outputs WARN messages to `/diagnostics`, it means that the EKF output is significantly different from the integrated twist values. +In other words, WARN outputs indicate that the vehicle has moved to a place outside the expected range based on the twist values. This discrepancy suggests that there may be an issue with either the estimated pose or the input twist. -The following diagram provides an overview of what the timeline of this process looks like: +The following diagram provides an overview of how the procedure looks like: + +![procedure](./media/pose_instabilty_detector_procedure.svg) + +## Dead reckoning algorithm + +Dead reckoning is a method of estimating the position of a vehicle based on its previous position and velocity. +The procedure for dead reckoning is as follows: + +1. Capture the necessary twist values from the `/input/twist` topic. +2. Integrate the twist values to calculate the pose transition. +3. Apply the pose transition to the previous pose to obtain the current pose. + +### Collecting twist values + +The `pose_instability_detector` node collects the twist values from the `~/input/twist` topic to perform dead reckoning. +Ideally, `pose_instability_detector` needs the twist values between the previous pose and the current pose. +Therefore, `pose_instability_detector` snips the twist buffer and apply interpolations and extrapolations to obtain the twist values at the desired time. + +![how_to_snip_necessary_twist](./media/how_to_snip_twist.png) + +### Linear transition and angular transition + +After the twist values are collected, the node calculates the linear transition and angular transition based on the twist values and add them to the previous pose. + +## Threshold definition + +The `pose_instability_detector` node compares the pose calculated by dead reckoning with the latest pose from the EKF output. +These two pose are ideally the same, but in reality, they are not due to the error in the twist values the pose observation. +If these two poses are significantly different so that the absolute value exceeds the threshold, the node outputs a WARN message to the `/diagnostics` topic. +There are six thresholds (x, y, z, roll, pitch, and yaw) to determine whether the poses are significantly different, and these thresholds are determined by the following subsections. + +### `diff_position_x` + +This threshold examines the difference in the longitudinal axis between the two poses, and check whether the vehicle goes beyond the expected error. +This threshold is a sum of "maximum longitudinal error due to velocity scale factor error" and "pose estimation error tolerance". + +$$ +\tau_x = v_{\rm max}\frac{\beta_v}{100} \Delta t + \epsilon_x\\ +$$ -![timeline](./media/timeline.drawio.svg) +| Symbol | Description | Unit | +| ------------- | -------------------------------------------------------------------------------- | ----- | +| $\tau_x$ | Threshold for the difference in the longitudinal axis | $m$ | +| $v_{\rm max}$ | Maximum velocity | $m/s$ | +| $\beta_v$ | Scale factor tolerance for the maximum velocity | $\%$ | +| $\Delta t$ | Time interval | $s$ | +| $\epsilon_x$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the longitudinal axis | $m$ | + +### `diff_position_y` and `diff_position_z` + +These thresholds examine the difference in the lateral and vertical axes between the two poses, and check whether the vehicle goes beyond the expected error. +The `pose_instability_detector` calculates the possible range where the vehicle goes, and get the maximum difference between the nominal dead reckoning pose and the maximum limit pose. + +![lateral_threshold_calculation](./media/lateral_threshold_calculation.png) + +Addition to this, the `pose_instability_detector` node considers the pose estimation error tolerance to determine the threshold. + +$$ +\tau_y = l + \epsilon_y +$$ + +| Symbol | Description | Unit | +| ------------ | ----------------------------------------------------------------------------------------------- | ---- | +| $\tau_y$ | Threshold for the difference in the lateral axis | $m$ | +| $l$ | Maximum lateral distance described in the image above (See the appendix how this is calculated) | $m$ | +| $\epsilon_y$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the lateral axis | $m$ | + +Note that `pose_instability_detector` sets the threshold for the vertical axis as the same as the lateral axis. Only the pose estimator error tolerance is different. + +### `diff_angle_x`, `diff_angle_y`, and `diff_angle_z` + +These thresholds examine the difference in the roll, pitch, and yaw angles between the two poses. +This threshold is a sum of "maximum angular error due to velocity scale factor error and bias error" and "pose estimation error tolerance". + +$$ +\tau_\phi = \tau_\theta = \tau_\psi = \left(\omega_{\rm max}\frac{\beta_\omega}{100} + b \right) \Delta t + \epsilon_\psi +$$ + +| Symbol | Description | Unit | +| ------------------ | ------------------------------------------------------------------------ | ------------- | +| $\tau_\phi$ | Threshold for the difference in the roll angle | ${\rm rad}$ | +| $\tau_\theta$ | Threshold for the difference in the pitch angle | ${\rm rad}$ | +| $\tau_\psi$ | Threshold for the difference in the yaw angle | ${\rm rad}$ | +| $\omega_{\rm max}$ | Maximum angular velocity | ${\rm rad}/s$ | +| $\beta_\omega$ | Scale factor tolerance for the maximum angular velocity | $\%$ | +| $b$ | Bias tolerance of the angular velocity | ${\rm rad}/s$ | +| $\Delta t$ | Time interval | $s$ | +| $\epsilon_\psi$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the yaw angle | ${\rm rad}$ | ## Parameters @@ -34,4 +125,44 @@ The following diagram provides an overview of what the timeline of this process | `~/debug/diff_pose` | geometry_msgs::msg::PoseStamped | diff_pose | | `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | Diagnostics | -![rqt_runtime_monitor](./media/rqt_runtime_monitor.png) +## Appendix + +On calculating the maximum lateral distance $l$, the `pose_instability_detector` node will estimate the following poses. + +| Pose | heading velocity $v$ | angular velocity $\omega$ | +| ------------------------------- | ------------------------------------------------ | -------------------------------------------------------------- | +| Nominal dead reckoning pose | $v_{\rm max}$ | $\omega_{\rm max}$ | +| Dead reckoning pose of corner A | $\left(1+\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1+\frac{\beta_\omega}{100}\right) \omega_{\rm max} + b$ | +| Dead reckoning pose of corner B | $\left(1-\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1+\frac{\beta_\omega}{100}\right) \omega_{\rm max} + b$ | +| Dead reckoning pose of corner C | $\left(1-\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1-\frac{\beta_\omega}{100}\right) \omega_{\rm max} - b$ | +| Dead reckoning pose of corner D | $\left(1+\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1-\frac{\beta_\omega}{100}\right) \omega_{\rm max} - b$ | + +Given a heading velocity $v$ and $\omega$, the 2D theoretical variation seen from the previous pose is calculated as follows: + +$$ +\begin{align*} +\left[ + \begin{matrix} + \Delta x\\ + \Delta y + \end{matrix} +\right] +&= +\left[ + \begin{matrix} + \int_{0}^{\Delta t} v \cos(\omega t) dt\\ + \int_{0}^{\Delta t} v \sin(\omega t) dt + \end{matrix} +\right] +\\ +&= +\left[ + \begin{matrix} + \frac{v}{\omega} \sin(\omega \Delta t)\\ + \frac{v}{\omega} \left(1 - \cos(\omega \Delta t)\right) + \end{matrix} +\right] +\end{align*} +$$ + +We calculate this variation for each corner and get the maximum value of the lateral distance $l$ by comparing the distance between the nominal dead reckoning pose and the corner poses. diff --git a/localization/pose_instability_detector/config/pose_instability_detector.param.yaml b/localization/pose_instability_detector/config/pose_instability_detector.param.yaml index d94de020a4a12..d9b11b78885c9 100644 --- a/localization/pose_instability_detector/config/pose_instability_detector.param.yaml +++ b/localization/pose_instability_detector/config/pose_instability_detector.param.yaml @@ -1,9 +1,15 @@ /**: ros__parameters: - interval_sec: 0.5 # [sec] - threshold_diff_position_x: 1.0 # [m] - threshold_diff_position_y: 1.0 # [m] - threshold_diff_position_z: 1.0 # [m] - threshold_diff_angle_x: 1.0 # [rad] - threshold_diff_angle_y: 1.0 # [rad] - threshold_diff_angle_z: 1.0 # [rad] + timer_period: 0.5 # [sec] + + heading_velocity_maximum: 16.667 # [m/s] + heading_velocity_scale_factor_tolerance: 3.0 # [%] + + angular_velocity_maximum: 0.523 # [rad/s] + angular_velocity_scale_factor_tolerance: 0.2 # [%] + angular_velocity_bias_tolerance: 0.00698 # [rad/s] + + pose_estimator_longitudinal_tolerance: 0.11 # [m] + pose_estimator_lateral_tolerance: 0.11 # [m] + pose_estimator_vertical_tolerance: 0.11 # [m] + pose_estimator_angular_tolerance: 0.0175 # [rad] diff --git a/localization/pose_instability_detector/src/pose_instability_detector.hpp b/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp similarity index 57% rename from localization/pose_instability_detector/src/pose_instability_detector.hpp rename to localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp index 761a10b7a6bf7..0a55a5005dde1 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.hpp +++ b/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_INSTABILITY_DETECTOR_HPP_ -#define POSE_INSTABILITY_DETECTOR_HPP_ +#ifndef AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_ +#define AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_ #include @@ -22,6 +22,8 @@ #include #include +#include +#include #include class PoseInstabilityDetector : public rclcpp::Node @@ -37,13 +39,31 @@ class PoseInstabilityDetector : public rclcpp::Node using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; public: + struct ThresholdValues + { + double position_x; + double position_y; + double position_z; + double angle_x; + double angle_y; + double angle_z; + }; + explicit PoseInstabilityDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ThresholdValues calculate_threshold(double interval_sec); + void dead_reckon( + PoseStamped::SharedPtr & initial_pose, const rclcpp::Time & end_time, + const std::deque & twist_deque, Pose::SharedPtr & estimated_pose); private: void callback_odometry(Odometry::ConstSharedPtr odometry_msg_ptr); void callback_twist(TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr); void callback_timer(); + std::deque clip_out_necessary_twist( + const std::deque & twist_buffer, const rclcpp::Time & start_time, + const rclcpp::Time & end_time); + // subscribers and timer rclcpp::Subscription::SharedPtr odometry_sub_; rclcpp::Subscription::SharedPtr twist_sub_; @@ -54,17 +74,26 @@ class PoseInstabilityDetector : public rclcpp::Node rclcpp::Publisher::SharedPtr diagnostics_pub_; // parameters - const double threshold_diff_position_x_; - const double threshold_diff_position_y_; - const double threshold_diff_position_z_; - const double threshold_diff_angle_x_; - const double threshold_diff_angle_y_; - const double threshold_diff_angle_z_; + const double timer_period_; // [sec] + + ThresholdValues threshold_values_; + + const double heading_velocity_maximum_; // [m/s] + const double heading_velocity_scale_factor_tolerance_; // [%] + + const double angular_velocity_maximum_; // [rad/s] + const double angular_velocity_scale_factor_tolerance_; // [%] + const double angular_velocity_bias_tolerance_; // [rad/s] + + const double pose_estimator_longitudinal_tolerance_; // [m] + const double pose_estimator_lateral_tolerance_; // [m] + const double pose_estimator_vertical_tolerance_; // [m] + const double pose_estimator_angular_tolerance_; // [rad] // variables std::optional latest_odometry_ = std::nullopt; std::optional prev_odometry_ = std::nullopt; - std::vector twist_buffer_; + std::deque twist_buffer_; }; -#endif // POSE_INSTABILITY_DETECTOR_HPP_ +#endif // AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_ diff --git a/localization/pose_instability_detector/media/how_to_snip_twist.png b/localization/pose_instability_detector/media/how_to_snip_twist.png new file mode 100644 index 0000000000000..6dc66a480e769 Binary files /dev/null and b/localization/pose_instability_detector/media/how_to_snip_twist.png differ diff --git a/localization/pose_instability_detector/media/lateral_threshold_calculation.png b/localization/pose_instability_detector/media/lateral_threshold_calculation.png new file mode 100644 index 0000000000000..cd919cdcb0383 Binary files /dev/null and b/localization/pose_instability_detector/media/lateral_threshold_calculation.png differ diff --git a/localization/pose_instability_detector/media/pose_instability_detector_overview.png b/localization/pose_instability_detector/media/pose_instability_detector_overview.png new file mode 100644 index 0000000000000..ea8f7a81700ae Binary files /dev/null and b/localization/pose_instability_detector/media/pose_instability_detector_overview.png differ diff --git a/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg b/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg new file mode 100644 index 0000000000000..ba45b1f52600b --- /dev/null +++ b/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg @@ -0,0 +1,316 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Get necessary subsequence +
+ from twist buffer +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ Update +
+ + latest pose +
+ +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ Calculate integration of +
+ the twist subsequence +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+
+ Calculate relative difference between +
+
+
+ + latest pose +
+ +
+
and
+
+ previous pose + twist integration +
+
+
+
+
+ + + + + + + + + + + + + + + + +
+
+
+
+ Compare pose difference with thresholds +
+
+
+ Output results as + /diagnostics +
+
+
+
+
+ + + + + + + + + + + + +
+
+
+
+ + previous pose ← latest pose +
+ +
+
+
+
+ + + + + + + + + + + + +
+
+
+ Timer Callback +
+
+
+ + + + + + + + + + + + +
+
+
+ Odometry Subscription Callback +
+
+
+ + + + + + + diff --git a/localization/pose_instability_detector/media/rqt_runtime_monitor.png b/localization/pose_instability_detector/media/rqt_runtime_monitor.png index b3ad402e48ba7..9bad665db17e5 100644 Binary files a/localization/pose_instability_detector/media/rqt_runtime_monitor.png and b/localization/pose_instability_detector/media/rqt_runtime_monitor.png differ diff --git a/localization/pose_instability_detector/schema/pose_instability_detector.schema.json b/localization/pose_instability_detector/schema/pose_instability_detector.schema.json index 560d39a2d5bff..53380f8e7f252 100644 --- a/localization/pose_instability_detector/schema/pose_instability_detector.schema.json +++ b/localization/pose_instability_detector/schema/pose_instability_detector.schema.json @@ -1,62 +1,83 @@ { "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for Pose Instability Detector Nodes", + "title": "Parameters for Pose Instability Detector Node", "type": "object", "definitions": { "pose_instability_detector_node": { "type": "object", "properties": { - "interval_sec": { + "timer_period": { "type": "number", "default": 0.5, "exclusiveMinimum": 0, - "description": "The interval of timer_callback in seconds." + "description": "The period of timer_callback (sec)." }, - "threshold_diff_position_x": { + "heading_velocity_maximum": { "type": "number", - "default": 1.0, + "default": 16.667, "minimum": 0.0, - "description": "The threshold of diff_position x (m)." + "description": "The maximum of heading velocity (m/s)." }, - "threshold_diff_position_y": { + "heading_velocity_scale_factor_tolerance": { "type": "number", - "default": 1.0, + "default": 3.0, "minimum": 0.0, - "description": "The threshold of diff_position y (m)." + "description": "The tolerance of heading velocity scale factor (%)." }, - "threshold_diff_position_z": { + "angular_velocity_maximum": { "type": "number", - "default": 1.0, + "default": 0.523, "minimum": 0.0, - "description": "The threshold of diff_position z (m)." + "description": "The maximum of angular velocity (rad/s)." }, - "threshold_diff_angle_x": { + "angular_velocity_scale_factor_tolerance": { "type": "number", - "default": 1.0, + "default": 0.2, "minimum": 0.0, - "description": "The threshold of diff_angle x (rad)." + "description": "The tolerance of angular velocity scale factor (%)." }, - "threshold_diff_angle_y": { + "angular_velocity_bias_tolerance": { "type": "number", - "default": 1.0, + "default": 0.00698, "minimum": 0.0, - "description": "The threshold of diff_angle y (rad)." + "description": "The tolerance of angular velocity bias (rad/s)." }, - "threshold_diff_angle_z": { + "pose_estimator_longitudinal_tolerance": { "type": "number", - "default": 1.0, + "default": 0.11, "minimum": 0.0, - "description": "The threshold of diff_angle z (rad)." + "description": "The tolerance of longitudinal position of pose estimator (m)." + }, + "pose_estimator_lateral_tolerance": { + "type": "number", + "default": 0.11, + "minimum": 0.0, + "description": "The tolerance of lateral position of pose estimator (m)." + }, + "pose_estimator_vertical_tolerance": { + "type": "number", + "default": 0.11, + "minimum": 0.0, + "description": "The tolerance of vertical position of pose estimator (m)." + }, + "pose_estimator_angular_tolerance": { + "type": "number", + "default": 0.0175, + "minimum": 0.0, + "description": "The tolerance of roll angle of pose estimator (rad)." } }, "required": [ - "interval_sec", - "threshold_diff_position_x", - "threshold_diff_position_y", - "threshold_diff_position_z", - "threshold_diff_angle_x", - "threshold_diff_angle_y", - "threshold_diff_angle_z" + "timer_period", + "heading_velocity_maximum", + "heading_velocity_scale_factor_tolerance", + "angular_velocity_maximum", + "angular_velocity_scale_factor_tolerance", + "angular_velocity_bias_tolerance", + "pose_estimator_longitudinal_tolerance", + "pose_estimator_lateral_tolerance", + "pose_estimator_vertical_tolerance", + "pose_estimator_angular_tolerance" ] } }, diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/pose_instability_detector/src/pose_instability_detector.cpp index c2bce6a3db288..fa7b2ecf16562 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.cpp +++ b/localization/pose_instability_detector/src/pose_instability_detector.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_instability_detector.hpp" +#include "autoware/pose_instability_detector/pose_instability_detector.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -20,17 +20,32 @@ #include +#include +#include +#include #include PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & options) : rclcpp::Node("pose_instability_detector", options), - threshold_diff_position_x_(this->declare_parameter("threshold_diff_position_x")), - threshold_diff_position_y_(this->declare_parameter("threshold_diff_position_y")), - threshold_diff_position_z_(this->declare_parameter("threshold_diff_position_z")), - threshold_diff_angle_x_(this->declare_parameter("threshold_diff_angle_x")), - threshold_diff_angle_y_(this->declare_parameter("threshold_diff_angle_y")), - threshold_diff_angle_z_(this->declare_parameter("threshold_diff_angle_z")) + timer_period_(this->declare_parameter("timer_period")), + heading_velocity_maximum_(this->declare_parameter("heading_velocity_maximum")), + heading_velocity_scale_factor_tolerance_( + this->declare_parameter("heading_velocity_scale_factor_tolerance")), + angular_velocity_maximum_(this->declare_parameter("angular_velocity_maximum")), + angular_velocity_scale_factor_tolerance_( + this->declare_parameter("angular_velocity_scale_factor_tolerance")), + angular_velocity_bias_tolerance_( + this->declare_parameter("angular_velocity_bias_tolerance")), + pose_estimator_longitudinal_tolerance_( + this->declare_parameter("pose_estimator_longitudinal_tolerance")), + pose_estimator_lateral_tolerance_( + this->declare_parameter("pose_estimator_lateral_tolerance")), + pose_estimator_vertical_tolerance_( + this->declare_parameter("pose_estimator_vertical_tolerance")), + pose_estimator_angular_tolerance_( + this->declare_parameter("pose_estimator_angular_tolerance")) { + // Define subscribers, publishers and a timer. odometry_sub_ = this->create_subscription( "~/input/odometry", 10, std::bind(&PoseInstabilityDetector::callback_odometry, this, std::placeholders::_1)); @@ -39,9 +54,8 @@ PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & opt "~/input/twist", 10, std::bind(&PoseInstabilityDetector::callback_twist, this, std::placeholders::_1)); - const double interval_sec = this->declare_parameter("interval_sec"); timer_ = rclcpp::create_timer( - this, this->get_clock(), std::chrono::duration(interval_sec), + this, this->get_clock(), std::chrono::duration(timer_period_), std::bind(&PoseInstabilityDetector::callback_timer, this)); diff_pose_pub_ = this->create_publisher("~/debug/diff_pose", 10); @@ -61,6 +75,7 @@ void PoseInstabilityDetector::callback_twist( void PoseInstabilityDetector::callback_timer() { + // odometry callback and timer callback has to be called at least once if (latest_odometry_ == std::nullopt) { return; } @@ -69,6 +84,16 @@ void PoseInstabilityDetector::callback_timer() return; } + // twist callback has to be called at least once + if (twist_buffer_.empty()) { + return; + } + + // time variables + const rclcpp::Time latest_odometry_time = rclcpp::Time(latest_odometry_->header.stamp); + const rclcpp::Time prev_odometry_time = rclcpp::Time(prev_odometry_->header.stamp); + + // define lambda function to convert quaternion to rpy auto quat_to_rpy = [](const Quaternion & quat) { tf2::Quaternion tf2_quat(quat.x, quat.y, quat.z, quat.w); tf2::Matrix3x3 mat(tf2_quat); @@ -79,70 +104,48 @@ void PoseInstabilityDetector::callback_timer() return std::make_tuple(roll, pitch, yaw); }; - Pose pose = prev_odometry_->pose.pose; - rclcpp::Time prev_time = rclcpp::Time(prev_odometry_->header.stamp); - for (const TwistWithCovarianceStamped & twist_with_cov : twist_buffer_) { - const Twist twist = twist_with_cov.twist.twist; - - const rclcpp::Time curr_time = rclcpp::Time(twist_with_cov.header.stamp); - if (curr_time > latest_odometry_->header.stamp) { + // delete twist data older than prev_odometry_ (but preserve the one right before prev_odometry_) + while (twist_buffer_.size() > 1) { + if (rclcpp::Time(twist_buffer_[1].header.stamp) < prev_odometry_time) { + twist_buffer_.pop_front(); + } else { break; } + } - const rclcpp::Duration time_diff = curr_time - prev_time; - const double time_diff_sec = time_diff.seconds(); - if (time_diff_sec < 0.0) { - continue; - } - - // quat to rpy - auto [ang_x, ang_y, ang_z] = quat_to_rpy(pose.orientation); - - // rpy update - ang_x += twist.angular.x * time_diff_sec; - ang_y += twist.angular.y * time_diff_sec; - ang_z += twist.angular.z * time_diff_sec; - tf2::Quaternion quat; - quat.setRPY(ang_x, ang_y, ang_z); + // dead reckoning from prev_odometry_ to latest_odometry_ + PoseStamped::SharedPtr prev_pose = std::make_shared(); + prev_pose->header = prev_odometry_->header; + prev_pose->pose = prev_odometry_->pose.pose; - // Convert twist to world frame - tf2::Vector3 linear_velocity(twist.linear.x, twist.linear.y, twist.linear.z); - linear_velocity = tf2::quatRotate(quat, linear_velocity); - - // update - pose.position.x += linear_velocity.x() * time_diff_sec; - pose.position.y += linear_velocity.y() * time_diff_sec; - pose.position.z += linear_velocity.z() * time_diff_sec; - pose.orientation.x = quat.x(); - pose.orientation.y = quat.y(); - pose.orientation.z = quat.z(); - pose.orientation.w = quat.w(); - prev_time = curr_time; - } + Pose::SharedPtr DR_pose = std::make_shared(); + dead_reckon(prev_pose, latest_odometry_time, twist_buffer_, DR_pose); - // compare pose and latest_odometry_ + // compare dead reckoning pose and latest_odometry_ const Pose latest_ekf_pose = latest_odometry_->pose.pose; - const Pose ekf_to_odom = tier4_autoware_utils::inverseTransformPose(pose, latest_ekf_pose); - const geometry_msgs::msg::Point pos = ekf_to_odom.position; - const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_odom.orientation); + const Pose ekf_to_DR = tier4_autoware_utils::inverseTransformPose(*DR_pose, latest_ekf_pose); + const geometry_msgs::msg::Point pos = ekf_to_DR.position; + const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_DR.orientation); const std::vector values = {pos.x, pos.y, pos.z, ang_x, ang_y, ang_z}; - const rclcpp::Time stamp = latest_odometry_->header.stamp; - // publish diff_pose for debug PoseStamped diff_pose; - diff_pose.header = latest_odometry_->header; - diff_pose.pose = ekf_to_odom; + diff_pose.header.stamp = latest_odometry_time; + diff_pose.header.frame_id = "base_link"; + diff_pose.pose = ekf_to_DR; diff_pose_pub_->publish(diff_pose); - const std::vector thresholds = {threshold_diff_position_x_, threshold_diff_position_y_, - threshold_diff_position_z_, threshold_diff_angle_x_, - threshold_diff_angle_y_, threshold_diff_angle_z_}; + // publish diagnostics + ThresholdValues threshold_values = + calculate_threshold((latest_odometry_time - prev_odometry_time).seconds()); + + const std::vector thresholds = {threshold_values.position_x, threshold_values.position_y, + threshold_values.position_z, threshold_values.angle_x, + threshold_values.angle_y, threshold_values.angle_z}; const std::vector labels = {"diff_position_x", "diff_position_y", "diff_position_z", "diff_angle_x", "diff_angle_y", "diff_angle_z"}; - // publish diagnostics DiagnosticStatus status; status.name = "localization: pose_instability_detector"; status.hardware_id = this->get_name(); @@ -166,13 +169,239 @@ void PoseInstabilityDetector::callback_timer() status.message = (all_ok ? "OK" : "WARN"); DiagnosticArray diagnostics; - diagnostics.header.stamp = stamp; + diagnostics.header.stamp = latest_odometry_time; diagnostics.status.emplace_back(status); diagnostics_pub_->publish(diagnostics); // prepare for next loop prev_odometry_ = latest_odometry_; - twist_buffer_.clear(); +} + +PoseInstabilityDetector::ThresholdValues PoseInstabilityDetector::calculate_threshold( + double interval_sec) +{ + // Calculate maximum longitudinal difference + const double longitudinal_difference = + heading_velocity_maximum_ * heading_velocity_scale_factor_tolerance_ * 0.01 * interval_sec; + + // Calculate maximum lateral and vertical difference + double lateral_difference = 0.0; + + const std::vector heading_velocity_signs = {1.0, -1.0, -1.0, 1.0}; + const std::vector angular_velocity_signs = {1.0, 1.0, -1.0, -1.0}; + + const double nominal_variation_x = heading_velocity_maximum_ / angular_velocity_maximum_ * + sin(angular_velocity_maximum_ * interval_sec); + const double nominal_variation_y = heading_velocity_maximum_ / angular_velocity_maximum_ * + (1 - cos(angular_velocity_maximum_ * interval_sec)); + + for (int i = 0; i < 4; i++) { + const double edge_heading_velocity = + heading_velocity_maximum_ * + (1 + heading_velocity_signs[i] * heading_velocity_scale_factor_tolerance_ * 0.01); + const double edge_angular_velocity = + angular_velocity_maximum_ * + (1 + angular_velocity_signs[i] * angular_velocity_scale_factor_tolerance_ * 0.01) + + angular_velocity_signs[i] * angular_velocity_bias_tolerance_; + + const double edge_variation_x = + edge_heading_velocity / edge_angular_velocity * sin(edge_angular_velocity * interval_sec); + const double edge_variation_y = edge_heading_velocity / edge_angular_velocity * + (1 - cos(edge_angular_velocity * interval_sec)); + + const double diff_variation_x = edge_variation_x - nominal_variation_x; + const double diff_variation_y = edge_variation_y - nominal_variation_y; + + const double lateral_difference_candidate = abs( + diff_variation_x * sin(angular_velocity_maximum_ * interval_sec) - + diff_variation_y * cos(angular_velocity_maximum_ * interval_sec)); + lateral_difference = std::max(lateral_difference, lateral_difference_candidate); + } + + const double vertical_difference = lateral_difference; + + // Calculate maximum angular difference + const double roll_difference = + (angular_velocity_maximum_ * angular_velocity_scale_factor_tolerance_ * 0.01 + + angular_velocity_bias_tolerance_) * + interval_sec; + const double pitch_difference = roll_difference; + const double yaw_difference = roll_difference; + + // Set thresholds + ThresholdValues result_values; + result_values.position_x = longitudinal_difference + pose_estimator_longitudinal_tolerance_; + result_values.position_y = lateral_difference + pose_estimator_lateral_tolerance_; + result_values.position_z = vertical_difference + pose_estimator_vertical_tolerance_; + result_values.angle_x = roll_difference + pose_estimator_angular_tolerance_; + result_values.angle_y = pitch_difference + pose_estimator_angular_tolerance_; + result_values.angle_z = yaw_difference + pose_estimator_angular_tolerance_; + + return result_values; +} + +void PoseInstabilityDetector::dead_reckon( + PoseStamped::SharedPtr & initial_pose, const rclcpp::Time & end_time, + const std::deque & twist_deque, Pose::SharedPtr & estimated_pose) +{ + // get start time + rclcpp::Time start_time = rclcpp::Time(initial_pose->header.stamp); + + // initialize estimated_pose + estimated_pose->position = initial_pose->pose.position; + estimated_pose->orientation = initial_pose->pose.orientation; + + // cut out necessary twist data + std::deque sliced_twist_deque = + clip_out_necessary_twist(twist_deque, start_time, end_time); + + // dead reckoning + rclcpp::Time prev_odometry_time = rclcpp::Time(sliced_twist_deque.front().header.stamp); + tf2::Quaternion prev_orientation; + tf2::fromMsg(estimated_pose->orientation, prev_orientation); + + for (size_t i = 1; i < sliced_twist_deque.size(); ++i) { + const rclcpp::Time curr_time = rclcpp::Time(sliced_twist_deque[i].header.stamp); + const double time_diff_sec = (curr_time - prev_odometry_time).seconds(); + + const Twist twist = sliced_twist_deque[i - 1].twist.twist; + + // variation of orientation (rpy update) + tf2::Quaternion delta_orientation; + tf2::Vector3 rotation_axis(twist.angular.x, twist.angular.y, twist.angular.z); + if (rotation_axis.length() > 0.0) { + delta_orientation.setRotation( + rotation_axis.normalized(), rotation_axis.length() * time_diff_sec); + } else { + delta_orientation.setValue(0.0, 0.0, 0.0, 1.0); + } + + tf2::Quaternion curr_orientation; + curr_orientation = prev_orientation * delta_orientation; + curr_orientation.normalize(); + + // average quaternion of two frames + tf2::Quaternion average_quat = prev_orientation.slerp(curr_orientation, 0.5); + + // Convert twist to world frame (take average of two frames) + tf2::Vector3 linear_velocity(twist.linear.x, twist.linear.y, twist.linear.z); + linear_velocity = tf2::quatRotate(average_quat, linear_velocity); + + // xyz update + estimated_pose->position.x += linear_velocity.x() * time_diff_sec; + estimated_pose->position.y += linear_velocity.y() * time_diff_sec; + estimated_pose->position.z += linear_velocity.z() * time_diff_sec; + + // update previous variables + prev_odometry_time = curr_time; + prev_orientation = curr_orientation; + } + estimated_pose->orientation.x = prev_orientation.x(); + estimated_pose->orientation.y = prev_orientation.y(); + estimated_pose->orientation.z = prev_orientation.z(); + estimated_pose->orientation.w = prev_orientation.w(); +} + +std::deque +PoseInstabilityDetector::clip_out_necessary_twist( + const std::deque & twist_buffer, const rclcpp::Time & start_time, + const rclcpp::Time & end_time) +{ + // If there is only one element in the twist_buffer, return a deque that has the same twist + // from the start till the end + if (twist_buffer.size() == 1) { + TwistWithCovarianceStamped twist = twist_buffer.front(); + std::deque simple_twist_deque; + + twist.header.stamp = start_time; + simple_twist_deque.push_back(twist); + + twist.header.stamp = end_time; + simple_twist_deque.push_back(twist); + + return simple_twist_deque; + } + + // get iterator to the element that is right before start_time (if it does not exist, start_it = + // twist_buffer.begin()) + auto start_it = twist_buffer.begin(); + + for (auto it = twist_buffer.begin(); it != twist_buffer.end(); ++it) { + if (rclcpp::Time(it->header.stamp) > start_time) { + break; + } + start_it = it; + } + + // get iterator to the element that is right after end_time (if it does not exist, end_it = + // twist_buffer.end()) + auto end_it = twist_buffer.end(); + end_it--; + for (auto it = end_it; it != twist_buffer.begin(); --it) { + if (rclcpp::Time(it->header.stamp) < end_time) { + break; + } + end_it = it; + } + + // Create result deque + std::deque result_deque(start_it, end_it); + + // If the first element is later than start_time, add the first element to the front of the + // result_deque + if (rclcpp::Time(result_deque.front().header.stamp) > start_time) { + TwistWithCovarianceStamped start_twist = *start_it; + start_twist.header.stamp = start_time; + result_deque.push_front(start_twist); + } else { + // If the first element is earlier than start_time, interpolate the first element + rclcpp::Time time0 = rclcpp::Time(result_deque[0].header.stamp); + rclcpp::Time time1 = rclcpp::Time(result_deque[1].header.stamp); + double ratio = (start_time - time0).seconds() / (time1 - time0).seconds(); + Twist twist0 = result_deque[0].twist.twist; + Twist twist1 = result_deque[1].twist.twist; + result_deque[0].twist.twist.linear.x = twist1.linear.x * ratio + twist0.linear.x * (1 - ratio); + result_deque[0].twist.twist.linear.y = twist1.linear.y * ratio + twist0.linear.y * (1 - ratio); + result_deque[0].twist.twist.linear.z = twist1.linear.z * ratio + twist0.linear.z * (1 - ratio); + result_deque[0].twist.twist.angular.x = + twist1.angular.x * ratio + twist0.angular.x * (1 - ratio); + result_deque[0].twist.twist.angular.y = + twist1.angular.y * ratio + twist0.angular.y * (1 - ratio); + result_deque[0].twist.twist.angular.z = + twist1.angular.z * ratio + twist0.angular.z * (1 - ratio); + + result_deque[0].header.stamp = start_time; + } + + // If the last element is earlier than end_time, add the last element to the back of the + // result_deque + if (rclcpp::Time(result_deque.back().header.stamp) < end_time) { + TwistWithCovarianceStamped end_twist = *end_it; + end_twist.header.stamp = end_time; + result_deque.push_back(end_twist); + } else { + // If the last element is later than end_time, interpolate the last element + rclcpp::Time time0 = rclcpp::Time(result_deque[result_deque.size() - 2].header.stamp); + rclcpp::Time time1 = rclcpp::Time(result_deque[result_deque.size() - 1].header.stamp); + double ratio = (end_time - time0).seconds() / (time1 - time0).seconds(); + Twist twist0 = result_deque[result_deque.size() - 2].twist.twist; + Twist twist1 = result_deque[result_deque.size() - 1].twist.twist; + result_deque[result_deque.size() - 1].twist.twist.linear.x = + twist1.linear.x * ratio + twist0.linear.x * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.linear.y = + twist1.linear.y * ratio + twist0.linear.y * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.linear.z = + twist1.linear.z * ratio + twist0.linear.z * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.angular.x = + twist1.angular.x * ratio + twist0.angular.x * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.angular.y = + twist1.angular.y * ratio + twist0.angular.y * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.angular.z = + twist1.angular.z * ratio + twist0.angular.z * (1 - ratio); + + result_deque[result_deque.size() - 1].header.stamp = end_time; + } + return result_deque; } #include diff --git a/localization/pose_instability_detector/test/test.cpp b/localization/pose_instability_detector/test/test.cpp index 5ea03859d7731..9300984967d4b 100644 --- a/localization/pose_instability_detector/test/test.cpp +++ b/localization/pose_instability_detector/test/test.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "../src/pose_instability_detector.hpp" +#include "autoware/pose_instability_detector/pose_instability_detector.hpp" #include "test_message_helper_node.hpp" #include @@ -81,6 +81,11 @@ TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry) // N timestamp.nanosec = 0; helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0); + // send the twist message1 (move 1m in x direction) + timestamp.sec = 0; + timestamp.nanosec = 5e8; + helper_->send_twist_message(timestamp, 2.0, 0.0, 0.0); + // process the above message (by timer_callback) helper_->received_diagnostic_array_flag = false; while (!helper_->received_diagnostic_array_flag) { @@ -88,11 +93,6 @@ TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry) // N std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - // send the twist message1 (move 1m in x direction) - timestamp.sec = 0; - timestamp.nanosec = 5e8; - helper_->send_twist_message(timestamp, 2.0, 0.0, 0.0); - // send the twist message2 (move 1m in x direction) timestamp.sec = 1; timestamp.nanosec = 0; @@ -101,7 +101,9 @@ TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry) // N // send the second odometry message (finish x = 12) timestamp.sec = 2; timestamp.nanosec = 0; - helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0); + helper_->send_odometry_message(timestamp, 14.0, 0.0, 0.0); + + executor_.spin_some(); // process the above messages (by timer_callback) helper_->received_diagnostic_array_flag = false; @@ -124,6 +126,11 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small) // NOL timestamp.nanosec = 0; helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0); + // send the twist message1 (move 0.1m in x direction) + timestamp.sec = 0; + timestamp.nanosec = 5e8; + helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); + // process the above message (by timer_callback) helper_->received_diagnostic_array_flag = false; while (!helper_->received_diagnostic_array_flag) { @@ -131,11 +138,6 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small) // NOL std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - // send the twist message1 (move 0.1m in x direction) - timestamp.sec = 0; - timestamp.nanosec = 5e8; - helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); - // send the twist message2 (move 0.1m in x direction) timestamp.sec = 1; timestamp.nanosec = 0; @@ -144,7 +146,9 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small) // NOL // send the second odometry message (finish x = 12) timestamp.sec = 2; timestamp.nanosec = 0; - helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0); + helper_->send_odometry_message(timestamp, 14.0, 0.0, 0.0); + + executor_.spin_some(); // process the above messages (by timer_callback) helper_->received_diagnostic_array_flag = false; diff --git a/localization/stop_filter/include/stop_filter/stop_filter.hpp b/localization/stop_filter/include/stop_filter/stop_filter.hpp index 24145a7920d91..e8428788820b2 100644 --- a/localization/stop_filter/include/stop_filter/stop_filter.hpp +++ b/localization/stop_filter/include/stop_filter/stop_filter.hpp @@ -52,6 +52,6 @@ class StopFilter : public rclcpp::Node /** * @brief set odometry measurement */ - void callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); + void callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg); }; #endif // STOP_FILTER__STOP_FILTER_HPP_ diff --git a/localization/stop_filter/src/stop_filter.cpp b/localization/stop_filter/src/stop_filter.cpp index 4d6b2c6240867..c1c4de2fb6b6e 100644 --- a/localization/stop_filter/src/stop_filter.cpp +++ b/localization/stop_filter/src/stop_filter.cpp @@ -31,13 +31,13 @@ StopFilter::StopFilter(const rclcpp::NodeOptions & node_options) wz_threshold_ = declare_parameter("wz_threshold"); sub_odom_ = create_subscription( - "input/odom", 1, std::bind(&StopFilter::callbackOdometry, this, _1)); + "input/odom", 1, std::bind(&StopFilter::callback_odometry, this, _1)); pub_odom_ = create_publisher("output/odom", 1); pub_stop_flag_ = create_publisher("debug/stop_flag", 1); } -void StopFilter::callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) +void StopFilter::callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg) { tier4_debug_msgs::msg::BoolStamped stop_flag_msg; stop_flag_msg.stamp = msg->header.stamp; diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index de675da5cc2d4..d4c8b71fa6b66 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -29,6 +29,7 @@ sophus std_msgs tf2_ros + tier4_autoware_utils visualization_msgs ament_lint_auto diff --git a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp index d0dfc87067f7f..1126d6260b911 100644 --- a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp +++ b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp @@ -15,7 +15,7 @@ #include "yabloc_common/ll2_decomposer/ll2_decomposer.hpp" #include -#include +#include #include #include @@ -198,7 +198,7 @@ Ll2Decomposer::MarkerArray Ll2Decomposer::make_sign_marker_msg( marker.header.frame_id = "map"; marker.header.stamp = get_clock()->now(); marker.type = Marker::LINE_STRIP; - marker.color = common::Color(0.6f, 0.6f, 0.6f, 0.999f); + marker.color = tier4_autoware_utils::createMarkerColor(0.6f, 0.6f, 0.6f, 0.999f); marker.scale.x = 0.1; marker.ns = ns; marker.id = id++; @@ -228,7 +228,7 @@ Ll2Decomposer::MarkerArray Ll2Decomposer::make_polygon_marker_msg( marker.header.frame_id = "map"; marker.header.stamp = get_clock()->now(); marker.type = Marker::LINE_STRIP; - marker.color = common::Color(0.4f, 0.4f, 0.8f, 0.999f); + marker.color = tier4_autoware_utils::createMarkerColor(0.4f, 0.4f, 0.8f, 0.999f); marker.scale.x = 0.2; marker.ns = ns; marker.id = id++; diff --git a/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp index 90cd62883e339..e6e59a836f6db 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp @@ -93,7 +93,8 @@ class ParticleVisualize : public rclcpp::Node marker.scale.y = 0.1; marker.scale.z = 0.1; - marker.color = common::color_scale::rainbow(bound_weight(p.weight)); + marker.color = + static_cast(common::color_scale::rainbow(bound_weight(p.weight))); marker.pose.orientation = p.pose.orientation; marker.pose.position.x = p.pose.position.x; marker.pose.position.y = p.pose.position.y; diff --git a/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp b/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp index 955ed84fdea2d..888baf0bea0ac 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp @@ -45,7 +45,8 @@ void ParticleVisualizer::publish(const ParticleArray & msg) marker.scale.x = 0.3; marker.scale.y = 0.1; marker.scale.z = 0.1; - marker.color = common::color_scale::rainbow(boundWeight(p.weight)); + marker.color = + static_cast(common::color_scale::rainbow(boundWeight(p.weight))); marker.pose.orientation = p.pose.orientation; marker.pose.position.x = p.pose.position.x; marker.pose.position.y = p.pose.position.y; diff --git a/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp index b321986240b69..e8c8890561a7f 100644 --- a/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp @@ -136,7 +136,7 @@ void GnssParticleCorrector::publish_marker(const Eigen::Vector3f & position, boo marker.pose.position.z = latest_height_.data; float prob = i / 4.0f; - marker.color = common::color_scale::rainbow(prob); + marker.color = static_cast(common::color_scale::rainbow(prob)); marker.color.a = 0.5f; marker.scale.x = 0.1; drawCircle(marker.points, weight_manager_.inverse_normal_pdf(prob, is_rtk_fixed)); diff --git a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp index ebd9c424edf5e..62d8ed826b373 100644 --- a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include @@ -169,7 +169,7 @@ HierarchicalCostMap::MarkerArray HierarchicalCostMap::show_map_range() const marker.header.frame_id = "map"; marker.id = id++; marker.type = Marker::LINE_STRIP; - marker.color = common::Color(0, 0, 1.0f, 1.0f); + marker.color = tier4_autoware_utils::createMarkerColor(0, 0, 1.0f, 1.0f); marker.scale.x = 0.1; Eigen::Vector2f xy = area.real_scale(); marker.points.push_back(point_msg(xy.x(), xy.y())); diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp index c6c34f0e28a42..8a5fc65e2fac3 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp @@ -42,7 +42,8 @@ void MarkerModule::publish_marker( marker.type = Marker::ARROW; marker.id = i; marker.ns = "arrow"; - marker.color = common::color_scale::rainbow(normalize(scores.at(i))); + marker.color = + static_cast(common::color_scale::rainbow(normalize(scores.at(i)))); marker.color.a = 0.5; marker.pose.position.x = position.x(); diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 838d7b1c8e316..52e104f572f76 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -157,7 +157,7 @@ PredictedPath PathGenerator::generatePathForOnLaneVehicle( return generateStraightPath(object, duration); } - return generatePolynomialPath(object, ref_paths, speed_limit, duration, lateral_duration); + return generatePolynomialPath(object, ref_paths, duration, lateral_duration, speed_limit); } PredictedPath PathGenerator::generateStraightPath( diff --git a/planning/.pages b/planning/.pages index 48dc8f32edf71..6e3c6d5d49104 100644 --- a/planning/.pages +++ b/planning/.pages @@ -14,8 +14,8 @@ nav: - 'Dynamic Obstacle Avoidance': planning/autoware_behavior_path_dynamic_obstacle_avoidance_module - 'Goal Planner': planning/behavior_path_goal_planner_module - 'Lane Change': planning/behavior_path_lane_change_module - - 'Side Shift': planning/behavior_path_side_shift_module - - 'Start Planner': planning/behavior_path_start_planner_module + - 'Side Shift': planning/autoware_behavior_path_side_shift_module + - 'Start Planner': planning/autoware_behavior_path_start_planner_module - 'Static Obstacle Avoidance': planning/autoware_behavior_path_static_obstacle_avoidance_module - 'Behavior Velocity Planner': - 'About Behavior Velocity': planning/autoware_behavior_velocity_planner @@ -25,7 +25,7 @@ nav: - 'Crosswalk': planning/behavior_velocity_crosswalk_module - 'Detection Area': planning/behavior_velocity_detection_area_module - 'Dynamic Obstacle Stop': planning/behavior_velocity_dynamic_obstacle_stop_module - - 'Intersection': planning/behavior_velocity_intersection_module + - 'Intersection': planning/autoware_behavior_velocity_intersection_module - 'No Drivable Lane': planning/behavior_velocity_no_drivable_lane_module - 'No Stopping Area': planning/behavior_velocity_no_stopping_area_module - 'Occlusion Spot': planning/behavior_velocity_occlusion_spot_module @@ -35,7 +35,7 @@ nav: - 'Stop Line': planning/behavior_velocity_stop_line_module - 'Traffic Light': planning/behavior_velocity_traffic_light_module - 'Virtual Traffic Light': planning/autoware_behavior_velocity_virtual_traffic_light_module - - 'Walkway': planning/behavior_velocity_walkway_module + - 'Walkway': planning/autoware_behavior_velocity_walkway_module - 'Parking': - 'Freespace Planner': - 'About Freespace Planner': planning/freespace_planner @@ -56,13 +56,13 @@ nav: - 'About Path Smoother': planning/path_smoother - 'Elastic Band': planning/path_smoother/docs/eb - 'Sampling Based Planner': - - 'Path Sample': planning/sampling_based_planner/path_sampler - - 'Common library': planning/sampling_based_planner/sampler_common - - 'Frenet Planner': planning/sampling_based_planner/frenet_planner - - 'Bezier Sampler': planning/sampling_based_planner/bezier_sampler + - 'Path Sample': planning/sampling_based_planner/autoware_path_sampler + - 'Common library': planning/sampling_based_planner/autoware_sampler_common + - 'Frenet Planner': planning/sampling_based_planner/autoware_frenet_planner + - 'Bezier Sampler': planning/sampling_based_planner/autoware_bezier_sampler - 'Surround Obstacle Checker': - - 'About Surround Obstacle Checker': planning/surround_obstacle_checker - - 'About Surround Obstacle Checker (Japanese)': planning/surround_obstacle_checker/surround_obstacle_checker-design.ja + - 'About Surround Obstacle Checker': planning/autoware_surround_obstacle_checker + - 'About Surround Obstacle Checker (Japanese)': planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja - 'Motion Velocity Planner': - 'About Motion Velocity Planner': planning/autoware_motion_velocity_planner_node/ - 'Available Modules': @@ -74,7 +74,7 @@ nav: - 'Static Centerline Generator': planning/autoware_static_centerline_generator - 'API and Library': - 'Costmap Generator': planning/costmap_generator - - 'External Velocity Limit Selector': planning/external_velocity_limit_selector + - 'External Velocity Limit Selector': planning/autoware_external_velocity_limit_selector - 'Objects of Interest Marker Interface': planning/objects_of_interest_marker_interface - 'Route Handler': planning/route_handler - 'RTC Interface': planning/rtc_interface @@ -86,5 +86,5 @@ nav: - 'Stop Reason Visualizer': https://github.com/autowarefoundation/autoware_tools/blob/main/planning/planning_debug_tools/doc-stop-reason-visualizer.md - 'Planning Test Utils': planning/planning_test_utils - 'Planning Test Manager': planning/autoware_planning_test_manager - - 'Planning Topic Converter': planning/planning_topic_converter - - 'Planning Validator': planning/planning_validator + - 'Planning Topic Converter': planning/autoware_planning_topic_converter + - 'Planning Validator': planning/autoware_planning_validator diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp index 867e76f0db614..fe8dc4c25869a 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp @@ -18,7 +18,7 @@ #include "autoware_behavior_path_planner_common/data_manager.hpp" #include "autoware_behavior_path_planner_common/parameters.hpp" -#include +#include #include #include @@ -75,7 +75,8 @@ class GeometricParallelParking bool planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start, - const std::shared_ptr lane_departure_checker); + const std::shared_ptr + autoware_lane_departure_checker); void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; } void setPlannerData(const std::shared_ptr & planner_data) { @@ -119,7 +120,8 @@ class GeometricParallelParking const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, const bool left_side_parking, const double end_pose_offset, const double lane_departure_margin, const double arc_path_interval, - const std::shared_ptr lane_departure_checker); + const std::shared_ptr + autoware_lane_departure_checker); PathWithLaneId generateArcPath( const Pose & center, const double radius, const double start_yaw, double end_yaw, const double arc_path_interval, const bool is_left_turn, const bool is_forward); diff --git a/planning/autoware_behavior_path_planner_common/package.xml b/planning/autoware_behavior_path_planner_common/package.xml index 21da8dcef1c2d..24d369e475e61 100644 --- a/planning/autoware_behavior_path_planner_common/package.xml +++ b/planning/autoware_behavior_path_planner_common/package.xml @@ -43,12 +43,12 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_lane_departure_checker autoware_perception_msgs autoware_planning_msgs freespace_planning_algorithms geometry_msgs interpolation - lane_departure_checker lanelet2_extension magic_enum motion_utils diff --git a/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index 26c089ee0ba09..73f513c9e65eb 100644 --- a/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -223,7 +223,8 @@ bool GeometricParallelParking::planPullOver( bool GeometricParallelParking::planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start, - const std::shared_ptr lane_departure_checker) + const std::shared_ptr + lane_departure_checker) { constexpr bool is_forward = false; // parking backward means pull_out forward constexpr double start_pose_offset = 0.0; // start_pose is current_pose @@ -364,7 +365,8 @@ std::vector GeometricParallelParking::planOneTrial( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, const bool left_side_parking, const double end_pose_offset, const double lane_departure_margin, const double arc_path_interval, - const std::shared_ptr lane_departure_checker) + const std::shared_ptr + lane_departure_checker) { clearPaths(); diff --git a/planning/behavior_path_side_shift_module/CMakeLists.txt b/planning/autoware_behavior_path_side_shift_module/CMakeLists.txt similarity index 92% rename from planning/behavior_path_side_shift_module/CMakeLists.txt rename to planning/autoware_behavior_path_side_shift_module/CMakeLists.txt index 28009e48a4cc4..c36a4ffd73254 100644 --- a/planning/behavior_path_side_shift_module/CMakeLists.txt +++ b/planning/autoware_behavior_path_side_shift_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_path_side_shift_module) +project(autoware_behavior_path_side_shift_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_path_side_shift_module/README.md b/planning/autoware_behavior_path_side_shift_module/README.md similarity index 100% rename from planning/behavior_path_side_shift_module/README.md rename to planning/autoware_behavior_path_side_shift_module/README.md diff --git a/planning/behavior_path_side_shift_module/config/side_shift.param.yaml b/planning/autoware_behavior_path_side_shift_module/config/side_shift.param.yaml similarity index 100% rename from planning/behavior_path_side_shift_module/config/side_shift.param.yaml rename to planning/autoware_behavior_path_side_shift_module/config/side_shift.param.yaml diff --git a/planning/behavior_path_side_shift_module/images/side_shift_status.drawio.svg b/planning/autoware_behavior_path_side_shift_module/images/side_shift_status.drawio.svg similarity index 100% rename from planning/behavior_path_side_shift_module/images/side_shift_status.drawio.svg rename to planning/autoware_behavior_path_side_shift_module/images/side_shift_status.drawio.svg diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/data_structs.hpp b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/data_structs.hpp similarity index 87% rename from planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/data_structs.hpp rename to planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/data_structs.hpp index 8059c46423dac..227b81e3d006f 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/data_structs.hpp +++ b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/data_structs.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ -#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ #include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -51,4 +51,4 @@ struct SideShiftDebugData }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/manager.hpp b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/manager.hpp similarity index 85% rename from planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/manager.hpp rename to planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/manager.hpp index 21621e1c7128c..7e03c77c757e8 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/manager.hpp +++ b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/manager.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ -#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ #include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" -#include "behavior_path_side_shift_module/scene.hpp" +#include "autoware_behavior_path_side_shift_module/scene.hpp" #include @@ -50,4 +50,4 @@ class SideShiftModuleManager : public SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/scene.hpp similarity index 94% rename from planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp rename to planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/scene.hpp index accba00b81439..5c8442284e513 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp +++ b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/scene.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ -#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ #include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" #include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "behavior_path_side_shift_module/data_structs.hpp" +#include "autoware_behavior_path_side_shift_module/data_structs.hpp" #include @@ -131,4 +131,4 @@ class SideShiftModule : public SceneModuleInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/utils.hpp similarity index 88% rename from planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp rename to planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/utils.hpp index f53bd42363b55..e5973948af9b3 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp +++ b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ -#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ #include #include @@ -41,4 +41,4 @@ Point transformToGrid( } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ diff --git a/planning/behavior_path_side_shift_module/package.xml b/planning/autoware_behavior_path_side_shift_module/package.xml similarity index 91% rename from planning/behavior_path_side_shift_module/package.xml rename to planning/autoware_behavior_path_side_shift_module/package.xml index af2b3c121f090..98d765fbe2fc7 100644 --- a/planning/behavior_path_side_shift_module/package.xml +++ b/planning/autoware_behavior_path_side_shift_module/package.xml @@ -1,9 +1,9 @@ - behavior_path_side_shift_module + autoware_behavior_path_side_shift_module 0.1.0 - The behavior_path_side_shift_module package + The autoware_behavior_path_side_shift_module package Satoshi Ota Fumiya Watanabe diff --git a/planning/behavior_path_side_shift_module/plugins.xml b/planning/autoware_behavior_path_side_shift_module/plugins.xml similarity index 71% rename from planning/behavior_path_side_shift_module/plugins.xml rename to planning/autoware_behavior_path_side_shift_module/plugins.xml index 2688e3a17c2b6..7be57efc71739 100644 --- a/planning/behavior_path_side_shift_module/plugins.xml +++ b/planning/autoware_behavior_path_side_shift_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_path_side_shift_module/src/manager.cpp b/planning/autoware_behavior_path_side_shift_module/src/manager.cpp similarity index 97% rename from planning/behavior_path_side_shift_module/src/manager.cpp rename to planning/autoware_behavior_path_side_shift_module/src/manager.cpp index fa8d579da3d09..bee9d3c78bafc 100644 --- a/planning/behavior_path_side_shift_module/src/manager.cpp +++ b/planning/autoware_behavior_path_side_shift_module/src/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_side_shift_module/manager.hpp" +#include "autoware_behavior_path_side_shift_module/manager.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" diff --git a/planning/behavior_path_side_shift_module/src/scene.cpp b/planning/autoware_behavior_path_side_shift_module/src/scene.cpp similarity index 99% rename from planning/behavior_path_side_shift_module/src/scene.cpp rename to planning/autoware_behavior_path_side_shift_module/src/scene.cpp index c2b239b5dd4fa..e5ea0e4cca29c 100644 --- a/planning/behavior_path_side_shift_module/src/scene.cpp +++ b/planning/autoware_behavior_path_side_shift_module/src/scene.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_side_shift_module/scene.hpp" +#include "autoware_behavior_path_side_shift_module/scene.hpp" #include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" #include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware_behavior_path_planner_common/utils/path_utils.hpp" #include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_side_shift_module/utils.hpp" +#include "autoware_behavior_path_side_shift_module/utils.hpp" #include diff --git a/planning/behavior_path_side_shift_module/src/utils.cpp b/planning/autoware_behavior_path_side_shift_module/src/utils.cpp similarity index 97% rename from planning/behavior_path_side_shift_module/src/utils.cpp rename to planning/autoware_behavior_path_side_shift_module/src/utils.cpp index 888a2e9e24d9f..507259d9e6e23 100644 --- a/planning/behavior_path_side_shift_module/src/utils.cpp +++ b/planning/autoware_behavior_path_side_shift_module/src/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_side_shift_module/utils.hpp" +#include "autoware_behavior_path_side_shift_module/utils.hpp" #include "autoware_behavior_path_planner_common/utils/utils.hpp" diff --git a/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp b/planning/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp similarity index 87% rename from planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp rename to planning/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp index db1f5ee3560b1..244c59bf5e6f9 100644 --- a/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp @@ -57,14 +57,15 @@ std::shared_ptr generateNode() node_options.parameter_overrides(params); test_utils::updateNodeOptions( - node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", - planning_test_utils_dir + "/config/test_nearest_search.param.yaml", - planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", - behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", - behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", - behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - ament_index_cpp::get_package_share_directory("behavior_path_side_shift_module") + - "/config/side_shift.param.yaml"}); + node_options, + {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + ament_index_cpp::get_package_share_directory("autoware_behavior_path_side_shift_module") + + "/config/side_shift.param.yaml"}); return std::make_shared(node_options); } diff --git a/planning/behavior_path_start_planner_module/CMakeLists.txt b/planning/autoware_behavior_path_start_planner_module/CMakeLists.txt similarity index 88% rename from planning/behavior_path_start_planner_module/CMakeLists.txt rename to planning/autoware_behavior_path_start_planner_module/CMakeLists.txt index 6bb2f76cb9842..b2882727f50d8 100644 --- a/planning/behavior_path_start_planner_module/CMakeLists.txt +++ b/planning/autoware_behavior_path_start_planner_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_path_start_planner_module) +project(autoware_behavior_path_start_planner_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/autoware_behavior_path_start_planner_module/README.md similarity index 99% rename from planning/behavior_path_start_planner_module/README.md rename to planning/autoware_behavior_path_start_planner_module/README.md index 71aa0a1abe704..bcb3b1bec1fd9 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/autoware_behavior_path_start_planner_module/README.md @@ -526,7 +526,7 @@ If a safe path cannot be generated from the current position, search backwards f ### **freespace pull out** If the vehicle gets stuck with pull out along lanes, execute freespace pull out. -To run this feature, you need to set `parking_lot` to the map, `activate_by_scenario` of [costmap_generator](../costmap_generator/README.md) to `false` and `enable_freespace_planner` to `true` +To run this feature, you need to set `parking_lot` to the map, `activate_by_scenario` of [costmap_generator](../autoware_costmap_generator/README.md) to `false` and `enable_freespace_planner` to `true` diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml similarity index 100% rename from planning/behavior_path_start_planner_module/config/start_planner.param.yaml rename to planning/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml diff --git a/planning/behavior_path_start_planner_module/images/collision_check_range_shift_pull_out.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/collision_check_range_shift_pull_out.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/collision_check_range_shift_pull_out.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/collision_check_range_shift_pull_out.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/freespace_pull_out.png b/planning/autoware_behavior_path_start_planner_module/images/freespace_pull_out.png similarity index 100% rename from planning/behavior_path_start_planner_module/images/freespace_pull_out.png rename to planning/autoware_behavior_path_start_planner_module/images/freespace_pull_out.png diff --git a/planning/behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/priority_order.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/priority_order.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/priority_order.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/shift_pull_out.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/shift_pull_out.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/shift_pull_out.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/shift_pull_out.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/shift_pull_out_path.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/shift_pull_out_path.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/start_planner_example.png b/planning/autoware_behavior_path_start_planner_module/images/start_planner_example.png similarity index 100% rename from planning/behavior_path_start_planner_module/images/start_planner_example.png rename to planning/autoware_behavior_path_start_planner_module/images/start_planner_example.png diff --git a/planning/behavior_path_start_planner_module/images/start_pose_candidate.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/start_pose_candidate.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/start_pose_candidate.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/start_pose_candidate.drawio.svg diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/data_structs.hpp similarity index 95% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/data_structs.hpp index d8b94bb33d00b..95c98c3c89733 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/data_structs.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ #include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" @@ -134,4 +134,4 @@ struct StartPlannerParameters } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/debug.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/debug.hpp similarity index 80% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/debug.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/debug.hpp index 6aa5584807d90..c252a8f22eda5 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/debug.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/debug.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ -#include "behavior_path_start_planner_module/data_structs.hpp" +#include "autoware_behavior_path_start_planner_module/data_structs.hpp" #include #include @@ -36,4 +36,4 @@ void updateSafetyCheckDebugData( } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/freespace_pull_out.hpp similarity index 83% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/freespace_pull_out.hpp index 561ef337fa68c..df640b04e5a72 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/freespace_pull_out.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ -#include "behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp" #include #include @@ -49,4 +49,4 @@ class FreespacePullOut : public PullOutPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/geometric_pull_out.hpp similarity index 65% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/geometric_pull_out.hpp index ccf9e933f9383..510fee8fe3dc6 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/geometric_pull_out.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ #include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include "behavior_path_start_planner_module/pull_out_path.hpp" -#include "behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp" -#include +#include #include @@ -32,15 +32,16 @@ class GeometricPullOut : public PullOutPlannerBase public: explicit GeometricPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const std::shared_ptr lane_departure_checker); + const std::shared_ptr + lane_departure_checker); PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; }; std::optional plan(const Pose & start_pose, const Pose & goal_pose) override; GeometricParallelParking planner_; ParallelParkingParameters parallel_parking_parameters_; - std::shared_ptr lane_departure_checker_; + std::shared_ptr lane_departure_checker_; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/manager.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/manager.hpp similarity index 84% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/manager.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/manager.hpp index ac752918a0e32..a7fcd6d820202 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/manager.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/manager.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ #include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" -#include "behavior_path_start_planner_module/start_planner_module.hpp" +#include "autoware_behavior_path_start_planner_module/start_planner_module.hpp" #include @@ -53,4 +53,4 @@ class StartPlannerModuleManager : public SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_path.hpp similarity index 83% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_path.hpp index f7f443c297c61..5674087cdb3df 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_path.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ #include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -34,4 +34,4 @@ struct PullOutPath Pose end_pose{}; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp similarity index 89% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp index f2c437883b8a1..8cbe3569d13fe 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ #include "autoware_behavior_path_planner_common/data_manager.hpp" #include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_start_planner_module/data_structs.hpp" -#include "behavior_path_start_planner_module/pull_out_path.hpp" -#include "behavior_path_start_planner_module/util.hpp" +#include "autoware_behavior_path_start_planner_module/data_structs.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" +#include "autoware_behavior_path_start_planner_module/util.hpp" #include #include @@ -102,4 +102,4 @@ class PullOutPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/shift_pull_out.hpp similarity index 80% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/shift_pull_out.hpp index 3bded7ee99fbb..29a74ce0e14ef 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/shift_pull_out.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ -#include "behavior_path_start_planner_module/pull_out_path.hpp" -#include "behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp" -#include +#include #include @@ -27,7 +27,7 @@ namespace behavior_path_planner { -using lane_departure_checker::LaneDepartureChecker; +using autoware::lane_departure_checker::LaneDepartureChecker; class ShiftPullOut : public PullOutPlannerBase { @@ -61,4 +61,4 @@ class ShiftPullOut : public PullOutPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/start_planner_module.hpp similarity index 94% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/start_planner_module.hpp index c8e74009d267a..7a98a43efec5b 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/start_planner_module.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ #include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" #include "autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" @@ -21,13 +21,13 @@ #include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_start_planner_module/data_structs.hpp" -#include "behavior_path_start_planner_module/freespace_pull_out.hpp" -#include "behavior_path_start_planner_module/geometric_pull_out.hpp" -#include "behavior_path_start_planner_module/pull_out_path.hpp" -#include "behavior_path_start_planner_module/shift_pull_out.hpp" +#include "autoware_behavior_path_start_planner_module/data_structs.hpp" +#include "autoware_behavior_path_start_planner_module/freespace_pull_out.hpp" +#include "autoware_behavior_path_start_planner_module/geometric_pull_out.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" +#include "autoware_behavior_path_start_planner_module/shift_pull_out.hpp" -#include +#include #include #include @@ -46,13 +46,13 @@ namespace behavior_path_planner { +using autoware::lane_departure_checker::LaneDepartureChecker; using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using geometry_msgs::msg::PoseArray; -using lane_departure_checker::LaneDepartureChecker; using PriorityOrder = std::vector>>; struct PullOutStatus @@ -336,4 +336,4 @@ class StartPlannerModule : public SceneModuleInterface }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/util.hpp similarity index 89% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/util.hpp index 9185873cfd770..b9f0b964ed428 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/util.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ #include "autoware_behavior_path_planner_common/data_manager.hpp" #include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_start_planner_module/pull_out_path.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" #include @@ -55,4 +55,4 @@ std::optional extractCollisionCheckSection( const PullOutPath & path, const double collision_check_distance_from_end); } // namespace behavior_path_planner::start_planner_utils -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/behavior_path_start_planner_module/package.xml b/planning/autoware_behavior_path_start_planner_module/package.xml similarity index 91% rename from planning/behavior_path_start_planner_module/package.xml rename to planning/autoware_behavior_path_start_planner_module/package.xml index 23c57b6d6024a..8504241c908ce 100644 --- a/planning/behavior_path_start_planner_module/package.xml +++ b/planning/autoware_behavior_path_start_planner_module/package.xml @@ -1,9 +1,9 @@ - behavior_path_start_planner_module + autoware_behavior_path_start_planner_module 0.1.0 - The behavior_path_start_planner_module package + The autoware_behavior_path_start_planner_module package Kosuke Takeuchi Kyoichi Sugahara diff --git a/planning/behavior_path_start_planner_module/plugins.xml b/planning/autoware_behavior_path_start_planner_module/plugins.xml similarity index 70% rename from planning/behavior_path_start_planner_module/plugins.xml rename to planning/autoware_behavior_path_start_planner_module/plugins.xml index fde8b673be5bc..92498d5158ae5 100644 --- a/planning/behavior_path_start_planner_module/plugins.xml +++ b/planning/autoware_behavior_path_start_planner_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_path_start_planner_module/src/debug.cpp b/planning/autoware_behavior_path_start_planner_module/src/debug.cpp similarity index 94% rename from planning/behavior_path_start_planner_module/src/debug.cpp rename to planning/autoware_behavior_path_start_planner_module/src/debug.cpp index 0f95470d86d40..4b4aa7c386925 100644 --- a/planning/behavior_path_start_planner_module/src/debug.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/debug.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_start_planner_module/debug.hpp" +#include "autoware_behavior_path_start_planner_module/debug.hpp" namespace behavior_path_planner { diff --git a/planning/behavior_path_start_planner_module/src/freespace_pull_out.cpp b/planning/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp similarity index 97% rename from planning/behavior_path_start_planner_module/src/freespace_pull_out.cpp rename to planning/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp index d29047d23fffd..1dfacd45b5218 100644 --- a/planning/behavior_path_start_planner_module/src/freespace_pull_out.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_start_planner_module/freespace_pull_out.hpp" +#include "autoware_behavior_path_start_planner_module/freespace_pull_out.hpp" #include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "autoware_behavior_path_planner_common/utils/path_utils.hpp" #include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_start_planner_module/util.hpp" +#include "autoware_behavior_path_start_planner_module/util.hpp" #include diff --git a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp similarity index 95% rename from planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp rename to planning/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp index 18d02ade489e8..d299b3f4799b2 100644 --- a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_start_planner_module/geometric_pull_out.hpp" +#include "autoware_behavior_path_start_planner_module/geometric_pull_out.hpp" #include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware_behavior_path_planner_common/utils/path_utils.hpp" #include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_start_planner_module/util.hpp" +#include "autoware_behavior_path_start_planner_module/util.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -32,7 +32,8 @@ using start_planner_utils::getPullOutLanes; GeometricPullOut::GeometricPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const std::shared_ptr lane_departure_checker) + const std::shared_ptr + lane_departure_checker) : PullOutPlannerBase{node, parameters}, parallel_parking_parameters_{parameters.parallel_parking_parameters}, lane_departure_checker_(lane_departure_checker) diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/autoware_behavior_path_start_planner_module/src/manager.cpp similarity index 99% rename from planning/behavior_path_start_planner_module/src/manager.cpp rename to planning/autoware_behavior_path_start_planner_module/src/manager.cpp index d5731f54224b6..23cca1ef05190 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_start_planner_module/manager.hpp" +#include "autoware_behavior_path_start_planner_module/manager.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp similarity index 99% rename from planning/behavior_path_start_planner_module/src/shift_pull_out.cpp rename to planning/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index 298510126a4c3..d0a7b827a2285 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_start_planner_module/shift_pull_out.hpp" +#include "autoware_behavior_path_start_planner_module/shift_pull_out.hpp" #include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware_behavior_path_planner_common/utils/path_utils.hpp" #include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_start_planner_module/util.hpp" +#include "autoware_behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp similarity index 99% rename from planning/behavior_path_start_planner_module/src/start_planner_module.cpp rename to planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 82a4709c6b5be..b30af17eaa898 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_start_planner_module/start_planner_module.hpp" +#include "autoware_behavior_path_start_planner_module/start_planner_module.hpp" #include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" #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 "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_start_planner_module/debug.hpp" -#include "behavior_path_start_planner_module/util.hpp" +#include "autoware_behavior_path_start_planner_module/debug.hpp" +#include "autoware_behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include @@ -65,7 +65,7 @@ StartPlannerModule::StartPlannerModule( { lane_departure_checker_ = std::make_shared(); lane_departure_checker_->setVehicleInfo(vehicle_info_); - lane_departure_checker::Param lane_departure_checker_params; + autoware::lane_departure_checker::Param lane_departure_checker_params; lane_departure_checker_params.footprint_extra_margin = parameters->lane_departure_check_expansion_margin; diff --git a/planning/behavior_path_start_planner_module/src/util.cpp b/planning/autoware_behavior_path_start_planner_module/src/util.cpp similarity index 98% rename from planning/behavior_path_start_planner_module/src/util.cpp rename to planning/autoware_behavior_path_start_planner_module/src/util.cpp index b1bcf59db86cb..51a6093936154 100644 --- a/planning/behavior_path_start_planner_module/src/util.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/util.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_start_planner_module/util.hpp" +#include "autoware_behavior_path_start_planner_module/util.hpp" #include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "autoware_behavior_path_planner_common/utils/path_utils.hpp" diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 767c1d21dfc90..9972263cb4fd2 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -1208,32 +1208,45 @@ bool StaticObstacleAvoidanceModule::isValidShiftLine( } } + const auto is_return_shift = + [](const double start_shift_length, const double end_shift_length, const double threshold) { + return std::abs(start_shift_length) > threshold && std::abs(end_shift_length) < threshold; + }; + // check if the vehicle is in road. (yaw angle is not considered) { const auto minimum_distance = 0.5 * planner_data_->parameters.vehicle_width + parameters_->hard_drivable_bound_margin - parameters_->max_deviation_from_lane; - const size_t start_idx = shift_lines.front().start_idx; - const size_t end_idx = shift_lines.back().end_idx; + for (const auto & shift_line : shift_lines) { + const size_t start_idx = shift_line.start_idx; + const size_t end_idx = shift_line.end_idx; - const auto path = shifter_for_validate.getReferencePath(); - const auto left_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.left_bound)); - const auto right_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.right_bound)); - for (size_t i = start_idx; i <= end_idx; ++i) { - const auto p = getPoint(path.points.at(i)); - lanelet::BasicPoint2d basic_point{p.x, p.y}; - - const auto shift_length = proposed_shift_path.shift_length.at(i); - const auto THRESHOLD = minimum_distance + std::abs(shift_length); + if (is_return_shift( + shift_line.start_shift_length, shift_line.end_shift_length, + parameters_->lateral_small_shift_threshold)) { + continue; + } - if ( - boost::geometry::distance(basic_point, (shift_length > 0.0 ? left_bound : right_bound)) < - THRESHOLD) { - RCLCPP_DEBUG_THROTTLE( - getLogger(), *clock_, 1000, - "following latest new shift line may cause deviation from drivable area."); - return false; + const auto path = shifter_for_validate.getReferencePath(); + const auto left_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.left_bound)); + const auto right_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.right_bound)); + for (size_t i = start_idx; i <= end_idx; ++i) { + const auto p = getPoint(path.points.at(i)); + lanelet::BasicPoint2d basic_point{p.x, p.y}; + + const auto shift_length = proposed_shift_path.shift_length.at(i); + const auto THRESHOLD = minimum_distance + std::abs(shift_length); + + if ( + boost::geometry::distance(basic_point, (shift_length > 0.0 ? left_bound : right_bound)) < + THRESHOLD) { + RCLCPP_DEBUG_THROTTLE( + getLogger(), *clock_, 1000, + "following latest new shift line may cause deviation from drivable area."); + return false; + } } } } diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/autoware_behavior_velocity_intersection_module/CMakeLists.txt similarity index 93% rename from planning/behavior_velocity_intersection_module/CMakeLists.txt rename to planning/autoware_behavior_velocity_intersection_module/CMakeLists.txt index c02b5a3852722..f3b31001978e9 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_intersection_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_intersection_module) +project(autoware_behavior_velocity_intersection_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/autoware_behavior_velocity_intersection_module/README.md similarity index 100% rename from planning/behavior_velocity_intersection_module/README.md rename to planning/autoware_behavior_velocity_intersection_module/README.md diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/autoware_behavior_velocity_intersection_module/config/intersection.param.yaml similarity index 100% rename from planning/behavior_velocity_intersection_module/config/intersection.param.yaml rename to planning/autoware_behavior_velocity_intersection_module/config/intersection.param.yaml diff --git a/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/data-structure.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/data-structure.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-stoplines.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/intersection-stoplines.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/intersection-stoplines.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/intersection-stoplines.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-topology.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/intersection-topology.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/intersection-topology.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/intersection-topology.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/merge_from_private.png b/planning/autoware_behavior_velocity_intersection_module/docs/merge_from_private.png similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/merge_from_private.png rename to planning/autoware_behavior_velocity_intersection_module/docs/merge_from_private.png diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion-with-tl.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/occlusion-with-tl.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/occlusion-with-tl.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/occlusion-with-tl.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion-wo-tl-creeping.png b/planning/autoware_behavior_velocity_intersection_module/docs/occlusion-wo-tl-creeping.png similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/occlusion-wo-tl-creeping.png rename to planning/autoware_behavior_velocity_intersection_module/docs/occlusion-wo-tl-creeping.png diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/signal-phase-group.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/signal-phase-group.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/signal-phase-group.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/signal-phase-group.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/static-occlusion-timeout.png b/planning/autoware_behavior_velocity_intersection_module/docs/static-occlusion-timeout.png similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/static-occlusion-timeout.png rename to planning/autoware_behavior_velocity_intersection_module/docs/static-occlusion-timeout.png diff --git a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/ttc.gif b/planning/autoware_behavior_velocity_intersection_module/docs/ttc.gif similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/ttc.gif rename to planning/autoware_behavior_velocity_intersection_module/docs/ttc.gif diff --git a/planning/behavior_velocity_intersection_module/docs/ugly-intersection.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/ugly-intersection.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/ugly-intersection.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/ugly-intersection.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/upstream-velocity.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/upstream-velocity.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/upstream-velocity.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/upstream-velocity.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/autoware_behavior_velocity_intersection_module/package.xml similarity index 88% rename from planning/behavior_velocity_intersection_module/package.xml rename to planning/autoware_behavior_velocity_intersection_module/package.xml index 3694a395b3a53..401e53586c649 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/autoware_behavior_velocity_intersection_module/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_intersection_module + autoware_behavior_velocity_intersection_module 0.1.0 - The behavior_velocity_intersection_module package + The autoware_behavior_velocity_intersection_module package Mamoru Sobue Takayuki Murooka @@ -18,9 +18,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common fmt geometry_msgs interpolation diff --git a/planning/autoware_behavior_velocity_intersection_module/plugins.xml b/planning/autoware_behavior_velocity_intersection_module/plugins.xml new file mode 100644 index 0000000000000..d30fbb46e050e --- /dev/null +++ b/planning/autoware_behavior_velocity_intersection_module/plugins.xml @@ -0,0 +1,4 @@ + + + + diff --git a/planning/behavior_velocity_intersection_module/scripts/ttc.py b/planning/autoware_behavior_velocity_intersection_module/scripts/ttc.py similarity index 100% rename from planning/behavior_velocity_intersection_module/scripts/ttc.py rename to planning/autoware_behavior_velocity_intersection_module/scripts/ttc.py diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/autoware_behavior_velocity_intersection_module/src/debug.cpp similarity index 97% rename from planning/behavior_velocity_intersection_module/src/debug.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/debug.cpp index 252d7c2a9f61e..ab09ca2e6e746 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/debug.cpp @@ -15,8 +15,8 @@ #include "scene_intersection.hpp" #include "scene_merge_from_private_road.hpp" -#include -#include +#include +#include #include #include @@ -45,7 +45,7 @@ visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( visualization_msgs::msg::MarkerArray msg; int32_t i = 0; - int32_t uid = behavior_velocity_planner::planning_utils::bitShift(lane_id); + int32_t uid = autoware::behavior_velocity_planner::planning_utils::bitShift(lane_id); for (const auto & polygon : polygons) { visualization_msgs::msg::Marker marker{}; marker.header.frame_id = "map"; @@ -224,7 +224,7 @@ constexpr std::tuple light_blue() } } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createMarkerColor; @@ -472,7 +472,7 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMark const auto state = state_machine_.getState(); - int32_t uid = behavior_velocity_planner::planning_utils::bitShift(module_id_); + int32_t uid = autoware::behavior_velocity_planner::planning_utils::bitShift(module_id_); const auto now = this->clock_->now(); if (state == StateMachine::State::STOP) { appendMarkerArray( @@ -496,4 +496,4 @@ motion_utils::VirtualWalls MergeFromPrivateRoadModule::createVirtualWalls() } return virtual_walls; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.cpp b/planning/autoware_behavior_velocity_intersection_module/src/decision_result.cpp similarity index 96% rename from planning/behavior_velocity_intersection_module/src/decision_result.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/decision_result.cpp index 7ed896d1b4b55..5d1f1a1fcfca2 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/decision_result.cpp @@ -14,7 +14,7 @@ #include "decision_result.hpp" -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { std::string formatDecisionResult(const DecisionResult & decision_result) { @@ -65,4 +65,4 @@ std::string formatDecisionResult(const DecisionResult & decision_result) return ""; } -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.hpp b/planning/autoware_behavior_velocity_intersection_module/src/decision_result.hpp similarity index 97% rename from planning/behavior_velocity_intersection_module/src/decision_result.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/decision_result.hpp index 5f642db3a462d..26f78616a7b61 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/decision_result.hpp @@ -19,7 +19,7 @@ #include #include -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { /** @@ -172,6 +172,6 @@ using DecisionResult = std::variant< std::string formatDecisionResult(const DecisionResult & decision_result); -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner #endif // DECISION_RESULT_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp b/planning/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp similarity index 93% rename from planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp index b6ae84aa8488b..d8bada002e37b 100644 --- a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp @@ -23,7 +23,7 @@ #include #include -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { /** @@ -43,6 +43,6 @@ struct InterpolatedPathInfo std::optional> lane_id_interval{std::nullopt}; }; -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner #endif // INTERPOLATED_PATH_INFO_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp b/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp similarity index 97% rename from planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp index 555ea424dfef0..be7e9e25cc8bc 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp @@ -21,7 +21,7 @@ #include -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { void IntersectionLanelets::update( @@ -79,4 +79,4 @@ void IntersectionLanelets::update( } } } -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp b/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp similarity index 98% rename from planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp index 9624d375de122..a7ca5eb3b0bc1 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { /** @@ -190,6 +190,6 @@ struct PathLanelets // conflicting lanelets plus the next lane part of the // path }; -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner #endif // INTERSECTION_LANELETS_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp b/planning/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp similarity index 95% rename from planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp index 99d79d4468b38..98325f0aa8735 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp @@ -17,7 +17,7 @@ #include -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { /** @@ -72,6 +72,6 @@ struct IntersectionStopLines */ size_t occlusion_wo_tl_pass_judge_line{0}; }; -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner #endif // INTERSECTION_STOPLINES_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/autoware_behavior_velocity_intersection_module/src/manager.cpp similarity index 98% rename from planning/behavior_velocity_intersection_module/src/manager.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/manager.cpp index ac32ad553d2b7..8f540603791cb 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -14,8 +14,8 @@ #include "manager.hpp" -#include -#include +#include +#include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::getOrDeclareParameter; @@ -589,11 +589,12 @@ bool MergeFromPrivateModuleManager::hasSameParentLaneletAndTurnDirectionWithRegi return false; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::IntersectionModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::IntersectionModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::MergeFromPrivateModulePlugin, - behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::MergeFromPrivateModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_intersection_module/src/manager.hpp b/planning/autoware_behavior_velocity_intersection_module/src/manager.hpp similarity index 90% rename from planning/behavior_velocity_intersection_module/src/manager.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/manager.hpp index 7bbc8d51bbe9e..ecf6bb810f313 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/manager.hpp @@ -18,9 +18,9 @@ #include "scene_intersection.hpp" #include "scene_merge_from_private_road.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -32,7 +32,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC { @@ -90,6 +90,6 @@ class MergeFromPrivateModulePlugin : public PluginWrapper ObjectInfoManager::registerObject( const bool belong_intersection_area, const bool is_parked_vehicle) { if (objects_info_.count(uuid) == 0) { - auto object = std::make_shared(uuid); + auto object = std::make_shared(uuid); objects_info_[uuid] = object; } auto object = objects_info_[uuid]; @@ -219,7 +219,7 @@ std::shared_ptr ObjectInfoManager::registerObject( void ObjectInfoManager::registerExistingObject( const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, const bool belong_intersection_area, const bool is_parked_vehicle, - std::shared_ptr object) + std::shared_ptr object) { objects_info_[uuid] = object; if (belong_attention_area) { @@ -249,7 +249,7 @@ std::vector> ObjectInfoManager::allObjects() const return all_objects; } -std::optional findPassageInterval( +std::optional findPassageInterval( const autoware_perception_msgs::msg::PredictedPath & predicted_path, const autoware_perception_msgs::msg::Shape & shape, const lanelet::BasicPolygon2d & ego_lane_poly, const std::optional & first_attention_lane_opt, @@ -284,25 +284,25 @@ std::optional findPassageInterval( if (lanelet::geometry::inside( first_attention_lane_opt.value(), lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { - return intersection::CollisionInterval::LanePosition::FIRST; + return CollisionInterval::LanePosition::FIRST; } } if (second_attention_lane_opt) { if (lanelet::geometry::inside( second_attention_lane_opt.value(), lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { - return intersection::CollisionInterval::LanePosition::SECOND; + return CollisionInterval::LanePosition::SECOND; } } - return intersection::CollisionInterval::LanePosition::ELSE; + return CollisionInterval::LanePosition::ELSE; }(); std::vector path; for (const auto & pose : predicted_path.path) { path.push_back(pose); } - return intersection::CollisionInterval{ + return CollisionInterval{ lane_position, path, {enter_idx, exit_idx}, {object_enter_time, object_exit_time}}; } -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.hpp b/planning/autoware_behavior_velocity_intersection_module/src/object_manager.hpp similarity index 96% rename from planning/behavior_velocity_intersection_module/src/object_manager.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/object_manager.hpp index 180496bd6b18d..0ba9947fdb318 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/object_manager.hpp @@ -52,7 +52,7 @@ struct hash }; } // namespace std -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { /** @@ -234,8 +234,7 @@ class ObjectInfoManager void registerExistingObject( const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, - const bool belong_intersection_area, const bool is_parked, - std::shared_ptr object); + const bool belong_intersection_area, const bool is_parked, std::shared_ptr object); void clearObjects(); @@ -282,12 +281,12 @@ class ObjectInfoManager /** * @brief return the CollisionInterval struct if the predicted path collides ego path geometrically */ -std::optional findPassageInterval( +std::optional findPassageInterval( const autoware_perception_msgs::msg::PredictedPath & predicted_path, const autoware_perception_msgs::msg::Shape & shape, const lanelet::BasicPolygon2d & ego_lane_poly, const std::optional & first_attention_lane_opt, const std::optional & second_attention_lane_opt); -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner #endif // OBJECT_MANAGER_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/result.hpp b/planning/autoware_behavior_velocity_intersection_module/src/result.hpp similarity index 93% rename from planning/behavior_velocity_intersection_module/src/result.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/result.hpp index 5d82183cee2fb..1a833b268a5d8 100644 --- a/planning/behavior_velocity_intersection_module/src/result.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/result.hpp @@ -18,7 +18,7 @@ #include #include -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { template @@ -48,6 +48,6 @@ Result make_err(Args &&... args) return Result(Error{std::forward(args)...}); } -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner #endif // RESULT_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp similarity index 92% rename from planning/behavior_velocity_intersection_module/src/scene_intersection.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index c27483ea1fa51..beed0aab8e382 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -16,8 +16,8 @@ #include "util.hpp" -#include // for toGeomPoly -#include +#include // for toGeomPoly +#include #include #include #include @@ -38,13 +38,10 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using intersection::make_err; -using intersection::make_ok; -using intersection::Result; using motion_utils::VelocityFactorInterface; IntersectionModule::IntersectionModule( @@ -105,8 +102,8 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * prev_decision_result_ = decision_result; { - const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " + - intersection::formatDecisionResult(decision_result); + const std::string decision_type = + "intersection" + std::to_string(module_id_) + " : " + formatDecisionResult(decision_result); internal_debug_data_.decision_type = decision_type; } @@ -147,7 +144,7 @@ static std::string formatOcclusionType(const IntersectionModule::OcclusionType & return "RTCOccluded"; } -intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( +DecisionResult IntersectionModule::modifyPathVelocityDetail( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { const auto prepare_data = prepareIntersectionData(path); @@ -187,18 +184,18 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( // ego path has just entered the entry of this intersection // ========================================================================================== if (!intersection_lanelets.first_attention_area()) { - return intersection::InternalError{"attention area is empty"}; + return InternalError{"attention area is empty"}; } const auto first_attention_area = intersection_lanelets.first_attention_area().value(); const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; if (!default_stopline_idx_opt) { - return intersection::InternalError{"default stop line is null"}; + return InternalError{"default stop line is null"}; } const auto default_stopline_idx = default_stopline_idx_opt.value(); const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; if (!first_attention_stopline_idx_opt || !occlusion_peeking_stopline_idx_opt) { - return intersection::InternalError{"occlusion stop line is null"}; + return InternalError{"occlusion stop line is null"}; } const auto first_attention_stopline_idx = first_attention_stopline_idx_opt.value(); const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value(); @@ -275,9 +272,9 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto closest_idx = intersection_stoplines.closest_idx; const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis( *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects); - return intersection::OverPassJudge{safety_diag, evasive_diag}; + return OverPassJudge{safety_diag, evasive_diag}; } - return intersection::OverPassJudge{ + return OverPassJudge{ "no collision is detected", "ego can safely pass the intersection at this rate"}; } @@ -288,8 +285,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( // ========================================================================================== std::string safety_report = safety_diag; if (const bool collision_on_1st_attention_lane = - has_collision && - (collision_position == intersection::CollisionInterval::LanePosition::FIRST); + has_collision && (collision_position == CollisionInterval::LanePosition::FIRST); is_over_1st_pass_judge_line && is_over_2nd_pass_judge_line.has_value() && !is_over_2nd_pass_judge_line.value() && collision_on_1st_attention_lane) { safety_report += @@ -329,19 +325,18 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_state_machine_.getState() == StateMachine::State::STOP; if (is_prioritized) { - return intersection::FullyPrioritized{ + return FullyPrioritized{ has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx, safety_report}; } // Safe if (!is_occlusion_state && !has_collision_with_margin) { - return intersection::Safe{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; + return Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; } // Only collision if (!is_occlusion_state && has_collision_with_margin) { - return intersection::NonOccludedCollisionStop{ + return NonOccludedCollisionStop{ closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; } // Occluded @@ -400,16 +395,16 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto closest_idx = intersection_stoplines.closest_idx; const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis( *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects); - return intersection::OverPassJudge{ + return OverPassJudge{ "already passed maximum peeking line in the absence of traffic light.\n" + safety_report, evasive_diag}; } - return intersection::OverPassJudge{ + return OverPassJudge{ "already passed maximum peeking line in the absence of traffic light safely", "no evasive action required"}; } - return intersection::OccludedAbsenceTrafficLight{ + return OccludedAbsenceTrafficLight{ is_occlusion_cleared_with_margin, has_collision_with_margin, temporal_stop_before_attention_required, @@ -439,8 +434,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool release_static_occlusion_stuck = (static_occlusion_timeout_state_machine_.getState() == StateMachine::State::GO); if (!has_collision_with_margin && release_static_occlusion_stuck) { - return intersection::Safe{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; + return Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; } // occlusion_status is either STATICALLY_OCCLUDED or DYNAMICALLY_OCCLUDED const double max_timeout = @@ -453,7 +447,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( occlusion_stop_state_machine_.getDuration()) : (is_static_occlusion ? std::make_optional(max_timeout) : std::nullopt); if (has_collision_with_margin) { - return intersection::OccludedCollisionStop{ + return OccludedCollisionStop{ is_occlusion_cleared_with_margin, temporal_stop_before_attention_required, closest_idx, @@ -463,7 +457,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( static_occlusion_timeout, occlusion_diag}; } else { - return intersection::PeekingTowardOcclusion{ + return PeekingTowardOcclusion{ is_occlusion_cleared_with_margin, temporal_stop_before_attention_required, closest_idx, @@ -478,7 +472,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) ? first_attention_stopline_idx : occlusion_stopline_idx; - return intersection::FirstWaitBeforeOcclusion{ + return FirstWaitBeforeOcclusion{ is_occlusion_cleared_with_margin, closest_idx, default_stopline_idx, occlusion_stopline, occlusion_diag}; } @@ -505,7 +499,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - [[maybe_unused]] const intersection::InternalError & result, + [[maybe_unused]] const InternalError & result, [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) @@ -515,7 +509,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - [[maybe_unused]] const intersection::OverPassJudge & result, + [[maybe_unused]] const OverPassJudge & result, [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) @@ -525,7 +519,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::StuckStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + const StuckStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -545,9 +539,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::YieldStuckStop & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) + const YieldStuckStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + [[maybe_unused]] double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "YieldStuckStop"); const auto closest_idx = result.closest_idx; @@ -560,9 +554,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::NonOccludedCollisionStop & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const NonOccludedCollisionStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "NonOccludedCollisionStop"); const auto closest_idx = result.closest_idx; @@ -579,9 +573,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::FirstWaitBeforeOcclusion & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const FirstWaitBeforeOcclusion & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "FirstWaitBeforeOcclusion"); const auto closest_idx = result.closest_idx; @@ -598,9 +592,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::PeekingTowardOcclusion & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const PeekingTowardOcclusion & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "PeekingTowardOcclusion"); const auto closest_idx = result.closest_idx; @@ -617,9 +611,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::OccludedAbsenceTrafficLight & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const OccludedAbsenceTrafficLight & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedAbsenceTrafficLight"); const auto closest_idx = result.closest_idx; @@ -634,9 +628,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::OccludedCollisionStop & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const OccludedCollisionStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedCollisionStop"); const auto closest_idx = result.closest_idx; @@ -653,9 +647,8 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::Safe & result, const tier4_planning_msgs::msg::PathWithLaneId & path, - bool * default_safety, double * default_distance, bool * occlusion_safety, - double * occlusion_distance) + const Safe & result, const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "Safe"); const auto closest_idx = result.closest_idx; @@ -672,9 +665,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::FullyPrioritized & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const FullyPrioritized & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "FullyPrioritized"); const auto closest_idx = result.closest_idx; @@ -690,8 +683,7 @@ void prepareRTCByDecisionResult( } void IntersectionModule::prepareRTCStatus( - const intersection::DecisionResult & decision_result, - const tier4_planning_msgs::msg::PathWithLaneId & path) + const DecisionResult & decision_result, const tier4_planning_msgs::msg::PathWithLaneId & path) { bool default_safety = true; double default_distance = std::numeric_limits::lowest(); @@ -705,7 +697,7 @@ void IntersectionModule::prepareRTCStatus( setSafe(default_safety); setDistance(default_distance); occlusion_first_stop_required_ = - std::holds_alternative(decision_result); + std::holds_alternative(decision_result); } template @@ -723,7 +715,7 @@ template <> void reactRTCApprovalByDecisionResult( [[maybe_unused]] const bool rtc_default_approved, [[maybe_unused]] const bool rtc_occlusion_approved, - [[maybe_unused]] const intersection::InternalError & decision_result, + [[maybe_unused]] const InternalError & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, @@ -738,7 +730,7 @@ template <> void reactRTCApprovalByDecisionResult( [[maybe_unused]] const bool rtc_default_approved, [[maybe_unused]] const bool rtc_occlusion_approved, - [[maybe_unused]] const intersection::OverPassJudge & decision_result, + [[maybe_unused]] const OverPassJudge & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, @@ -752,7 +744,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::StuckStop & decision_result, + const StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -799,7 +791,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::YieldStuckStop & decision_result, + const YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -832,7 +824,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::NonOccludedCollisionStop & decision_result, + const NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -876,7 +868,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::FirstWaitBeforeOcclusion & decision_result, + const FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) @@ -927,7 +919,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::PeekingTowardOcclusion & decision_result, + const PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) @@ -983,7 +975,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::OccludedCollisionStop & decision_result, + const OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -1031,7 +1023,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::OccludedAbsenceTrafficLight & decision_result, + const OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -1084,8 +1076,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( - const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::Safe & decision_result, + const bool rtc_default_approved, const bool rtc_occlusion_approved, const Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -1128,7 +1119,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::FullyPrioritized & decision_result, + const FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -1170,8 +1161,8 @@ void reactRTCApprovalByDecisionResult( } void IntersectionModule::reactRTCApproval( - const intersection::DecisionResult & decision_result, - tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) + const DecisionResult & decision_result, tier4_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason) { const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; std::visit( @@ -1293,7 +1284,7 @@ void IntersectionModule::updateTrafficSignalObservation() IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, - const intersection::IntersectionStopLines & intersection_stoplines) + const IntersectionStopLines & intersection_stoplines) { const auto & current_pose = planner_data_->current_odometry->pose; const auto closest_idx = intersection_stoplines.closest_idx; @@ -1346,12 +1337,11 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat // as "was_safe" // ========================================================================================== const bool was_safe = [&]() { - if (std::holds_alternative(prev_decision_result_)) { + if (std::holds_alternative(prev_decision_result_)) { return true; } - if (std::holds_alternative(prev_decision_result_)) { - const auto & state = - std::get(prev_decision_result_); + if (std::holds_alternative(prev_decision_result_)) { + const auto & state = std::get(prev_decision_result_); return !state.collision_detected; } return false; @@ -1422,4 +1412,4 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat safely_passed_1st_judge_line_first_time, safely_passed_2nd_judge_line_first_time}; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp similarity index 89% rename from planning/behavior_velocity_intersection_module/src/scene_intersection.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index b86fd77491f54..e25f41a18028a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -22,8 +22,8 @@ #include "object_manager.hpp" #include "result.hpp" -#include -#include +#include +#include #include #include @@ -43,7 +43,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class IntersectionModule : public SceneModuleInterface @@ -295,11 +295,9 @@ class IntersectionModule : public SceneModuleInterface BLAME_AT_SECOND_PASS_JUDGE, }; const bool collision_detected; - const intersection::CollisionInterval::LanePosition collision_position; - const std::vector>> - too_late_detect_objects; - const std::vector>> - misjudge_objects; + const CollisionInterval::LanePosition collision_position; + const std::vector>> too_late_detect_objects; + const std::vector>> misjudge_objects; }; IntersectionModule( @@ -377,7 +375,7 @@ class IntersectionModule : public SceneModuleInterface */ //! cache IntersectionLanelets struct - std::optional intersection_lanelets_{std::nullopt}; + std::optional intersection_lanelets_{std::nullopt}; //! cache discretized occlusion detection lanelets std::optional> occlusion_attention_divisions_{ @@ -400,7 +398,7 @@ class IntersectionModule : public SceneModuleInterface bool is_permanent_go_{false}; //! for checking if ego is over the pass judge lines because previously the situation was SAFE - intersection::DecisionResult prev_decision_result_{intersection::InternalError{""}}; + DecisionResult prev_decision_result_{InternalError{""}}; //! flag if ego passed the 1st_pass_judge_line while peeking. If this is true, 1st_pass_judge_line //! is treated as the same position as occlusion_peeking_stopline @@ -427,7 +425,7 @@ class IntersectionModule : public SceneModuleInterface StateMachine collision_state_machine_; //! container for storing safety status of objects on the attention area - intersection::ObjectInfoManager object_info_manager_; + ObjectInfoManager object_info_manager_; /** @} */ private: @@ -506,21 +504,20 @@ class IntersectionModule : public SceneModuleInterface /** * @brief analyze traffic_light/occupancy/objects context and return DecisionResult */ - intersection::DecisionResult modifyPathVelocityDetail( - PathWithLaneId * path, StopReason * stop_reason); + DecisionResult modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); /** * @brief set RTC value according to calculated DecisionResult */ void prepareRTCStatus( - const intersection::DecisionResult &, const tier4_planning_msgs::msg::PathWithLaneId & path); + const DecisionResult &, const tier4_planning_msgs::msg::PathWithLaneId & path); /** * @brief act based on current RTC approval */ void reactRTCApproval( - const intersection::DecisionResult & decision_result, - tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason); + const DecisionResult & decision_result, tier4_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason); /** @}*/ private: @@ -536,9 +533,9 @@ class IntersectionModule : public SceneModuleInterface */ struct BasicData { - intersection::InterpolatedPathInfo interpolated_path_info; - intersection::IntersectionStopLines intersection_stoplines; - intersection::PathLanelets path_lanelets; + InterpolatedPathInfo interpolated_path_info; + IntersectionStopLines intersection_stoplines; + PathLanelets path_lanelets; }; /** @@ -549,31 +546,30 @@ class IntersectionModule : public SceneModuleInterface * * To simplify modifyPathVelocityDetail(), this function is used at first */ - intersection::Result prepareIntersectionData( - PathWithLaneId * path); + Result prepareIntersectionData(PathWithLaneId * path); /** * @brief find the associated stopline road marking of assigned lanelet */ std::optional getStopLineIndexFromMap( - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, lanelet::ConstLanelet assigned_lanelet) const; /** * @brief generate IntersectionStopLines */ - std::optional generateIntersectionStopLines( + std::optional generateIntersectionStopLines( lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, tier4_planning_msgs::msg::PathWithLaneId * original_path) const; /** * @brief generate IntersectionLanelets */ - intersection::IntersectionLanelets generateObjectiveLanelets( + IntersectionLanelets generateObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const lanelet::ConstLanelet assigned_lanelet) const; @@ -581,9 +577,9 @@ class IntersectionModule : public SceneModuleInterface /** * @brief generate PathLanelets */ - std::optional generatePathLanelets( + std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, const lanelet::CompoundPolygon3d & first_conflicting_area, const std::vector & conflicting_areas, const std::optional & first_attention_area, @@ -637,10 +633,9 @@ class IntersectionModule : public SceneModuleInterface * @attention this function has access to value() of intersection_lanelets_, * intersection_lanelets.first_conflicting_lane(). They are ensured in prepareIntersectionData() */ - std::optional isStuckStatus( + std::optional isStuckStatus( const tier4_planning_msgs::msg::PathWithLaneId & path, - const intersection::IntersectionStopLines & intersection_stoplines, - const intersection::PathLanelets & path_lanelets) const; + const IntersectionStopLines & intersection_stoplines, const PathLanelets & path_lanelets) const; bool isTargetStuckVehicleType( const autoware_perception_msgs::msg::PredictedObject & object) const; @@ -651,7 +646,7 @@ class IntersectionModule : public SceneModuleInterface /** * @brief check stuck */ - bool checkStuckVehicleInIntersection(const intersection::PathLanelets & path_lanelets) const; + bool checkStuckVehicleInIntersection(const PathLanelets & path_lanelets) const; /** @} */ private: @@ -667,16 +662,16 @@ class IntersectionModule : public SceneModuleInterface * @attention this function has access to value() of intersection_lanelets_, * intersection_stoplines.default_stopline, intersection_stoplines.first_attention_stopline */ - std::optional isYieldStuckStatus( + std::optional isYieldStuckStatus( const tier4_planning_msgs::msg::PathWithLaneId & path, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionStopLines & intersection_stoplines) const; + const InterpolatedPathInfo & interpolated_path_info, + const IntersectionStopLines & intersection_stoplines) const; /** * @brief check yield stuck */ bool checkYieldStuckVehicleInIntersection( - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, const lanelet::ConstLanelets & attention_lanelets) const; /** @} */ @@ -698,15 +693,14 @@ class IntersectionModule : public SceneModuleInterface bool /* reconciled occlusion disapproval */> getOcclusionStatus( const TrafficPrioritizedLevel & traffic_prioritized_level, - const intersection::InterpolatedPathInfo & interpolated_path_info); + const InterpolatedPathInfo & interpolated_path_info); /** * @brief calculate detected occlusion status(NOT | STATICALLY | DYNAMICALLY) * @attention this function has access to value() of intersection_lanelets_, * intersection_lanelets.first_attention_area(), occlusion_attention_divisions_ */ - OcclusionType detectOcclusion( - const intersection::InterpolatedPathInfo & interpolated_path_info) const; + OcclusionType detectOcclusion(const InterpolatedPathInfo & interpolated_path_info) const; /** @} */ private: @@ -726,7 +720,7 @@ class IntersectionModule : public SceneModuleInterface */ PassJudgeStatus isOverPassJudgeLinesStatus( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, - const intersection::IntersectionStopLines & intersection_stoplines); + const IntersectionStopLines & intersection_stoplines); /** @} */ private: @@ -751,7 +745,7 @@ class IntersectionModule : public SceneModuleInterface * @attention this function has access to value() of intersection_lanelets_ */ void updateObjectInfoManagerCollision( - const intersection::PathLanelets & path_lanelets, const TimeDistanceArray & time_distance_array, + const PathLanelets & path_lanelets, const TimeDistanceArray & time_distance_array, const TrafficPrioritizedLevel & traffic_prioritized_level, const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time, tier4_debug_msgs::msg::Float64MultiArrayStamped * object_ttc_time_array); @@ -768,20 +762,18 @@ class IntersectionModule : public SceneModuleInterface * @attention this function has access to value() of * intersection_stoplines.occlusion_peeking_stopline */ - std::optional isGreenPseudoCollisionStatus( + std::optional isGreenPseudoCollisionStatus( const size_t closest_idx, const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines) const; + const IntersectionStopLines & intersection_stoplines) const; /** * @brief generate the message explaining why too_late_detect_objects/misjudge_objects exist and * blame past perception fault */ std::string generateDetectionBlameDiagnosis( - const std::vector< - std::pair>> & + const std::vector>> & too_late_detect_objects, - const std::vector< - std::pair>> & + const std::vector>> & misjudge_objects) const; /** @@ -791,11 +783,9 @@ class IntersectionModule : public SceneModuleInterface std::string generateEgoRiskEvasiveDiagnosis( const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const TimeDistanceArray & ego_time_distance_array, - const std::vector< - std::pair>> & + const std::vector>> & too_late_detect_objects, - const std::vector< - std::pair>> & + const std::vector>> & misjudge_objects) const; /** @@ -818,7 +808,7 @@ class IntersectionModule : public SceneModuleInterface */ TimeDistanceArray calcIntersectionPassingTime( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, - const intersection::IntersectionStopLines & intersection_stoplines, + const IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const; /** @} */ @@ -828,6 +818,6 @@ class IntersectionModule : public SceneModuleInterface rclcpp::Publisher::SharedPtr object_ttc_pub_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_INTERSECTION_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp similarity index 93% rename from planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 131081c5e8ca0..fb06ed9d811c8 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -15,8 +15,8 @@ #include "scene_intersection.hpp" #include "util.hpp" -#include // for toGeomPoly -#include // for smoothPath +#include // for toGeomPoly +#include // for smoothPath #include #include #include @@ -30,7 +30,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -140,7 +140,7 @@ void IntersectionModule::updateObjectInfoManagerArea() } void IntersectionModule::updateObjectInfoManagerCollision( - const intersection::PathLanelets & path_lanelets, + const PathLanelets & path_lanelets, const IntersectionModule::TimeDistanceArray & time_distance_array, const IntersectionModule::TrafficPrioritizedLevel & traffic_prioritized_level, const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time, @@ -243,8 +243,8 @@ void IntersectionModule::updateObjectInfoManagerCollision( // path, both will be null, which is interpreted as SAFE. if any of the path is "normal", either // of them has value, not both // ========================================================================================== - std::optional unsafe_interval{std::nullopt}; - std::optional safe_interval{std::nullopt}; + std::optional unsafe_interval{std::nullopt}; + std::optional safe_interval{std::nullopt}; std::optional> object_debug_info{std::nullopt}; for (const auto & predicted_path_ptr : sorted_predicted_paths) { @@ -256,7 +256,7 @@ void IntersectionModule::updateObjectInfoManagerCollision( } cutPredictPathWithinDuration( planner_data_->predicted_objects->header.stamp, passing_time, &predicted_path); - const auto object_passage_interval_opt = intersection::findPassageInterval( + const auto object_passage_interval_opt = findPassageInterval( predicted_path, predicted_object.shape, ego_poly, intersection_lanelets.first_attention_lane(), intersection_lanelets.second_attention_lane()); @@ -362,27 +362,25 @@ void IntersectionModule::updateObjectInfoManagerCollision( } object_info->update_safety(unsafe_interval, safe_interval, safe_under_traffic_control); if (passed_1st_judge_line_first_time) { - object_info->setDecisionAt1stPassJudgeLinePassage(intersection::CollisionKnowledge{ + object_info->setDecisionAt1stPassJudgeLinePassage(CollisionKnowledge{ clock_->now(), // stamp unsafe_interval - ? intersection::CollisionKnowledge::SafeType::UNSAFE - : (safe_under_traffic_control - ? intersection::CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL - : intersection::CollisionKnowledge::SafeType::SAFE), // safe - unsafe_interval ? unsafe_interval : safe_interval, // interval + ? CollisionKnowledge::SafeType::UNSAFE + : (safe_under_traffic_control ? CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL + : CollisionKnowledge::SafeType::SAFE), // safe + unsafe_interval ? unsafe_interval : safe_interval, // interval predicted_object.kinematics.initial_twist_with_covariance.twist.linear .x // observed_velocity }); } if (passed_2nd_judge_line_first_time) { - object_info->setDecisionAt2ndPassJudgeLinePassage(intersection::CollisionKnowledge{ + object_info->setDecisionAt2ndPassJudgeLinePassage(CollisionKnowledge{ clock_->now(), // stamp unsafe_interval - ? intersection::CollisionKnowledge::SafeType::UNSAFE - : (safe_under_traffic_control - ? intersection::CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL - : intersection::CollisionKnowledge::SafeType::SAFE), // safe - unsafe_interval ? unsafe_interval : safe_interval, // interval + ? CollisionKnowledge::SafeType::UNSAFE + : (safe_under_traffic_control ? CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL + : CollisionKnowledge::SafeType::SAFE), // safe + unsafe_interval ? unsafe_interval : safe_interval, // interval predicted_object.kinematics.initial_twist_with_covariance.twist.linear .x // observed_velocity }); @@ -415,10 +413,9 @@ void IntersectionModule::cutPredictPathWithinDuration( } } -std::optional -IntersectionModule::isGreenPseudoCollisionStatus( +std::optional IntersectionModule::isGreenPseudoCollisionStatus( const size_t closest_idx, const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines) const + const IntersectionStopLines & intersection_stoplines) const { // ========================================================================================== // if there are any vehicles on the attention area when ego entered the intersection on green @@ -440,7 +437,7 @@ IntersectionModule::isGreenPseudoCollisionStatus( }); if (exist_close_vehicles) { const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); - return intersection::NonOccludedCollisionStop{ + return NonOccludedCollisionStop{ closest_idx, collision_stopline_idx, occlusion_stopline_idx, std::string("")}; } } @@ -448,11 +445,11 @@ IntersectionModule::isGreenPseudoCollisionStatus( } std::string IntersectionModule::generateDetectionBlameDiagnosis( - const std::vector>> & + const std::vector< + std::pair>> & too_late_detect_objects, - const std::vector>> & + const std::vector< + std::pair>> & misjudge_objects) const { std::string diag; @@ -598,11 +595,11 @@ std::string IntersectionModule::generateDetectionBlameDiagnosis( std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const IntersectionModule::TimeDistanceArray & ego_time_distance_array, - const std::vector>> & + const std::vector< + std::pair>> & too_late_detect_objects, - [[maybe_unused]] const std::vector>> & + [[maybe_unused]] const std::vector< + std::pair>> & misjudge_objects) const { static constexpr double min_vel = 1e-2; @@ -684,9 +681,8 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( // that case is both "too late to stop" and "too late to go" for the planner. and basically // detection side is responsible for this fault // ========================================================================================== - std::vector>> - misjudge_objects; - std::vector>> + std::vector>> misjudge_objects; + std::vector>> too_late_detect_objects; for (const auto & object_info : object_info_manager_.attentionObjects()) { if (object_info->is_safe_under_traffic_control()) { @@ -709,14 +705,14 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( // visualized as "misjudge" // ========================================================================================== auto * debug_container = &debug_data_.unsafe_targets.objects; - if (unsafe_info.lane_position == intersection::CollisionInterval::LanePosition::FIRST) { + if (unsafe_info.lane_position == CollisionInterval::LanePosition::FIRST) { collision_at_first_lane = true; } else { collision_at_non_first_lane = true; } if ( is_over_1st_pass_judge_line && - unsafe_info.lane_position == intersection::CollisionInterval::LanePosition::FIRST) { + unsafe_info.lane_position == CollisionInterval::LanePosition::FIRST) { const auto & decision_at_1st_pass_judge_opt = object_info->decision_at_1st_pass_judge_line_passage(); if (!decision_at_1st_pass_judge_opt) { @@ -725,9 +721,7 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( debug_container = &debug_data_.too_late_detect_targets.objects; } else { const auto & decision_at_1st_pass_judge = decision_at_1st_pass_judge_opt.value(); - if ( - decision_at_1st_pass_judge.safe_type != - intersection::CollisionKnowledge::SafeType::UNSAFE) { + if (decision_at_1st_pass_judge.safe_type != CollisionKnowledge::SafeType::UNSAFE) { misjudge_objects.emplace_back( CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); debug_container = &debug_data_.misjudge_targets.objects; @@ -747,9 +741,7 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( debug_container = &debug_data_.too_late_detect_targets.objects; } else { const auto & decision_at_2nd_pass_judge = decision_at_2nd_pass_judge_opt.value(); - if ( - decision_at_2nd_pass_judge.safe_type != - intersection::CollisionKnowledge::SafeType::UNSAFE) { + if (decision_at_2nd_pass_judge.safe_type != CollisionKnowledge::SafeType::UNSAFE) { misjudge_objects.emplace_back( CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); debug_container = &debug_data_.misjudge_targets.objects; @@ -763,12 +755,11 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( debug_container->emplace_back(object_info->predicted_object()); } if (collision_at_first_lane) { - return { - true, intersection::CollisionInterval::FIRST, too_late_detect_objects, misjudge_objects}; + return {true, CollisionInterval::FIRST, too_late_detect_objects, misjudge_objects}; } else if (collision_at_non_first_lane) { - return {true, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; + return {true, CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; } - return {false, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; + return {false, CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; } std::optional IntersectionModule::checkAngleForTargetLanelets( @@ -811,7 +802,7 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, - const intersection::IntersectionStopLines & intersection_stoplines, + const IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const { const double intersection_velocity = @@ -973,4 +964,4 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin return time_distance_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp similarity index 98% rename from planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index b741d43bb025a..c43f8617897b9 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -25,7 +25,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -34,7 +34,7 @@ std::tuple< bool /* reconciled occlusion disapproval */> IntersectionModule::getOcclusionStatus( const TrafficPrioritizedLevel & traffic_prioritized_level, - const intersection::InterpolatedPathInfo & interpolated_path_info) + const InterpolatedPathInfo & interpolated_path_info) { const auto & intersection_lanelets = intersection_lanelets_.value(); const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); @@ -99,7 +99,7 @@ IntersectionModule::getOcclusionStatus( } IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( - const intersection::InterpolatedPathInfo & interpolated_path_info) const + const InterpolatedPathInfo & interpolated_path_info) const { const auto & intersection_lanelets = intersection_lanelets_.value(); const auto & adjacent_lanelets = intersection_lanelets.adjacent(); @@ -449,4 +449,4 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( debug_data_.static_occlusion = true; return StaticallyOccluded{min_dist}; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp similarity index 95% rename from planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 9ea5360c3a176..223e8eca84fe8 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -15,8 +15,8 @@ #include "scene_intersection.hpp" #include "util.hpp" -#include // for to_bg2d -#include // for planning_utils:: +#include // for to_bg2d +#include // for planning_utils:: #include #include // for lanelet::autoware::RoadMarking #include @@ -106,7 +106,7 @@ std::optional> getFirstPoi const auto & p = path.points.at(i).point.pose.position; for (const auto & polygon : polygons) { const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(behavior_velocity_planner::to_bg2d(p), polygon_2d); + is_in_lanelet = bg::within(autoware::behavior_velocity_planner::to_bg2d(p), polygon_2d); if (is_in_lanelet) { return std::make_optional>( i, polygon); @@ -122,7 +122,7 @@ std::optional> getFirstPoi const auto & p = path.points.at(i).point.pose.position; for (const auto & polygon : polygons) { const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(behavior_velocity_planner::to_bg2d(p), polygon_2d); + is_in_lanelet = bg::within(autoware::behavior_velocity_planner::to_bg2d(p), polygon_2d); if (is_in_lanelet) { return std::make_optional>( i, polygon); @@ -157,16 +157,12 @@ double getHighestCurvature(const lanelet::ConstLineString3d & centerline) } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using intersection::make_err; -using intersection::make_ok; -using intersection::Result; - -Result -IntersectionModule::prepareIntersectionData(PathWithLaneId * path) +Result IntersectionModule::prepareIntersectionData( + PathWithLaneId * path) { const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); @@ -191,13 +187,12 @@ IntersectionModule::prepareIntersectionData(PathWithLaneId * path) const auto interpolated_path_info_opt = util::generateInterpolatedPath( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); if (!interpolated_path_info_opt) { - return make_err( - "splineInterpolate failed"); + return make_err("splineInterpolate failed"); } const auto & interpolated_path_info = interpolated_path_info_opt.value(); if (!interpolated_path_info.lane_id_interval) { - return make_err( + return make_err( "Path has no interval on intersection lane " + std::to_string(lane_id_)); } @@ -231,8 +226,7 @@ IntersectionModule::prepareIntersectionData(PathWithLaneId * path) const auto & first_conflicting_lane_opt = intersection_lanelets.first_conflicting_lane(); if (conflicting_lanelets.empty() || !first_conflicting_area_opt || !first_conflicting_lane_opt) { // this is abnormal - return make_err( - "conflicting area is empty"); + return make_err("conflicting area is empty"); } const auto & first_conflicting_lane = first_conflicting_lane_opt.value(); const auto & first_conflicting_area = first_conflicting_area_opt.value(); @@ -250,7 +244,7 @@ IntersectionModule::prepareIntersectionData(PathWithLaneId * path) assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, second_attention_area_opt, interpolated_path_info, path); if (!intersection_stoplines_opt) { - return make_err( + return make_err( "failed to generate intersection_stoplines"); } const auto & intersection_stoplines = intersection_stoplines_opt.value(); @@ -265,7 +259,7 @@ IntersectionModule::prepareIntersectionData(PathWithLaneId * path) lanelets_on_path, interpolated_path_info, first_conflicting_area, conflicting_area, first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx); if (!path_lanelets_opt.has_value()) { - return make_err( + return make_err( "failed to generate PathLanelets"); } const auto & path_lanelets = path_lanelets_opt.value(); @@ -294,13 +288,12 @@ IntersectionModule::prepareIntersectionData(PathWithLaneId * path) } } - return make_ok( + return make_ok( interpolated_path_info, intersection_stoplines, path_lanelets); } std::optional IntersectionModule::getStopLineIndexFromMap( - const intersection::InterpolatedPathInfo & interpolated_path_info, - lanelet::ConstLanelet assigned_lanelet) const + const InterpolatedPathInfo & interpolated_path_info, lanelet::ConstLanelet assigned_lanelet) const { const auto & path = interpolated_path_info.path; const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); @@ -350,12 +343,11 @@ std::optional IntersectionModule::getStopLineIndexFromMap( planner_data_->ego_nearest_yaw_threshold); } -std::optional -IntersectionModule::generateIntersectionStopLines( +std::optional IntersectionModule::generateIntersectionStopLines( lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, tier4_planning_msgs::msg::PathWithLaneId * original_path) const { const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; @@ -555,7 +547,7 @@ IntersectionModule::generateIntersectionStopLines( intersection_stoplines_temp.default_stopline; } - intersection::IntersectionStopLines intersection_stoplines; + IntersectionStopLines intersection_stoplines; intersection_stoplines.closest_idx = intersection_stoplines_temp.closest_idx; if (stuck_stopline_valid) { intersection_stoplines.stuck_stopline = intersection_stoplines_temp.stuck_stopline; @@ -585,7 +577,7 @@ IntersectionModule::generateIntersectionStopLines( return intersection_stoplines; } -intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets( +IntersectionLanelets IntersectionModule::generateObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const lanelet::ConstLanelet assigned_lanelet) const { @@ -736,7 +728,7 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets auto [attention_lanelets, original_attention_lanelet_sequences] = util::mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); - intersection::IntersectionLanelets result; + IntersectionLanelets result; result.attention_ = std::move(attention_lanelets); for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that @@ -786,9 +778,9 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets return result; } -std::optional IntersectionModule::generatePathLanelets( +std::optional IntersectionModule::generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, const lanelet::CompoundPolygon3d & first_conflicting_area, const std::vector & conflicting_areas, const std::optional & first_attention_area, @@ -804,7 +796,7 @@ std::optional IntersectionModule::generatePathLanele const auto assigned_lane_interval = assigned_lane_interval_opt.value(); const auto & path = interpolated_path_info.path; - intersection::PathLanelets path_lanelets; + PathLanelets path_lanelets; // prev path_lanelets.prev = ::getPrevLanelets(lanelets_on_path, associative_ids_); path_lanelets.all = path_lanelets.prev; @@ -915,4 +907,4 @@ std::vector IntersectionModule::generateDetectionLan return detection_divisions; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp similarity index 94% rename from planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index 498a902c032db..eecb02d07c386 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -15,7 +15,7 @@ #include "scene_intersection.hpp" #include "util.hpp" -#include // for toGeomPoly +#include // for toGeomPoly #include #include @@ -114,14 +114,13 @@ lanelet::ConstLanelet createLaneletFromArcLength( } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -std::optional IntersectionModule::isStuckStatus( +std::optional IntersectionModule::isStuckStatus( const tier4_planning_msgs::msg::PathWithLaneId & path, - const intersection::IntersectionStopLines & intersection_stoplines, - const intersection::PathLanelets & path_lanelets) const + const IntersectionStopLines & intersection_stoplines, const PathLanelets & path_lanelets) const { const auto closest_idx = intersection_stoplines.closest_idx; auto fromEgoDist = [&](const size_t index) { @@ -162,8 +161,7 @@ std::optional IntersectionModule::isStuckStatus( } } if (stopline_idx) { - return intersection::StuckStop{ - closest_idx, stopline_idx.value(), occlusion_peeking_stopline_idx_opt}; + return StuckStop{closest_idx, stopline_idx.value(), occlusion_peeking_stopline_idx_opt}; } } } @@ -230,8 +228,7 @@ bool IntersectionModule::isTargetYieldStuckVehicleType( return false; } -bool IntersectionModule::checkStuckVehicleInIntersection( - const intersection::PathLanelets & path_lanelets) const +bool IntersectionModule::checkStuckVehicleInIntersection(const PathLanelets & path_lanelets) const { using lanelet::utils::getArcCoordinates; using lanelet::utils::getLaneletLength3d; @@ -306,10 +303,10 @@ bool IntersectionModule::checkStuckVehicleInIntersection( return false; } -std::optional IntersectionModule::isYieldStuckStatus( +std::optional IntersectionModule::isYieldStuckStatus( const tier4_planning_msgs::msg::PathWithLaneId & path, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionStopLines & intersection_stoplines) const + const InterpolatedPathInfo & interpolated_path_info, + const IntersectionStopLines & intersection_stoplines) const { const auto closest_idx = intersection_stoplines.closest_idx; auto fromEgoDist = [&](const size_t index) { @@ -342,14 +339,14 @@ std::optional IntersectionModule::isYieldStuckStat } } if (stopline_idx) { - return intersection::YieldStuckStop{closest_idx, stopline_idx.value(), std::string("")}; + return YieldStuckStop{closest_idx, stopline_idx.value(), std::string("")}; } } return std::nullopt; } bool IntersectionModule::checkYieldStuckVehicleInIntersection( - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, const lanelet::ConstLanelets & attention_lanelets) const { const bool yield_stuck_detection_direction = [&]() { @@ -425,4 +422,4 @@ bool IntersectionModule::checkYieldStuckVehicleInIntersection( } return false; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp similarity index 96% rename from planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 67da3c7a759fe..1aaed779e1b79 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -16,8 +16,8 @@ #include "util.hpp" -#include -#include +#include +#include #include #include #include @@ -32,7 +32,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -52,7 +52,7 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( static std::optional getFirstConflictingLanelet( const lanelet::ConstLanelets & conflicting_lanelets, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; @@ -206,4 +206,4 @@ lanelet::ConstLanelets MergeFromPrivateRoadModule::getAttentionLanelets() const return attention_lanelets; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp similarity index 91% rename from planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index 0b783cf2a7ebd..19e9ea44869ea 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -15,8 +15,8 @@ #ifndef SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ #define SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ -#include -#include +#include +#include #include #include @@ -35,7 +35,7 @@ * lanes before entering intersection */ -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class MergeFromPrivateRoadModule : public SceneModuleInterface { @@ -88,6 +88,6 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/autoware_behavior_velocity_intersection_module/src/util.cpp similarity index 95% rename from planning/behavior_velocity_intersection_module/src/util.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/util.cpp index 9c492e7a64cde..09f16bcada3c1 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/util.cpp @@ -16,9 +16,9 @@ #include "interpolated_path_info.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -40,7 +40,7 @@ #include #include -namespace behavior_velocity_planner::util +namespace autoware::behavior_velocity_planner::util { namespace bg = boost::geometry; @@ -129,8 +129,7 @@ std::optional> findLaneIdsInterval( } std::optional getFirstPointInsidePolygonByFootprint( - const lanelet::CompoundPolygon3d & polygon, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; @@ -154,7 +153,7 @@ std::optional> getFirstPointInsidePolygonsByFootprint( const std::vector & polygons, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; @@ -356,12 +355,12 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) return tl_id.has_value(); } -std::optional generateInterpolatedPath( +std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger) { - intersection::InterpolatedPathInfo interpolated_path_info; + InterpolatedPathInfo interpolated_path_info; if (!splineInterpolate(input_path, ds, interpolated_path_info.path, logger)) { return std::nullopt; } @@ -400,4 +399,4 @@ std::vector getPolygon3dFromLanelets( return polys; } -} // namespace behavior_velocity_planner::util +} // namespace autoware::behavior_velocity_planner::util diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/autoware_behavior_velocity_intersection_module/src/util.hpp similarity index 93% rename from planning/behavior_velocity_intersection_module/src/util.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/util.hpp index 878253e6943a7..ef826380afa69 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/util.hpp @@ -30,7 +30,7 @@ #include #include -namespace behavior_velocity_planner::util +namespace autoware::behavior_velocity_planner::util { /** @@ -98,7 +98,7 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); /** * @brief interpolate PathWithLaneId */ -std::optional generateInterpolatedPath( +std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger); @@ -124,8 +124,7 @@ mergeLaneletsByTopologicalSort( * polygon */ std::optional getFirstPointInsidePolygonByFootprint( - const lanelet::CompoundPolygon3d & polygon, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); /** @@ -136,12 +135,12 @@ std::optional> getFirstPointInsidePolygonsByFootprint( const std::vector & polygons, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); std::vector getPolygon3dFromLanelets( const lanelet::ConstLanelets & ll_vec); -} // namespace behavior_velocity_planner::util +} // namespace autoware::behavior_velocity_planner::util #endif // UTIL_HPP_ diff --git a/planning/autoware_behavior_velocity_planner/README.md b/planning/autoware_behavior_velocity_planner/README.md index bcef46e90c1ec..acf4c1ce78e56 100644 --- a/planning/autoware_behavior_velocity_planner/README.md +++ b/planning/autoware_behavior_velocity_planner/README.md @@ -9,9 +9,9 @@ It loads modules as plugins. Please refer to the links listed below for detail o - [Blind Spot](../behavior_velocity_blind_spot_module/README.md) - [Crosswalk](../behavior_velocity_crosswalk_module/README.md) -- [Walkway](../behavior_velocity_walkway_module/README.md) +- [Walkway](../autoware_behavior_velocity_walkway_module/README.md) - [Detection Area](../behavior_velocity_detection_area_module/README.md) -- [Intersection](../behavior_velocity_intersection_module/README.md) +- [Intersection](../autoware_behavior_velocity_intersection_module/README.md) - [MergeFromPrivate](../behavior_velocity_intersection_module/README.md#merge-from-private) - [Stop Line](../behavior_velocity_stop_line_module/README.md) - [Virtual Traffic Light](../autoware_behavior_velocity_virtual_traffic_light_module/README.md) diff --git a/planning/autoware_behavior_velocity_planner/package.xml b/planning/autoware_behavior_velocity_planner/package.xml index fff1fb8ff91c0..4d5dc17264df0 100644 --- a/planning/autoware_behavior_velocity_planner/package.xml +++ b/planning/autoware_behavior_velocity_planner/package.xml @@ -34,11 +34,11 @@ rosidl_default_generators + autoware_behavior_velocity_planner_common autoware_map_msgs autoware_perception_msgs autoware_planning_msgs autoware_velocity_smoother - behavior_velocity_planner_common diagnostic_msgs eigen geometry_msgs @@ -65,13 +65,14 @@ ament_cmake_ros ament_lint_auto + autoware_behavior_velocity_intersection_module autoware_behavior_velocity_run_out_module autoware_behavior_velocity_virtual_traffic_light_module + autoware_behavior_velocity_walkway_module autoware_lint_common behavior_velocity_blind_spot_module behavior_velocity_crosswalk_module behavior_velocity_detection_area_module - behavior_velocity_intersection_module behavior_velocity_no_drivable_lane_module behavior_velocity_no_stopping_area_module behavior_velocity_occlusion_spot_module @@ -79,7 +80,6 @@ behavior_velocity_speed_bump_module behavior_velocity_stop_line_module behavior_velocity_traffic_light_module - behavior_velocity_walkway_module rosidl_interface_packages diff --git a/planning/autoware_behavior_velocity_planner/src/node.cpp b/planning/autoware_behavior_velocity_planner/src/node.cpp index ffbc4ef9174dc..3ff4e813b3266 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/autoware_behavior_velocity_planner/src/node.cpp @@ -14,8 +14,8 @@ #include "node.hpp" +#include #include -#include #include #include #include @@ -310,7 +310,7 @@ void BehaviorVelocityPlannerNode::onParam() // constructed. It would be required if it was a callback. std::lock_guard // lock(mutex_); planner_data_.velocity_smoother_ = - std::make_unique(*this); + std::make_unique(*this); planner_data_.velocity_smoother_->setWheelBase(planner_data_.vehicle_info_.wheel_base_m); } @@ -439,14 +439,14 @@ autoware_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath( // screening const auto filtered_path = - ::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); + autoware::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - const auto interpolated_path_msg = ::behavior_velocity_planner::interpolatePath( + const auto interpolated_path_msg = autoware::behavior_velocity_planner::interpolatePath( filtered_path, forward_path_length_, behavior_output_path_interval_); // check stop point - output_path_msg = ::behavior_velocity_planner::filterStopPathPoint(interpolated_path_msg); + output_path_msg = autoware::behavior_velocity_planner::filterStopPathPoint(interpolated_path_msg); output_path_msg.header.frame_id = "map"; output_path_msg.header.stamp = this->now(); diff --git a/planning/autoware_behavior_velocity_planner/src/node.hpp b/planning/autoware_behavior_velocity_planner/src/node.hpp index 62ceef5f04ea6..b2157bfb818ce 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/autoware_behavior_velocity_planner/src/node.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -49,7 +49,6 @@ namespace autoware::behavior_velocity_planner using autoware_behavior_velocity_planner::srv::LoadPlugin; using autoware_behavior_velocity_planner::srv::UnloadPlugin; using autoware_map_msgs::msg::LaneletMapBin; -using ::behavior_velocity_planner::TrafficSignalStamped; using tier4_planning_msgs::msg::VelocityLimit; class BehaviorVelocityPlannerNode : public rclcpp::Node diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp index f462fc963f17b..93209a10180be 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -50,7 +50,8 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( } // namespace BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() -: plugin_loader_("autoware_behavior_velocity_planner", "behavior_velocity_planner::PluginInterface") +: plugin_loader_( + "autoware_behavior_velocity_planner", "autoware::behavior_velocity_planner::PluginInterface") { } diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp index 9e7f2942bb067..73193a002918d 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp @@ -15,8 +15,8 @@ #ifndef PLANNER_MANAGER_HPP_ #define PLANNER_MANAGER_HPP_ -#include -#include +#include +#include #include #include @@ -38,8 +38,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::PlannerData; -using ::behavior_velocity_planner::PluginInterface; class BehaviorVelocityPlannerManager { diff --git a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index e741d054afbaf..cdff8af4ac56e 100644 --- a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -68,21 +68,21 @@ std::shared_ptr generateNode() }; std::vector module_names; - module_names.emplace_back("behavior_velocity_planner::CrosswalkModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::WalkwayModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::TrafficLightModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::IntersectionModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::MergeFromPrivateModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::BlindSpotModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::DetectionAreaModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::CrosswalkModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::WalkwayModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::TrafficLightModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::IntersectionModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::MergeFromPrivateModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::BlindSpotModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::DetectionAreaModulePlugin"); module_names.emplace_back("autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::NoStoppingAreaModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::StopLineModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::OcclusionSpotModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::NoStoppingAreaModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::StopLineModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::OcclusionSpotModulePlugin"); module_names.emplace_back("autoware::behavior_velocity_planner::RunOutModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::SpeedBumpModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::OutOfLaneModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::NoDrivableLaneModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::SpeedBumpModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::OutOfLaneModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::NoDrivableLaneModulePlugin"); std::vector params; params.emplace_back("launch_modules", module_names); @@ -98,9 +98,9 @@ std::shared_ptr generateNode() behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", get_behavior_velocity_module_config_no_prefix("blind_spot"), get_behavior_velocity_module_config_no_prefix("crosswalk"), - get_behavior_velocity_module_config_no_prefix("walkway"), + get_behavior_velocity_module_config("walkway"), get_behavior_velocity_module_config_no_prefix("detection_area"), - get_behavior_velocity_module_config_no_prefix("intersection"), + get_behavior_velocity_module_config("intersection"), get_behavior_velocity_module_config_no_prefix("no_stopping_area"), get_behavior_velocity_module_config_no_prefix("occlusion_spot"), get_behavior_velocity_module_config("run_out"), diff --git a/planning/behavior_velocity_planner_common/CMakeLists.txt b/planning/autoware_behavior_velocity_planner_common/CMakeLists.txt similarity index 93% rename from planning/behavior_velocity_planner_common/CMakeLists.txt rename to planning/autoware_behavior_velocity_planner_common/CMakeLists.txt index c8847164851e8..9cb992312f52a 100644 --- a/planning/behavior_velocity_planner_common/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_planner_common/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_planner_common) +project(autoware_behavior_velocity_planner_common) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_velocity_planner_common/README.md b/planning/autoware_behavior_velocity_planner_common/README.md similarity index 100% rename from planning/behavior_velocity_planner_common/README.md rename to planning/autoware_behavior_velocity_planner_common/README.md diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/planner_data.hpp similarity index 92% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/planner_data.hpp index 51511b94f3e33..c69f23215a369 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/planner_data.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ #include "route_handler/route_handler.hpp" +#include #include -#include #include #include @@ -45,7 +45,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class BehaviorVelocityPlannerNode; struct PlannerData @@ -88,7 +88,7 @@ struct PlannerData bool is_simulation = false; // velocity smoother - std::shared_ptr velocity_smoother_; + std::shared_ptr velocity_smoother_; // route handler std::shared_ptr route_handler_; // parameters @@ -148,6 +148,6 @@ struct PlannerData return std::make_optional(traffic_light_id_map.at(id)); } }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_interface.hpp similarity index 75% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_interface.hpp index dcdb4a7052cc0..86579f06790b2 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_interface.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ -#include +#include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class PluginInterface @@ -38,6 +38,6 @@ class PluginInterface virtual const char * getModuleName() = 0; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_wrapper.hpp similarity index 78% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_wrapper.hpp index abb14dd8b2356..e82211937e55b 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_wrapper.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ -#include +#include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { template @@ -48,6 +48,6 @@ class PluginWrapper : public PluginInterface std::unique_ptr scene_manager_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/scene_module_interface.hpp similarity index 95% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/scene_module_interface.hpp index 3e7992207f3f1..53b6e064d73b4 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/scene_module_interface.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ -#include +#include #include #include #include @@ -45,7 +45,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using builtin_interfaces::msg::Time; @@ -276,6 +276,6 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface } }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/arc_lane_util.hpp similarity index 94% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/arc_lane_util.hpp index c9d292536ac13..5ea1ae9fffcc1 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ -#include +#include #include #include @@ -27,7 +27,7 @@ #define EIGEN_MPL2_ONLY #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -195,6 +195,6 @@ std::optional createTargetPoint( const size_t lane_id, const double margin, const double vehicle_offset); } // namespace arc_lane_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp similarity index 88% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp index bf238ecad55cb..22bba2b90dcc2 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ #include @@ -46,7 +46,7 @@ BOOST_GEOMETRY_REGISTER_POINT_3D( autoware_planning_msgs::msg::TrajectoryPoint, double, cs::cartesian, pose.position.x, pose.position.y, pose.position.z) -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -77,6 +77,6 @@ Polygon2d upScalePolygon( geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/debug.hpp similarity index 86% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/debug.hpp index ab44af265fbaa..c6e5d45ec4eb6 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/debug.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ #include @@ -26,7 +26,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace debug { @@ -47,5 +47,5 @@ visualization_msgs::msg::MarkerArray createPointsMarkerArray( const int64_t module_id, const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b); } // namespace debug -} // namespace behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ +} // namespace autoware::behavior_velocity_planner +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/path_utilization.hpp similarity index 78% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/path_utilization.hpp index 55a82db1ae390..e9bccc42e1dc4 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ #include @@ -22,7 +22,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { bool splineInterpolate( const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, @@ -33,6 +33,6 @@ autoware_planning_msgs::msg::Path filterLitterPathPoint( const autoware_planning_msgs::msg::Path & path); autoware_planning_msgs::msg::Path filterStopPathPoint( const autoware_planning_msgs::msg::Path & path); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/state_machine.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/state_machine.hpp similarity index 87% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/state_machine.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/state_machine.hpp index 73b0fa7d553a3..12dd4db930745 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/state_machine.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/state_machine.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ #include @@ -21,7 +21,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { /** * @brief Manage stop-go states with safety margin time. @@ -92,5 +92,5 @@ class StateMachine std::shared_ptr start_time_; //! first time received GO when STOP state }; -} // namespace behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ +} // namespace autoware::behavior_velocity_planner +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/trajectory_utils.hpp similarity index 76% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/trajectory_utils.hpp index 2aadb7883a857..c5040f055c243 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/trajectory_utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ -#include +#include #include #include @@ -26,7 +26,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -40,6 +40,6 @@ bool smoothPath( const PathWithLaneId & in_path, PathWithLaneId & out_path, const std::shared_ptr & planner_data); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/util.hpp similarity index 93% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/util.hpp index 4ef4bb91a295d..2fe728847b8f2 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/util.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ #include @@ -35,7 +35,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { struct DetectionRange { @@ -155,11 +155,13 @@ std::unordered_map, lanelet::ConstLanelet> get std::vector unique_lane_ids; if (nearest_lane_id) { // Add subsequent lane_ids from nearest lane_id - unique_lane_ids = behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( - path, *nearest_lane_id); + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( + path, *nearest_lane_id); } else { // Add all lane_ids in path - unique_lane_ids = behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); } for (const auto lane_id : unique_lane_ids) { @@ -239,6 +241,6 @@ lanelet::ConstLanelets getConstLaneletsFromIds( } } // namespace planning_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/autoware_behavior_velocity_planner_common/package.xml similarity index 93% rename from planning/behavior_velocity_planner_common/package.xml rename to planning/autoware_behavior_velocity_planner_common/package.xml index ba847d8b1f853..524387034e4b6 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/autoware_behavior_velocity_planner_common/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_planner_common + autoware_behavior_velocity_planner_common 0.1.0 - The behavior_velocity_planner_common package + The autoware_behavior_velocity_planner_common package Tomoya Kimura Shumpei Wakabayashi diff --git a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp similarity index 97% rename from planning/behavior_velocity_planner_common/src/scene_module_interface.cpp rename to planning/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index 3092d33418c8b..94cef496f4f75 100644 --- a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include #include #include #include @@ -21,7 +21,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::StopWatch; @@ -281,4 +281,4 @@ void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( } } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp similarity index 94% rename from planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp rename to planning/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp index e5705b1367e0e..d7e88f5f33901 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -62,7 +62,7 @@ return p * v; */ } // namespace -namespace behavior_velocity_planner::arc_lane_utils +namespace autoware::behavior_velocity_planner::arc_lane_utils { double calcSignedDistance(const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2) @@ -134,4 +134,4 @@ std::optional createTargetPoint( const auto front_idx = offset_segment->first; return std::make_pair(front_idx, target_pose); } -} // namespace behavior_velocity_planner::arc_lane_utils +} // namespace autoware::behavior_velocity_planner::arc_lane_utils diff --git a/planning/behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp b/planning/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp similarity index 91% rename from planning/behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp rename to planning/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp index 903cf5aab80e8..50f7cd269a904 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line) @@ -63,4 +63,4 @@ geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon) } return polygon_msg; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp similarity index 94% rename from planning/behavior_velocity_planner_common/src/utilization/debug.cpp rename to planning/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp index 00d746c56db85..a8e6828a048ae 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace debug { @@ -124,4 +124,4 @@ visualization_msgs::msg::MarkerArray createPointsMarkerArray( return msg; } } // namespace debug -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp b/planning/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp similarity index 96% rename from planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp rename to planning/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp index fe956e9be9512..eb17242c06b1e 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include #include @@ -23,7 +23,7 @@ constexpr double DOUBLE_EPSILON = 1e-6; -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { bool splineInterpolate( const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, @@ -168,4 +168,4 @@ autoware_planning_msgs::msg::Path filterStopPathPoint( } return filtered_path; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp similarity index 91% rename from planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp rename to planning/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index ecc314bca2009..1dc448bea5b86 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -// #include +// #include #include "motion_utils/trajectory/conversion.hpp" +#include #include -#include #include #include @@ -33,7 +33,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -82,11 +82,11 @@ bool smoothPath( traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); if (external_v_limit) { - autoware_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + autoware::velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); } out_path = motion_utils::convertToPathWithLaneId(traj_smoothed); return true; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/util.cpp b/planning/autoware_behavior_velocity_planner_common/src/utilization/util.cpp similarity index 97% rename from planning/behavior_velocity_planner_common/src/utilization/util.cpp rename to planning/autoware_behavior_velocity_planner_common/src/utilization/util.cpp index b724d01346f1e..4578676aeeee7 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/utilization/util.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include #include #include @@ -89,7 +89,7 @@ geometry_msgs::msg::Pose transformRelCoordinate2D( } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace planning_utils { @@ -545,11 +545,13 @@ std::vector getLaneletsOnPath( std::vector unique_lane_ids; if (nearest_lane_id) { // Add subsequent lane_ids from nearest lane_id - unique_lane_ids = behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( - path, *nearest_lane_id); + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( + path, *nearest_lane_id); } else { // Add all lane_ids in path - unique_lane_ids = behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); } std::vector lanelets; @@ -692,4 +694,4 @@ std::set getAssociativeIntersectionLanelets( } } // namespace planning_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp b/planning/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp similarity index 93% rename from planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp rename to planning/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp index fb2418ba9c829..b835aa0034ec4 100644 --- a/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp +++ b/planning/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp @@ -14,17 +14,18 @@ #include "utils.hpp" -#include -#include +#include +#include #include #include -using PathIndexWithPoint2d = behavior_velocity_planner::arc_lane_utils::PathIndexWithPoint2d; -using LineString2d = behavior_velocity_planner::LineString2d; -using Point2d = behavior_velocity_planner::Point2d; -namespace arc_lane_utils = behavior_velocity_planner::arc_lane_utils; +using PathIndexWithPoint2d = + autoware::behavior_velocity_planner::arc_lane_utils::PathIndexWithPoint2d; +using LineString2d = autoware::behavior_velocity_planner::LineString2d; +using Point2d = autoware::behavior_velocity_planner::Point2d; +namespace arc_lane_utils = autoware::behavior_velocity_planner::arc_lane_utils; namespace { @@ -121,7 +122,7 @@ TEST(findOffsetSegment, case_backward_offset_segment) TEST(checkCollision, various_cases) { - using behavior_velocity_planner::arc_lane_utils::checkCollision; + using autoware::behavior_velocity_planner::arc_lane_utils::checkCollision; constexpr double epsilon = 1e-6; { // normal case with collision diff --git a/planning/behavior_velocity_planner_common/test/src/test_state_machine.cpp b/planning/autoware_behavior_velocity_planner_common/test/src/test_state_machine.cpp similarity index 92% rename from planning/behavior_velocity_planner_common/test/src/test_state_machine.cpp rename to planning/autoware_behavior_velocity_planner_common/test/src/test_state_machine.cpp index ac7c6655ced87..fe6a1e00496b1 100644 --- a/planning/behavior_velocity_planner_common/test/src/test_state_machine.cpp +++ b/planning/autoware_behavior_velocity_planner_common/test/src/test_state_machine.cpp @@ -14,7 +14,7 @@ #include "utils.hpp" -#include +#include #include #include @@ -23,8 +23,8 @@ #include #include -using StateMachine = behavior_velocity_planner::StateMachine; -using State = behavior_velocity_planner::StateMachine::State; +using StateMachine = autoware::behavior_velocity_planner::StateMachine; +using State = autoware::behavior_velocity_planner::StateMachine::State; int enumToInt(State s) { diff --git a/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp b/planning/autoware_behavior_velocity_planner_common/test/src/test_utilization.cpp similarity index 92% rename from planning/behavior_velocity_planner_common/test/src/test_utilization.cpp rename to planning/autoware_behavior_velocity_planner_common/test/src/test_utilization.cpp index fbc5f5d709c5c..5e6c92b662820 100644 --- a/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp +++ b/planning/autoware_behavior_velocity_planner_common/test/src/test_utilization.cpp @@ -15,9 +15,9 @@ #include "motion_utils/trajectory/trajectory.hpp" #include "utils.hpp" -#include -#include -#include +#include +#include +#include #include @@ -35,7 +35,7 @@ TEST(is_ahead_of, nominal) { - using behavior_velocity_planner::planning_utils::isAheadOf; + using autoware::behavior_velocity_planner::planning_utils::isAheadOf; geometry_msgs::msg::Pose target = test::generatePose(0); geometry_msgs::msg::Pose origin = test::generatePose(1); bool is_ahead = isAheadOf(target, origin); @@ -47,7 +47,8 @@ TEST(is_ahead_of, nominal) TEST(smoothDeceleration, calculateMaxSlowDownVelocity) { - using behavior_velocity_planner::planning_utils::calcDecelerationVelocityFromDistanceToTarget; + using autoware::behavior_velocity_planner::planning_utils:: + calcDecelerationVelocityFromDistanceToTarget; const double current_accel = 1.0; const double current_velocity = 5.0; const double max_slow_down_jerk = -1.0; @@ -82,9 +83,9 @@ TEST(smoothDeceleration, calculateMaxSlowDownVelocity) TEST(specialInterpolation, specialInterpolation) { + using autoware::behavior_velocity_planner::interpolatePath; using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PathPoint; - using behavior_velocity_planner::interpolatePath; using motion_utils::calcSignedArcLength; using motion_utils::searchZeroVelocityIndex; diff --git a/planning/behavior_velocity_planner_common/test/src/utils.hpp b/planning/autoware_behavior_velocity_planner_common/test/src/utils.hpp similarity index 100% rename from planning/behavior_velocity_planner_common/test/src/utils.hpp rename to planning/autoware_behavior_velocity_planner_common/test/src/utils.hpp diff --git a/planning/autoware_behavior_velocity_run_out_module/package.xml b/planning/autoware_behavior_velocity_run_out_module/package.xml index cb2c2df58ffe2..330880b77d3da 100644 --- a/planning/autoware_behavior_velocity_run_out_module/package.xml +++ b/planning/autoware_behavior_velocity_run_out_module/package.xml @@ -19,10 +19,10 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs behavior_velocity_crosswalk_module - behavior_velocity_planner_common eigen geometry_msgs libboost-dev diff --git a/planning/autoware_behavior_velocity_run_out_module/plugins.xml b/planning/autoware_behavior_velocity_run_out_module/plugins.xml index 1ddcaf8e620e1..0fbaf5091d0f7 100644 --- a/planning/autoware_behavior_velocity_run_out_module/plugins.xml +++ b/planning/autoware_behavior_velocity_run_out_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/autoware_behavior_velocity_run_out_module/src/debug.hpp b/planning/autoware_behavior_velocity_run_out_module/src/debug.hpp index 3c6d475950e7c..c38dd63fadba9 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/debug.hpp @@ -26,7 +26,6 @@ #include namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::Polygon2d; using sensor_msgs::msg::PointCloud2; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_debug_msgs::msg::Int32Stamped; diff --git a/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index d204c7ec6f2fc..5e8b0382a07c0 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -32,8 +32,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::Point2d; -using ::behavior_velocity_planner::splineInterpolate; namespace { // create quaternion facing to the nearest trajectory point diff --git a/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp index b7ed815829a2b..8fb79fb9d8eaa 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp @@ -18,9 +18,9 @@ #include "debug.hpp" #include "utils.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -47,7 +47,6 @@ namespace autoware::behavior_velocity_planner using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; -using ::behavior_velocity_planner::PlannerData; using run_out_utils::DynamicObstacle; using run_out_utils::DynamicObstacleData; using run_out_utils::DynamicObstacleParam; @@ -56,7 +55,6 @@ using run_out_utils::PredictedPath; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; using PathPointsWithLaneId = std::vector; -using ::behavior_velocity_planner::Polygons2d; /** * @brief base class for creating dynamic obstacles from multiple types of input diff --git a/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp b/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp index 5ee50863fd162..002a49abae611 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp @@ -22,7 +22,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::SceneModuleManagerInterface; using tier4_autoware_utils::getOrDeclareParameter; RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -203,4 +202,4 @@ void RunOutModuleManager::setDynamicObstacleCreator( #include PLUGINLIB_EXPORT_CLASS( autoware::behavior_velocity_planner::RunOutModulePlugin, - behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/autoware_behavior_velocity_run_out_module/src/manager.hpp b/planning/autoware_behavior_velocity_run_out_module/src/manager.hpp index f0c49b99c99e5..8cdb17e21d74b 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/manager.hpp @@ -17,17 +17,14 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::PluginWrapper; -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::SceneModuleManagerInterface; class RunOutModuleManager : public SceneModuleManagerInterface { public: diff --git a/planning/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/autoware_behavior_velocity_run_out_module/src/scene.cpp index 351ed8e7a09b8..e492bc99be615 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -17,8 +17,8 @@ #include "behavior_velocity_crosswalk_module/util.hpp" #include "path_utils.hpp" -#include -#include +#include +#include #include #include #include @@ -37,11 +37,8 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using ::behavior_velocity_planner::PlanningBehavior; using object_recognition_utils::convertLabelToString; -namespace planning_utils = ::behavior_velocity_planner::planning_utils; -using ::behavior_velocity_planner::getCrosswalksOnPath; -using ::behavior_velocity_planner::Polygon2d; +namespace planning_utils = autoware::behavior_velocity_planner::planning_utils; RunOutModule::RunOutModule( const int64_t module_id, const std::shared_ptr & planner_data, diff --git a/planning/autoware_behavior_velocity_run_out_module/src/scene.hpp b/planning/autoware_behavior_velocity_run_out_module/src/scene.hpp index 85da1340a5a5d..e3fd30a4e36ce 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/scene.hpp @@ -21,7 +21,7 @@ #include "state_machine.hpp" #include "utils.hpp" -#include +#include #include #include @@ -39,11 +39,6 @@ using tier4_debug_msgs::msg::Float32Stamped; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; using BasicPolygons2d = std::vector; -using ::behavior_velocity_planner::PathWithLaneId; -using ::behavior_velocity_planner::PlannerData; -using ::behavior_velocity_planner::Polygon2d; -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::StopReason; class RunOutModule : public SceneModuleInterface { diff --git a/planning/autoware_behavior_velocity_run_out_module/src/utils.cpp b/planning/autoware_behavior_velocity_run_out_module/src/utils.cpp index ec1cd460e1bb0..ed745cbb92679 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/utils.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/utils.cpp @@ -35,9 +35,6 @@ #include namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::DetectionRange; -using ::behavior_velocity_planner::PathPointWithLaneId; -namespace planning_utils = ::behavior_velocity_planner::planning_utils; namespace run_out_utils { Polygon2d createBoostPolyFromMsg(const std::vector & input_poly) diff --git a/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp b/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp index 10f856b257a61..28bfa9569c66d 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp @@ -17,8 +17,8 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" -#include -#include +#include +#include #include #include @@ -38,8 +38,6 @@ using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; using autoware_planning_msgs::msg::PathPoint; -using ::behavior_velocity_planner::PlannerData; -using ::behavior_velocity_planner::Polygons2d; using tier4_autoware_utils::Box2d; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; diff --git a/planning/autoware_behavior_velocity_template_module/package.xml b/planning/autoware_behavior_velocity_template_module/package.xml index a6bcbf5c34e76..eb6a6668fcee9 100644 --- a/planning/autoware_behavior_velocity_template_module/package.xml +++ b/planning/autoware_behavior_velocity_template_module/package.xml @@ -13,8 +13,8 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/autoware_behavior_velocity_template_module/src/manager.cpp b/planning/autoware_behavior_velocity_template_module/src/manager.cpp index 8f6621aaab835..8aaf6be9bbfe9 100644 --- a/planning/autoware_behavior_velocity_template_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_template_module/src/manager.cpp @@ -61,4 +61,4 @@ TemplateModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( autoware::behavior_velocity_planner::TemplateModulePlugin, - behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/autoware_behavior_velocity_template_module/src/manager.hpp b/planning/autoware_behavior_velocity_template_module/src/manager.hpp index 8e95f516c337d..b92913b2a95ce 100644 --- a/planning/autoware_behavior_velocity_template_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_template_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -37,7 +37,8 @@ namespace autoware::behavior_velocity_planner * * @param node A reference to the ROS node. */ -class TemplateModuleManager : public ::behavior_velocity_planner::SceneModuleManagerInterface +class TemplateModuleManager +: public autoware::behavior_velocity_planner::SceneModuleManagerInterface { public: explicit TemplateModuleManager(rclcpp::Node & node); @@ -85,7 +86,7 @@ class TemplateModuleManager : public ::behavior_velocity_planner::SceneModuleMan * Velocity Planner. */ class TemplateModulePlugin -: public ::behavior_velocity_planner::PluginWrapper +: public autoware::behavior_velocity_planner::PluginWrapper { }; diff --git a/planning/autoware_behavior_velocity_template_module/src/scene.hpp b/planning/autoware_behavior_velocity_template_module/src/scene.hpp index 3ce5ddbd8729d..0fc4e7dd9ae91 100644 --- a/planning/autoware_behavior_velocity_template_module/src/scene.hpp +++ b/planning/autoware_behavior_velocity_template_module/src/scene.hpp @@ -15,7 +15,7 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ -#include +#include #include #include @@ -23,8 +23,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::StopReason; using tier4_planning_msgs::msg::PathWithLaneId; class TemplateModule : public SceneModuleInterface diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml b/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml index cd35d4308c26a..cda3abbd0eee9 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml @@ -17,8 +17,8 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_behavior_velocity_planner_common autoware_planning_msgs - behavior_velocity_planner_common geometry_msgs lanelet2_extension motion_utils diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml b/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml index 2402fc13469b9..2e2abef259778 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp index 5a32cfd2f74f0..39dca1f8f6303 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -28,7 +28,7 @@ namespace autoware::behavior_velocity_planner { using lanelet::autoware::VirtualTrafficLight; using tier4_autoware_utils::getOrDeclareParameter; -namespace planning_utils = ::behavior_velocity_planner::planning_utils; +namespace planning_utils = autoware::behavior_velocity_planner::planning_utils; VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -82,4 +82,4 @@ VirtualTrafficLightModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin, - behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp index c73bb0d706008..6b0436a4290ac 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -29,9 +29,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::PluginWrapper; -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::SceneModuleManagerInterface; class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface { public: diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 7d91a954171dd..a87d8867adf7e 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -14,8 +14,8 @@ #include "scene.hpp" -#include -#include +#include +#include #include #include @@ -25,11 +25,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::PlanningBehavior; -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::VelocityFactor; -namespace arc_lane_utils = ::behavior_velocity_planner::arc_lane_utils; -namespace planning_utils = ::behavior_velocity_planner::planning_utils; namespace { using tier4_autoware_utils::calcDistance2d; @@ -181,7 +176,8 @@ std::optional insertStopVelocityAtCollision( auto insert_point = path->points.at(insert_index); insert_point.point.pose = interpolated_pose; // Insert 0 velocity after stop point or replace velocity with 0 - behavior_velocity_planner::planning_utils::insertVelocity(*path, insert_point, 0.0, insert_index); + autoware::behavior_velocity_planner::planning_utils::insertVelocity( + *path, insert_point, 0.0, insert_index); return insert_index; } } // namespace diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index c83ff4e0607ef..9775e64145529 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -15,7 +15,7 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ -#include +#include #include #include #include @@ -32,10 +32,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::PathWithLaneId; -using ::behavior_velocity_planner::Pose; -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::StopReason; class VirtualTrafficLightModule : public SceneModuleInterface { public: diff --git a/planning/behavior_velocity_walkway_module/CMakeLists.txt b/planning/autoware_behavior_velocity_walkway_module/CMakeLists.txt similarity index 86% rename from planning/behavior_velocity_walkway_module/CMakeLists.txt rename to planning/autoware_behavior_velocity_walkway_module/CMakeLists.txt index 351a240743402..11504d9c8999c 100644 --- a/planning/behavior_velocity_walkway_module/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_walkway_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_walkway_module) +project(autoware_behavior_velocity_walkway_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_velocity_walkway_module/README.md b/planning/autoware_behavior_velocity_walkway_module/README.md similarity index 100% rename from planning/behavior_velocity_walkway_module/README.md rename to planning/autoware_behavior_velocity_walkway_module/README.md diff --git a/planning/behavior_velocity_walkway_module/config/walkway.param.yaml b/planning/autoware_behavior_velocity_walkway_module/config/walkway.param.yaml similarity index 100% rename from planning/behavior_velocity_walkway_module/config/walkway.param.yaml rename to planning/autoware_behavior_velocity_walkway_module/config/walkway.param.yaml diff --git a/planning/behavior_velocity_walkway_module/package.xml b/planning/autoware_behavior_velocity_walkway_module/package.xml similarity index 86% rename from planning/behavior_velocity_walkway_module/package.xml rename to planning/autoware_behavior_velocity_walkway_module/package.xml index ea6c7803717f9..bbe87ffb8b2cd 100644 --- a/planning/behavior_velocity_walkway_module/package.xml +++ b/planning/autoware_behavior_velocity_walkway_module/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_walkway_module + autoware_behavior_velocity_walkway_module 0.1.0 - The behavior_velocity_walkway_module package + The autoware_behavior_velocity_walkway_module package Satoshi Ota Tomoya Kimura @@ -17,9 +17,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_planning_msgs behavior_velocity_crosswalk_module - behavior_velocity_planner_common geometry_msgs lanelet2_extension libboost-dev diff --git a/planning/autoware_behavior_velocity_walkway_module/plugins.xml b/planning/autoware_behavior_velocity_walkway_module/plugins.xml new file mode 100644 index 0000000000000..9a5803807ae8d --- /dev/null +++ b/planning/autoware_behavior_velocity_walkway_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_walkway_module/src/debug.cpp b/planning/autoware_behavior_velocity_walkway_module/src/debug.cpp similarity index 95% rename from planning/behavior_velocity_walkway_module/src/debug.cpp rename to planning/autoware_behavior_velocity_walkway_module/src/debug.cpp index 8814fa8271866..80badf59434da 100644 --- a/planning/behavior_velocity_walkway_module/src/debug.cpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/debug.cpp @@ -14,7 +14,7 @@ #include "scene_walkway.hpp" -#include +#include #include #include #include @@ -22,7 +22,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using motion_utils::createSlowDownVirtualWallMarker; @@ -98,4 +98,4 @@ visualization_msgs::msg::MarkerArray WalkwayModule::createDebugMarkerArray() return debug_marker_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_walkway_module/src/manager.cpp b/planning/autoware_behavior_velocity_walkway_module/src/manager.cpp similarity index 92% rename from planning/behavior_velocity_walkway_module/src/manager.cpp rename to planning/autoware_behavior_velocity_walkway_module/src/manager.cpp index d427e57009cf6..cf3efe3700a05 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -23,7 +23,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::autoware::Crosswalk; @@ -100,8 +100,9 @@ WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) return walkway_id_set.count(scene_module->getModuleId()) == 0; }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::WalkwayModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::WalkwayModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_walkway_module/src/manager.hpp b/planning/autoware_behavior_velocity_walkway_module/src/manager.hpp similarity index 82% rename from planning/behavior_velocity_walkway_module/src/manager.hpp rename to planning/autoware_behavior_velocity_walkway_module/src/manager.hpp index 5228445bb4e03..592181dd3ac75 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene_walkway.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -31,9 +31,8 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { - using tier4_planning_msgs::msg::PathWithLaneId; class WalkwayModuleManager : public SceneModuleManagerInterface @@ -56,6 +55,6 @@ class WalkwayModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_walkway_module/src/scene_walkway.cpp b/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp similarity index 97% rename from planning/behavior_velocity_walkway_module/src/scene_walkway.cpp rename to planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp index 4a777d49f4fe6..73ae5a52e0183 100644 --- a/planning/behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -14,12 +14,12 @@ #include "scene_walkway.hpp" -#include +#include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; using motion_utils::calcLongitudinalOffsetPose; @@ -165,4 +165,4 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_ return true; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_walkway_module/src/scene_walkway.hpp b/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp similarity index 92% rename from planning/behavior_velocity_walkway_module/src/scene_walkway.hpp rename to planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp index c372a5835f795..e6c8f61ba656e 100644 --- a/planning/behavior_velocity_walkway_module/src/scene_walkway.hpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp @@ -18,7 +18,7 @@ #include "behavior_velocity_crosswalk_module/util.hpp" #include "scene_walkway.hpp" -#include +#include #include #include @@ -31,9 +31,8 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { - class WalkwayModule : public SceneModuleInterface { public: @@ -77,6 +76,6 @@ class WalkwayModule : public SceneModuleInterface // flag to use regulatory element const bool use_regulatory_element_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_WALKWAY_HPP_ diff --git a/planning/costmap_generator/CMakeLists.txt b/planning/autoware_costmap_generator/CMakeLists.txt similarity index 80% rename from planning/costmap_generator/CMakeLists.txt rename to planning/autoware_costmap_generator/CMakeLists.txt index 620a997990001..b8b15a7312324 100644 --- a/planning/costmap_generator/CMakeLists.txt +++ b/planning/autoware_costmap_generator/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(costmap_generator) +project(autoware_costmap_generator) find_package(autoware_cmake REQUIRED) autoware_package() @@ -16,9 +16,9 @@ include_directories( ) ament_auto_add_library(costmap_generator_lib SHARED - nodes/costmap_generator/points_to_costmap.cpp - nodes/costmap_generator/objects_to_costmap.cpp - nodes/costmap_generator/object_map_utils.cpp + nodes/autoware_costmap_generator/points_to_costmap.cpp + nodes/autoware_costmap_generator/objects_to_costmap.cpp + nodes/autoware_costmap_generator/object_map_utils.cpp ) target_link_libraries(costmap_generator_lib ${PCL_LIBRARIES} @@ -33,7 +33,7 @@ if(${PCL_VERSION} GREATER_EQUAL 1.12.1) endif() ament_auto_add_library(costmap_generator_node SHARED - nodes/costmap_generator/costmap_generator_node.cpp + nodes/autoware_costmap_generator/costmap_generator_node.cpp ) target_link_libraries(costmap_generator_node ${PCL_LIBRARIES} @@ -41,7 +41,7 @@ target_link_libraries(costmap_generator_node ) rclcpp_components_register_node(costmap_generator_node - PLUGIN "CostmapGenerator" + PLUGIN "autoware::costmap_generator::CostmapGenerator" EXECUTABLE costmap_generator ) diff --git a/planning/costmap_generator/README.md b/planning/autoware_costmap_generator/README.md similarity index 100% rename from planning/costmap_generator/README.md rename to planning/autoware_costmap_generator/README.md diff --git a/planning/costmap_generator/config/costmap_generator.param.yaml b/planning/autoware_costmap_generator/config/costmap_generator.param.yaml similarity index 100% rename from planning/costmap_generator/config/costmap_generator.param.yaml rename to planning/autoware_costmap_generator/config/costmap_generator.param.yaml diff --git a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp b/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp similarity index 94% rename from planning/costmap_generator/include/costmap_generator/costmap_generator.hpp rename to planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp index 31c8a05cc3c60..093c88541e9c8 100644 --- a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp +++ b/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp @@ -42,11 +42,11 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#ifndef COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ -#define COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ +#ifndef AUTOWARE_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ +#define AUTOWARE_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ -#include "costmap_generator/objects_to_costmap.hpp" -#include "costmap_generator/points_to_costmap.hpp" +#include "autoware_costmap_generator/objects_to_costmap.hpp" +#include "autoware_costmap_generator/points_to_costmap.hpp" #include #include @@ -72,6 +72,8 @@ #include #include +namespace autoware::costmap_generator +{ class CostmapGenerator : public rclcpp::Node { public: @@ -197,5 +199,6 @@ class CostmapGenerator : public rclcpp::Node /// \brief calculate cost for final output grid_map::Matrix generateCombinedCostmap(); }; +} // namespace autoware::costmap_generator -#endif // COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ +#endif // AUTOWARE_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ diff --git a/planning/costmap_generator/include/costmap_generator/object_map_utils.hpp b/planning/autoware_costmap_generator/include/autoware_costmap_generator/object_map_utils.hpp similarity index 95% rename from planning/costmap_generator/include/costmap_generator/object_map_utils.hpp rename to planning/autoware_costmap_generator/include/autoware_costmap_generator/object_map_utils.hpp index 282bd6dcc1beb..f4911cc428d36 100644 --- a/planning/costmap_generator/include/costmap_generator/object_map_utils.hpp +++ b/planning/autoware_costmap_generator/include/autoware_costmap_generator/object_map_utils.hpp @@ -30,8 +30,8 @@ * */ -#ifndef COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ -#define COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ +#ifndef AUTOWARE_COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ +#define AUTOWARE_COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ #include #include @@ -98,4 +98,4 @@ void FillPolygonAreas( } // namespace object_map -#endif // COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ +#endif // AUTOWARE_COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ diff --git a/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp b/planning/autoware_costmap_generator/include/autoware_costmap_generator/objects_to_costmap.hpp similarity index 95% rename from planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp rename to planning/autoware_costmap_generator/include/autoware_costmap_generator/objects_to_costmap.hpp index a88bb97a623e2..aa11a98830dc3 100644 --- a/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp +++ b/planning/autoware_costmap_generator/include/autoware_costmap_generator/objects_to_costmap.hpp @@ -42,8 +42,8 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#ifndef COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ -#define COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ +#ifndef AUTOWARE_COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ +#define AUTOWARE_COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ #include @@ -57,6 +57,8 @@ #include +namespace autoware::costmap_generator +{ class ObjectsToCostmap { public: @@ -123,5 +125,5 @@ class ObjectsToCostmap const grid_map::Polygon & polygon, const std::string & gridmap_layer_name, const float score, grid_map::GridMap & objects_costmap); }; - -#endif // COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ +} // namespace autoware::costmap_generator +#endif // AUTOWARE_COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ diff --git a/planning/costmap_generator/include/costmap_generator/points_to_costmap.hpp b/planning/autoware_costmap_generator/include/autoware_costmap_generator/points_to_costmap.hpp similarity index 95% rename from planning/costmap_generator/include/costmap_generator/points_to_costmap.hpp rename to planning/autoware_costmap_generator/include/autoware_costmap_generator/points_to_costmap.hpp index 3b179d63f1d2a..0e3abbd69ec20 100644 --- a/planning/costmap_generator/include/costmap_generator/points_to_costmap.hpp +++ b/planning/autoware_costmap_generator/include/autoware_costmap_generator/points_to_costmap.hpp @@ -42,8 +42,8 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#ifndef COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ -#define COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ +#ifndef AUTOWARE_COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ +#define AUTOWARE_COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ #include @@ -52,6 +52,9 @@ #include #include +namespace autoware::costmap_generator + +{ class PointsToCostmap { public: @@ -115,5 +118,6 @@ class PointsToCostmap const std::string & gridmap_layer_name, const std::vector>> grid_vec); }; +} // namespace autoware::costmap_generator -#endif // COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ +#endif // AUTOWARE_COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ diff --git a/planning/costmap_generator/launch/costmap_generator.launch.xml b/planning/autoware_costmap_generator/launch/costmap_generator.launch.xml similarity index 84% rename from planning/costmap_generator/launch/costmap_generator.launch.xml rename to planning/autoware_costmap_generator/launch/costmap_generator.launch.xml index ba8a71f5f428a..e7c378565dd0f 100644 --- a/planning/costmap_generator/launch/costmap_generator.launch.xml +++ b/planning/autoware_costmap_generator/launch/costmap_generator.launch.xml @@ -6,9 +6,9 @@ - + - + diff --git a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp similarity index 98% rename from planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp rename to planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp index 777dfc7a50a6e..4302ac6ada645 100644 --- a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp @@ -42,8 +42,8 @@ * OF private_node SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#include "costmap_generator/costmap_generator.hpp" -#include "costmap_generator/object_map_utils.hpp" +#include "autoware_costmap_generator/costmap_generator.hpp" +#include "autoware_costmap_generator/object_map_utils.hpp" #include #include @@ -157,6 +157,8 @@ pcl::PointCloud getTransformedPointCloud( } // namespace +namespace autoware::costmap_generator +{ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) : Node("costmap_generator", node_options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { @@ -476,6 +478,7 @@ void CostmapGenerator::publishCostmap(const grid_map::GridMap & costmap) out_gridmap_msg->header = header; pub_costmap_->publish(*out_gridmap_msg); } +} // namespace autoware::costmap_generator #include -RCLCPP_COMPONENTS_REGISTER_NODE(CostmapGenerator) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::costmap_generator::CostmapGenerator) diff --git a/planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/object_map_utils.cpp similarity index 98% rename from planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp rename to planning/autoware_costmap_generator/nodes/autoware_costmap_generator/object_map_utils.cpp index 6955798be3e8a..4ce452814008c 100644 --- a/planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/object_map_utils.cpp @@ -30,7 +30,7 @@ * */ -#include "costmap_generator/object_map_utils.hpp" +#include "autoware_costmap_generator/object_map_utils.hpp" #include #include diff --git a/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/objects_to_costmap.cpp similarity index 98% rename from planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp rename to planning/autoware_costmap_generator/nodes/autoware_costmap_generator/objects_to_costmap.cpp index f6f024fb92a4e..afb2dbb9874c9 100644 --- a/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/objects_to_costmap.cpp @@ -42,13 +42,15 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#include "costmap_generator/objects_to_costmap.hpp" +#include "autoware_costmap_generator/objects_to_costmap.hpp" #include #include #include +namespace autoware::costmap_generator +{ // Constructor ObjectsToCostmap::ObjectsToCostmap() : NUMBER_OF_POINTS(4), @@ -196,3 +198,4 @@ grid_map::Matrix ObjectsToCostmap::makeCostmapFromObjects( return objects_costmap[OBJECTS_COSTMAP_LAYER_]; } +} // namespace autoware::costmap_generator diff --git a/planning/costmap_generator/nodes/costmap_generator/points_to_costmap.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/points_to_costmap.cpp similarity index 97% rename from planning/costmap_generator/nodes/costmap_generator/points_to_costmap.cpp rename to planning/autoware_costmap_generator/nodes/autoware_costmap_generator/points_to_costmap.cpp index e875f68973a12..723c27201ac8a 100644 --- a/planning/costmap_generator/nodes/costmap_generator/points_to_costmap.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/points_to_costmap.cpp @@ -42,11 +42,14 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#include "costmap_generator/points_to_costmap.hpp" +#include "autoware_costmap_generator/points_to_costmap.hpp" #include #include +namespace autoware::costmap_generator +{ + void PointsToCostmap::initGridmapParam(const grid_map::GridMap & gridmap) { grid_length_x_ = gridmap.getLength().x(); @@ -140,3 +143,5 @@ grid_map::Matrix PointsToCostmap::makeCostmapFromPoints( gridmap_layer_name, grid_vec); return costmap; } + +} // namespace autoware::costmap_generator diff --git a/planning/costmap_generator/package.xml b/planning/autoware_costmap_generator/package.xml similarity index 92% rename from planning/costmap_generator/package.xml rename to planning/autoware_costmap_generator/package.xml index dc94d74dce8ae..aa1ae80eaf953 100644 --- a/planning/costmap_generator/package.xml +++ b/planning/autoware_costmap_generator/package.xml @@ -1,9 +1,9 @@ - costmap_generator + autoware_costmap_generator 0.1.0 - The costmap_generator package + The autoware_costmap_generator package Kosuke Takeuchi Takamasa Horibe Takayuki Murooka diff --git a/planning/costmap_generator/schema/costmap_generator.schema.json b/planning/autoware_costmap_generator/schema/costmap_generator.schema.json similarity index 100% rename from planning/costmap_generator/schema/costmap_generator.schema.json rename to planning/autoware_costmap_generator/schema/costmap_generator.schema.json diff --git a/planning/costmap_generator/test/test_objects_to_costmap.cpp b/planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp similarity index 97% rename from planning/costmap_generator/test/test_objects_to_costmap.cpp rename to planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp index 15b853d0782aa..8d437865b8793 100644 --- a/planning/costmap_generator/test/test_objects_to_costmap.cpp +++ b/planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include +namespace autoware::costmap_generator +{ using LABEL = autoware_perception_msgs::msg::ObjectClassification; class ObjectsToCostMapTest : public ::testing::Test @@ -134,3 +136,4 @@ TEST_F(ObjectsToCostMapTest, TestMakeCostmapFromObjects) } } } +} // namespace autoware::costmap_generator diff --git a/planning/costmap_generator/test/test_points_to_costmap.cpp b/planning/autoware_costmap_generator/test/test_points_to_costmap.cpp similarity index 98% rename from planning/costmap_generator/test/test_points_to_costmap.cpp rename to planning/autoware_costmap_generator/test/test_points_to_costmap.cpp index 2366dffc7fc6f..6c52e2dc6ce9d 100644 --- a/planning/costmap_generator/test/test_points_to_costmap.cpp +++ b/planning/autoware_costmap_generator/test/test_points_to_costmap.cpp @@ -12,9 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include + +namespace autoware::costmap_generator +{ using pointcloud = pcl::PointCloud; class PointsToCostmapTest : public ::testing::Test { @@ -216,3 +219,4 @@ TEST_F(PointsToCostmapTest, TestMakeCostmapFromPoints_invalidPoints_outOfGrid) EXPECT_EQ(nonempty_grid_cell_num, 0); } +} // namespace autoware::costmap_generator diff --git a/planning/external_velocity_limit_selector/CMakeLists.txt b/planning/autoware_external_velocity_limit_selector/CMakeLists.txt similarity index 73% rename from planning/external_velocity_limit_selector/CMakeLists.txt rename to planning/autoware_external_velocity_limit_selector/CMakeLists.txt index db83dcf3cad4a..40f0ebb2a3eba 100644 --- a/planning/external_velocity_limit_selector/CMakeLists.txt +++ b/planning/autoware_external_velocity_limit_selector/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(external_velocity_limit_selector) +project(autoware_external_velocity_limit_selector) find_package(autoware_cmake REQUIRED) autoware_package() @@ -9,7 +9,7 @@ ament_auto_add_library(external_velocity_limit_selector_node SHARED ) rclcpp_components_register_node(external_velocity_limit_selector_node - PLUGIN "ExternalVelocityLimitSelectorNode" + PLUGIN "autoware::external_velocity_limit_selector::ExternalVelocityLimitSelectorNode" EXECUTABLE external_velocity_limit_selector ) diff --git a/planning/external_velocity_limit_selector/README.md b/planning/autoware_external_velocity_limit_selector/README.md similarity index 100% rename from planning/external_velocity_limit_selector/README.md rename to planning/autoware_external_velocity_limit_selector/README.md diff --git a/planning/external_velocity_limit_selector/config/default.param.yaml b/planning/autoware_external_velocity_limit_selector/config/default.param.yaml similarity index 100% rename from planning/external_velocity_limit_selector/config/default.param.yaml rename to planning/autoware_external_velocity_limit_selector/config/default.param.yaml diff --git a/planning/external_velocity_limit_selector/config/default_common.param.yaml b/planning/autoware_external_velocity_limit_selector/config/default_common.param.yaml similarity index 100% rename from planning/external_velocity_limit_selector/config/default_common.param.yaml rename to planning/autoware_external_velocity_limit_selector/config/default_common.param.yaml diff --git a/planning/external_velocity_limit_selector/image/external_velocity_limit_selector.png b/planning/autoware_external_velocity_limit_selector/image/external_velocity_limit_selector.png similarity index 100% rename from planning/external_velocity_limit_selector/image/external_velocity_limit_selector.png rename to planning/autoware_external_velocity_limit_selector/image/external_velocity_limit_selector.png diff --git a/planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp b/planning/autoware_external_velocity_limit_selector/include/autoware_external_velocity_limit_selector/external_velocity_limit_selector_node.hpp similarity index 87% rename from planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp rename to planning/autoware_external_velocity_limit_selector/include/autoware_external_velocity_limit_selector/external_velocity_limit_selector_node.hpp index 93fad135ac678..e4043f1aeb986 100644 --- a/planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp +++ b/planning/autoware_external_velocity_limit_selector/include/autoware_external_velocity_limit_selector/external_velocity_limit_selector_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ -#define EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ +#ifndef AUTOWARE_EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ +#define AUTOWARE_EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ #include @@ -26,6 +26,9 @@ #include #include +namespace autoware::external_velocity_limit_selector +{ + using tier4_debug_msgs::msg::StringStamped; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; @@ -77,5 +80,6 @@ class ExternalVelocityLimitSelectorNode : public rclcpp::Node VelocityLimit hardest_limit_{}; VelocityLimitTable velocity_limit_table_; }; +} // namespace autoware::external_velocity_limit_selector -#endif // EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ +#endif // AUTOWARE_EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ diff --git a/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml b/planning/autoware_external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml similarity index 74% rename from planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml rename to planning/autoware_external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml index 5ef089f3d3ee7..42c8709b6663a 100644 --- a/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml +++ b/planning/autoware_external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml @@ -1,7 +1,7 @@ - - + + @@ -10,7 +10,7 @@ - + diff --git a/planning/external_velocity_limit_selector/package.xml b/planning/autoware_external_velocity_limit_selector/package.xml similarity index 88% rename from planning/external_velocity_limit_selector/package.xml rename to planning/autoware_external_velocity_limit_selector/package.xml index e83966c7b876d..803e32237e938 100644 --- a/planning/external_velocity_limit_selector/package.xml +++ b/planning/autoware_external_velocity_limit_selector/package.xml @@ -1,9 +1,9 @@ - external_velocity_limit_selector + autoware_external_velocity_limit_selector 0.1.0 - The external_velocity_limit_selector ROS 2 package + The autoware_external_velocity_limit_selector ROS 2 package Satoshi Ota Shinnosuke Hirakawa Shumpei Wakabayashi diff --git a/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp similarity index 96% rename from planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp rename to planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp index 5e557167ac3e1..008b58df303b1 100644 --- a/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp +++ b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "external_velocity_limit_selector/external_velocity_limit_selector_node.hpp" +#include "autoware_external_velocity_limit_selector/external_velocity_limit_selector_node.hpp" #include #include @@ -21,6 +21,9 @@ #include #include +namespace autoware::external_velocity_limit_selector +{ + namespace { VelocityLimit getHardestLimit( @@ -236,6 +239,8 @@ void ExternalVelocityLimitSelectorNode::updateVelocityLimit() hardest_limit_ = getHardestLimit(velocity_limit_table_, node_param_); } +} // namespace autoware::external_velocity_limit_selector #include -RCLCPP_COMPONENTS_REGISTER_NODE(ExternalVelocityLimitSelectorNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::external_velocity_limit_selector::ExternalVelocityLimitSelectorNode) diff --git a/planning/autoware_path_optimizer/CMakeLists.txt b/planning/autoware_path_optimizer/CMakeLists.txt index 3ceeae1022b10..78077e6c42720 100644 --- a/planning/autoware_path_optimizer/CMakeLists.txt +++ b/planning/autoware_path_optimizer/CMakeLists.txt @@ -41,7 +41,7 @@ target_include_directories(autoware_path_optimizer # register node rclcpp_components_register_node(autoware_path_optimizer - PLUGIN "autoware_path_optimizer::PathOptimizer" + PLUGIN "autoware::path_optimizer::PathOptimizer" EXECUTABLE path_optimizer_node ) diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp index b27eff787ca5a..d2de8d201d9fa 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp @@ -25,7 +25,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { struct ReferencePoint; struct Bounds; @@ -153,6 +153,6 @@ struct EgoNearestParam double dist_threshold{0.0}; double yaw_threshold{0.0}; }; -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__COMMON_STRUCTS_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp index d1d2abaeaad61..edca8d706047b 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp @@ -25,11 +25,11 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { MarkerArray getDebugMarker( const DebugData & debug_data, const std::vector & optimized_points, const vehicle_info_util::VehicleInfo & vehicle_info, const bool publish_extra_marker); -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__DEBUG_MARKER_HPP_ 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 33da339c62e40..8c207a9a3830f 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 @@ -34,7 +34,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { struct Bounds { @@ -308,5 +308,5 @@ class MPTOptimizer size_t getNumberOfSlackVariables() const; std::optional calcNormalizedAvoidanceCost(const ReferencePoint & ref_point) const; }; -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp index a0e776628e3cb..30c95debe11cb 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp @@ -33,7 +33,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { class PathOptimizer : public rclcpp::Node { @@ -141,6 +141,6 @@ class PathOptimizer : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__NODE_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp index 8270bb631e0ae..26d9599f07091 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp @@ -24,7 +24,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { class ReplanChecker { @@ -66,6 +66,6 @@ class ReplanChecker bool isPathGoalChanged( const PlannerData & planner_data, const std::vector & prev_traj_points) const; }; -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__REPLAN_CHECKER_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp index 6ee4fcb0ab7a5..0c41da0ee5f62 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp @@ -22,7 +22,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { struct ReferencePoint; @@ -58,5 +58,5 @@ class StateEquationGenerator std::unique_ptr vehicle_model_ptr_; mutable std::shared_ptr time_keeper_ptr_; }; -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp index 755d0b2ace297..63a5840789a6b 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp @@ -28,7 +28,7 @@ #include "tier4_debug_msgs/msg/string_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" -namespace autoware_path_optimizer +namespace autoware::path_optimizer { // std_msgs using std_msgs::msg::Header; @@ -45,6 +45,6 @@ using visualization_msgs::msg::MarkerArray; // debug using tier4_debug_msgs::msg::Float64Stamped; using tier4_debug_msgs::msg::StringStamped; -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__TYPE_ALIAS_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp index 32ef8cd5941fc..588b68f52a094 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp @@ -39,13 +39,13 @@ namespace tier4_autoware_utils { template <> -geometry_msgs::msg::Point getPoint(const autoware_path_optimizer::ReferencePoint & p); +geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p); template <> -geometry_msgs::msg::Pose getPose(const autoware_path_optimizer::ReferencePoint & p); +geometry_msgs::msg::Pose getPose(const autoware::path_optimizer::ReferencePoint & p); } // namespace tier4_autoware_utils -namespace autoware_path_optimizer +namespace autoware::path_optimizer { namespace geometry_utils { @@ -68,5 +68,5 @@ bool isOutsideDrivableAreaFromRectangleFootprint( const vehicle_info_util::VehicleInfo & vehicle_info, const bool use_footprint_polygon_for_outside_drivable_area_check); } // namespace geometry_utils -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp index 1056d80ef37aa..f3f528df2ec42 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp @@ -38,16 +38,16 @@ namespace tier4_autoware_utils { template <> -geometry_msgs::msg::Point getPoint(const autoware_path_optimizer::ReferencePoint & p); +geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p); template <> -geometry_msgs::msg::Pose getPose(const autoware_path_optimizer::ReferencePoint & p); +geometry_msgs::msg::Pose getPose(const autoware::path_optimizer::ReferencePoint & p); template <> -double getLongitudinalVelocity(const autoware_path_optimizer::ReferencePoint & p); +double getLongitudinalVelocity(const autoware::path_optimizer::ReferencePoint & p); } // namespace tier4_autoware_utils -namespace autoware_path_optimizer +namespace autoware::path_optimizer { namespace trajectory_utils { @@ -214,5 +214,5 @@ void insertStopPoint( std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, const size_t stop_seg_idx); } // namespace trajectory_utils -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ diff --git a/planning/autoware_path_optimizer/src/debug_marker.cpp b/planning/autoware_path_optimizer/src/debug_marker.cpp index 3127d521160c2..7c644f4448a0c 100644 --- a/planning/autoware_path_optimizer/src/debug_marker.cpp +++ b/planning/autoware_path_optimizer/src/debug_marker.cpp @@ -18,7 +18,7 @@ #include "visualization_msgs/msg/marker_array.hpp" -namespace autoware_path_optimizer +namespace autoware::path_optimizer { using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; @@ -432,4 +432,4 @@ MarkerArray getDebugMarker( return marker_array; } -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index ece301e64c97e..2ab622c6b4b58 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -29,7 +29,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { namespace { @@ -1783,4 +1783,4 @@ std::optional MPTOptimizer::calcNormalizedAvoidanceCost( } return std::clamp(-negative_avoidance_cost / mpt_param_.max_avoidance_cost, 0.0, 1.0); } -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index 49d41e6b07884..816c0d459d95f 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -25,7 +25,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { namespace { @@ -666,7 +666,7 @@ void PathOptimizer::publishDebugData(const Header & header) const time_keeper_ptr_->toc(__func__, " "); } -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(autoware_path_optimizer::PathOptimizer) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::path_optimizer::PathOptimizer) diff --git a/planning/autoware_path_optimizer/src/replan_checker.cpp b/planning/autoware_path_optimizer/src/replan_checker.cpp index 797041ee75416..6745872f50b02 100644 --- a/planning/autoware_path_optimizer/src/replan_checker.cpp +++ b/planning/autoware_path_optimizer/src/replan_checker.cpp @@ -21,7 +21,7 @@ #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param) : ego_nearest_param_(ego_nearest_param), logger_(node->get_logger().get_child("replan_checker")) @@ -212,4 +212,4 @@ bool ReplanChecker::isPathGoalChanged( return true; } -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/state_equation_generator.cpp b/planning/autoware_path_optimizer/src/state_equation_generator.cpp index 7712fbbf6c3cf..ec6a9e575be95 100644 --- a/planning/autoware_path_optimizer/src/state_equation_generator.cpp +++ b/planning/autoware_path_optimizer/src/state_equation_generator.cpp @@ -16,7 +16,7 @@ #include "autoware_path_optimizer/mpt_optimizer.hpp" -namespace autoware_path_optimizer +namespace autoware::path_optimizer { // state equation: x = B u + W (u includes x_0) // NOTE: Originally, x_t+1 = Ad x_t + Bd u + Wd. @@ -69,4 +69,4 @@ Eigen::VectorXd StateEquationGenerator::predict( { return mat.B * U + mat.W; } -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp index 9d93cdc26a7ed..45302c0b729a9 100644 --- a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp @@ -36,7 +36,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { namespace bg = boost::geometry; using tier4_autoware_utils::LinearRing2d; @@ -207,4 +207,4 @@ bool isOutsideDrivableAreaFromRectangleFootprint( return false; } } // namespace geometry_utils -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp index 7983c5c2a3c2f..75e2b75b232e0 100644 --- a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp @@ -36,25 +36,25 @@ namespace tier4_autoware_utils { template <> -geometry_msgs::msg::Point getPoint(const autoware_path_optimizer::ReferencePoint & p) +geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p) { return p.pose.position; } template <> -geometry_msgs::msg::Pose getPose(const autoware_path_optimizer::ReferencePoint & p) +geometry_msgs::msg::Pose getPose(const autoware::path_optimizer::ReferencePoint & p) { return p.pose; } template <> -double getLongitudinalVelocity(const autoware_path_optimizer::ReferencePoint & p) +double getLongitudinalVelocity(const autoware::path_optimizer::ReferencePoint & p) { return p.longitudinal_velocity_mps; } } // namespace tier4_autoware_utils -namespace autoware_path_optimizer +namespace autoware::path_optimizer { namespace trajectory_utils { @@ -242,4 +242,4 @@ void insertStopPoint( traj_points.insert(traj_points.begin() + stop_seg_idx + 1, additional_traj_point); } } // namespace trajectory_utils -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp index 8ef099ba09f24..4af88539597be 100644 --- a/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp +++ b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp @@ -42,7 +42,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) planning_test_utils_dir + "/config/test_nearest_search.param.yaml", "--params-file", path_optimizer_dir + "/config/path_optimizer.param.yaml"}); - auto test_target_node = std::make_shared(node_options); + auto test_target_node = std::make_shared(node_options); // publish necessary topics from test_manager test_manager->publishOdometry(test_target_node, "path_optimizer/input/odometry"); diff --git a/planning/autoware_planning_test_manager/README.md b/planning/autoware_planning_test_manager/README.md index fe5484ca498ee..97eb2fd8d94fd 100644 --- a/planning/autoware_planning_test_manager/README.md +++ b/planning/autoware_planning_test_manager/README.md @@ -41,7 +41,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) node_options.arguments( {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", - planning_validator_dir + "/config/planning_validator.param.yaml"}); + autoware_planning_validator_dir + "/config/planning_validator.param.yaml"}); // instantiate the TargetNode with node_options auto test_target_node = std::make_shared(node_options); @@ -70,18 +70,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) ## Implemented tests -| Node | Test name | exceptional input | output | Exceptional input pattern | -| ------------------------- | ----------------------------------------------------------------------------------------- | ----------------- | -------------- | ------------------------------------------------------------------------------------- | -| planning_validator | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| velocity_smoother | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_cruise_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_stop_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_velocity_limiter | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| path_optimizer | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| scenario_selector | NodeTestWithExceptionTrajectoryLaneDrivingMode NodeTestWithExceptionTrajectoryParkingMode | trajectory | scenario | Empty, single point, path with duplicate points for scenarios:LANEDRIVING and PARKING | -| freespace_planner | NodeTestWithExceptionRoute | route | trajectory | Empty route | -| behavior_path_planner | NodeTestWithExceptionRoute NodeTestWithOffTrackEgoPose | route | route odometry | Empty route Off-lane ego-position | -| behavior_velocity_planner | NodeTestWithExceptionPathWithLaneID | path_with_lane_id | path | Empty path | +| Node | Test name | exceptional input | output | Exceptional input pattern | +| --------------------------- | ----------------------------------------------------------------------------------------- | ----------------- | -------------- | ------------------------------------------------------------------------------------- | +| autoware_planning_validator | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| velocity_smoother | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| obstacle_cruise_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| obstacle_stop_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| obstacle_velocity_limiter | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| path_optimizer | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| scenario_selector | NodeTestWithExceptionTrajectoryLaneDrivingMode NodeTestWithExceptionTrajectoryParkingMode | trajectory | scenario | Empty, single point, path with duplicate points for scenarios:LANEDRIVING and PARKING | +| freespace_planner | NodeTestWithExceptionRoute | route | trajectory | Empty route | +| behavior_path_planner | NodeTestWithExceptionRoute NodeTestWithOffTrackEgoPose | route | route odometry | Empty route Off-lane ego-position | +| behavior_velocity_planner | NodeTestWithExceptionPathWithLaneID | path_with_lane_id | path | Empty path | ## Important Notes diff --git a/planning/planning_topic_converter/CMakeLists.txt b/planning/autoware_planning_topic_converter/CMakeLists.txt similarity index 83% rename from planning/planning_topic_converter/CMakeLists.txt rename to planning/autoware_planning_topic_converter/CMakeLists.txt index 75b63d26c316f..ae1b585895e5e 100644 --- a/planning/planning_topic_converter/CMakeLists.txt +++ b/planning/autoware_planning_topic_converter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(planning_topic_converter) +project(autoware_planning_topic_converter) ### Compile options if(NOT CMAKE_CXX_STANDARD) @@ -17,7 +17,7 @@ ament_auto_add_library(planning_topic_converter SHARED ) rclcpp_components_register_node(planning_topic_converter - PLUGIN "planning_topic_converter::PathToTrajectory" + PLUGIN "autoware::planning_topic_converter::PathToTrajectory" EXECUTABLE path_to_trajectory_converter ) diff --git a/planning/planning_topic_converter/README.md b/planning/autoware_planning_topic_converter/README.md similarity index 86% rename from planning/planning_topic_converter/README.md rename to planning/autoware_planning_topic_converter/README.md index 74137cda0197d..e5036c6c0903c 100644 --- a/planning/planning_topic_converter/README.md +++ b/planning/autoware_planning_topic_converter/README.md @@ -12,7 +12,7 @@ The tools in this package are provided as composable ROS 2 component nodes, so t ```xml - + diff --git a/planning/planning_topic_converter/include/planning_topic_converter/converter_base.hpp b/planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/converter_base.hpp similarity index 83% rename from planning/planning_topic_converter/include/planning_topic_converter/converter_base.hpp rename to planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/converter_base.hpp index a093d9a952029..e3b0fe5069e92 100644 --- a/planning/planning_topic_converter/include/planning_topic_converter/converter_base.hpp +++ b/planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/converter_base.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ -#define PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ +#ifndef AUTOWARE_PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ +#define AUTOWARE_PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ #include "rclcpp/rclcpp.hpp" #include #include -namespace planning_topic_converter +namespace autoware::planning_topic_converter { template @@ -46,6 +46,6 @@ class ConverterBase : public rclcpp::Node private: }; -} // namespace planning_topic_converter +} // namespace autoware::planning_topic_converter -#endif // PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ +#endif // AUTOWARE_PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ diff --git a/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp b/planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/path_to_trajectory.hpp similarity index 75% rename from planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp rename to planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/path_to_trajectory.hpp index 708f624e97898..7ca50d6c4e50c 100644 --- a/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp +++ b/planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/path_to_trajectory.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ -#define PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ +#ifndef AUTOWARE_PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ +#define AUTOWARE_PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ -#include "planning_topic_converter/converter_base.hpp" +#include "autoware_planning_topic_converter/converter_base.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -23,7 +23,7 @@ #include -namespace planning_topic_converter +namespace autoware::planning_topic_converter { using autoware_planning_msgs::msg::Path; @@ -40,6 +40,6 @@ class PathToTrajectory : public ConverterBase void process(const Path::ConstSharedPtr msg) override; }; -} // namespace planning_topic_converter +} // namespace autoware::planning_topic_converter -#endif // PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ +#endif // AUTOWARE_PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ diff --git a/planning/planning_topic_converter/package.xml b/planning/autoware_planning_topic_converter/package.xml similarity index 86% rename from planning/planning_topic_converter/package.xml rename to planning/autoware_planning_topic_converter/package.xml index 3699f3e53900c..be44523fa9852 100644 --- a/planning/planning_topic_converter/package.xml +++ b/planning/autoware_planning_topic_converter/package.xml @@ -1,8 +1,8 @@ - planning_topic_converter + autoware_planning_topic_converter 0.1.0 - The planning_topic_converter package + The autoware_planning_topic_converter package Satoshi OTA Shumpei Wakabayashi diff --git a/planning/planning_topic_converter/src/path_to_trajectory.cpp b/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp similarity index 88% rename from planning/planning_topic_converter/src/path_to_trajectory.cpp rename to planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp index 757dcd0d9f632..d7aceb1abd106 100644 --- a/planning/planning_topic_converter/src/path_to_trajectory.cpp +++ b/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_topic_converter/path_to_trajectory.hpp" +#include "autoware_planning_topic_converter/path_to_trajectory.hpp" #include #include -namespace planning_topic_converter +namespace autoware::planning_topic_converter { namespace { @@ -54,7 +54,7 @@ void PathToTrajectory::process(const Path::ConstSharedPtr msg) pub_->publish(output); } -} // namespace planning_topic_converter +} // namespace autoware::planning_topic_converter #include -RCLCPP_COMPONENTS_REGISTER_NODE(planning_topic_converter::PathToTrajectory) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::planning_topic_converter::PathToTrajectory) diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp index 9eb9a2b432a21..0705f0b51ddd3 100644 --- a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -130,7 +130,7 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra const auto eb_path_smoother_ptr = path_smoother::ElasticBandSmoother(create_node_options()).getElasticBandSmoother(); const auto mpt_optimizer_ptr = - autoware_path_optimizer::PathOptimizer(create_node_options()).getMPTOptimizer(); + autoware::path_optimizer::PathOptimizer(create_node_options()).getMPTOptimizer(); // NOTE: The optimization is executed every valid_optimized_traj_points_num points. constexpr int valid_optimized_traj_points_num = 10; @@ -158,7 +158,7 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra // road collision avoidance by model predictive trajectory in the autoware_path_optimizer // package - const autoware_path_optimizer::PlannerData planner_data{ + const autoware::path_optimizer::PlannerData planner_data{ raw_path.header, smoothed_traj_points, raw_path.left_bound, raw_path.right_bound, virtual_ego_pose}; const auto optimized_traj_points = mpt_optimizer_ptr->optimizeTrajectory(planner_data); diff --git a/planning/surround_obstacle_checker/CMakeLists.txt b/planning/autoware_surround_obstacle_checker/CMakeLists.txt similarity index 82% rename from planning/surround_obstacle_checker/CMakeLists.txt rename to planning/autoware_surround_obstacle_checker/CMakeLists.txt index 0d7b636646783..141b2c7ce4aa2 100644 --- a/planning/surround_obstacle_checker/CMakeLists.txt +++ b/planning/autoware_surround_obstacle_checker/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(surround_obstacle_checker) +project(autoware_surround_obstacle_checker) find_package(autoware_cmake REQUIRED) autoware_package() @@ -23,7 +23,7 @@ target_link_libraries(${PROJECT_NAME} ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "surround_obstacle_checker::SurroundObstacleCheckerNode" + PLUGIN "autoware::surround_obstacle_checker::SurroundObstacleCheckerNode" EXECUTABLE ${PROJECT_NAME}_node ) diff --git a/planning/surround_obstacle_checker/README.md b/planning/autoware_surround_obstacle_checker/README.md similarity index 98% rename from planning/surround_obstacle_checker/README.md rename to planning/autoware_surround_obstacle_checker/README.md index 5cf7c8a419b61..1f4bf77145624 100644 --- a/planning/surround_obstacle_checker/README.md +++ b/planning/autoware_surround_obstacle_checker/README.md @@ -101,7 +101,7 @@ As mentioned in stop condition section, it prevents chattering by changing thres ## Parameters -{{ json_to_markdown("planning/surround_obstacle_checker/schema/surround_obstacle_checker.schema.json") }} +{{ json_to_markdown("planning/autoware_surround_obstacle_checker/schema/surround_obstacle_checker.schema.json") }} | Name | Type | Description | Default value | | :----------------------------------- | :------- | :----------------------------------------------------------------------------------------------------------------------------------------------- | :------------------------------------------- | diff --git a/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml b/planning/autoware_surround_obstacle_checker/config/surround_obstacle_checker.param.yaml similarity index 100% rename from planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml rename to planning/autoware_surround_obstacle_checker/config/surround_obstacle_checker.param.yaml diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp b/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/debug_marker.hpp similarity index 93% rename from planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp rename to planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/debug_marker.hpp index 07a23a4ac2e67..d2e055bbccaa2 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp +++ b/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/debug_marker.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ -#define SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ +#ifndef AUTOWARE_SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ +#define AUTOWARE_SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ #include #include @@ -31,7 +31,7 @@ #include #include -namespace surround_obstacle_checker +namespace autoware::surround_obstacle_checker { using autoware_adapi_v1_msgs::msg::PlanningBehavior; @@ -98,5 +98,5 @@ class SurroundObstacleCheckerDebugNode std::shared_ptr stop_pose_ptr_; rclcpp::Clock::SharedPtr clock_; }; -} // namespace surround_obstacle_checker -#endif // SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ +} // namespace autoware::surround_obstacle_checker +#endif // AUTOWARE_SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/node.hpp similarity index 94% rename from planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp rename to planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/node.hpp index d5b2c49df51cb..4ccd097b40837 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/node.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SURROUND_OBSTACLE_CHECKER__NODE_HPP_ -#define SURROUND_OBSTACLE_CHECKER__NODE_HPP_ +#ifndef AUTOWARE_SURROUND_OBSTACLE_CHECKER__NODE_HPP_ +#define AUTOWARE_SURROUND_OBSTACLE_CHECKER__NODE_HPP_ -#include "surround_obstacle_checker/debug_marker.hpp" +#include "autoware_surround_obstacle_checker/debug_marker.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/polling_subscriber.hpp" @@ -43,7 +43,7 @@ #include #include -namespace surround_obstacle_checker +namespace autoware::surround_obstacle_checker { using autoware_perception_msgs::msg::PredictedObjects; @@ -149,6 +149,6 @@ class SurroundObstacleCheckerNode : public rclcpp::Node std::unordered_map label_map_; }; -} // namespace surround_obstacle_checker +} // namespace autoware::surround_obstacle_checker -#endif // SURROUND_OBSTACLE_CHECKER__NODE_HPP_ +#endif // AUTOWARE_SURROUND_OBSTACLE_CHECKER__NODE_HPP_ diff --git a/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml b/planning/autoware_surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml similarity index 80% rename from planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml rename to planning/autoware_surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml index 53f26a05c87c3..0ba7d29090916 100644 --- a/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml +++ b/planning/autoware_surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml @@ -1,5 +1,5 @@ - + @@ -7,7 +7,7 @@ - + diff --git a/planning/surround_obstacle_checker/media/check_distance.drawio.svg b/planning/autoware_surround_obstacle_checker/media/check_distance.drawio.svg similarity index 100% rename from planning/surround_obstacle_checker/media/check_distance.drawio.svg rename to planning/autoware_surround_obstacle_checker/media/check_distance.drawio.svg diff --git a/planning/surround_obstacle_checker/package.xml b/planning/autoware_surround_obstacle_checker/package.xml similarity index 91% rename from planning/surround_obstacle_checker/package.xml rename to planning/autoware_surround_obstacle_checker/package.xml index a1ef2c408ce3b..3bb961766e9f3 100644 --- a/planning/surround_obstacle_checker/package.xml +++ b/planning/autoware_surround_obstacle_checker/package.xml @@ -1,9 +1,9 @@ - surround_obstacle_checker + autoware_surround_obstacle_checker 0.1.0 - The surround_obstacle_checker package + The autoware_surround_obstacle_checker package Satoshi Ota Go Sakayori Apache License 2.0 diff --git a/planning/surround_obstacle_checker/schema/surround_obstacle_checker.schema.json b/planning/autoware_surround_obstacle_checker/schema/surround_obstacle_checker.schema.json similarity index 99% rename from planning/surround_obstacle_checker/schema/surround_obstacle_checker.schema.json rename to planning/autoware_surround_obstacle_checker/schema/surround_obstacle_checker.schema.json index 00a8ae6a12991..85458fb7cdbc6 100644 --- a/planning/surround_obstacle_checker/schema/surround_obstacle_checker.schema.json +++ b/planning/autoware_surround_obstacle_checker/schema/surround_obstacle_checker.schema.json @@ -3,7 +3,7 @@ "title": "Parameters for Surround Obstacle Checker Node", "type": "object", "definitions": { - "surround_obstacle_checker": { + "autoware_surround_obstacle_checker": { "type": "object", "properties": { "pointcloud": { @@ -385,7 +385,7 @@ "type": "object", "properties": { "ros__parameters": { - "$ref": "#/definitions/surround_obstacle_checker" + "$ref": "#/definitions/autoware_surround_obstacle_checker" } }, "required": ["ros__parameters"], diff --git a/planning/surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp similarity index 98% rename from planning/surround_obstacle_checker/src/debug_marker.cpp rename to planning/autoware_surround_obstacle_checker/src/debug_marker.cpp index a0ca15d490e9c..ba561a4b8cf35 100644 --- a/planning/surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "surround_obstacle_checker/debug_marker.hpp" +#include "autoware_surround_obstacle_checker/debug_marker.hpp" #include #include @@ -25,7 +25,7 @@ #include -namespace surround_obstacle_checker +namespace autoware::surround_obstacle_checker { namespace { @@ -245,4 +245,4 @@ void SurroundObstacleCheckerDebugNode::updateFootprintMargin( surround_check_back_distance_ = back_distance; } -} // namespace surround_obstacle_checker +} // namespace autoware::surround_obstacle_checker diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/autoware_surround_obstacle_checker/src/node.cpp similarity index 98% rename from planning/surround_obstacle_checker/src/node.cpp rename to planning/autoware_surround_obstacle_checker/src/node.cpp index af6dba8fcc0c9..0744ebecd370c 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "surround_obstacle_checker/node.hpp" +#include "autoware_surround_obstacle_checker/node.hpp" #include #include @@ -46,7 +46,7 @@ #include #include -namespace surround_obstacle_checker +namespace autoware::surround_obstacle_checker { namespace bg = boost::geometry; using Point2d = bg::model::d2::point_xy; @@ -541,7 +541,7 @@ bool SurroundObstacleCheckerNode::isStopRequired( return false; } -} // namespace surround_obstacle_checker +} // namespace autoware::surround_obstacle_checker #include -RCLCPP_COMPONENTS_REGISTER_NODE(surround_obstacle_checker::SurroundObstacleCheckerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::surround_obstacle_checker::SurroundObstacleCheckerNode) diff --git a/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md b/planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja.md similarity index 100% rename from planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md rename to planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja.md diff --git a/planning/autoware_velocity_smoother/CMakeLists.txt b/planning/autoware_velocity_smoother/CMakeLists.txt index fee13513a7a97..9dd0c62c4537e 100644 --- a/planning/autoware_velocity_smoother/CMakeLists.txt +++ b/planning/autoware_velocity_smoother/CMakeLists.txt @@ -41,7 +41,7 @@ target_link_libraries(${PROJECT_NAME}_node ) rclcpp_components_register_node(${PROJECT_NAME}_node - PLUGIN "autoware_velocity_smoother::VelocitySmootherNode" + PLUGIN "autoware::velocity_smoother::VelocitySmootherNode" EXECUTABLE velocity_smoother_node ) 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 aecfab342e7e0..8ab7acd5ec9b3 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp @@ -53,7 +53,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -275,6 +275,6 @@ class VelocitySmootherNode : public rclcpp::Node std::unique_ptr logger_configure_; std::unique_ptr published_time_publisher_; }; -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #endif // AUTOWARE_VELOCITY_SMOOTHER__NODE_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp index dc85c2b2f336f..24a20ca8a588f 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp @@ -20,7 +20,7 @@ #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { namespace resampling { @@ -48,6 +48,6 @@ TrajectoryPoints resampleTrajectory( const double nearest_dist_threshold, const double nearest_yaw_threshold, const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v = true); } // namespace resampling -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #endif // AUTOWARE_VELOCITY_SMOOTHER__RESAMPLE_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index e54a6e7b8afad..a5baa59f5d034 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -28,7 +28,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { class AnalyticalJerkConstrainedSmoother : public SmootherBase { @@ -113,7 +113,7 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase std::string strTimes(const std::vector & times) const; std::string strStartIndices(const std::vector & start_indices) const; }; -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother // clang-format off #endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp index 3ab4fce11fd0b..0c18e102f6f66 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp @@ -25,7 +25,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { namespace analytical_velocity_planning_utils { @@ -51,7 +51,7 @@ double integ_x(double x0, double v0, double a0, double j0, double t); double integ_v(double v0, double a0, double j0, double t); double integ_a(double a0, double j0, double t); } // namespace analytical_velocity_planning_utils -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother // clang-format off #endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp index 09c1e7e96be7b..4e93fcd339140 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -26,7 +26,7 @@ #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { class JerkFilteredSmoother : public SmootherBase { @@ -69,6 +69,6 @@ class JerkFilteredSmoother : public SmootherBase const double v0, const double a0, const double a_min, const double j_min, const TrajectoryPoints & forward_filtered, const TrajectoryPoints & backward_filtered) const; }; -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ 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 066e0acef67f6..44fa263e71fb9 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 @@ -26,7 +26,7 @@ #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { class L2PseudoJerkSmoother : public SmootherBase { @@ -56,6 +56,6 @@ class L2PseudoJerkSmoother : public SmootherBase autoware::common::osqp::OSQPInterface qp_solver_; rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("l2_pseudo_jerk_smoother")}; }; -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ 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 b54de318e9702..f638e03658e05 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 @@ -26,7 +26,7 @@ #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { class LinfPseudoJerkSmoother : public SmootherBase { @@ -56,6 +56,6 @@ class LinfPseudoJerkSmoother : public SmootherBase autoware::common::osqp::OSQPInterface qp_solver_; rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("linf_pseudo_jerk_smoother")}; }; -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp index 43ccfcf193f14..0c17ef5bdbb9d 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp @@ -23,7 +23,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; @@ -86,6 +86,6 @@ class SmootherBase protected: BaseParam base_param_; }; -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp index d35f80fb7fad8..f7999d1c8b04c 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp @@ -23,7 +23,7 @@ #include #include -namespace autoware_velocity_smoother::trajectory_utils +namespace autoware::velocity_smoother::trajectory_utils { using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; @@ -77,6 +77,6 @@ std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit( double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closest); -} // namespace autoware_velocity_smoother::trajectory_utils +} // namespace autoware::velocity_smoother::trajectory_utils #endif // AUTOWARE_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 0ea10534eb126..0fd83871153f0 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -32,7 +32,7 @@ #include // clang-format on -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_options) : Node("velocity_smoother", node_options) @@ -1099,7 +1099,7 @@ TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPointFromEgo( return calcProjectedTrajectoryPoint(trajectory, current_odometry_ptr_->pose.pose); } -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(autoware_velocity_smoother::VelocitySmootherNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::velocity_smoother::VelocitySmootherNode) diff --git a/planning/autoware_velocity_smoother/src/resample.cpp b/planning/autoware_velocity_smoother/src/resample.cpp index 17ea1eb9fc9bd..af3b222a71ef9 100644 --- a/planning/autoware_velocity_smoother/src/resample.cpp +++ b/planning/autoware_velocity_smoother/src/resample.cpp @@ -22,7 +22,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { namespace resampling { @@ -264,4 +264,4 @@ TrajectoryPoints resampleTrajectory( } } // namespace resampling -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index 9cdfc515d51b7..dc5f4f19ce771 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -64,7 +64,7 @@ bool applyMaxVelocity( } // namespace -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(rclcpp::Node & node) : SmootherBase(node) @@ -658,4 +658,4 @@ std::string AnalyticalJerkConstrainedSmoother::strStartIndices( return ss.str(); } -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index 84ac29d14bdc5..e3edcfbbec681 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -19,7 +19,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { namespace analytical_velocity_planning_utils { @@ -353,4 +353,4 @@ double integ_a(double a0, double j0, double t) } } // namespace analytical_velocity_planning_utils -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index d212cfc0bcedf..d759930eb4320 100644 --- a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -27,7 +27,7 @@ #define VERBOSE_TRAJECTORY_VELOCITY false -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { JerkFilteredSmoother::JerkFilteredSmoother(rclcpp::Node & node) : SmootherBase(node) { @@ -480,4 +480,4 @@ TrajectoryPoints JerkFilteredSmoother::resampleTrajectory( smoother_param_.jerk_filter_ds); } -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index 3c2872f2e58e3..fff1166567aee 100644 --- a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -23,7 +23,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { L2PseudoJerkSmoother::L2PseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) { @@ -242,4 +242,4 @@ TrajectoryPoints L2PseudoJerkSmoother::resampleTrajectory( base_param_.resample_param); } -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index f434d8d4514ca..b00936f3092ad 100644 --- a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -23,7 +23,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { LinfPseudoJerkSmoother::LinfPseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) { @@ -254,4 +254,4 @@ TrajectoryPoints LinfPseudoJerkSmoother::resampleTrajectory( base_param_.resample_param); } -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp index 5704113067244..e1d72889203cc 100644 --- a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp @@ -26,7 +26,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { namespace @@ -270,4 +270,4 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit( return output; } -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp index aff1a0b0e3213..6ebf246748666 100644 --- a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp @@ -25,7 +25,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { using geometry_msgs::msg::Point; namespace trajectory_utils @@ -610,4 +610,4 @@ double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closes } } // namespace trajectory_utils -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp index 51ee84f535ca4..f1b7dbb197ea2 100644 --- a/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp +++ b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp @@ -18,8 +18,8 @@ #include +using autoware::velocity_smoother::trajectory_utils::TrajectoryPoints; using autoware_planning_msgs::msg::TrajectoryPoint; -using autoware_velocity_smoother::trajectory_utils::TrajectoryPoints; TrajectoryPoints genStraightTrajectory(const size_t size) { @@ -48,7 +48,7 @@ TEST(TestTrajectoryUtils, CalcTrajectoryCurvatureFrom3Points) const auto checkOutputSize = [](const size_t trajectory_size, const size_t idx_dist) { const auto trajectory_points = genStraightTrajectory(trajectory_size); const auto curvatures = - autoware_velocity_smoother::trajectory_utils::calcTrajectoryCurvatureFrom3Points( + autoware::velocity_smoother::trajectory_utils::calcTrajectoryCurvatureFrom3Points( trajectory_points, idx_dist); EXPECT_EQ(curvatures.size(), trajectory_size) << ", idx_dist = " << idx_dist; }; diff --git a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp index f0145ea5a32a7..30a64955366e4 100644 --- a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp +++ b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp @@ -22,7 +22,7 @@ #include -using autoware_velocity_smoother::VelocitySmootherNode; +using autoware::velocity_smoother::VelocitySmootherNode; using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp index 0009fbccdb5d8..903655b64e347 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp @@ -18,7 +18,7 @@ #include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" -#include +#include #include @@ -27,7 +27,7 @@ namespace behavior_path_planner { -using lane_departure_checker::LaneDepartureChecker; +using autoware::lane_departure_checker::LaneDepartureChecker; class GeometricPullOver : public PullOverPlannerBase { public: diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 8938e48e0ceb5..b7c4a24e52974 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -28,9 +28,9 @@ #include "behavior_path_goal_planner_module/goal_searcher.hpp" #include "behavior_path_goal_planner_module/shift_pull_over.hpp" +#include #include #include -#include #include #include @@ -50,9 +50,9 @@ namespace behavior_path_planner { +using autoware::lane_departure_checker::LaneDepartureChecker; using autoware_vehicle_msgs::msg::HazardLightsCommand; using geometry_msgs::msg::PoseArray; -using lane_departure_checker::LaneDepartureChecker; using nav_msgs::msg::OccupancyGrid; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp index 635816f17497f..e7b297548083a 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp @@ -17,7 +17,7 @@ #include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" -#include +#include #include @@ -26,7 +26,7 @@ namespace behavior_path_planner { -using lane_departure_checker::LaneDepartureChecker; +using autoware::lane_departure_checker::LaneDepartureChecker; class ShiftPullOver : public PullOverPlannerBase { diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp index 83e817a8d5a45..d6c9215e495ae 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp @@ -18,7 +18,7 @@ #include "behavior_path_goal_planner_module/goal_searcher_base.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include +#include #include "visualization_msgs/msg/detail/marker_array__struct.hpp" #include diff --git a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp index efd9eda322585..b56ae44dcf476 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -32,7 +32,7 @@ namespace behavior_path_planner { -using lane_departure_checker::LaneDepartureChecker; +using autoware::lane_departure_checker::LaneDepartureChecker; using lanelet::autoware::NoParkingArea; using lanelet::autoware::NoStoppingArea; using tier4_autoware_utils::calcOffsetPose; diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 2e79589c9f1bd..70190731ac1a7 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -33,8 +33,8 @@ Behavior Path Planner has following scene modules | Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](../behavior_path_lane_change_module/README.md) | | External Lane Change | WIP | LINK | | Goal Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](../behavior_path_goal_planner_module/README.md) | -| Start Planner | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](../behavior_path_start_planner_module/README.md) | -| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](../behavior_path_side_shift_module/README.md) | +| Start Planner | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](../autoware_behavior_path_start_planner_module/README.md) | +| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](../autoware_behavior_path_side_shift_module/README.md) | !!! Note diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index e4fe3d0e9d3e4..f51fa99578d7b 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -38,23 +38,23 @@ autoware_adapi_v1_msgs autoware_behavior_path_planner_common + autoware_frenet_planner + autoware_lane_departure_checker + autoware_path_sampler autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager autoware_vehicle_msgs behaviortree_cpp_v3 freespace_planning_algorithms - frenet_planner geometry_msgs interpolation - lane_departure_checker lanelet2_extension libboost-dev libopencv-dev magic_enum motion_utils object_recognition_utils - path_sampler pluginlib rclcpp rclcpp_components diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp index 3f3d144952d8e..0a5dec8d07b4f 100644 --- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp +++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp @@ -21,19 +21,19 @@ #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 "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware_bezier_sampler/bezier_sampling.hpp" +#include "autoware_frenet_planner/frenet_planner.hpp" +#include "autoware_sampler_common/constraints/footprint.hpp" +#include "autoware_sampler_common/constraints/hard_constraint.hpp" +#include "autoware_sampler_common/constraints/soft_constraint.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" #include "behavior_path_sampling_planner_module/sampling_planner_parameters.hpp" #include "behavior_path_sampling_planner_module/util.hpp" -#include "bezier_sampler/bezier_sampling.hpp" -#include "frenet_planner/frenet_planner.hpp" #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "rclcpp/rclcpp.hpp" -#include "sampler_common/constraints/footprint.hpp" -#include "sampler_common/constraints/hard_constraint.hpp" -#include "sampler_common/constraints/soft_constraint.hpp" -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/math/constants.hpp" @@ -73,7 +73,7 @@ struct SamplingPlannerData struct SamplingPlannerDebugData { - std::vector sampled_candidates{}; + std::vector sampled_candidates{}; size_t previous_sampled_candidates_nb = 0UL; std::vector obstacles{}; std::vector footprints{}; @@ -231,25 +231,25 @@ class SamplingPlannerModule : public SceneModuleInterface void updateDebugMarkers(); void prepareConstraints( - sampler_common::Constraints & constraints, + autoware::sampler_common::Constraints & constraints, const PredictedObjects::ConstSharedPtr & predicted_objects, const std::vector & left_bound, const std::vector & right_bound) const; - frenet_planner::SamplingParameters prepareSamplingParameters( - const sampler_common::State & initial_state, - const sampler_common::transform::Spline2D & path_spline, + autoware::frenet_planner::SamplingParameters prepareSamplingParameters( + const autoware::sampler_common::State & initial_state, + const autoware::sampler_common::transform::Spline2D & path_spline, const SamplingPlannerInternalParameters & internal_params_); PathWithLaneId convertFrenetPathToPathWithLaneID( - const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets, + const autoware::frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets, const double path_z); // member // std::shared_ptr params_; std::shared_ptr internal_params_; vehicle_info_util::VehicleInfo vehicle_info_{}; - std::optional prev_sampling_path_ = std::nullopt; + std::optional prev_sampling_path_ = std::nullopt; // move to utils void extendOutputDrivableArea( diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp index 6e00fefe89574..4ddfbf436ffaf 100644 --- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp +++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp @@ -15,8 +15,8 @@ #ifndef BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ #define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ -#include "bezier_sampler/bezier_sampling.hpp" -#include "sampler_common/structures.hpp" +#include "autoware_bezier_sampler/bezier_sampling.hpp" +#include "autoware_sampler_common/structures.hpp" #include namespace behavior_path_planner @@ -78,12 +78,12 @@ struct Sampling std::vector target_lateral_positions{}; int nb_target_lateral_positions{}; Frenet frenet; - bezier_sampler::SamplingParameters bezier{}; + autoware::bezier_sampler::SamplingParameters bezier{}; }; struct SamplingPlannerInternalParameters { - sampler_common::Constraints constraints; + autoware::sampler_common::Constraints constraints; Sampling sampling; Preprocessing preprocessing{}; }; diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp index ac8ba788cb9bd..2825ead2a9fee 100644 --- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp +++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp @@ -14,8 +14,8 @@ #ifndef BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_ #define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_ -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" #include @@ -40,18 +40,20 @@ struct SoftConstraintsInputs lanelet::ConstLanelets closest_lanelets_to_goal; behavior_path_planner::PlanResult reference_path; behavior_path_planner::PlanResult prev_module_path; - std::optional prev_path; + std::optional prev_path; lanelet::ConstLanelets current_lanes; }; using SoftConstraintsFunctionVector = std::vector>; + autoware::sampler_common::Path &, const autoware::sampler_common::Constraints &, + const SoftConstraintsInputs &)>>; using HardConstraintsFunctionVector = std::vector>; + autoware::sampler_common::Path &, const autoware::sampler_common::Constraints &, + const MultiPoint2d &)>>; inline std::vector evaluateSoftConstraints( - sampler_common::Path & path, const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, const autoware::sampler_common::Constraints & constraints, const SoftConstraintsFunctionVector & soft_constraints_functions, const SoftConstraintsInputs & input_data) { @@ -71,7 +73,7 @@ inline std::vector evaluateSoftConstraints( } inline std::vector evaluateHardConstraints( - sampler_common::Path & path, const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, const autoware::sampler_common::Constraints & constraints, const MultiPoint2d & footprint, const HardConstraintsFunctionVector & hard_constraints) { std::vector constraints_results; @@ -86,11 +88,11 @@ inline std::vector evaluateHardConstraints( return constraints_results; } -inline sampler_common::State getInitialState( +inline autoware::sampler_common::State getInitialState( const geometry_msgs::msg::Pose & pose, - const sampler_common::transform::Spline2D & reference_spline) + const autoware::sampler_common::transform::Spline2D & reference_spline) { - sampler_common::State initial_state; + autoware::sampler_common::State initial_state; Point2d initial_state_pose{pose.position.x, pose.position.y}; const auto rpy = tier4_autoware_utils::getRPY(pose.orientation); initial_state.pose = initial_state_pose; diff --git a/planning/behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_sampling_planner_module/package.xml index e6fb0d4d0f573..0583056db4293 100644 --- a/planning/behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_sampling_planner_module/package.xml @@ -14,17 +14,17 @@ autoware_cmake autoware_behavior_path_planner_common + autoware_bezier_sampler + autoware_frenet_planner + autoware_path_sampler autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager autoware_vehicle_msgs - bezier_sampler - frenet_planner geometry_msgs interpolation lanelet2_extension motion_utils - path_sampler pluginlib rclcpp rclcpp_components diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index 4b0ecdcd24e14..22187652bd663 100644 --- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -37,21 +37,22 @@ SamplingPlannerModule::SamplingPlannerModule( : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} { - internal_params_ = - std::shared_ptr(new SamplingPlannerInternalParameters{}); + internal_params_ = std::make_shared(); updateModuleParams(parameters); // check if the path is empty hard_constraints_.emplace_back( []( - sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + [[maybe_unused]] const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const MultiPoint2d & footprint) -> bool { return !path.points.empty() && !path.poses.empty(); }); hard_constraints_.emplace_back( []( - sampler_common::Path & path, const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + const autoware::sampler_common::Constraints & constraints, const MultiPoint2d & footprint) -> bool { if (!footprint.empty()) { path.constraint_results.inside_drivable_area = @@ -71,7 +72,8 @@ SamplingPlannerModule::SamplingPlannerModule( hard_constraints_.emplace_back( []( - sampler_common::Path & path, const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const MultiPoint2d & footprint) -> bool { if (path.curvatures.empty()) { path.constraint_results.valid_curvature = false; @@ -96,8 +98,8 @@ SamplingPlannerModule::SamplingPlannerModule( // Yaw difference soft constraint cost -> Considering implementation // soft_constraints_.emplace_back( // [&]( - // sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & - // constraints, + // autoware::sampler_common::Path & path, [[maybe_unused]] const + // autoware::sampler_common::Constraints & constraints, // [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { // if (path.points.empty()) return 0.0; // const auto & goal_pose_yaw = @@ -109,7 +111,8 @@ SamplingPlannerModule::SamplingPlannerModule( // Remaining path length soft_constraints_.emplace_back( [&]( - sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + [[maybe_unused]] const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { if (path.points.empty()) return 0.0; if (path.poses.empty()) return 0.0; @@ -142,8 +145,8 @@ SamplingPlannerModule::SamplingPlannerModule( // Distance to centerline soft_constraints_.emplace_back( [&]( - [[maybe_unused]] sampler_common::Path & path, - [[maybe_unused]] const sampler_common::Constraints & constraints, + [[maybe_unused]] autoware::sampler_common::Path & path, + [[maybe_unused]] const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { if (path.poses.empty()) return 0.0; const auto & last_pose = path.poses.back(); @@ -159,7 +162,8 @@ SamplingPlannerModule::SamplingPlannerModule( // // Curvature cost soft_constraints_.emplace_back( []( - sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + [[maybe_unused]] const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { if (path.curvatures.empty()) return std::numeric_limits::max(); @@ -244,7 +248,7 @@ bool SamplingPlannerModule::isReferencePathSafe() const std::vector hard_constraints_results; auto transform_to_sampling_path = [](const PlanResult plan) { - sampler_common::Path path; + autoware::sampler_common::Path path; for (size_t i = 0; i < plan->points.size(); ++i) { const auto x = plan->points[i].point.pose.position.x; const auto y = plan->points[i].point.pose.position.y; @@ -258,17 +262,20 @@ bool SamplingPlannerModule::isReferencePathSafe() const } return path; }; - sampler_common::Path reference_path = transform_to_sampling_path(prev_module_reference_path); - const auto footprint = sampler_common::constraints::buildFootprintPoints( + autoware::sampler_common::Path reference_path = + transform_to_sampling_path(prev_module_reference_path); + const auto footprint = autoware::sampler_common::constraints::buildFootprintPoints( reference_path, internal_params_->constraints); behavior_path_planner::HardConstraintsFunctionVector hard_constraints_reference_path; hard_constraints_reference_path.emplace_back( []( - sampler_common::Path & path, const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + const autoware::sampler_common::Constraints & constraints, const MultiPoint2d & footprint) -> bool { path.constraint_results.collision_free = - !sampler_common::constraints::has_collision(footprint, constraints.obstacle_polygons); + !autoware::sampler_common::constraints::has_collision( + footprint, constraints.obstacle_polygons); return path.constraint_results.collision_free; }); evaluateHardConstraints( @@ -295,7 +302,7 @@ SamplingPlannerData SamplingPlannerModule::createPlannerData( } PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID( - const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets, + const autoware::frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets, const double path_z) { auto quaternion_from_rpy = [](double roll, double pitch, double yaw) -> tf2::Quaternion { @@ -348,12 +355,12 @@ PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID( } void SamplingPlannerModule::prepareConstraints( - sampler_common::Constraints & constraints, + autoware::sampler_common::Constraints & constraints, const PredictedObjects::ConstSharedPtr & predicted_objects, const std::vector & left_bound, const std::vector & right_bound) const { - constraints.obstacle_polygons = sampler_common::MultiPolygon2d(); + constraints.obstacle_polygons = autoware::sampler_common::MultiPolygon2d(); constraints.rtree.clear(); size_t i = 0; for (const auto & o : predicted_objects->objects) { @@ -370,7 +377,7 @@ void SamplingPlannerModule::prepareConstraints( // TODO(Maxime): directly use lines instead of polygon - sampler_common::Polygon2d drivable_area_polygon; + autoware::sampler_common::Polygon2d drivable_area_polygon; for (const auto & p : right_bound) { drivable_area_polygon.outer().emplace_back(p.x, p.y); } @@ -392,7 +399,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan() if (reference_path_ptr->points.empty()) { return {}; } - auto reference_spline = [&]() -> sampler_common::transform::Spline2D { + auto reference_spline = [&]() -> autoware::sampler_common::transform::Spline2D { std::vector x; std::vector y; x.reserve(reference_path_ptr->points.size()); @@ -404,19 +411,19 @@ BehaviorModuleOutput SamplingPlannerModule::plan() return {x, y}; }(); - frenet_planner::FrenetState frenet_initial_state; - frenet_planner::SamplingParameters sampling_parameters; + autoware::frenet_planner::FrenetState frenet_initial_state; + autoware::frenet_planner::SamplingParameters sampling_parameters; const auto & pose = planner_data_->self_odometry->pose.pose; - sampler_common::State initial_state = + autoware::sampler_common::State initial_state = behavior_path_planner::getInitialState(pose, reference_spline); sampling_parameters = prepareSamplingParameters(initial_state, reference_spline, *internal_params_); auto set_frenet_state = []( - const sampler_common::State & initial_state, - const sampler_common::transform::Spline2D & reference_spline, - frenet_planner::FrenetState & frenet_initial_state) + const autoware::sampler_common::State & initial_state, + const autoware::sampler_common::transform::Spline2D & reference_spline, + autoware::frenet_planner::FrenetState & frenet_initial_state) { frenet_initial_state.position = initial_state.frenet; @@ -496,14 +503,16 @@ BehaviorModuleOutput SamplingPlannerModule::plan() const int path_divisions = internal_params_->sampling.previous_path_reuse_points_nb; const bool is_extend_previous_path = path_divisions > 0; - std::vector frenet_paths; + std::vector frenet_paths; // Extend prev path if (prev_path_is_valid && is_extend_previous_path) { - frenet_planner::Path prev_path_frenet = prev_sampling_path_.value(); + autoware::frenet_planner::Path prev_path_frenet = prev_sampling_path_.value(); frenet_paths.push_back(prev_path_frenet); - auto get_subset = [](const frenet_planner::Path & path, size_t offset) -> frenet_planner::Path { - frenet_planner::Path s; + auto get_subset = []( + const autoware::frenet_planner::Path & path, + size_t offset) -> autoware::frenet_planner::Path { + autoware::frenet_planner::Path s; s.points = {path.points.begin(), path.points.begin() + offset}; s.curvatures = {path.curvatures.begin(), path.curvatures.begin() + offset}; s.yaws = {path.yaws.begin(), path.yaws.begin() + offset}; @@ -512,7 +521,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan() return s; }; - sampler_common::State current_state; + autoware::sampler_common::State current_state; current_state.pose = {ego_pose.position.x, ego_pose.position.y}; const auto closest_iter = std::min_element( @@ -537,22 +546,22 @@ BehaviorModuleOutput SamplingPlannerModule::plan() const auto reused_path = get_subset(prev_path_frenet, reuse_idx); geometry_msgs::msg::Pose future_pose = reused_path.poses.back(); - sampler_common::State future_state = + autoware::sampler_common::State future_state = behavior_path_planner::getInitialState(future_pose, reference_spline); - frenet_planner::FrenetState frenet_reuse_state; + autoware::frenet_planner::FrenetState frenet_reuse_state; set_frenet_state(future_state, reference_spline, frenet_reuse_state); - frenet_planner::SamplingParameters extension_sampling_parameters = + autoware::frenet_planner::SamplingParameters extension_sampling_parameters = prepareSamplingParameters(future_state, reference_spline, *internal_params_); - auto extension_frenet_paths = frenet_planner::generatePaths( + auto extension_frenet_paths = autoware::frenet_planner::generatePaths( reference_spline, frenet_reuse_state, extension_sampling_parameters); for (auto & p : extension_frenet_paths) { if (!p.points.empty()) frenet_paths.push_back(reused_path.extend(p)); } } } else { - frenet_paths = - frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters); + frenet_paths = autoware::frenet_planner::generatePaths( + reference_spline, frenet_initial_state, sampling_parameters); } const auto path_for_calculating_bounds = getPreviousModuleOutput().reference_path; @@ -587,8 +596,8 @@ BehaviorModuleOutput SamplingPlannerModule::plan() std::vector> hard_constraints_results_full; std::vector> soft_constraints_results_full; for (auto & path : frenet_paths) { - const auto footprint = - sampler_common::constraints::buildFootprintPoints(path, internal_params_->constraints); + const auto footprint = autoware::sampler_common::constraints::buildFootprintPoints( + path, internal_params_->constraints); std::vector hard_constraints_results = evaluateHardConstraints(path, internal_params_->constraints, footprint, hard_constraints_); path.constraint_results.valid_curvature = true; @@ -598,7 +607,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan() soft_constraints_results_full.push_back(soft_constraints_results); } - std::vector candidate_paths; + std::vector candidate_paths; const auto move_to_paths = [&candidate_paths](auto & paths_to_move) { candidate_paths.insert( candidate_paths.end(), std::make_move_iterator(paths_to_move.begin()), @@ -917,9 +926,9 @@ DrivableLanes SamplingPlannerModule::generateExpandDrivableLanes( return current_drivable_lanes; } -frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParameters( - const sampler_common::State & initial_state, - const sampler_common::transform::Spline2D & path_spline, +autoware::frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParameters( + const autoware::sampler_common::State & initial_state, + const autoware::sampler_common::transform::Spline2D & path_spline, const SamplingPlannerInternalParameters & params_) { // calculate target lateral positions @@ -953,10 +962,10 @@ frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParamet } else { target_lateral_positions = params_.sampling.target_lateral_positions; } - frenet_planner::SamplingParameters sampling_parameters; + autoware::frenet_planner::SamplingParameters sampling_parameters; sampling_parameters.resolution = params_.sampling.resolution; const auto max_s = path_spline.lastS(); - frenet_planner::SamplingParameter p; + autoware::frenet_planner::SamplingParameter p; for (const auto target_length : params_.sampling.target_lengths) { p.target_state.position.s = std::min(max_s, path_spline.frenet(initial_state.pose).s + std::max(0.0, target_length)); diff --git a/planning/behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_blind_spot_module/package.xml index ebf760405a256..67947398afb01 100644 --- a/planning/behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_blind_spot_module/package.xml @@ -16,9 +16,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common geometry_msgs lanelet2_extension motion_utils diff --git a/planning/behavior_velocity_blind_spot_module/plugins.xml b/planning/behavior_velocity_blind_spot_module/plugins.xml index 7dda59ea2fdbe..fb5019de5df08 100644 --- a/planning/behavior_velocity_blind_spot_module/plugins.xml +++ b/planning/behavior_velocity_blind_spot_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp index a30836ab9f6a9..c357df481bca8 100644 --- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp @@ -14,8 +14,8 @@ #include "scene.hpp" -#include -#include +#include +#include #include #include @@ -25,7 +25,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createMarkerColor; @@ -107,4 +107,4 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray() return debug_marker_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_blind_spot_module/src/decisions.cpp index 6e4135c50b253..6b963d86c9bbf 100644 --- a/planning/behavior_velocity_blind_spot_module/src/decisions.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/decisions.cpp @@ -16,7 +16,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { /* @@ -150,4 +150,4 @@ void BlindSpotModule::reactRTCApprovalByDecision( return; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index f1b0a56018d9b..7f2f7ebd89baa 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -14,8 +14,8 @@ #include "manager.hpp" -#include -#include +#include +#include #include @@ -25,7 +25,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) @@ -95,8 +95,9 @@ BlindSpotModuleManager::getModuleExpiredFunction( }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::BlindSpotModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::BlindSpotModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.hpp b/planning/behavior_velocity_blind_spot_module/src/manager.hpp index 7d6f936cc9d5b..12dccd846affc 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC { @@ -49,6 +49,6 @@ class BlindSpotModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index 14ebe70216d5a..eed767fb891f9 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -14,9 +14,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include #include @@ -40,7 +40,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -704,4 +704,4 @@ bool BlindSpotModule::isTargetObjectType( return false; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index b3bf90f5b91d1..cfb6bfdfeeded 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -15,9 +15,9 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ -#include -#include -#include +#include +#include +#include #include #include @@ -34,7 +34,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { /** * @brief wrapper class of interpolated path with lane id @@ -237,6 +237,6 @@ class BlindSpotModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp index 690eca196d536..ca07636c2f60e 100644 --- a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp +++ b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp @@ -33,12 +33,12 @@ #define EIGEN_MPL2_ONLY #include #include -#include +#include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathPointWithLaneId; @@ -108,6 +108,6 @@ std::vector getLinestringIntersects( std::optional getStopLineFromMap( const lanelet::Id lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const std::string & attribute_name); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // BEHAVIOR_VELOCITY_CROSSWALK_MODULE__UTIL_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 2fd5e2bb514c3..eebf9c4d4a14f 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -21,9 +21,9 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs grid_map_core diff --git a/planning/behavior_velocity_crosswalk_module/plugins.xml b/planning/behavior_velocity_crosswalk_module/plugins.xml index 3d1d720857c17..e38ab0d85a63a 100644 --- a/planning/behavior_velocity_crosswalk_module/plugins.xml +++ b/planning/behavior_velocity_crosswalk_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_crosswalk_module/src/debug.cpp b/planning/behavior_velocity_crosswalk_module/src/debug.cpp index 4bee98db2fa56..1418d46081fcd 100644 --- a/planning/behavior_velocity_crosswalk_module/src/debug.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/debug.cpp @@ -14,14 +14,14 @@ #include "scene_crosswalk.hpp" -#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using motion_utils::createSlowDownVirtualWallMarker; @@ -207,4 +207,4 @@ visualization_msgs::msg::MarkerArray CrosswalkModule::createDebugMarkerArray() return debug_marker_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index fe4282893674c..428ec2510c7f4 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -24,7 +24,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::autoware::Crosswalk; @@ -229,8 +229,9 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) return crosswalk_id_set.count(scene_module->getModuleId()) == 0; }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::CrosswalkModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::CrosswalkModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_crosswalk_module/src/manager.hpp index ba7e75193deab..520dab49b347f 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene_crosswalk.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -32,7 +32,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathWithLaneId; @@ -56,6 +56,6 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC class CrosswalkModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index b6adb32f642eb..619e389f3fcf8 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -22,11 +22,11 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { bool is_occluded( const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx, - const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) + const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params) { grid_map::Index idx_offset; for (idx_offset.x() = 0; idx_offset.x() < min_nb_of_cells; ++idx_offset.x()) { @@ -140,7 +140,7 @@ bool is_crosswalk_occluded( const nav_msgs::msg::OccupancyGrid & occupancy_grid, const geometry_msgs::msg::Point & path_intersection, const double detection_range, const std::vector & dynamic_objects, - const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) + const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params) { grid_map::GridMap grid_map; grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); @@ -170,4 +170,4 @@ double calculate_detection_range( const auto time_to_crosswalk = dist_ego_to_crosswalk / std::max(min_ego_velocity, ego_velocity); return time_to_crosswalk > 0.0 ? time_to_crosswalk / occluded_object_velocity : 20.0; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index 4aab9d3bfc888..c4f2c1a23c57e 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -29,7 +29,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { /// @brief check if the gridmap is occluded at the given index /// @param [in] grid_map input grid map @@ -40,7 +40,7 @@ namespace behavior_velocity_planner /// @return true if the index is occluded bool is_occluded( const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx, - const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); + const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params); /// @brief interpolate a point beyond the end of the given segment /// @param [in] segment input segment @@ -62,7 +62,7 @@ bool is_crosswalk_occluded( const nav_msgs::msg::OccupancyGrid & occupancy_grid, const geometry_msgs::msg::Point & path_intersection, const double detection_range, const std::vector & dynamic_objects, - const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); + const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params); /// @brief calculate the distance away from the crosswalk that should be checked for occlusions /// @param occluded_objects_velocity assumed velocity of the objects coming out of occlusions @@ -89,6 +89,6 @@ std::vector select_and_inflate_o void clear_occlusions_behind_objects( grid_map::GridMap & grid_map, const std::vector & objects); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // OCCLUDED_CROSSWALK_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 191eea8feabed..91893c5224550 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -16,8 +16,8 @@ #include "occluded_crosswalk.hpp" -#include -#include +#include +#include #include #include #include @@ -38,7 +38,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; using motion_utils::calcArcLength; @@ -1281,4 +1281,4 @@ void CrosswalkModule::planStop( ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose, VelocityFactor::UNKNOWN); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 107a904dd076b..13ecddafb6490 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -17,7 +17,7 @@ #include "behavior_velocity_crosswalk_module/util.hpp" -#include +#include #include #include #include @@ -46,7 +46,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; using autoware_perception_msgs::msg::ObjectClassification; @@ -459,6 +459,6 @@ class CrosswalkModule : public SceneModuleInterface std::optional current_initial_occlusion_time_; std::optional most_recent_occlusion_time_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_CROSSWALK_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp index d276ae95e06b6..883b40b89730f 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp @@ -14,7 +14,7 @@ #include "behavior_velocity_crosswalk_module/util.hpp" -#include +#include #include #include #include @@ -45,7 +45,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; using motion_utils::calcSignedArcLength; @@ -62,16 +62,19 @@ std::vector> getCrosswalksOnPath( // Add current lane id const auto nearest_lane_id = - behavior_velocity_planner::planning_utils::getNearestLaneId(path, lanelet_map, current_pose); + autoware::behavior_velocity_planner::planning_utils::getNearestLaneId( + path, lanelet_map, current_pose); std::vector unique_lane_ids; if (nearest_lane_id) { // Add subsequent lane_ids from nearest lane_id - unique_lane_ids = behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( - path, *nearest_lane_id); + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( + path, *nearest_lane_id); } else { // Add all lane_ids in path - unique_lane_ids = behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); } for (const auto lane_id : unique_lane_ids) { @@ -229,4 +232,4 @@ std::optional getStopLineFromMap( return stop_line.front(); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_detection_area_module/package.xml b/planning/behavior_velocity_detection_area_module/package.xml index 6bcca43edc0cb..b2d370380829a 100644 --- a/planning/behavior_velocity_detection_area_module/package.xml +++ b/planning/behavior_velocity_detection_area_module/package.xml @@ -17,8 +17,8 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_detection_area_module/plugins.xml b/planning/behavior_velocity_detection_area_module/plugins.xml index 73497c8bfdf2a..9356c863ec26e 100644 --- a/planning/behavior_velocity_detection_area_module/plugins.xml +++ b/planning/behavior_velocity_detection_area_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_detection_area_module/src/debug.cpp b/planning/behavior_velocity_detection_area_module/src/debug.cpp index ee2af54e5ea2a..1ce982cd8df52 100644 --- a/planning/behavior_velocity_detection_area_module/src/debug.cpp +++ b/planning/behavior_velocity_detection_area_module/src/debug.cpp @@ -14,8 +14,8 @@ #include "scene.hpp" -#include -#include +#include +#include #include #include #include @@ -29,7 +29,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using std_msgs::msg::ColorRGBA; using tier4_autoware_utils::appendMarkerArray; @@ -191,4 +191,4 @@ motion_utils::VirtualWalls DetectionAreaModule::createVirtualWalls() return virtual_walls; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp index feb5bf6bb50ef..336a1710ed593 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::autoware::DetectionArea; using tier4_autoware_utils::getOrDeclareParameter; @@ -84,8 +84,9 @@ DetectionAreaModuleManager::getModuleExpiredFunction( }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::DetectionAreaModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::DetectionAreaModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_detection_area_module/src/manager.hpp b/planning/behavior_velocity_detection_area_module/src/manager.hpp index 71cfa0a5eef96..64a76171814b9 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.hpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC { @@ -49,6 +49,6 @@ class DetectionAreaModulePlugin : public PluginWrapper -#include +#include +#include #include #ifdef ROS_DISTRO_GALACTIC #include @@ -30,7 +30,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; using motion_utils::calcLongitudinalOffsetPose; @@ -339,4 +339,4 @@ bool DetectionAreaModule::hasEnoughBrakingDistance( return arc_lane_utils::calcSignedDistance(self_pose, line_pose.position) > pass_judge_line_distance; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_detection_area_module/src/scene.hpp index f135c3b2660cb..f577cd92dea6e 100644 --- a/planning/behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_detection_area_module/src/scene.hpp @@ -23,15 +23,15 @@ #define EIGEN_MPL2_ONLY #include -#include -#include +#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using PathIndexWithPose = std::pair; // front index, pose using PathIndexWithPoint2d = std::pair; // front index, point2d @@ -101,6 +101,6 @@ class DetectionAreaModule : public SceneModuleInterface // Debug DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml index 84d01d04a09a4..6b9db657d6d8e 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml @@ -12,9 +12,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common geometry_msgs libboost-dev motion_utils diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml index 2f5662c7998ac..ba2d2ba61ad50 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp index 8497369917232..10843fd3aef2b 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp @@ -24,7 +24,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { std::optional find_closest_collision_point( @@ -80,4 +80,4 @@ std::vector find_collisions( return collisions; } -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp index 27a48afa032b1..8431221a27d6b 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp @@ -20,7 +20,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { /// @brief find the collision point closest to ego along an object footprint /// @param [in] ego_data ego data including its path and footprints used for finding a collision @@ -41,6 +41,6 @@ std::vector find_collisions( const std::vector & objects, const tier4_autoware_utils::MultiPolygon2d & obstacle_forward_footprints); -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop #endif // COLLISION_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp index 367a1c2be1d5a..5a457d651cd9e 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp @@ -23,7 +23,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop::debug { std::vector make_delete_markers( @@ -110,4 +110,4 @@ std::vector make_polygon_markers( } return markers; } -} // namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop::debug diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp index 7397f63ca079c..c567948927089 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop::debug { std::vector make_delete_markers( const size_t from, const size_t to, const std::string & ns); @@ -40,6 +40,6 @@ std::vector make_collision_markers( const rclcpp::Time & now); std::vector make_polygon_markers( const tier4_autoware_utils::MultiPolygon2d & footprints, const std::string & ns, const double z); -} // namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop::debug #endif // DEBUG_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp index a58e4e9b88c75..5f005348c4cea 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -26,7 +26,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { tier4_autoware_utils::MultiPolygon2d make_forward_footprints( const std::vector & obstacles, @@ -81,4 +81,4 @@ void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params) } } -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp index 0050a4e2c9259..c75d7bb9cc49e 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp @@ -21,7 +21,7 @@ #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { /// @brief create the footprint of the given obstacles and their projection over a fixed time /// horizon @@ -50,6 +50,6 @@ tier4_autoware_utils::Polygon2d project_to_pose( /// @param [inout] ego_data ego data with its path and the rtree to populate /// @param [in] params parameters void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params); -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop #endif // FOOTPRINT_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp index 63f1f3025f94a..ddb8c842fb92d 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp @@ -20,7 +20,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::getOrDeclareParameter; @@ -68,9 +68,9 @@ DynamicObstacleStopModuleManager::getModuleExpiredFunction( return false; }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::DynamicObstacleStopModulePlugin, - behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::DynamicObstacleStopModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp index eb7bf4c6dbc98..d5c403e26792c 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene_dynamic_obstacle_stop.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class DynamicObstacleStopModuleManager : public SceneModuleManagerInterface { @@ -53,6 +53,6 @@ class DynamicObstacleStopModulePlugin : public PluginWrapper #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { /// @brief filter the given predicted objects @@ -91,4 +91,4 @@ std::vector filter_predicted_obj } return filtered_objects; } -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp index 5daa0cda44203..71da351d2f10b 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp @@ -21,7 +21,7 @@ #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { /// @brief filter the given predicted objects @@ -34,6 +34,6 @@ std::vector filter_predicted_obj const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis); -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop #endif // OBJECT_FILTERING_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp index 1df8a1ed6a4a3..0f6fdbfadbf86 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp @@ -18,7 +18,7 @@ #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { void update_object_map( ObjectStopDecisionMap & object_map, const std::vector & collisions, @@ -66,4 +66,4 @@ std::optional find_earliest_collision( return earliest_collision; } -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp index 5756c8589fb73..cc13614e80782 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp @@ -24,7 +24,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { /// @brief representation of a decision to stop for a dynamic object struct ObjectStopDecision @@ -71,6 +71,6 @@ void update_object_map( std::optional find_earliest_collision( const ObjectStopDecisionMap & object_map, const EgoData & ego_data); -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop #endif // OBJECT_STOP_DECISION_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp index 11ec540e3980f..9c5eced187aa8 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp @@ -21,8 +21,8 @@ #include "object_stop_decision.hpp" #include "types.hpp" -#include -#include +#include +#include #include #include #include @@ -33,7 +33,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { using visualization_msgs::msg::Marker; @@ -163,4 +163,4 @@ motion_utils::VirtualWalls DynamicObstacleStopModule::createVirtualWalls() return virtual_walls; } -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp index 79911c40a195d..8018412ba2b4b 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp @@ -18,7 +18,7 @@ #include "object_stop_decision.hpp" #include "types.hpp" -#include +#include #include #include @@ -30,7 +30,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { class DynamicObstacleStopModule : public SceneModuleInterface { @@ -57,6 +57,6 @@ class DynamicObstacleStopModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop #endif // SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp index 532d770608dd3..32ef89413b2f8 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp @@ -29,7 +29,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { using BoxIndexPair = std::pair; using Rtree = boost::geometry::index::rtree>; @@ -84,6 +84,6 @@ struct Collision geometry_msgs::msg::Point point; std::string object_uuid; }; -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop #endif // TYPES_HPP_ diff --git a/planning/behavior_velocity_intersection_module/plugins.xml b/planning/behavior_velocity_intersection_module/plugins.xml deleted file mode 100644 index 206f0324231ec..0000000000000 --- a/planning/behavior_velocity_intersection_module/plugins.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/planning/behavior_velocity_no_drivable_lane_module/package.xml b/planning/behavior_velocity_no_drivable_lane_module/package.xml index 79eea68142cc8..4c3d6dbf78153 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/package.xml +++ b/planning/behavior_velocity_no_drivable_lane_module/package.xml @@ -19,8 +19,8 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_no_drivable_lane_module/plugins.xml b/planning/behavior_velocity_no_drivable_lane_module/plugins.xml index 33b0d5d6e7469..e3e9efb254fba 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/plugins.xml +++ b/planning/behavior_velocity_no_drivable_lane_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp index 0c6bb747a854b..0908fe712dbf5 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp @@ -14,14 +14,14 @@ #include "scene.hpp" -#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; @@ -102,4 +102,4 @@ visualization_msgs::msg::MarkerArray NoDrivableLaneModule::createDebugMarkerArra return debug_marker_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp index 0b7a8a8c28dd5..8c3fc5ef0f3ce 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -14,12 +14,12 @@ #include "manager.hpp" -#include +#include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::getOrDeclareParameter; @@ -66,8 +66,9 @@ NoDrivableLaneModuleManager::getModuleExpiredFunction( }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::NoDrivableLaneModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::NoDrivableLaneModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp index 90455bd4b1c62..4afa498a5679b 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class NoDrivableLaneModuleManager : public SceneModuleManagerInterface { @@ -49,6 +49,6 @@ class NoDrivableLaneModulePlugin : public PluginWrapper +#include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::createPoint; @@ -283,4 +283,4 @@ void NoDrivableLaneModule::initialize_debug_data( } } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp index fb90e023fedcd..6910e972a8685 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -17,14 +17,14 @@ #include "util.hpp" -#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathWithLaneId; @@ -85,6 +85,6 @@ class NoDrivableLaneModule : public SceneModuleInterface void initialize_debug_data( const lanelet::Lanelet & no_drivable_lane, const geometry_msgs::msg::Point & ego_pos); }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp index 4c5efad94dfc8..c9a6ce1ac1510 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp @@ -17,14 +17,14 @@ #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include +#include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -110,4 +110,4 @@ PathWithNoDrivableLanePolygonIntersection getPathIntersectionWithNoDrivableLaneP } return path_no_drivable_lane_polygon_intersection; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp index 11953fd5dd177..289b04a2ce96d 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -51,6 +51,6 @@ PathWithNoDrivableLanePolygonIntersection getPathIntersectionWithNoDrivableLaneP const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, const geometry_msgs::msg::Point & ego_pos, const size_t max_num); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // UTIL_HPP_ diff --git a/planning/behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_no_stopping_area_module/package.xml index 0991b37120a6f..baa51d6681490 100644 --- a/planning/behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_no_stopping_area_module/package.xml @@ -17,9 +17,9 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs interpolation diff --git a/planning/behavior_velocity_no_stopping_area_module/plugins.xml b/planning/behavior_velocity_no_stopping_area_module/plugins.xml index a9b07297cfa30..23d2a800c3cf5 100644 --- a/planning/behavior_velocity_no_stopping_area_module/plugins.xml +++ b/planning/behavior_velocity_no_stopping_area_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp index 9e51afca2d475..e90273d4ac5e4 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp @@ -14,8 +14,8 @@ #include "scene_no_stopping_area.hpp" -#include -#include +#include +#include #include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -175,4 +175,4 @@ motion_utils::VirtualWalls NoStoppingAreaModule::createVirtualWalls() } return virtual_walls; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp index faa265afe1559..6aeac1e9d8dd3 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::autoware::NoStoppingArea; using tier4_autoware_utils::getOrDeclareParameter; @@ -86,8 +86,9 @@ NoStoppingAreaModuleManager::getModuleExpiredFunction( }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::NoStoppingAreaModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::NoStoppingAreaModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp index baf5901ccc124..696115d4bda6d 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene_no_stopping_area.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC { @@ -49,6 +49,6 @@ class NoStoppingAreaModulePlugin : public PluginWrapper -#include -#include +#include +#include +#include #include #include #include @@ -36,7 +36,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -416,4 +416,4 @@ void NoStoppingAreaModule::insertStopPoint( // Insert stop point or replace with zero velocity planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 62ec0b88b328e..e7e66775f368e 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -18,9 +18,9 @@ #define EIGEN_MPL2_ONLY #include -#include -#include -#include +#include +#include +#include #include #include @@ -38,7 +38,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using PathIndexWithPose = std::pair; // front index, pose using PathIndexWithPoint2d = std::pair; // front index, point2d @@ -172,6 +172,6 @@ class NoStoppingAreaModule : public SceneModuleInterface // Debug DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_NO_STOPPING_AREA_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/package.xml b/planning/behavior_velocity_occlusion_spot_module/package.xml index 5817c2203cc5d..ceddd48b045a0 100644 --- a/planning/behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/behavior_velocity_occlusion_spot_module/package.xml @@ -16,9 +16,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common geometry_msgs grid_map_ros grid_map_utils diff --git a/planning/behavior_velocity_occlusion_spot_module/plugins.xml b/planning/behavior_velocity_occlusion_spot_module/plugins.xml index a38900d1a893b..2d85752cf2b3d 100644 --- a/planning/behavior_velocity_occlusion_spot_module/plugins.xml +++ b/planning/behavior_velocity_occlusion_spot_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp index 1170afc490a75..40e5b8f91e6b9 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp @@ -15,8 +15,8 @@ #include "occlusion_spot_utils.hpp" #include "scene_occlusion_spot.hpp" -#include -#include +#include +#include #include #include @@ -24,7 +24,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -227,4 +227,4 @@ motion_utils::VirtualWalls OcclusionSpotModule::createVirtualWalls() } return virtual_walls; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index c8adb324b9055..427a6f27be3c4 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp @@ -20,7 +20,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace grid_utils { @@ -381,4 +381,4 @@ void denoiseOccupancyGridCV( grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); } } // namespace grid_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp index 710f671df609f..5c8d77fc831df 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp @@ -15,8 +15,8 @@ #ifndef GRID_UTILS_HPP_ #define GRID_UTILS_HPP_ -#include -#include +#include +#include #include #include #include @@ -49,7 +49,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; @@ -118,6 +118,6 @@ void denoiseOccupancyGridCV( grid_map::GridMap & grid_map, const GridParam & param, const bool is_show_debug_window, const int num_iter, const bool use_object_footprints, const bool use_object_ray_casts); } // namespace grid_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // GRID_UTILS_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp index 64adc112aab2f..b1757c4142212 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -16,7 +16,7 @@ #include "scene_occlusion_spot.hpp" -#include +#include #include #include @@ -25,7 +25,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using occlusion_spot_utils::DETECTION_METHOD; using occlusion_spot_utils::PASS_JUDGE; @@ -136,8 +136,9 @@ OcclusionSpotModuleManager::getModuleExpiredFunction( return false; }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::OcclusionSpotModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::OcclusionSpotModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp index 0955e4ae9aab5..123d0ef32afc1 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp @@ -18,9 +18,9 @@ #include "occlusion_spot_utils.hpp" #include "scene_occlusion_spot.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -37,7 +37,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class OcclusionSpotModuleManager : public SceneModuleManagerInterface { @@ -63,6 +63,6 @@ class OcclusionSpotModulePlugin : public PluginWrapper -#include +#include +#include #include #include #include @@ -33,7 +33,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; namespace occlusion_spot_utils @@ -485,4 +485,4 @@ std::optional generateOneNotableCollisionFromOcclusionSpo } } // namespace occlusion_spot_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index 2b6d89680cd37..542c869919339 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp @@ -17,7 +17,7 @@ #include "grid_utils.hpp" -#include +#include #include #include #include @@ -46,7 +46,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; @@ -246,6 +246,6 @@ bool generatePossibleCollisionsFromGridMap( DebugData & debug_data); } // namespace occlusion_spot_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // OCCLUSION_SPOT_UTILS_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp index 96f497ca47afa..9fe06273ea617 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp @@ -16,12 +16,12 @@ #include "occlusion_spot_utils.hpp" -#include +#include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace occlusion_spot_utils { @@ -98,4 +98,4 @@ SafeMotion calculateSafeMotion(const Velocity & v, const double ttv) return sm; } } // namespace occlusion_spot_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp index 8d7e9d2fdedd5..9def3b0938998 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp @@ -22,7 +22,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace occlusion_spot_utils { @@ -38,6 +38,6 @@ void applySafeVelocityConsideringPossibleCollision( SafeMotion calculateSafeMotion(const Velocity & v, const double ttv); } // namespace occlusion_spot_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // RISK_PREDICTIVE_BRAKING_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index 1f5ca1bab904e..e5c5eaec346ff 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -17,10 +17,10 @@ #include "occlusion_spot_utils.hpp" #include "risk_predictive_braking.hpp" -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -41,7 +41,7 @@ namespace { -namespace utils = behavior_velocity_planner::occlusion_spot_utils; +namespace utils = autoware::behavior_velocity_planner::occlusion_spot_utils; using autoware_perception_msgs::msg::PredictedObject; std::vector extractStuckVehicle( const std::vector & vehicles, const double stop_velocity) @@ -56,7 +56,7 @@ std::vector extractStuckVehicle( } } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace utils = occlusion_spot_utils; @@ -201,4 +201,4 @@ bool OcclusionSpotModule::modifyPathVelocity( return true; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index b7da7c073cbd9..00126a2ebc39a 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -17,8 +17,8 @@ #include "occlusion_spot_utils.hpp" -#include -#include +#include +#include #include #include @@ -37,7 +37,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class OcclusionSpotModule : public SceneModuleInterface { @@ -70,6 +70,6 @@ class OcclusionSpotModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_OCCLUSION_SPOT_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp index bc526959386e9..a21e0333082a9 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp @@ -15,7 +15,7 @@ #include "grid_utils.hpp" #include "utils.hpp" -#include +#include #include #include @@ -38,11 +38,11 @@ struct indexEq } }; -using behavior_velocity_planner::LineString2d; -using behavior_velocity_planner::Point2d; -using behavior_velocity_planner::Polygon2d; -using behavior_velocity_planner::grid_utils::occlusion_cost_value::OCCUPIED; -using behavior_velocity_planner::grid_utils::occlusion_cost_value::UNKNOWN; +using autoware::behavior_velocity_planner::LineString2d; +using autoware::behavior_velocity_planner::Point2d; +using autoware::behavior_velocity_planner::Polygon2d; +using autoware::behavior_velocity_planner::grid_utils::occlusion_cost_value::OCCUPIED; +using autoware::behavior_velocity_planner::grid_utils::occlusion_cost_value::UNKNOWN; namespace bg = boost::geometry; Polygon2d pointsToPoly(const Point2d p0, const Point2d p1, const double radius) diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp index 05e73855f2642..60ef5d62a5b7b 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp @@ -16,8 +16,8 @@ #include "occlusion_spot_utils.hpp" #include "utils.hpp" -#include -#include +#include +#include #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/vector3.hpp" @@ -32,8 +32,9 @@ using tier4_planning_msgs::msg::PathWithLaneId; TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) { - using behavior_velocity_planner::occlusion_spot_utils::calcSlowDownPointsForPossibleCollision; - using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; + using autoware::behavior_velocity_planner::occlusion_spot_utils:: + calcSlowDownPointsForPossibleCollision; + using autoware::behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::high_resolution_clock; @@ -65,8 +66,9 @@ TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) TEST(calcSlowDownPointsForPossibleCollision, ConsiderSignedOffset) { - using behavior_velocity_planner::occlusion_spot_utils::calcSlowDownPointsForPossibleCollision; - using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; + using autoware::behavior_velocity_planner::occlusion_spot_utils:: + calcSlowDownPointsForPossibleCollision; + using autoware::behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::high_resolution_clock; diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp b/planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp index 9b1d92b71a111..4a0071be10b2d 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp @@ -23,7 +23,7 @@ TEST(safeMotion, delay_jerk_acceleration) { - namespace utils = behavior_velocity_planner::occlusion_spot_utils; + namespace utils = autoware::behavior_velocity_planner::occlusion_spot_utils; using utils::calculateSafeMotion; /** * @brief check if calculation is correct in below parameter diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp b/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp index f7bb3d4b1ad6f..4b8a7ae3b2f3f 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp @@ -49,17 +49,18 @@ inline grid_map::GridMap generateGrid(int w, int h, double res) grid_map::GridMap grid{}; grid_map::Length length(w * res, h * res); grid.setGeometry(length, res, grid_map::Position(length.x() / 2.0, length.y() / 2.0)); - grid.add("layer", behavior_velocity_planner::grid_utils::occlusion_cost_value::FREE_SPACE); + grid.add( + "layer", autoware::behavior_velocity_planner::grid_utils::occlusion_cost_value::FREE_SPACE); return grid; } -using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; +using autoware::behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; inline void generatePossibleCollisions( std::vector & possible_collisions, double x0, double y0, double x, double y, int nb_cols) { - using behavior_velocity_planner::occlusion_spot_utils::ObstacleInfo; - using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; + using autoware::behavior_velocity_planner::occlusion_spot_utils::ObstacleInfo; + using autoware::behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; const double lon = 0.0; // assume col_x = intersection_x const double lat = -1.0; const double velocity = 1.0; diff --git a/planning/behavior_velocity_out_of_lane_module/package.xml b/planning/behavior_velocity_out_of_lane_module/package.xml index d3742ea7d305f..6ea60dd1398f5 100644 --- a/planning/behavior_velocity_out_of_lane_module/package.xml +++ b/planning/behavior_velocity_out_of_lane_module/package.xml @@ -14,9 +14,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common geometry_msgs lanelet2_extension libboost-dev diff --git a/planning/behavior_velocity_out_of_lane_module/plugins.xml b/planning/behavior_velocity_out_of_lane_module/plugins.xml index 8c18fdce79480..f85eb50367ccc 100644 --- a/planning/behavior_velocity_out_of_lane_module/plugins.xml +++ b/planning/behavior_velocity_out_of_lane_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp index 0c9e6448bb374..5e3877753d203 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -26,7 +26,7 @@ #include #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { /// @brief estimate whether ego can decelerate without breaking the deceleration limit @@ -102,5 +102,5 @@ std::optional calculate_slowdown_point( } return std::nullopt; } -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane #endif // CALCULATE_SLOWDOWN_POINTS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp index 862ca351a118a..08e66f99e2a0c 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp @@ -18,7 +18,7 @@ #include -namespace behavior_velocity_planner::out_of_lane::debug +namespace autoware::behavior_velocity_planner::out_of_lane::debug { namespace { @@ -184,4 +184,4 @@ void add_range_markers( debug_marker_array.markers.push_back(debug_marker); } -} // namespace behavior_velocity_planner::out_of_lane::debug +} // namespace autoware::behavior_velocity_planner::out_of_lane::debug diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp index 0802ae78d7a26..05eed6b35c13c 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp @@ -23,7 +23,7 @@ #include -namespace behavior_velocity_planner::out_of_lane::debug +namespace autoware::behavior_velocity_planner::out_of_lane::debug { /// @brief add footprint markers to the given marker array /// @param [inout] debug_marker_array marker array @@ -64,6 +64,6 @@ void add_range_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t first_path_idx, const double z, const size_t prev_nb); -} // namespace behavior_velocity_planner::out_of_lane::debug +} // namespace autoware::behavior_velocity_planner::out_of_lane::debug #endif // DEBUG_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index 126c75c2f80b9..62416c8d38ed6 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -29,7 +29,7 @@ #include #include #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { double distance_along_path(const EgoData & ego_data, const size_t target_idx) { @@ -380,4 +380,4 @@ std::vector calculate_decisions( return decisions; } -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp index 4f2b0a6dad89b..27c5b00c96b3f 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp @@ -29,7 +29,7 @@ #include #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { /// @brief calculate the distance along the ego path between ego and some target path index /// @param [in] ego_data data related to the ego vehicle @@ -110,6 +110,6 @@ std::optional calculate_decision( /// @return return the calculated decisions to slowdown or stop std::vector calculate_decisions( const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger); -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane #endif // DECISIONS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index 80cd106bf52ab..bb352f625580f 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -24,7 +24,7 @@ #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { void cut_predicted_path_beyond_line( autoware_perception_msgs::msg::PredictedPath & predicted_path, @@ -143,4 +143,4 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( return filtered_objects; } -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp index be3e8809d2e3d..a12b37e4c7d6d 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -17,11 +17,11 @@ #include "types.hpp" -#include +#include #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { /// @brief cut a predicted path beyond the given stop line /// @param [inout] predicted_path predicted path to cut @@ -52,6 +52,6 @@ void cut_predicted_path_beyond_red_lights( /// @return filtered predicted objects autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params); -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane #endif // FILTER_PREDICTED_OBJECTS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp index d136f4a8598f3..48992e46ec74f 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp @@ -24,7 +24,7 @@ #include #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { tier4_autoware_utils::Polygon2d make_base_footprint( const PlannerParam & p, const bool ignore_offset) @@ -87,4 +87,4 @@ lanelet::BasicPolygon2d calculate_current_ego_footprint( footprint.emplace_back(p.x() + ego_data.pose.position.x, p.y() + ego_data.pose.position.y); return footprint; } -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/footprint.hpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.hpp index a573b6ff3981c..1a0d38600fe73 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/footprint.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/footprint.hpp @@ -21,7 +21,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace out_of_lane { @@ -54,6 +54,6 @@ std::vector calculate_path_footprints( lanelet::BasicPolygon2d calculate_current_ego_footprint( const EgoData & ego_data, const PlannerParam & params, const bool ignore_offset = false); } // namespace out_of_lane -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // FOOTPRINT_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp index 67d8a79a63f03..42df39853f4f5 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -14,7 +14,7 @@ #include "lanelets_selection.hpp" -#include +#include #include #include @@ -22,7 +22,7 @@ #include #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { lanelet::ConstLanelets consecutive_lanelets( @@ -127,4 +127,4 @@ lanelet::ConstLanelets calculate_other_lanelets( } return other_lanelets; } -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp index 87757a0cb2230..8985a49a12853 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp @@ -21,7 +21,7 @@ #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { /// @brief checks if a lanelet is already contained in a vector of lanelets /// @param [in] lanelets vector to check @@ -68,6 +68,6 @@ lanelet::ConstLanelets calculate_other_lanelets( const EgoData & ego_data, const lanelet::ConstLanelets & path_lanelets, const lanelet::ConstLanelets & ignored_lanelets, const route_handler::RouteHandler & route_handler, const PlannerParam & params); -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane #endif // LANELETS_SELECTION_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 840054d92252f..61ec1a832bf8e 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -16,7 +16,7 @@ #include "scene_out_of_lane.hpp" -#include +#include #include #include @@ -25,7 +25,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::getOrDeclareParameter; @@ -100,8 +100,9 @@ OutOfLaneModuleManager::getModuleExpiredFunction( return false; }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::OutOfLaneModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::OutOfLaneModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp index 9da1751576a7f..5ab7126d88d4d 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene_out_of_lane.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -36,7 +36,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class OutOfLaneModuleManager : public SceneModuleManagerInterface { @@ -61,6 +61,6 @@ class OutOfLaneModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp index 6dfb41ccfbfcb..a081df0b52028 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp @@ -24,7 +24,7 @@ #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { Overlap calculate_overlap( @@ -123,4 +123,4 @@ OverlapRanges calculate_overlapping_ranges( return ranges; } -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp index 2b0830acc28cc..afb71c11b3ae4 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp @@ -23,7 +23,7 @@ #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { /// @brief calculate the overlap between the given footprint and lanelet @@ -55,6 +55,6 @@ OverlapRanges calculate_overlapping_ranges( const std::vector & path_footprints, const lanelet::ConstLanelets & path_lanelets, const lanelet::ConstLanelets & lanelets, const PlannerParam & params); -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane #endif // OVERLAPPING_RANGE_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp index 067a5e3dc7698..3f34e8441e8be 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp @@ -23,8 +23,8 @@ #include "overlapping_range.hpp" #include "types.hpp" -#include -#include +#include +#include #include #include #include @@ -41,7 +41,7 @@ #include #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { using visualization_msgs::msg::Marker; @@ -241,4 +241,4 @@ motion_utils::VirtualWalls OutOfLaneModule::createVirtualWalls() return virtual_walls; } -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp index 1d51c45c6afd1..38101d6a9ba1d 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp @@ -17,7 +17,7 @@ #include "types.hpp" -#include +#include #include #include @@ -31,7 +31,7 @@ #include #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { class OutOfLaneModule : public SceneModuleInterface { @@ -60,6 +60,6 @@ class OutOfLaneModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane #endif // SCENE_OUT_OF_LANE_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp index cb2945f8b32b8..f1c48a6e96d17 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -31,7 +31,7 @@ #include #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { /// @brief parameters for the "out of lane" module struct PlannerParam @@ -230,6 +230,6 @@ struct DebugData } }; -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane #endif // TYPES_HPP_ diff --git a/planning/behavior_velocity_speed_bump_module/package.xml b/planning/behavior_velocity_speed_bump_module/package.xml index 9b1de66c95c92..62e215951757d 100644 --- a/planning/behavior_velocity_speed_bump_module/package.xml +++ b/planning/behavior_velocity_speed_bump_module/package.xml @@ -15,8 +15,8 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_speed_bump_module/plugins.xml b/planning/behavior_velocity_speed_bump_module/plugins.xml index 506d25669f8cf..48287a7f94b4b 100644 --- a/planning/behavior_velocity_speed_bump_module/plugins.xml +++ b/planning/behavior_velocity_speed_bump_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_speed_bump_module/src/debug.cpp b/planning/behavior_velocity_speed_bump_module/src/debug.cpp index 7409c24e7e8bf..acbe3e05debaa 100644 --- a/planning/behavior_velocity_speed_bump_module/src/debug.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/debug.cpp @@ -14,7 +14,7 @@ #include "scene.hpp" -#include +#include #include #include #include @@ -23,7 +23,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using motion_utils::createSlowDownVirtualWallMarker; using tier4_autoware_utils::appendMarkerArray; @@ -120,4 +120,4 @@ visualization_msgs::msg::MarkerArray SpeedBumpModule::createDebugMarkerArray() return debug_marker_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_speed_bump_module/src/manager.cpp index fcd000699b7ff..42f10586d8fec 100644 --- a/planning/behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::autoware::SpeedBump; using tier4_autoware_utils::getOrDeclareParameter; @@ -80,8 +80,9 @@ SpeedBumpModuleManager::getModuleExpiredFunction( }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::SpeedBumpModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::SpeedBumpModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.hpp b/planning/behavior_velocity_speed_bump_module/src/manager.hpp index 40fcfdd081c2e..41b9a593dde95 100644 --- a/planning/behavior_velocity_speed_bump_module/src/manager.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class SpeedBumpModuleManager : public SceneModuleManagerInterface { @@ -49,6 +49,6 @@ class SpeedBumpModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_speed_bump_module/src/scene.cpp index dc944cc0c292b..0ca4e0731b155 100644 --- a/planning/behavior_velocity_speed_bump_module/src/scene.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/scene.cpp @@ -20,7 +20,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using motion_utils::calcSignedArcLength; using tier4_autoware_utils::createPoint; @@ -169,4 +169,4 @@ bool SpeedBumpModule::applySlowDownSpeed( output.points, slow_start_point_idx, slow_end_point_idx, speed_bump_speed); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_speed_bump_module/src/scene.hpp index f227366127046..ffbcf5fdf9325 100644 --- a/planning/behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/scene.hpp @@ -17,14 +17,14 @@ #include "util.hpp" -#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathWithLaneId; @@ -81,6 +81,6 @@ class SpeedBumpModule : public SceneModuleInterface float speed_bump_slow_down_speed_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_speed_bump_module/src/util.cpp b/planning/behavior_velocity_speed_bump_module/src/util.cpp index d1533d7028dcf..ca4e7e379d1ea 100644 --- a/planning/behavior_velocity_speed_bump_module/src/util.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/util.cpp @@ -17,7 +17,7 @@ #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include +#include #include #include @@ -32,7 +32,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -172,4 +172,4 @@ float calcSlowDownSpeed(const Point32 & p1, const Point32 & p2, const float spee return std::clamp(speed, p2.y, p1.y); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_speed_bump_module/src/util.hpp b/planning/behavior_velocity_speed_bump_module/src/util.hpp index 2cd408735fd61..72bad3db86f73 100644 --- a/planning/behavior_velocity_speed_bump_module/src/util.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/util.hpp @@ -35,7 +35,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -70,6 +70,6 @@ std::optional insertPointWithOffset( // returns y (speed) for y=mx+b float calcSlowDownSpeed(const Point32 & p1, const Point32 & p2, const float speed_bump_height); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // UTIL_HPP_ diff --git a/planning/behavior_velocity_stop_line_module/package.xml b/planning/behavior_velocity_stop_line_module/package.xml index c71ecb83098fb..3132f60d1f342 100644 --- a/planning/behavior_velocity_stop_line_module/package.xml +++ b/planning/behavior_velocity_stop_line_module/package.xml @@ -18,8 +18,8 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_stop_line_module/plugins.xml b/planning/behavior_velocity_stop_line_module/plugins.xml index 51fb225fbebad..4b8ce9852fcec 100644 --- a/planning/behavior_velocity_stop_line_module/plugins.xml +++ b/planning/behavior_velocity_stop_line_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_stop_line_module/src/debug.cpp b/planning/behavior_velocity_stop_line_module/src/debug.cpp index f63bdca5068a2..bfcd5a5828556 100644 --- a/planning/behavior_velocity_stop_line_module/src/debug.cpp +++ b/planning/behavior_velocity_stop_line_module/src/debug.cpp @@ -14,7 +14,7 @@ #include "scene.hpp" -#include +#include #include #include #include @@ -24,7 +24,7 @@ #include #endif -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createMarkerColor; @@ -111,4 +111,4 @@ motion_utils::VirtualWalls StopLineModule::createVirtualWalls() } return virtual_walls; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_stop_line_module/src/manager.cpp index b37b3b119178c..80f48fa65efaf 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.cpp @@ -21,7 +21,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::TrafficSign; using tier4_autoware_utils::getOrDeclareParameter; @@ -102,8 +102,9 @@ StopLineModuleManager::getModuleExpiredFunction( }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::StopLineModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::StopLineModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_stop_line_module/src/manager.hpp b/planning/behavior_velocity_stop_line_module/src/manager.hpp index af9dbaa083c33..eca5132a75c5f 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.hpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -30,7 +30,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using StopLineWithLaneId = std::pair; @@ -62,6 +62,6 @@ class StopLineModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_stop_line_module/src/scene.cpp index 976acd98fe42b..1bf777338e43c 100644 --- a/planning/behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_stop_line_module/src/scene.cpp @@ -14,14 +14,14 @@ #include "scene.hpp" -#include -#include +#include +#include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -177,4 +177,4 @@ geometry_msgs::msg::Point StopLineModule::getCenterOfStopLine( center_point.z = (stop_line[0].z() + stop_line[1].z()) / 2.0; return center_point; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_stop_line_module/src/scene.hpp index 70c67df623c85..0ae0a885f036f 100644 --- a/planning/behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_stop_line_module/src/scene.hpp @@ -24,16 +24,16 @@ #define EIGEN_MPL2_ONLY #include #include -#include -#include -#include +#include +#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class StopLineModule : public SceneModuleInterface { @@ -106,6 +106,6 @@ class StopLineModule : public SceneModuleInterface // Debug DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml index ff7b5a269db32..067dcaca6a638 100644 --- a/planning/behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -19,9 +19,9 @@ eigen3_cmake_module autoware_adapi_v1_msgs + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_traffic_light_module/plugins.xml b/planning/behavior_velocity_traffic_light_module/plugins.xml index b65cc66c5c232..9b5e84fff982d 100644 --- a/planning/behavior_velocity_traffic_light_module/plugins.xml +++ b/planning/behavior_velocity_traffic_light_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_traffic_light_module/src/debug.cpp index b3dedaa29d6ad..7ceded9c91934 100644 --- a/planning/behavior_velocity_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/debug.cpp @@ -14,7 +14,7 @@ #include "scene.hpp" -#include +#include #include #include #include @@ -24,7 +24,7 @@ #include #endif -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { visualization_msgs::msg::MarkerArray TrafficLightModule::createDebugMarkerArray() { @@ -54,4 +54,4 @@ motion_utils::VirtualWalls TrafficLightModule::createVirtualWalls() return virtual_walls; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 01c3acf84cd63..ef5f8b2b36016 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -24,7 +24,7 @@ #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::TrafficLight; using tier4_autoware_utils::getOrDeclareParameter; @@ -188,8 +188,9 @@ bool TrafficLightModuleManager::hasSameTrafficLight( return false; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::TrafficLightModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::TrafficLightModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp index 97340f8592a7d..c40056a0ba10c 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC { @@ -65,6 +65,6 @@ class TrafficLightModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index c886578dc65e4..3ec597f1de249 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -14,7 +14,7 @@ #include "scene.hpp" -#include +#include #include #include @@ -34,7 +34,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -432,4 +432,4 @@ tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( return modified_path; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index 8385a1210d421..220145a2c7163 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -23,15 +23,15 @@ #define EIGEN_MPL2_ONLY #include #include -#include -#include +#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class TrafficLightModule : public SceneModuleInterface { @@ -129,6 +129,6 @@ class TrafficLightModule : public SceneModuleInterface // Traffic Light State TrafficSignal looking_tl_state_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_walkway_module/plugins.xml b/planning/behavior_velocity_walkway_module/plugins.xml deleted file mode 100644 index 971a49f2cb044..0000000000000 --- a/planning/behavior_velocity_walkway_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp index add6b5ef392ea..87aa4ac483feb 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp @@ -81,7 +81,7 @@ struct PlannerData tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; // velocity smoother - std::shared_ptr velocity_smoother_{}; + std::shared_ptr velocity_smoother_{}; // parameters vehicle_info_util::VehicleInfo vehicle_info_; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml index 2f4e7241d60ee..a115d28ee6d86 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -15,10 +15,10 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs autoware_velocity_smoother - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 4d152afa250b7..262c32fe1ea47 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -241,7 +241,7 @@ void MotionVelocityPlannerNode::on_acceleration( void MotionVelocityPlannerNode::set_velocity_smoother_params() { planner_data_.velocity_smoother_ = - std::make_shared(*this); + std::make_shared(*this); } void MotionVelocityPlannerNode::on_lanelet_map( @@ -399,7 +399,7 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); if (external_v_limit) { - autoware_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + autoware::velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); } return traj_smoothed; diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 2ccd000657948..8b338e9d0cd9a 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -88,7 +88,7 @@ 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 + max_lat_margin_against_unknown: 0.3 # lateral margin between the unknown obstacles and ego's footprint crossing_obstacle: collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] @@ -218,3 +218,15 @@ lpf_gain_slow_down_vel: 0.99 # low-pass filter gain for slow down velocity lpf_gain_lat_dist: 0.999 # low-pass filter gain for lateral distance from obstacle to ego's path lpf_gain_dist_to_slow_down: 0.7 # low-pass filter gain for distance to slow down start + stop: + type_specified_params: + labels: # For the listed types, the node try to read the following type specified values + - "default" + - "unknown" + # default: For the default type, which denotes the other types listed above, the following param is defined implicitly, and the other type-specified parameters are not defined. + # limit_min_acc: common_param.yaml/limit.min_acc + unknown: + limit_min_acc: -1.2 # overwrite the deceleration limit, in usually, common_param.yaml/limit.min_acc is referred. + sudden_object_acc_threshold: -1.1 # If a stop can be achieved by a deceleration smaller than this value, it is not considered as “sudden stop". + sudden_object_dist_threshold: 1000.0 # If a stop distance is longer than this value, it is not considered as “sudden stop". + abandon_to_stop: false # If true, the planner gives up to stop when it cannot avoid to run over while maintaining the deceleration limit. diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index 052e7bb721592..34ae20980d3fd 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -113,14 +113,15 @@ struct StopObstacle : public TargetObstacleInterface { StopObstacle( const std::string & arg_uuid, const rclcpp::Time & arg_stamp, - const geometry_msgs::msg::Pose & arg_pose, const Shape & arg_shape, - const double arg_lon_velocity, const double arg_lat_velocity, + const ObjectClassification & object_classification, const geometry_msgs::msg::Pose & arg_pose, + const Shape & arg_shape, const double arg_lon_velocity, const double arg_lat_velocity, const geometry_msgs::msg::Point arg_collision_point, const double arg_dist_to_collide_on_decimated_traj) : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity), shape(arg_shape), collision_point(arg_collision_point), - dist_to_collide_on_decimated_traj(arg_dist_to_collide_on_decimated_traj) + dist_to_collide_on_decimated_traj(arg_dist_to_collide_on_decimated_traj), + classification(object_classification) { } Shape shape; @@ -129,6 +130,7 @@ struct StopObstacle : public TargetObstacleInterface // calculateMarginFromObstacleOnCurve() and should be removed as it can be // replaced by ”dist_to_collide_on_decimated_traj” double dist_to_collide_on_decimated_traj; + ObjectClassification classification; }; struct CruiseObstacle : public TargetObstacleInterface diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index fd65446408db1..3aea741e3b2f4 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -272,8 +272,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool is_driving_forward_{true}; bool enable_slow_down_planning_{false}; - // previous closest obstacle - std::shared_ptr prev_closest_stop_obstacle_ptr_{nullptr}; + std::vector prev_closest_stop_obstacles_{}; std::unique_ptr logger_configure_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 416c36b116059..7a060657e16af 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -23,6 +23,8 @@ #include "tier4_autoware_utils/ros/update_param.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" +#include +#include #include #include #include @@ -42,7 +44,8 @@ class PlannerInterface vehicle_info_(vehicle_info), ego_nearest_param_(ego_nearest_param), debug_data_ptr_(debug_data_ptr), - slow_down_param_(SlowDownParam(node)) + slow_down_param_(SlowDownParam(node)), + stop_param_(StopParam(node, longitudinal_info)) { stop_reasons_pub_ = node.create_publisher("~/output/stop_reasons", 1); velocity_factors_pub_ = @@ -91,6 +94,7 @@ class PlannerInterface updateCommonParam(parameters); updateCruiseParam(parameters); slow_down_param_.onParam(parameters); + stop_param_.onParam(parameters, longitudinal_info_); } Float32MultiArrayStamped getStopPlanningDebugMessage(const rclcpp::Time & current_time) const @@ -333,6 +337,84 @@ class PlannerInterface double lpf_gain_dist_to_slow_down; }; SlowDownParam slow_down_param_; + struct StopParam + { + struct ObstacleSpecificParams + { + double limit_min_acc; + double sudden_object_acc_threshold; + double sudden_object_dist_threshold; + bool abandon_to_stop; + }; + const std::unordered_map types_maps = { + {ObjectClassification::UNKNOWN, "unknown"}, {ObjectClassification::CAR, "car"}, + {ObjectClassification::TRUCK, "truck"}, {ObjectClassification::BUS, "bus"}, + {ObjectClassification::TRAILER, "trailer"}, {ObjectClassification::MOTORCYCLE, "motorcycle"}, + {ObjectClassification::BICYCLE, "bicycle"}, {ObjectClassification::PEDESTRIAN, "pedestrian"}}; + std::unordered_map type_specified_param_list; + explicit StopParam(rclcpp::Node & node, const LongitudinalInfo & longitudinal_info) + { + const std::string param_prefix = "stop.type_specified_params."; + std::vector obstacle_labels{"default"}; + obstacle_labels = + node.declare_parameter>(param_prefix + "labels", obstacle_labels); + + for (const auto & type_str : obstacle_labels) { + if (type_str != "default") { + ObstacleSpecificParams param{ + node.declare_parameter(param_prefix + type_str + ".limit_min_acc"), + node.declare_parameter( + param_prefix + type_str + ".sudden_object_acc_threshold"), + node.declare_parameter( + param_prefix + type_str + ".sudden_object_dist_threshold"), + node.declare_parameter(param_prefix + type_str + ".abandon_to_stop")}; + + param.sudden_object_acc_threshold = + std::min(param.sudden_object_acc_threshold, longitudinal_info.limit_min_accel); + param.limit_min_acc = std::min(param.limit_min_acc, param.sudden_object_acc_threshold); + + type_specified_param_list.emplace(type_str, param); + } + } + } + void onParam( + const std::vector & parameters, const LongitudinalInfo & longitudinal_info) + { + const std::string param_prefix = "stop.type_specified_params."; + for (auto & [type_str, param] : type_specified_param_list) { + if (type_str == "default") { + continue; + } + tier4_autoware_utils::updateParam( + parameters, param_prefix + type_str + ".limit_min_acc", param.limit_min_acc); + tier4_autoware_utils::updateParam( + parameters, param_prefix + type_str + ".sudden_object_acc_threshold", + param.sudden_object_acc_threshold); + tier4_autoware_utils::updateParam( + parameters, param_prefix + type_str + ".sudden_object_dist_threshold", + param.sudden_object_dist_threshold); + tier4_autoware_utils::updateParam( + parameters, param_prefix + type_str + ".abandon_to_stop", param.abandon_to_stop); + + param.sudden_object_acc_threshold = + std::min(param.sudden_object_acc_threshold, longitudinal_info.limit_min_accel); + param.limit_min_acc = std::min(param.limit_min_acc, param.sudden_object_acc_threshold); + } + } + std::string getParamType(const ObjectClassification label) + { + const auto type_str = types_maps.at(label.label); + if (type_specified_param_list.count(type_str) == 0) { + return "default"; + } + return type_str; + } + ObstacleSpecificParams getParam(const ObjectClassification label) + { + return type_specified_param_list.at(getParamType(label)); + } + }; + StopParam stop_param_; double moving_object_speed_threshold; double moving_object_hysteresis_range; std::vector prev_slow_down_output_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp index 3b4ab577c2988..48bab514cbf77 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp @@ -36,8 +36,7 @@ PoseWithStamp getCurrentObjectPose( const PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time, const bool use_prediction); -std::optional getClosestStopObstacle( - const std::vector & stop_obstacles); +std::vector getClosestStopObstacles(const std::vector & stop_obstacles); template size_t getIndexWithLongitudinalOffset( diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index c5f0c73a13b78..41ae29b7d470f 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -1253,9 +1253,9 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( } const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); - return StopObstacle{ - obstacle.uuid, obstacle.stamp, obstacle.pose, obstacle.shape, - tangent_vel, normal_vel, collision_point->first, collision_point->second}; + return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification, + obstacle.pose, obstacle.shape, tangent_vel, + normal_vel, collision_point->first, collision_point->second}; } std::optional ObstacleCruisePlannerNode::createSlowDownObstacle( @@ -1386,50 +1386,36 @@ void ObstacleCruisePlannerNode::checkConsistency( const rclcpp::Time & current_time, const PredictedObjects & predicted_objects, std::vector & stop_obstacles) { - const auto current_closest_stop_obstacle = - obstacle_cruise_utils::getClosestStopObstacle(stop_obstacles); - - if (!prev_closest_stop_obstacle_ptr_) { - if (current_closest_stop_obstacle) { - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); + for (const auto & prev_closest_stop_obstacle : prev_closest_stop_obstacles_) { + const auto predicted_object_itr = std::find_if( + predicted_objects.objects.begin(), predicted_objects.objects.end(), + [&prev_closest_stop_obstacle](const PredictedObject & po) { + return tier4_autoware_utils::toHexString(po.object_id) == prev_closest_stop_obstacle.uuid; + }); + // If previous closest obstacle disappear from the perception result, do nothing anymore. + if (predicted_object_itr == predicted_objects.objects.end()) { + continue; } - return; - } - const auto predicted_object_itr = std::find_if( - predicted_objects.objects.begin(), predicted_objects.objects.end(), - [&](PredictedObject predicted_object) { - return tier4_autoware_utils::toHexString(predicted_object.object_id) == - prev_closest_stop_obstacle_ptr_->uuid; - }); - // If previous closest obstacle disappear from the perception result, do nothing anymore. - if (predicted_object_itr == predicted_objects.objects.end()) { - return; - } - - const auto is_disappeared_from_stop_obstacle = std::none_of( - stop_obstacles.begin(), stop_obstacles.end(), - [&](const auto & obstacle) { return obstacle.uuid == prev_closest_stop_obstacle_ptr_->uuid; }); - if (is_disappeared_from_stop_obstacle) { - // re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop" - // condition is satisfied - const double elapsed_time = (current_time - prev_closest_stop_obstacle_ptr_->stamp).seconds(); - if ( - predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x < - behavior_determination_param_.obstacle_velocity_threshold_from_stop_to_cruise && - elapsed_time < behavior_determination_param_.stop_obstacle_hold_time_threshold) { - stop_obstacles.push_back(*prev_closest_stop_obstacle_ptr_); - return; + const auto is_disappeared_from_stop_obstacle = std::none_of( + stop_obstacles.begin(), stop_obstacles.end(), + [&prev_closest_stop_obstacle](const StopObstacle & so) { + return so.uuid == prev_closest_stop_obstacle.uuid; + }); + if (is_disappeared_from_stop_obstacle) { + // re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop" + // condition is satisfied + const double elapsed_time = (current_time - prev_closest_stop_obstacle.stamp).seconds(); + if ( + predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x < + behavior_determination_param_.obstacle_velocity_threshold_from_stop_to_cruise && + elapsed_time < behavior_determination_param_.stop_obstacle_hold_time_threshold) { + stop_obstacles.push_back(prev_closest_stop_obstacle); + } } } - if (current_closest_stop_obstacle) { - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); - } else { - prev_closest_stop_obstacle_ptr_ = nullptr; - } + prev_closest_stop_obstacles_ = obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles); } bool ObstacleCruisePlannerNode::isObstacleCrossing( diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index f7ce218cf3ccf..6e1de97378d16 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -259,129 +259,171 @@ std::vector PlannerInterface::generateStopTrajectory( rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), enable_debug_info_, "stop planning"); - // Get Closest Stop Obstacle - const auto closest_stop_obstacle = obstacle_cruise_utils::getClosestStopObstacle(stop_obstacles); - if (!closest_stop_obstacle) { - // delete marker - const auto markers = - motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); - - prev_stop_distance_info_ = std::nullopt; - return planner_data.traj_points; - } - - const auto ego_segment_idx = - ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose); - const double dist_to_collide_on_ref_traj = - motion_utils::calcSignedArcLength(planner_data.traj_points, 0, ego_segment_idx) + - closest_stop_obstacle->dist_to_collide_on_decimated_traj; - - const double margin_from_obstacle_considering_behavior_module = [&]() { - const double margin_from_obstacle = - calculateMarginFromObstacleOnCurve(planner_data, *closest_stop_obstacle); - // Use terminal margin (terminal_safe_distance_margin) for obstacle stop - const auto ref_traj_length = motion_utils::calcSignedArcLength( - planner_data.traj_points, 0, planner_data.traj_points.size() - 1); - if (dist_to_collide_on_ref_traj > ref_traj_length) { - return longitudinal_info_.terminal_safe_distance_margin; - } - - // If behavior stop point is ahead of the closest_obstacle_stop point within a certain margin - // we set closest_obstacle_stop_distance to closest_behavior_stop_distance - const auto closest_behavior_stop_idx = - motion_utils::searchZeroVelocityIndex(planner_data.traj_points, ego_segment_idx + 1); - if (closest_behavior_stop_idx) { - const double closest_behavior_stop_dist_on_ref_traj = - motion_utils::calcSignedArcLength(planner_data.traj_points, 0, *closest_behavior_stop_idx); - const double stop_dist_diff = closest_behavior_stop_dist_on_ref_traj - - (dist_to_collide_on_ref_traj - margin_from_obstacle); - if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) { - return min_behavior_stop_margin_; + std::optional determined_stop_obstacle{}; + std::optional determined_zero_vel_dist{}; + std::optional determined_desired_margin{}; + + const auto closest_stop_obstacles = + obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles); + for (const auto & stop_obstacle : closest_stop_obstacles) { + const auto ego_segment_idx = + ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose); + const double dist_to_collide_on_ref_traj = + motion_utils::calcSignedArcLength(planner_data.traj_points, 0, ego_segment_idx) + + stop_obstacle.dist_to_collide_on_decimated_traj; + + const double desired_margin = [&]() { + const double margin_from_obstacle = + calculateMarginFromObstacleOnCurve(planner_data, stop_obstacle); + // Use terminal margin (terminal_safe_distance_margin) for obstacle stop + const auto ref_traj_length = motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, planner_data.traj_points.size() - 1); + if (dist_to_collide_on_ref_traj > ref_traj_length) { + return longitudinal_info_.terminal_safe_distance_margin; } - } - return margin_from_obstacle; - }(); - // Generate Output Trajectory - const auto [zero_vel_dist, will_collide_with_obstacle] = [&]() { - double candidate_zero_vel_dist = - std::max(0.0, dist_to_collide_on_ref_traj - margin_from_obstacle_considering_behavior_module); + // If behavior stop point is ahead of the closest_obstacle_stop point within a certain + // margin we set closest_obstacle_stop_distance to closest_behavior_stop_distance + const auto closest_behavior_stop_idx = + motion_utils::searchZeroVelocityIndex(planner_data.traj_points, ego_segment_idx + 1); + if (closest_behavior_stop_idx) { + const double closest_behavior_stop_dist_on_ref_traj = motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, *closest_behavior_stop_idx); + const double stop_dist_diff = closest_behavior_stop_dist_on_ref_traj - + (dist_to_collide_on_ref_traj - margin_from_obstacle); + if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) { + return min_behavior_stop_margin_; + } + } + return margin_from_obstacle; + }(); - // Check feasibility to stop + // calc stop point against the obstacle + double candidate_zero_vel_dist = std::max(0.0, dist_to_collide_on_ref_traj - desired_margin); if (suppress_sudden_obstacle_stop_) { - // Calculate feasible stop margin (Check the feasibility) - const double feasible_stop_dist = - calcMinimumDistanceToStop( - planner_data.ego_vel, longitudinal_info_.limit_max_accel, - longitudinal_info_.limit_min_accel) + - motion_utils::calcSignedArcLength( - planner_data.traj_points, 0, planner_data.ego_pose.position); + const auto acceptable_stop_acc = [&]() -> std::optional { + if (stop_param_.getParamType(stop_obstacle.classification) == "default") { + return longitudinal_info_.limit_min_accel; + } + const double distance_to_judge_suddenness = std::min( + calcMinimumDistanceToStop( + planner_data.ego_vel, longitudinal_info_.limit_max_accel, + stop_param_.getParam(stop_obstacle.classification).sudden_object_acc_threshold), + stop_param_.getParam(stop_obstacle.classification).sudden_object_dist_threshold); + if (candidate_zero_vel_dist > distance_to_judge_suddenness) { + return longitudinal_info_.limit_min_accel; + } + if (stop_param_.getParam(stop_obstacle.classification).abandon_to_stop) { + RCLCPP_WARN( + rclcpp::get_logger("ObstacleCruisePlanner::StopPlanner"), + "[Cruise] abandon to stop against %s object", + stop_param_.types_maps.at(stop_obstacle.classification.label).c_str()); + return std::nullopt; + } else { + return stop_param_.getParam(stop_obstacle.classification).limit_min_acc; + } + }(); + if (!acceptable_stop_acc) { + continue; + } - if (candidate_zero_vel_dist < feasible_stop_dist) { - candidate_zero_vel_dist = feasible_stop_dist; - return std::make_pair(candidate_zero_vel_dist, true); + const double acceptable_stop_pos = + motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, planner_data.ego_pose.position) + + calcMinimumDistanceToStop( + planner_data.ego_vel, longitudinal_info_.limit_max_accel, acceptable_stop_acc.value()); + if (acceptable_stop_pos > candidate_zero_vel_dist) { + candidate_zero_vel_dist = acceptable_stop_pos; } } - // Hold previous stop distance if necessary - if ( - std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold && - prev_stop_distance_info_) { - // NOTE: We assume that the current trajectory's front point is ahead of the previous - // trajectory's front point. - const size_t traj_front_point_prev_seg_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prev_stop_distance_info_->first, planner_data.traj_points.front().pose); - const double diff_dist_front_points = motion_utils::calcSignedArcLength( - prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position, - traj_front_point_prev_seg_idx); - - const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points; + if (determined_stop_obstacle) { + const bool is_same_param_types = + (stop_obstacle.classification.label == determined_stop_obstacle->classification.label); if ( - std::abs(prev_zero_vel_dist - candidate_zero_vel_dist) < - longitudinal_info_.hold_stop_distance_threshold) { - candidate_zero_vel_dist = prev_zero_vel_dist; + (is_same_param_types && stop_obstacle.dist_to_collide_on_decimated_traj > + determined_stop_obstacle->dist_to_collide_on_decimated_traj) || + (!is_same_param_types && candidate_zero_vel_dist > determined_zero_vel_dist)) { + continue; } } - return std::make_pair(candidate_zero_vel_dist, false); - }(); + determined_zero_vel_dist = candidate_zero_vel_dist; + determined_stop_obstacle = stop_obstacle; + determined_desired_margin = desired_margin; + } + + if (!determined_zero_vel_dist) { + // delete marker + const auto markers = + motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); + tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + + prev_stop_distance_info_ = std::nullopt; + return planner_data.traj_points; + } + + // Hold previous stop distance if necessary + if ( + std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold && + prev_stop_distance_info_) { + // NOTE: We assume that the current trajectory's front point is ahead of the previous + // trajectory's front point. + const size_t traj_front_point_prev_seg_idx = + motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_stop_distance_info_->first, planner_data.traj_points.front().pose); + const double diff_dist_front_points = motion_utils::calcSignedArcLength( + prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position, + traj_front_point_prev_seg_idx); + + const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points; + if ( + std::abs(prev_zero_vel_dist - determined_zero_vel_dist.value()) < + longitudinal_info_.hold_stop_distance_threshold) { + determined_zero_vel_dist.value() = prev_zero_vel_dist; + } + } // Insert stop point auto output_traj_points = planner_data.traj_points; - const auto zero_vel_idx = motion_utils::insertStopPoint(0, zero_vel_dist, output_traj_points); + const auto zero_vel_idx = + motion_utils::insertStopPoint(0, *determined_zero_vel_dist, output_traj_points); if (zero_vel_idx) { // virtual wall marker for stop obstacle const auto markers = motion_utils::createStopVirtualWallMarker( output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", planner_data.current_time, 0, abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); - debug_data_ptr_->obstacles_to_stop.push_back(*closest_stop_obstacle); + debug_data_ptr_->obstacles_to_stop.push_back(*determined_stop_obstacle); // Publish Stop Reason const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; const auto stop_reasons_msg = - makeStopReasonArray(planner_data, stop_pose, *closest_stop_obstacle); + makeStopReasonArray(planner_data, stop_pose, *determined_stop_obstacle); stop_reasons_pub_->publish(stop_reasons_msg); velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose)); - // Publish if ego vehicle collides with the obstacle with a limit acceleration + // Publish if ego vehicle will over run against the stop point with a limit acceleration + + const bool will_over_run = determined_zero_vel_dist.value() > + motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, planner_data.ego_pose.position) + + determined_stop_obstacle->dist_to_collide_on_decimated_traj + + determined_desired_margin.value() + 0.1; const auto stop_speed_exceeded_msg = - createStopSpeedExceededMsg(planner_data.current_time, will_collide_with_obstacle); + createStopSpeedExceededMsg(planner_data.current_time, will_over_run); stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg); - prev_stop_distance_info_ = std::make_pair(output_traj_points, zero_vel_dist); + prev_stop_distance_info_ = std::make_pair(output_traj_points, determined_zero_vel_dist.value()); } stop_planning_debug_info_.set( StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE, - closest_stop_obstacle->dist_to_collide_on_decimated_traj); + determined_stop_obstacle->dist_to_collide_on_decimated_traj); stop_planning_debug_info_.set( - StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, closest_stop_obstacle->velocity); - + StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, + determined_stop_obstacle->velocity); stop_planning_debug_info_.set( - StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, - margin_from_obstacle_considering_behavior_module); + StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, determined_desired_margin.value()); stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_VELOCITY, 0.0); stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_ACCELERATION, 0.0); diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 6b32ccafc7f42..6c1b3999e40c1 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -95,16 +95,21 @@ PoseWithStamp getCurrentObjectPose( return PoseWithStamp{obj_base_time, *interpolated_pose}; } -std::optional getClosestStopObstacle(const std::vector & stop_obstacles) +std::vector getClosestStopObstacles(const std::vector & stop_obstacles) { - std::optional candidate_obstacle = std::nullopt; + std::vector candidates{}; for (const auto & stop_obstacle : stop_obstacles) { - if ( - !candidate_obstacle || stop_obstacle.dist_to_collide_on_decimated_traj < - candidate_obstacle->dist_to_collide_on_decimated_traj) { - candidate_obstacle = stop_obstacle; + const auto itr = + std::find_if(candidates.begin(), candidates.end(), [&stop_obstacle](const StopObstacle & co) { + return co.classification.label == stop_obstacle.classification.label; + }); + if (itr == candidates.end()) { + candidates.emplace_back(stop_obstacle); + } else if ( + stop_obstacle.dist_to_collide_on_decimated_traj < itr->dist_to_collide_on_decimated_traj) { + *itr = stop_obstacle; } } - return candidate_obstacle; + return candidates; } } // namespace obstacle_cruise_utils diff --git a/planning/planning_validator/CMakeLists.txt b/planning/planning_validator/CMakeLists.txt index 455dded2d7e32..da780cfdebdcf 100644 --- a/planning/planning_validator/CMakeLists.txt +++ b/planning/planning_validator/CMakeLists.txt @@ -1,22 +1,22 @@ cmake_minimum_required(VERSION 3.22) -project(planning_validator) +project(autoware_planning_validator) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(planning_validator_helpers SHARED +ament_auto_add_library(autoware_planning_validator_helpers SHARED src/utils.cpp src/debug_marker.cpp ) # planning validator -ament_auto_add_library(planning_validator_component SHARED - include/planning_validator/planning_validator.hpp +ament_auto_add_library(autoware_planning_validator_component SHARED + include/autoware_planning_validator/planning_validator.hpp src/planning_validator.cpp ) -target_link_libraries(planning_validator_component planning_validator_helpers) -rclcpp_components_register_node(planning_validator_component - PLUGIN "planning_validator::PlanningValidator" +target_link_libraries(autoware_planning_validator_component autoware_planning_validator_helpers) +rclcpp_components_register_node(autoware_planning_validator_component + PLUGIN "autoware::planning_validator::PlanningValidator" EXECUTABLE planning_validator_node ) @@ -25,7 +25,7 @@ ament_auto_add_library(invalid_trajectory_publisher_node SHARED src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp ) rclcpp_components_register_node(invalid_trajectory_publisher_node - PLUGIN "planning_validator::InvalidTrajectoryPublisherNode" + PLUGIN "autoware::planning_validator::InvalidTrajectoryPublisherNode" EXECUTABLE invalid_trajectory_publisher ) @@ -37,29 +37,29 @@ rosidl_generate_interfaces( # to use a message defined in the same package if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(planning_validator_component + rosidl_target_interfaces(autoware_planning_validator_component ${PROJECT_NAME} "rosidl_typesupport_cpp") else() rosidl_get_typesupport_target( cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") - target_link_libraries(planning_validator_component "${cpp_typesupport_target}") + target_link_libraries(autoware_planning_validator_component "${cpp_typesupport_target}") endif() if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_planning_validator + ament_add_ros_isolated_gtest(test_autoware_planning_validator test/src/test_main.cpp test/src/test_planning_validator_functions.cpp test/src/test_planning_validator_helper.cpp test/src/test_planning_validator_pubsub.cpp - test/src/test_${PROJECT_NAME}_node_interface.cpp + test/src/test_planning_validator_node_interface.cpp ) - ament_target_dependencies(test_planning_validator + ament_target_dependencies(test_autoware_planning_validator rclcpp autoware_planning_msgs ) - target_link_libraries(test_planning_validator - planning_validator_component + target_link_libraries(test_autoware_planning_validator + autoware_planning_validator_component ) endif() diff --git a/planning/planning_validator/README.md b/planning/planning_validator/README.md index 9d70e7f78a5bb..92c9b86c8b31c 100644 --- a/planning/planning_validator/README.md +++ b/planning/planning_validator/README.md @@ -1,8 +1,8 @@ # Planning Validator -The `planning_validator` is a module that checks the validity of a trajectory before it is published. The status of the validation can be viewed in the `/diagnostics` and `/validation_status` topics. When an invalid trajectory is detected, the `planning_validator` will process the trajectory following the selected option: "0. publish the trajectory as it is", "1. stop publishing the trajectory", "2. publish the last validated trajectory". +The `autoware_planning_validator` is a module that checks the validity of a trajectory before it is published. The status of the validation can be viewed in the `/diagnostics` and `/validation_status` topics. When an invalid trajectory is detected, the `autoware_planning_validator` will process the trajectory following the selected option: "0. publish the trajectory as it is", "1. stop publishing the trajectory", "2. publish the last validated trajectory". -![planning_validator](./image/planning_validator.drawio.svg) +![autoware_planning_validator](./image/planning_validator.drawio.svg) ## Supported features @@ -29,7 +29,7 @@ The following features are to be implemented. ### Inputs -The `planning_validator` takes in the following inputs: +The `autoware_planning_validator` takes in the following inputs: | Name | Type | Description | | -------------------- | --------------------------------- | ---------------------------------------------- | @@ -48,7 +48,7 @@ It outputs the following: ## Parameters -The following parameters can be set for the `planning_validator`: +The following parameters can be set for the `autoware_planning_validator`: ### System parameters diff --git a/planning/planning_validator/config/planning_validator_plotjugler_config.xml b/planning/planning_validator/config/planning_validator_plotjugler_config.xml index d6afc0b017d32..04d5d4d8a0e38 100644 --- a/planning/planning_validator/config/planning_validator_plotjugler_config.xml +++ b/planning/planning_validator/config/planning_validator_plotjugler_config.xml @@ -9,21 +9,21 @@ - + - + - + @@ -32,21 +32,21 @@ - + - + - + @@ -55,21 +55,21 @@ - + - + - + @@ -78,7 +78,7 @@ - + @@ -86,23 +86,23 @@ - - - - - + + + + + - - - - - - + + + + + + diff --git a/planning/planning_validator/image/planning_validator.drawio.svg b/planning/planning_validator/image/planning_validator.drawio.svg index 070c1a6d68b81..0a2267cba159e 100644 --- a/planning/planning_validator/image/planning_validator.drawio.svg +++ b/planning/planning_validator/image/planning_validator.drawio.svg @@ -38,12 +38,12 @@ >
- planning_validator + autoware_planning_validator
- planning_validator + autoware_planning_validator diff --git a/planning/planning_validator/include/planning_validator/debug_marker.hpp b/planning/planning_validator/include/autoware_planning_validator/debug_marker.hpp similarity index 91% rename from planning/planning_validator/include/planning_validator/debug_marker.hpp rename to planning/planning_validator/include/autoware_planning_validator/debug_marker.hpp index 4671a0b0d7297..955ba5bbf64ab 100644 --- a/planning/planning_validator/include/planning_validator/debug_marker.hpp +++ b/planning/planning_validator/include/autoware_planning_validator/debug_marker.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ -#define PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ +#ifndef AUTOWARE_PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ +#define AUTOWARE_PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ #include @@ -57,4 +57,4 @@ class PlanningValidatorDebugMarkerPublisher } }; -#endif // PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ +#endif // AUTOWARE_PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp similarity index 91% rename from planning/planning_validator/include/planning_validator/planning_validator.hpp rename to planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp index fed6e161c4d90..113c70fecb62c 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ -#define PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ +#ifndef AUTOWARE_PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ +#define AUTOWARE_PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ -#include "planning_validator/debug_marker.hpp" -#include "planning_validator/msg/planning_validator_status.hpp" +#include "autoware_planning_validator/debug_marker.hpp" +#include "autoware_planning_validator/msg/planning_validator_status.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -33,14 +33,14 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_validator::msg::PlanningValidatorStatus; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; using nav_msgs::msg::Odometry; -using planning_validator::msg::PlanningValidatorStatus; using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::Float64Stamped; @@ -141,6 +141,6 @@ class PlanningValidator : public rclcpp::Node StopWatch stop_watch_; }; -} // namespace planning_validator +} // namespace autoware::planning_validator -#endif // PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ +#endif // AUTOWARE_PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ diff --git a/planning/planning_validator/include/planning_validator/utils.hpp b/planning/planning_validator/include/autoware_planning_validator/utils.hpp similarity index 90% rename from planning/planning_validator/include/planning_validator/utils.hpp rename to planning/planning_validator/include/autoware_planning_validator/utils.hpp index 11eaf4f37f400..1c943f5a60e08 100644 --- a/planning/planning_validator/include/planning_validator/utils.hpp +++ b/planning/planning_validator/include/autoware_planning_validator/utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_VALIDATOR__UTILS_HPP_ -#define PLANNING_VALIDATOR__UTILS_HPP_ +#ifndef AUTOWARE_PLANNING_VALIDATOR__UTILS_HPP_ +#define AUTOWARE_PLANNING_VALIDATOR__UTILS_HPP_ #include @@ -23,7 +23,7 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -65,6 +65,6 @@ bool checkFinite(const TrajectoryPoint & point); void shiftPose(geometry_msgs::msg::Pose & pose, double longitudinal); -} // namespace planning_validator +} // namespace autoware::planning_validator -#endif // PLANNING_VALIDATOR__UTILS_HPP_ +#endif // AUTOWARE_PLANNING_VALIDATOR__UTILS_HPP_ diff --git a/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml b/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml index 3f73e0f462cc2..a1cdc7c7ecc55 100644 --- a/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml +++ b/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/planning/planning_validator/launch/planning_validator.launch.xml b/planning/planning_validator/launch/planning_validator.launch.xml index 81573adb4dc24..fa1475a0787e4 100644 --- a/planning/planning_validator/launch/planning_validator.launch.xml +++ b/planning/planning_validator/launch/planning_validator.launch.xml @@ -1,9 +1,9 @@ - + - + diff --git a/planning/planning_validator/package.xml b/planning/planning_validator/package.xml index 5a413f27dc7a3..7fe57e68928d9 100644 --- a/planning/planning_validator/package.xml +++ b/planning/planning_validator/package.xml @@ -1,9 +1,9 @@ - planning_validator + autoware_planning_validator 0.1.0 - ros node for planning_validator + ros node for autoware_planning_validator Takamasa Horibe Kosuke Takeuchi Apache License 2.0 diff --git a/planning/planning_validator/src/debug_marker.cpp b/planning/planning_validator/src/debug_marker.cpp index 82117d199c36e..413611a72b5ab 100644 --- a/planning/planning_validator/src/debug_marker.cpp +++ b/planning/planning_validator/src/debug_marker.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/debug_marker.hpp" +#include "autoware_planning_validator/debug_marker.hpp" #include #include @@ -90,7 +90,7 @@ void PlanningValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs: { const auto now = node_->get_clock()->now(); const auto stop_wall_marker = - motion_utils::createStopVirtualWallMarker(pose, "planning_validator", now, 0); + motion_utils::createStopVirtualWallMarker(pose, "autoware_planning_validator", now, 0); tier4_autoware_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); } diff --git a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp index 6cca99cd9f14c..645d6c92e3698 100644 --- a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp +++ b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp @@ -18,7 +18,7 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { InvalidTrajectoryPublisherNode::InvalidTrajectoryPublisherNode( const rclcpp::NodeOptions & node_options) @@ -72,7 +72,7 @@ void InvalidTrajectoryPublisherNode::onCurrentTrajectory(const Trajectory::Const traj_sub_.reset(); } -} // namespace planning_validator +} // namespace autoware::planning_validator #include -RCLCPP_COMPONENTS_REGISTER_NODE(planning_validator::InvalidTrajectoryPublisherNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::planning_validator::InvalidTrajectoryPublisherNode) diff --git a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp index 1d1e47ad10407..0025d759217bf 100644 --- a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp +++ b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp @@ -21,7 +21,7 @@ #include -namespace planning_validator +namespace autoware::planning_validator { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -41,6 +41,6 @@ class InvalidTrajectoryPublisherNode : public rclcpp::Node Trajectory::ConstSharedPtr current_trajectory_ = nullptr; }; -} // namespace planning_validator +} // namespace autoware::planning_validator #endif // INVALID_TRAJECTORY_PUBLISHER__INVALID_TRAJECTORY_PUBLISHER_HPP_ diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index d14b1d9ff37d2..aac50e24c5d5c 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/planning_validator.hpp" +#include "autoware_planning_validator/planning_validator.hpp" -#include "planning_validator/utils.hpp" +#include "autoware_planning_validator/utils.hpp" #include #include @@ -23,7 +23,7 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { using diagnostic_msgs::msg::DiagnosticStatus; @@ -584,7 +584,7 @@ void PlanningValidator::displayStatus() warn(s.is_valid_forward_trajectory_length, "planning trajectory forward length is not enough!!"); } -} // namespace planning_validator +} // namespace autoware::planning_validator #include -RCLCPP_COMPONENTS_REGISTER_NODE(planning_validator::PlanningValidator) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::planning_validator::PlanningValidator) diff --git a/planning/planning_validator/src/utils.cpp b/planning/planning_validator/src/utils.cpp index aada0bea2da9b..4710d3ae84ea3 100644 --- a/planning/planning_validator/src/utils.cpp +++ b/planning/planning_validator/src/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/utils.hpp" +#include "autoware_planning_validator/utils.hpp" #include #include @@ -22,7 +22,7 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { using tier4_autoware_utils::calcCurvature; using tier4_autoware_utils::calcDistance2d; @@ -324,4 +324,4 @@ void shiftPose(geometry_msgs::msg::Pose & pose, double longitudinal) pose.position.y += std::sin(yaw) * longitudinal; } -} // namespace planning_validator +} // namespace autoware::planning_validator diff --git a/planning/planning_validator/test/src/test_planning_validator_functions.cpp b/planning/planning_validator/test/src/test_planning_validator_functions.cpp index e406b6c5d7da2..41ae41a80bdc5 100644 --- a/planning/planning_validator/test/src/test_planning_validator_functions.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_functions.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/debug_marker.hpp" -#include "planning_validator/planning_validator.hpp" +#include "autoware_planning_validator/debug_marker.hpp" +#include "autoware_planning_validator/planning_validator.hpp" #include "test_parameter.hpp" #include "test_planning_validator_helper.hpp" @@ -21,8 +21,8 @@ #include +using autoware::planning_validator::PlanningValidator; using autoware_planning_msgs::msg::Trajectory; -using planning_validator::PlanningValidator; TEST(PlanningValidatorTestSuite, checkValidFiniteValueFunction) { diff --git a/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp b/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp index 2b2a32bf54618..78f57e0fe104d 100644 --- a/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/planning_validator.hpp" +#include "autoware_planning_validator/planning_validator.hpp" #include #include @@ -22,8 +22,8 @@ #include +using autoware::planning_validator::PlanningValidator; using planning_test_utils::PlanningInterfaceTestManager; -using planning_validator::PlanningValidator; std::shared_ptr generateTestManager() { @@ -46,7 +46,7 @@ std::shared_ptr generateNode() const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto planning_validator_dir = - ament_index_cpp::get_package_share_directory("planning_validator"); + ament_index_cpp::get_package_share_directory("autoware_planning_validator"); node_options.arguments( {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", diff --git a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp index 2329a3b9bb6c8..8cf56525edbc5 100644 --- a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/planning_validator.hpp" +#include "autoware_planning_validator/planning_validator.hpp" #include "test_parameter.hpp" #include "test_planning_validator_helper.hpp" @@ -29,11 +29,11 @@ * This test checks the diagnostics message published from the planning_validator node */ +using autoware::planning_validator::PlanningValidator; using autoware_planning_msgs::msg::Trajectory; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; -using planning_validator::PlanningValidator; constexpr double epsilon = 0.001; constexpr double scale_margin = 1.1; @@ -101,7 +101,8 @@ bool hasError(const std::vector & diags, const throw std::runtime_error(name + " is not contained in the diagnostic message."); } -std::pair, std::shared_ptr> +std::pair< + std::shared_ptr, std::shared_ptr> prepareTest(const Trajectory & trajectory, const Odometry & ego_odom) { auto validator = std::make_shared(getNodeOptionsWithDefaultParams()); diff --git a/planning/sampling_based_planner/bezier_sampler/CMakeLists.txt b/planning/sampling_based_planner/autoware_bezier_sampler/CMakeLists.txt similarity index 80% rename from planning/sampling_based_planner/bezier_sampler/CMakeLists.txt rename to planning/sampling_based_planner/autoware_bezier_sampler/CMakeLists.txt index 76d0fc99843f1..8d5bd554ab282 100644 --- a/planning/sampling_based_planner/bezier_sampler/CMakeLists.txt +++ b/planning/sampling_based_planner/autoware_bezier_sampler/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(bezier_sampler) +project(autoware_bezier_sampler) find_package(autoware_cmake REQUIRED) autoware_package() @@ -7,7 +7,7 @@ autoware_package() find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) -ament_auto_add_library(bezier_sampler SHARED +ament_auto_add_library(autoware_bezier_sampler SHARED src/bezier.cpp src/bezier_sampling.cpp ) diff --git a/planning/sampling_based_planner/bezier_sampler/README.md b/planning/sampling_based_planner/autoware_bezier_sampler/README.md similarity index 100% rename from planning/sampling_based_planner/bezier_sampler/README.md rename to planning/sampling_based_planner/autoware_bezier_sampler/README.md diff --git a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp b/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier.hpp similarity index 94% rename from planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp rename to planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier.hpp index 93c6ec6b87f36..5d16c55d793af 100644 --- a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp +++ b/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEZIER_SAMPLER__BEZIER_HPP_ -#define BEZIER_SAMPLER__BEZIER_HPP_ +#ifndef AUTOWARE_BEZIER_SAMPLER__BEZIER_HPP_ +#define AUTOWARE_BEZIER_SAMPLER__BEZIER_HPP_ #include #include -namespace bezier_sampler +namespace autoware::bezier_sampler { // Coefficients for matrix calculation of the quintic Bézier curve. @@ -70,6 +70,6 @@ class Bezier /// @brief calculate the curvature for the given parameter t [[nodiscard]] double curvature(const double t) const; }; -} // namespace bezier_sampler +} // namespace autoware::bezier_sampler -#endif // BEZIER_SAMPLER__BEZIER_HPP_ +#endif // AUTOWARE_BEZIER_SAMPLER__BEZIER_HPP_ diff --git a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp b/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier_sampling.hpp similarity index 80% rename from planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp rename to planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier_sampling.hpp index 18df9dcb796d8..a832ef1f5cd39 100644 --- a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp +++ b/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier_sampling.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ -#define BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ +#ifndef AUTOWARE_BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ +#define AUTOWARE_BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ -#include +#include +#include #include #include -#include #include -namespace bezier_sampler +namespace autoware::bezier_sampler { struct SamplingParameters { @@ -38,13 +38,13 @@ struct SamplingParameters /// @details from Section IV of A. Artuñedoet al.: Real-Time Motion Planning Approach for Automated /// Driving in Urban Environments std::vector sample( - const sampler_common::State & initial, const sampler_common::State & final, + const autoware::sampler_common::State & initial, const autoware::sampler_common::State & final, const SamplingParameters & params); /// @brief generate a Bezier curve for the given states, velocities, and accelerations Bezier generate( const Eigen::Vector2d & initial_pose, const Eigen::Vector2d & final_pose, const Eigen::Vector2d & initial_velocity, const Eigen::Vector2d & initial_acceleration, const Eigen::Vector2d & final_velocity, const Eigen::Vector2d & final_acceleration); -} // namespace bezier_sampler +} // namespace autoware::bezier_sampler -#endif // BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ +#endif // AUTOWARE_BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ diff --git a/planning/sampling_based_planner/bezier_sampler/package.xml b/planning/sampling_based_planner/autoware_bezier_sampler/package.xml similarity index 89% rename from planning/sampling_based_planner/bezier_sampler/package.xml rename to planning/sampling_based_planner/autoware_bezier_sampler/package.xml index efe65e830871a..62dce46816119 100644 --- a/planning/sampling_based_planner/bezier_sampler/package.xml +++ b/planning/sampling_based_planner/autoware_bezier_sampler/package.xml @@ -1,6 +1,6 @@ - bezier_sampler + autoware_bezier_sampler 0.0.1 Package to sample trajectories using Bézier curves Maxime CLEMENT @@ -11,8 +11,8 @@ autoware_cmake + autoware_sampler_common eigen - sampler_common ament_cmake_ros ament_lint_auto diff --git a/planning/sampling_based_planner/bezier_sampler/src/bezier.cpp b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier.cpp similarity index 96% rename from planning/sampling_based_planner/bezier_sampler/src/bezier.cpp rename to planning/sampling_based_planner/autoware_bezier_sampler/src/bezier.cpp index fc4c09840f507..9a882ad06659a 100644 --- a/planning/sampling_based_planner/bezier_sampler/src/bezier.cpp +++ b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include -namespace bezier_sampler +namespace autoware::bezier_sampler { Bezier::Bezier(Eigen::Matrix control_points) : control_points_(std::move(control_points)) @@ -117,4 +117,4 @@ double Bezier::heading(const double t) const return std::atan2(vel.y(), vel.x()); } -} // namespace bezier_sampler +} // namespace autoware::bezier_sampler diff --git a/planning/sampling_based_planner/bezier_sampler/src/bezier_sampling.cpp b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp similarity index 94% rename from planning/sampling_based_planner/bezier_sampler/src/bezier_sampling.cpp rename to planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp index a9fa318980250..e9a1a9ef32de5 100644 --- a/planning/sampling_based_planner/bezier_sampler/src/bezier_sampling.cpp +++ b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include -namespace bezier_sampler +namespace autoware::bezier_sampler { std::vector sample( - const sampler_common::State & initial, const sampler_common::State & final, + const autoware::sampler_common::State & initial, const autoware::sampler_common::State & final, const SamplingParameters & params) { std::vector curves; @@ -75,4 +75,4 @@ Bezier generate( (1.0 / 20.0) * final_acceleration.transpose(); return Bezier(control_points); } -} // namespace bezier_sampler +} // namespace autoware::bezier_sampler diff --git a/planning/sampling_based_planner/frenet_planner/CMakeLists.txt b/planning/sampling_based_planner/autoware_frenet_planner/CMakeLists.txt similarity index 78% rename from planning/sampling_based_planner/frenet_planner/CMakeLists.txt rename to planning/sampling_based_planner/autoware_frenet_planner/CMakeLists.txt index 0d7918daa215b..f928702a387b9 100644 --- a/planning/sampling_based_planner/frenet_planner/CMakeLists.txt +++ b/planning/sampling_based_planner/autoware_frenet_planner/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(frenet_planner) +project(autoware_frenet_planner) find_package(autoware_cmake REQUIRED) autoware_package() @@ -7,7 +7,7 @@ autoware_package() find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) -ament_auto_add_library(frenet_planner SHARED +ament_auto_add_library(autoware_frenet_planner SHARED DIRECTORY src/ ) diff --git a/planning/sampling_based_planner/frenet_planner/README.md b/planning/sampling_based_planner/autoware_frenet_planner/README.md similarity index 100% rename from planning/sampling_based_planner/frenet_planner/README.md rename to planning/sampling_based_planner/autoware_frenet_planner/README.md diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/conversions.hpp b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/conversions.hpp similarity index 81% rename from planning/sampling_based_planner/frenet_planner/include/frenet_planner/conversions.hpp rename to planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/conversions.hpp index f8575d8f65936..dc504ba1386eb 100644 --- a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/conversions.hpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/conversions.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FRENET_PLANNER__CONVERSIONS_HPP_ -#define FRENET_PLANNER__CONVERSIONS_HPP_ +#ifndef AUTOWARE_FRENET_PLANNER__CONVERSIONS_HPP_ +#define AUTOWARE_FRENET_PLANNER__CONVERSIONS_HPP_ -#include "frenet_planner/polynomials.hpp" +#include "autoware_frenet_planner/polynomials.hpp" -#include +#include #include -namespace frenet_planner +namespace autoware::frenet_planner { /// @brief calculate the lengths and yaws vectors of the given trajectory @@ -44,6 +44,6 @@ inline void calculateLengthsAndYaws(Trajectory & trajectory) const auto last_yaw = trajectory.yaws.empty() ? 0.0 : trajectory.yaws.back(); trajectory.yaws.push_back(last_yaw); } -} // namespace frenet_planner +} // namespace autoware::frenet_planner -#endif // FRENET_PLANNER__CONVERSIONS_HPP_ +#endif // AUTOWARE_FRENET_PLANNER__CONVERSIONS_HPP_ diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/frenet_planner.hpp similarity index 67% rename from planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp rename to planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/frenet_planner.hpp index 7d60214a52722..88eb2f3244e40 100644 --- a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/frenet_planner.hpp @@ -12,34 +12,34 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FRENET_PLANNER__FRENET_PLANNER_HPP_ -#define FRENET_PLANNER__FRENET_PLANNER_HPP_ +#ifndef AUTOWARE_FRENET_PLANNER__FRENET_PLANNER_HPP_ +#define AUTOWARE_FRENET_PLANNER__FRENET_PLANNER_HPP_ -#include "frenet_planner/structures.hpp" -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" +#include "autoware_frenet_planner/structures.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include #include -namespace frenet_planner +namespace autoware::frenet_planner { /// @brief generate trajectories relative to the reference for the given initial state and sampling /// parameters std::vector generateTrajectories( - const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, - const SamplingParameters & sampling_parameters); + const autoware::sampler_common::transform::Spline2D & reference_spline, + const FrenetState & initial_state, const SamplingParameters & sampling_parameters); /// @brief generate trajectories relative to the reference for the given initial state and sampling /// parameters std::vector generateLowVelocityTrajectories( - const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, - const SamplingParameters & sampling_parameters); + const autoware::sampler_common::transform::Spline2D & reference_spline, + const FrenetState & initial_state, const SamplingParameters & sampling_parameters); /// @brief generate paths relative to the reference for the given initial state and sampling /// parameters std::vector generatePaths( - const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, - const SamplingParameters & sampling_parameters); + const autoware::sampler_common::transform::Spline2D & reference_spline, + const FrenetState & initial_state, const SamplingParameters & sampling_parameters); /// @brief generate a candidate path /// @details one polynomial for lateral motion (d) is calculated over the longitudinal displacement /// (s): d(s). @@ -58,11 +58,12 @@ Trajectory generateLowVelocityCandidate( const FrenetState & initial_state, const FrenetState & target_state, const double duration, const double time_resolution); /// @brief calculate the cartesian frame of the given path -void calculateCartesian(const sampler_common::transform::Spline2D & reference, Path & path); +void calculateCartesian( + const autoware::sampler_common::transform::Spline2D & reference, Path & path); /// @brief calculate the cartesian frame of the given trajectory void calculateCartesian( - const sampler_common::transform::Spline2D & reference, Trajectory & trajectory); + const autoware::sampler_common::transform::Spline2D & reference, Trajectory & trajectory); -} // namespace frenet_planner +} // namespace autoware::frenet_planner -#endif // FRENET_PLANNER__FRENET_PLANNER_HPP_ +#endif // AUTOWARE_FRENET_PLANNER__FRENET_PLANNER_HPP_ diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/polynomials.hpp b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/polynomials.hpp similarity index 88% rename from planning/sampling_based_planner/frenet_planner/include/frenet_planner/polynomials.hpp rename to planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/polynomials.hpp index 65157c76d5db3..02217ef8ad444 100644 --- a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/polynomials.hpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/polynomials.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FRENET_PLANNER__POLYNOMIALS_HPP_ -#define FRENET_PLANNER__POLYNOMIALS_HPP_ +#ifndef AUTOWARE_FRENET_PLANNER__POLYNOMIALS_HPP_ +#define AUTOWARE_FRENET_PLANNER__POLYNOMIALS_HPP_ -namespace frenet_planner +namespace autoware::frenet_planner { class Polynomial { @@ -50,6 +50,6 @@ class Polynomial /// @brief Get the jerk at the given time [[nodiscard]] double jerk(const double t) const; }; -} // namespace frenet_planner +} // namespace autoware::frenet_planner -#endif // FRENET_PLANNER__POLYNOMIALS_HPP_ +#endif // AUTOWARE_FRENET_PLANNER__POLYNOMIALS_HPP_ diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/structures.hpp similarity index 73% rename from planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp rename to planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/structures.hpp index c44cb5d814d71..d2a71d06a7933 100644 --- a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/structures.hpp @@ -12,52 +12,54 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FRENET_PLANNER__STRUCTURES_HPP_ -#define FRENET_PLANNER__STRUCTURES_HPP_ +#ifndef AUTOWARE_FRENET_PLANNER__STRUCTURES_HPP_ +#define AUTOWARE_FRENET_PLANNER__STRUCTURES_HPP_ -#include "frenet_planner/polynomials.hpp" +#include "autoware_frenet_planner/polynomials.hpp" -#include +#include #include #include -namespace frenet_planner +namespace autoware::frenet_planner { struct FrenetState { - sampler_common::FrenetPoint position = {0, 0}; + autoware::sampler_common::FrenetPoint position = {0, 0}; double lateral_velocity{}; double longitudinal_velocity{}; double lateral_acceleration{}; double longitudinal_acceleration{}; }; -struct Path : sampler_common::Path +struct Path : autoware::sampler_common::Path { - std::vector frenet_points{}; + std::vector frenet_points{}; std::optional lateral_polynomial{}; Path() = default; - explicit Path(const sampler_common::Path & path) : sampler_common::Path(path) {} + explicit Path(const autoware::sampler_common::Path & path) : autoware::sampler_common::Path(path) + { + } void clear() override { - sampler_common::Path::clear(); + autoware::sampler_common::Path::clear(); frenet_points.clear(); lateral_polynomial.reset(); } void reserve(const size_t size) override { - sampler_common::Path::reserve(size); + autoware::sampler_common::Path::reserve(size); frenet_points.reserve(size); } [[nodiscard]] Path extend(const Path & path) const { - Path extended_traj(sampler_common::Path::extend(path)); + Path extended_traj(autoware::sampler_common::Path::extend(path)); extended_traj.frenet_points.insert( extended_traj.frenet_points.end(), frenet_points.begin(), frenet_points.end()); extended_traj.frenet_points.insert( @@ -69,7 +71,7 @@ struct Path : sampler_common::Path [[nodiscard]] Path * subset(const size_t from_idx, const size_t to_idx) const override { - auto * subpath = new Path(*sampler_common::Path::subset(from_idx, to_idx)); + auto * subpath = new Path(*autoware::sampler_common::Path::subset(from_idx, to_idx)); assert(to_idx >= from_idx); subpath->reserve(to_idx - from_idx); @@ -87,19 +89,22 @@ struct SamplingParameter FrenetState target_state; }; -struct Trajectory : sampler_common::Trajectory +struct Trajectory : autoware::sampler_common::Trajectory { - std::vector frenet_points{}; + std::vector frenet_points{}; std::optional lateral_polynomial{}; std::optional longitudinal_polynomial{}; SamplingParameter sampling_parameter; Trajectory() = default; - explicit Trajectory(const sampler_common::Trajectory & traj) : sampler_common::Trajectory(traj) {} + explicit Trajectory(const autoware::sampler_common::Trajectory & traj) + : autoware::sampler_common::Trajectory(traj) + { + } void clear() override { - sampler_common::Trajectory::clear(); + autoware::sampler_common::Trajectory::clear(); frenet_points.clear(); lateral_polynomial.reset(); longitudinal_polynomial.reset(); @@ -107,13 +112,13 @@ struct Trajectory : sampler_common::Trajectory void reserve(const size_t size) override { - sampler_common::Trajectory::reserve(size); + autoware::sampler_common::Trajectory::reserve(size); frenet_points.reserve(size); } [[nodiscard]] Trajectory extend(const Trajectory & traj) const { - Trajectory extended_traj(sampler_common::Trajectory::extend(traj)); + Trajectory extended_traj(autoware::sampler_common::Trajectory::extend(traj)); extended_traj.frenet_points.insert( extended_traj.frenet_points.end(), frenet_points.begin(), frenet_points.end()); extended_traj.frenet_points.insert( @@ -126,7 +131,8 @@ struct Trajectory : sampler_common::Trajectory [[nodiscard]] Trajectory * subset(const size_t from_idx, const size_t to_idx) const override { - auto * sub_trajectory = new Trajectory(*sampler_common::Trajectory::subset(from_idx, to_idx)); + auto * sub_trajectory = + new Trajectory(*autoware::sampler_common::Trajectory::subset(from_idx, to_idx)); assert(to_idx >= from_idx); sub_trajectory->reserve(to_idx - from_idx); @@ -151,6 +157,6 @@ struct SamplingParameters std::vector parameters; double resolution; }; -} // namespace frenet_planner +} // namespace autoware::frenet_planner -#endif // FRENET_PLANNER__STRUCTURES_HPP_ +#endif // AUTOWARE_FRENET_PLANNER__STRUCTURES_HPP_ diff --git a/planning/sampling_based_planner/frenet_planner/launch/frenet_planner.launch b/planning/sampling_based_planner/autoware_frenet_planner/launch/frenet_planner.launch similarity index 78% rename from planning/sampling_based_planner/frenet_planner/launch/frenet_planner.launch rename to planning/sampling_based_planner/autoware_frenet_planner/launch/frenet_planner.launch index 9570d488eaecc..69e5961b9cdb8 100644 --- a/planning/sampling_based_planner/frenet_planner/launch/frenet_planner.launch +++ b/planning/sampling_based_planner/autoware_frenet_planner/launch/frenet_planner.launch @@ -3,7 +3,7 @@ - + diff --git a/planning/sampling_based_planner/frenet_planner/package.xml b/planning/sampling_based_planner/autoware_frenet_planner/package.xml similarity index 76% rename from planning/sampling_based_planner/frenet_planner/package.xml rename to planning/sampling_based_planner/autoware_frenet_planner/package.xml index c378c6ae25e9b..1ba7c8a148007 100644 --- a/planning/sampling_based_planner/frenet_planner/package.xml +++ b/planning/sampling_based_planner/autoware_frenet_planner/package.xml @@ -1,8 +1,8 @@ - frenet_planner + autoware_frenet_planner 0.0.1 - The frenet_planner package for trajectory generation in Frenet frame + The autoware_frenet_planner package for trajectory generation in Frenet frame Maxime CLEMENT Maxime CLEMENT Apache License 2.0 @@ -12,7 +12,7 @@ autoware_cmake autoware_auto_common - sampler_common + autoware_sampler_common ament_cmake_ros ament_lint_auto diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp similarity index 89% rename from planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp rename to planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp index 6f0bef882743c..ba3156c6b085d 100644 --- a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "frenet_planner/frenet_planner.hpp" +#include "autoware_frenet_planner/frenet_planner.hpp" #include +#include +#include +#include +#include +#include #include -#include -#include -#include -#include -#include #include "autoware_planning_msgs/msg/path.hpp" #include @@ -31,11 +31,11 @@ #include #include -namespace frenet_planner +namespace autoware::frenet_planner { std::vector generateTrajectories( - const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, - const SamplingParameters & sampling_parameters) + const autoware::sampler_common::transform::Spline2D & reference_spline, + const FrenetState & initial_state, const SamplingParameters & sampling_parameters) { std::vector trajectories; trajectories.reserve(sampling_parameters.parameters.size()); @@ -54,8 +54,8 @@ std::vector generateTrajectories( } std::vector generateLowVelocityTrajectories( - const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, - const SamplingParameters & sampling_parameters) + const autoware::sampler_common::transform::Spline2D & reference_spline, + const FrenetState & initial_state, const SamplingParameters & sampling_parameters) { std::vector trajectories; trajectories.reserve(sampling_parameters.parameters.size()); @@ -73,8 +73,8 @@ std::vector generateLowVelocityTrajectories( } std::vector generatePaths( - const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, - const SamplingParameters & sampling_parameters) + const autoware::sampler_common::transform::Spline2D & reference_spline, + const FrenetState & initial_state, const SamplingParameters & sampling_parameters) { std::vector candidates; candidates.reserve(sampling_parameters.parameters.size()); @@ -148,7 +148,8 @@ Path generateCandidate( return path; } -void calculateCartesian(const sampler_common::transform::Spline2D & reference, Path & path) +void calculateCartesian( + const autoware::sampler_common::transform::Spline2D & reference, Path & path) { if (!path.frenet_points.empty()) { path.points.reserve(path.frenet_points.size()); @@ -191,7 +192,7 @@ void calculateCartesian(const sampler_common::transform::Spline2D & reference, P } } void calculateCartesian( - const sampler_common::transform::Spline2D & reference, Trajectory & trajectory) + const autoware::sampler_common::transform::Spline2D & reference, Trajectory & trajectory) { if (!trajectory.frenet_points.empty()) { trajectory.points.reserve(trajectory.frenet_points.size()); @@ -240,4 +241,4 @@ void calculateCartesian( } } -} // namespace frenet_planner +} // namespace autoware::frenet_planner diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/polynomials.cpp b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/polynomials.cpp similarity index 94% rename from planning/sampling_based_planner/frenet_planner/src/frenet_planner/polynomials.cpp rename to planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/polynomials.cpp index 13f71dc88f9d6..b785255cd856c 100644 --- a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/polynomials.cpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/polynomials.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include -namespace frenet_planner +namespace autoware::frenet_planner { // @brief Create a polynomial connecting the given initial and target configuration over the given // duration @@ -72,4 +72,4 @@ double Polynomial::jerk(const double t) const const double t2 = t * t; return 60 * a_ * t2 + 24 * b_ * t + 6 * c_; } -} // namespace frenet_planner +} // namespace autoware::frenet_planner diff --git a/planning/sampling_based_planner/path_sampler/CMakeLists.txt b/planning/sampling_based_planner/autoware_path_sampler/CMakeLists.txt similarity index 52% rename from planning/sampling_based_planner/path_sampler/CMakeLists.txt rename to planning/sampling_based_planner/autoware_path_sampler/CMakeLists.txt index 5a5b99f0aff3a..170646b0c0c9a 100644 --- a/planning/sampling_based_planner/path_sampler/CMakeLists.txt +++ b/planning/sampling_based_planner/autoware_path_sampler/CMakeLists.txt @@ -1,16 +1,16 @@ cmake_minimum_required(VERSION 3.14) -project(path_sampler) +project(autoware_path_sampler) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(path_sampler SHARED +ament_auto_add_library(autoware_path_sampler SHARED DIRECTORY src ) # register node -rclcpp_components_register_node(path_sampler - PLUGIN "path_sampler::PathSampler" +rclcpp_components_register_node(autoware_path_sampler + PLUGIN "autoware::path_sampler::PathSampler" EXECUTABLE path_sampler_exe ) diff --git a/planning/sampling_based_planner/path_sampler/README.md b/planning/sampling_based_planner/autoware_path_sampler/README.md similarity index 100% rename from planning/sampling_based_planner/path_sampler/README.md rename to planning/sampling_based_planner/autoware_path_sampler/README.md diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp similarity index 87% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp index 7cf88cb75e13a..5a8183d8e5926 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__COMMON_STRUCTS_HPP_ -#define PATH_SAMPLER__COMMON_STRUCTS_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__COMMON_STRUCTS_HPP_ +#define AUTOWARE_PATH_SAMPLER__COMMON_STRUCTS_HPP_ -#include "path_sampler/type_alias.hpp" +#include "autoware_path_sampler/type_alias.hpp" +#include "autoware_sampler_common/structures.hpp" #include "rclcpp/rclcpp.hpp" -#include "sampler_common/structures.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" @@ -28,7 +28,7 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { struct PlannerData { @@ -61,7 +61,7 @@ struct TimeKeeper latest_stream.str(""); if (enable_calculation_time_info) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("path_sampler.time"), msg); + RCLCPP_INFO_STREAM(rclcpp::get_logger("autoware_path_sampler.time"), msg); } } @@ -87,7 +87,7 @@ struct TimeKeeper struct DebugData { - std::vector sampled_candidates{}; + std::vector sampled_candidates{}; size_t previous_sampled_candidates_nb = 0UL; std::vector obstacles{}; std::vector footprints{}; @@ -131,6 +131,6 @@ struct EgoNearestParam double dist_threshold{0.0}; double yaw_threshold{0.0}; }; -} // namespace path_sampler +} // namespace autoware::path_sampler -#endif // PATH_SAMPLER__COMMON_STRUCTS_HPP_ +#endif // AUTOWARE_PATH_SAMPLER__COMMON_STRUCTS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp similarity index 77% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp index ab9bc71bc79e4..e44c38aed2b53 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__NODE_HPP_ -#define PATH_SAMPLER__NODE_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__NODE_HPP_ +#define AUTOWARE_PATH_SAMPLER__NODE_HPP_ -#include "path_sampler/common_structs.hpp" -#include "path_sampler/parameters.hpp" -#include "path_sampler/type_alias.hpp" +#include "autoware_path_sampler/common_structs.hpp" +#include "autoware_path_sampler/parameters.hpp" +#include "autoware_path_sampler/type_alias.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" #include "rclcpp/rclcpp.hpp" -#include "sampler_common/transform/spline_transform.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" -#include +#include #include #include @@ -30,7 +30,7 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { class PathSampler : public rclcpp::Node { @@ -54,7 +54,7 @@ class PathSampler : public rclcpp::Node PredictedObjects::SharedPtr in_objects_ptr_ = std::make_shared(); // variables for previous information - std::optional prev_path_; + std::optional prev_path_; // interface publisher rclcpp::Publisher::SharedPtr traj_pub_; @@ -86,8 +86,9 @@ class PathSampler : public rclcpp::Node const std::vector & traj_points, const std::vector & optimized_points) const; void resetPreviousData(); - sampler_common::State getPlanningState( - sampler_common::State & state, const sampler_common::transform::Spline2D & path_spline) const; + autoware::sampler_common::State getPlanningState( + autoware::sampler_common::State & state, + const autoware::sampler_common::transform::Spline2D & path_spline) const; // sub-functions of generateTrajectory void copyZ( @@ -95,13 +96,14 @@ class PathSampler : public rclcpp::Node void copyVelocity( const std::vector & from_traj, std::vector & to_traj, const geometry_msgs::msg::Pose & ego_pose); - sampler_common::Path generatePath(const PlannerData & planner_data); - std::vector generateCandidatesFromPreviousPath( - const PlannerData & planner_data, const sampler_common::transform::Spline2D & path_spline); + autoware::sampler_common::Path generatePath(const PlannerData & planner_data); + std::vector generateCandidatesFromPreviousPath( + const PlannerData & planner_data, + const autoware::sampler_common::transform::Spline2D & path_spline); std::vector generateTrajectoryPoints(const PlannerData & planner_data); void publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const; void publishDebugMarker(const std::vector & traj_points) const; }; -} // namespace path_sampler +} // namespace autoware::path_sampler -#endif // PATH_SAMPLER__NODE_HPP_ +#endif // AUTOWARE_PATH_SAMPLER__NODE_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/parameters.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/parameters.hpp similarity index 77% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/parameters.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/parameters.hpp index 03887d8357e0d..0ce0ea875c066 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/parameters.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/parameters.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__PARAMETERS_HPP_ -#define PATH_SAMPLER__PARAMETERS_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__PARAMETERS_HPP_ +#define AUTOWARE_PATH_SAMPLER__PARAMETERS_HPP_ -#include "bezier_sampler/bezier_sampling.hpp" -#include "sampler_common/structures.hpp" +#include "autoware_bezier_sampler/bezier_sampling.hpp" +#include "autoware_sampler_common/structures.hpp" #include struct Parameters { - sampler_common::Constraints constraints; + autoware::sampler_common::Constraints constraints; struct { bool enable_frenet{}; @@ -37,7 +37,7 @@ struct Parameters std::vector target_lateral_velocities{}; std::vector target_lateral_accelerations{}; } frenet; - bezier_sampler::SamplingParameters bezier{}; + autoware::bezier_sampler::SamplingParameters bezier{}; } sampling; struct @@ -54,4 +54,4 @@ struct Parameters } path_reuse{}; }; -#endif // PATH_SAMPLER__PARAMETERS_HPP_ +#endif // AUTOWARE_PATH_SAMPLER__PARAMETERS_HPP_ diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/path_generation.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/path_generation.hpp new file mode 100644 index 0000000000000..ef234f1d8207a --- /dev/null +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/path_generation.hpp @@ -0,0 +1,52 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_PATH_SAMPLER__PATH_GENERATION_HPP_ +#define AUTOWARE_PATH_SAMPLER__PATH_GENERATION_HPP_ + +#include "autoware_bezier_sampler/bezier_sampling.hpp" +#include "autoware_frenet_planner/structures.hpp" +#include "autoware_path_sampler/parameters.hpp" +#include "autoware_sampler_common/constraints/hard_constraint.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" + +#include + +#include + +namespace autoware::path_sampler +{ +/** + * @brief generate candidate paths for the given problem inputs + * @param [in] initial_state initial ego state + * @param [in] path_spline spline of the reference path + * @param [in] base_length base length of the reuse path (= 0.0 if not reusing a previous path) + * @param [in] params parameters + * @return candidate paths + */ +std::vector generateCandidatePaths( + const autoware::sampler_common::State & initial_state, + const autoware::sampler_common::transform::Spline2D & path_spline, const double base_length, + const Parameters & params); + +std::vector generateBezierPaths( + const autoware::sampler_common::State & initial_state, const double base_length, + const autoware::sampler_common::transform::Spline2D & path_spline, const Parameters & params); +std::vector generateFrenetPaths( + const autoware::sampler_common::State & initial_state, const double base_length, + const autoware::sampler_common::transform::Spline2D & path_spline, const Parameters & params); +} // namespace autoware::path_sampler + +#endif // AUTOWARE_PATH_SAMPLER__PATH_GENERATION_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/prepare_inputs.hpp similarity index 64% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/prepare_inputs.hpp index a9a2728b27c49..1b10be1121481 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/prepare_inputs.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__PREPARE_INPUTS_HPP_ -#define PATH_SAMPLER__PREPARE_INPUTS_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__PREPARE_INPUTS_HPP_ +#define AUTOWARE_PATH_SAMPLER__PREPARE_INPUTS_HPP_ -#include "frenet_planner/structures.hpp" -#include "path_sampler/parameters.hpp" -#include "path_sampler/type_alias.hpp" -#include "sampler_common/transform/spline_transform.hpp" +#include "autoware_frenet_planner/structures.hpp" +#include "autoware_path_sampler/parameters.hpp" +#include "autoware_path_sampler/type_alias.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" #include #include @@ -35,20 +35,20 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { /// @brief prepare constraints void prepareConstraints( - sampler_common::Constraints & constraints, const PredictedObjects & predicted_objects, + autoware::sampler_common::Constraints & constraints, const PredictedObjects & predicted_objects, const std::vector & left_bound, const std::vector & right_bound); /// @brief prepare sampling parameters to generate Frenet paths -frenet_planner::SamplingParameters prepareSamplingParameters( - const sampler_common::State & initial_state, const double base_length, - const sampler_common::transform::Spline2D & path_spline, const Parameters & params); +autoware::frenet_planner::SamplingParameters prepareSamplingParameters( + const autoware::sampler_common::State & initial_state, const double base_length, + const autoware::sampler_common::transform::Spline2D & path_spline, const Parameters & params); /// @brief prepare the 2D spline representation of the given Path message -sampler_common::transform::Spline2D preparePathSpline( +autoware::sampler_common::transform::Spline2D preparePathSpline( const std::vector & path_msg, const bool smooth_path); -} // namespace path_sampler +} // namespace autoware::path_sampler -#endif // PATH_SAMPLER__PREPARE_INPUTS_HPP_ +#endif // AUTOWARE_PATH_SAMPLER__PREPARE_INPUTS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/type_alias.hpp similarity index 88% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/type_alias.hpp index 90e655c086444..95d833c4df47d 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/type_alias.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__TYPE_ALIAS_HPP_ -#define PATH_SAMPLER__TYPE_ALIAS_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__TYPE_ALIAS_HPP_ +#define AUTOWARE_PATH_SAMPLER__TYPE_ALIAS_HPP_ #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/path.hpp" @@ -28,7 +28,7 @@ #include "tier4_debug_msgs/msg/string_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" -namespace path_sampler +namespace autoware::path_sampler { // std_msgs using std_msgs::msg::Header; @@ -46,6 +46,6 @@ using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; // debug using tier4_debug_msgs::msg::StringStamped; -} // namespace path_sampler +} // namespace autoware::path_sampler -#endif // PATH_SAMPLER__TYPE_ALIAS_HPP_ +#endif // AUTOWARE_PATH_SAMPLER__TYPE_ALIAS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp similarity index 81% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp index 875b81b9cd49a..abd2fa367bc7b 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ -#define PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ +#define AUTOWARE_PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ +#include "autoware_path_sampler/common_structs.hpp" +#include "autoware_path_sampler/type_alias.hpp" #include "eigen3/Eigen/Core" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "path_sampler/common_structs.hpp" -#include "path_sampler/type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" @@ -35,7 +35,7 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { namespace geometry_utils { @@ -52,5 +52,5 @@ bool isSamePoint(const T1 & t1, const T2 & t2) return true; } } // namespace geometry_utils -} // namespace path_sampler -#endif // PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ +} // namespace autoware::path_sampler +#endif // AUTOWARE_PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp similarity index 90% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp index a036dcfe638c2..4f346ae910f44 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ -#define PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE_PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ +#include "autoware_path_sampler/common_structs.hpp" +#include "autoware_path_sampler/type_alias.hpp" +#include "autoware_sampler_common/structures.hpp" #include "eigen3/Eigen/Core" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "path_sampler/common_structs.hpp" -#include "path_sampler/type_alias.hpp" -#include "sampler_common/structures.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -33,7 +33,7 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { namespace trajectory_utils { @@ -95,7 +95,8 @@ std::vector convertToTrajectoryPoints(const std::vector & po return traj_points; } -inline std::vector convertToTrajectoryPoints(const sampler_common::Path & path) +inline std::vector convertToTrajectoryPoints( + const autoware::sampler_common::Path & path) { std::vector traj_points; for (auto i = 0UL; i < path.points.size(); ++i) { @@ -160,7 +161,7 @@ std::optional updateFrontPointForFix( motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); if (0 < lon_offset_to_prev_front) { RCLCPP_WARN( - rclcpp::get_logger("path_sampler.trajectory_utils"), + rclcpp::get_logger("autoware_path_sampler.trajectory_utils"), "Fixed point will not be inserted due to the error during calculation."); return std::nullopt; } @@ -171,7 +172,7 @@ std::optional updateFrontPointForFix( constexpr double max_lat_error = 3.0; if (max_lat_error < dist) { RCLCPP_WARN( - rclcpp::get_logger("path_sampler.trajectory_utils"), + rclcpp::get_logger("autoware_path_sampler.trajectory_utils"), "New Fixed point is too far from points %f [m]", dist); } @@ -193,5 +194,5 @@ void insertStopPoint( std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, const size_t stop_seg_idx); } // namespace trajectory_utils -} // namespace path_sampler -#endif // PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ +} // namespace autoware::path_sampler +#endif // AUTOWARE_PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/package.xml b/planning/sampling_based_planner/autoware_path_sampler/package.xml similarity index 92% rename from planning/sampling_based_planner/path_sampler/package.xml rename to planning/sampling_based_planner/autoware_path_sampler/package.xml index a57ff6e6033f6..011da8d469607 100644 --- a/planning/sampling_based_planner/path_sampler/package.xml +++ b/planning/sampling_based_planner/autoware_path_sampler/package.xml @@ -1,7 +1,7 @@ - path_sampler + autoware_path_sampler 0.1.0 Package for the sampling-based path planner Maxime CLEMENT @@ -13,10 +13,10 @@ autoware_cmake + autoware_bezier_sampler + autoware_frenet_planner autoware_perception_msgs autoware_planning_msgs - bezier_sampler - frenet_planner geometry_msgs interpolation motion_utils diff --git a/planning/sampling_based_planner/path_sampler/src/node.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp similarity index 94% rename from planning/sampling_based_planner/path_sampler/src/node.cpp rename to planning/sampling_based_planner/autoware_path_sampler/src/node.cpp index 97bafb68c47a8..a82c1b6a52a26 100644 --- a/planning/sampling_based_planner/path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp @@ -12,25 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "path_sampler/node.hpp" - +#include "autoware_path_sampler/node.hpp" + +#include "autoware_path_sampler/path_generation.hpp" +#include "autoware_path_sampler/prepare_inputs.hpp" +#include "autoware_path_sampler/utils/geometry_utils.hpp" +#include "autoware_path_sampler/utils/trajectory_utils.hpp" +#include "autoware_sampler_common/constraints/hard_constraint.hpp" +#include "autoware_sampler_common/constraints/soft_constraint.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/marker/marker_helper.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "path_sampler/path_generation.hpp" -#include "path_sampler/prepare_inputs.hpp" -#include "path_sampler/utils/geometry_utils.hpp" -#include "path_sampler/utils/trajectory_utils.hpp" #include "rclcpp/time.hpp" -#include "sampler_common/constraints/hard_constraint.hpp" -#include "sampler_common/constraints/soft_constraint.hpp" #include #include #include -namespace path_sampler +namespace autoware::path_sampler { namespace { @@ -220,8 +220,9 @@ void PathSampler::objectsCallback(const PredictedObjects::SharedPtr msg) in_objects_ptr_ = msg; } -sampler_common::State PathSampler::getPlanningState( - sampler_common::State & state, const sampler_common::transform::Spline2D & path_spline) const +autoware::sampler_common::State PathSampler::getPlanningState( + autoware::sampler_common::State & state, + const autoware::sampler_common::transform::Spline2D & path_spline) const { state.frenet = path_spline.frenet(state.pose); if (params_.preprocessing.force_zero_deviation) { @@ -390,13 +391,14 @@ std::vector PathSampler::generateTrajectory(const PlannerData & return generated_traj_points; } -std::vector PathSampler::generateCandidatesFromPreviousPath( - const PlannerData & planner_data, const sampler_common::transform::Spline2D & path_spline) +std::vector PathSampler::generateCandidatesFromPreviousPath( + const PlannerData & planner_data, + const autoware::sampler_common::transform::Spline2D & path_spline) { - std::vector candidates; + std::vector candidates; if (!prev_path_ || prev_path_->points.size() < 2) return candidates; // Update previous path - sampler_common::State current_state; + autoware::sampler_common::State current_state; current_state.pose = {planner_data.ego_pose.position.x, planner_data.ego_pose.position.y}; current_state.heading = tf2::getYaw(planner_data.ego_pose.orientation); const auto closest_iter = std::min_element( @@ -419,7 +421,7 @@ std::vector PathSampler::generateCandidatesFromPreviousPat const auto reuse_step = prev_path_length / params_.sampling.previous_path_reuse_points_nb; for (double reuse_length = reuse_step; reuse_length <= prev_path_length; reuse_length += reuse_step) { - sampler_common::State reuse_state; + autoware::sampler_common::State reuse_state; size_t reuse_idx = 0; for (reuse_idx = 0; reuse_idx + 1 < prev_path_->lengths.size() && prev_path_->lengths[reuse_idx] < reuse_length; @@ -438,13 +440,13 @@ std::vector PathSampler::generateCandidatesFromPreviousPat return candidates; } -sampler_common::Path PathSampler::generatePath(const PlannerData & planner_data) +autoware::sampler_common::Path PathSampler::generatePath(const PlannerData & planner_data) { time_keeper_ptr_->tic(__func__); - sampler_common::Path generated_path{}; + autoware::sampler_common::Path generated_path{}; if (prev_path_ && prev_path_->points.size() > 1) { - sampler_common::constraints::checkHardConstraints(*prev_path_, params_.constraints); + autoware::sampler_common::constraints::checkHardConstraints(*prev_path_, params_.constraints); if (prev_path_->constraint_results.isValid()) { const auto prev_path_spline = preparePathSpline(trajectory_utils::convertToTrajectoryPoints(*prev_path_), false); @@ -460,7 +462,7 @@ sampler_common::Path PathSampler::generatePath(const PlannerData & planner_data) } const auto path_spline = preparePathSpline(planner_data.traj_points, params_.preprocessing.smooth_reference); - sampler_common::State current_state; + autoware::sampler_common::State current_state; current_state.pose = {planner_data.ego_pose.position.x, planner_data.ego_pose.position.y}; current_state.heading = tf2::getYaw(planner_data.ego_pose.orientation); @@ -476,9 +478,9 @@ sampler_common::Path PathSampler::generatePath(const PlannerData & planner_data) debug_data_.footprints.clear(); for (auto & path : candidate_paths) { const auto footprint = - sampler_common::constraints::checkHardConstraints(path, params_.constraints); + autoware::sampler_common::constraints::checkHardConstraints(path, params_.constraints); debug_data_.footprints.push_back(footprint); - sampler_common::constraints::calculateCost(path, params_.constraints, path_spline); + autoware::sampler_common::constraints::calculateCost(path, params_.constraints, path_spline); } const auto best_path_idx = [](const auto & paths) { auto min_cost = std::numeric_limits::max(); @@ -701,7 +703,7 @@ std::vector PathSampler::extendTrajectory( time_keeper_ptr_->toc(__func__, " "); return resampled_traj_points; } -} // namespace path_sampler +} // namespace autoware::path_sampler #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(path_sampler::PathSampler) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::path_sampler::PathSampler) diff --git a/planning/sampling_based_planner/path_sampler/src/path_generation.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/path_generation.cpp similarity index 72% rename from planning/sampling_based_planner/path_sampler/src/path_generation.cpp rename to planning/sampling_based_planner/autoware_path_sampler/src/path_generation.cpp index 2d1587c7b45a3..63ca5e18f423b 100644 --- a/planning/sampling_based_planner/path_sampler/src/path_generation.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/path_generation.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "path_sampler/path_generation.hpp" +#include "autoware_path_sampler/path_generation.hpp" -#include "sampler_common/structures.hpp" +#include "autoware_sampler_common/structures.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -29,14 +29,14 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { -std::vector generateCandidatePaths( - const sampler_common::State & initial_state, - const sampler_common::transform::Spline2D & path_spline, const double base_length, +std::vector generateCandidatePaths( + const autoware::sampler_common::State & initial_state, + const autoware::sampler_common::transform::Spline2D & path_spline, const double base_length, const Parameters & params) { - std::vector paths; + std::vector paths; const auto move_to_paths = [&](auto & paths_to_move) { paths.insert( paths.end(), std::make_move_iterator(paths_to_move.begin()), @@ -53,27 +53,27 @@ std::vector generateCandidatePaths( } return paths; } -std::vector generateBezierPaths( - const sampler_common::State & initial_state, const double base_length, - const sampler_common::transform::Spline2D & path_spline, const Parameters & params) +std::vector generateBezierPaths( + const autoware::sampler_common::State & initial_state, const double base_length, + const autoware::sampler_common::transform::Spline2D & path_spline, const Parameters & params) { const auto initial_s = path_spline.frenet(initial_state.pose).s; const auto max_s = path_spline.lastS(); - std::vector bezier_paths; + std::vector bezier_paths; for (const auto target_length : params.sampling.target_lengths) { if (target_length <= base_length) continue; const auto target_s = std::min(max_s, initial_s + target_length - base_length); if (target_s >= max_s) break; - sampler_common::State target_state{}; + autoware::sampler_common::State target_state{}; target_state.pose = path_spline.cartesian({target_s, 0}); target_state.curvature = path_spline.curvature(target_s); target_state.heading = path_spline.yaw(target_s); const auto bezier_samples = - bezier_sampler::sample(initial_state, target_state, params.sampling.bezier); + autoware::bezier_sampler::sample(initial_state, target_state, params.sampling.bezier); const auto step = std::min(0.1, params.sampling.resolution / target_length); for (const auto & bezier : bezier_samples) { - sampler_common::Path path; + autoware::sampler_common::Path path; path.lengths.push_back(0.0); for (double t = 0.0; t <= 1.0; t += step) { const auto x_y = bezier.valueM(t); @@ -93,14 +93,14 @@ std::vector generateBezierPaths( return bezier_paths; } -std::vector generateFrenetPaths( - const sampler_common::State & initial_state, const double base_length, - const sampler_common::transform::Spline2D & path_spline, const Parameters & params) +std::vector generateFrenetPaths( + const autoware::sampler_common::State & initial_state, const double base_length, + const autoware::sampler_common::transform::Spline2D & path_spline, const Parameters & params) { const auto sampling_parameters = prepareSamplingParameters(initial_state, base_length, path_spline, params); - frenet_planner::FrenetState initial_frenet_state; + autoware::frenet_planner::FrenetState initial_frenet_state; initial_frenet_state.position = path_spline.frenet(initial_state.pose); const auto s = initial_frenet_state.position.s; const auto d = initial_frenet_state.position.d; @@ -121,6 +121,7 @@ std::vector generateFrenetPaths( ((1 - path_curvature * d) / (cos_yaw * cos_yaw)) * (initial_state.curvature * ((1 - path_curvature * d) / cos_yaw) - path_curvature); } - return frenet_planner::generatePaths(path_spline, initial_frenet_state, sampling_parameters); + return autoware::frenet_planner::generatePaths( + path_spline, initial_frenet_state, sampling_parameters); } -} // namespace path_sampler +} // namespace autoware::path_sampler diff --git a/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp similarity index 84% rename from planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp rename to planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp index fd52764950ca9..64a09087666ff 100644 --- a/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "path_sampler/prepare_inputs.hpp" +#include "autoware_path_sampler/prepare_inputs.hpp" -#include "frenet_planner/structures.hpp" -#include "path_sampler/utils/geometry_utils.hpp" -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" +#include "autoware_frenet_planner/structures.hpp" +#include "autoware_path_sampler/utils/geometry_utils.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -29,22 +29,22 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { void prepareConstraints( - sampler_common::Constraints & constraints, const PredictedObjects & predicted_objects, + autoware::sampler_common::Constraints & constraints, const PredictedObjects & predicted_objects, const std::vector & left_bound, const std::vector & right_bound) { - constraints.obstacle_polygons = sampler_common::MultiPolygon2d(); + constraints.obstacle_polygons = autoware::sampler_common::MultiPolygon2d(); for (const auto & o : predicted_objects.objects) if (o.kinematics.initial_twist_with_covariance.twist.linear.x < 0.5) // TODO(Maxime): param constraints.obstacle_polygons.push_back(tier4_autoware_utils::toPolygon2d(o)); constraints.dynamic_obstacles = {}; // TODO(Maxime): not implemented // TODO(Maxime): directly use lines instead of polygon - sampler_common::Polygon2d drivable_area_polygon; + autoware::sampler_common::Polygon2d drivable_area_polygon; for (const auto & p : right_bound) drivable_area_polygon.outer().emplace_back(p.x, p.y); for (auto it = left_bound.rbegin(); it != left_bound.rend(); ++it) drivable_area_polygon.outer().emplace_back(it->x, it->y); @@ -52,9 +52,9 @@ void prepareConstraints( constraints.drivable_polygons = {drivable_area_polygon}; } -frenet_planner::SamplingParameters prepareSamplingParameters( - const sampler_common::State & initial_state, const double base_length, - const sampler_common::transform::Spline2D & path_spline, const Parameters & params) +autoware::frenet_planner::SamplingParameters prepareSamplingParameters( + const autoware::sampler_common::State & initial_state, const double base_length, + const autoware::sampler_common::transform::Spline2D & path_spline, const Parameters & params) { // calculate target lateral positions std::vector target_lateral_positions; @@ -87,10 +87,10 @@ frenet_planner::SamplingParameters prepareSamplingParameters( } else { target_lateral_positions = params.sampling.target_lateral_positions; } - frenet_planner::SamplingParameters sampling_parameters; + autoware::frenet_planner::SamplingParameters sampling_parameters; sampling_parameters.resolution = params.sampling.resolution; const auto max_s = path_spline.lastS(); - frenet_planner::SamplingParameter p; + autoware::frenet_planner::SamplingParameter p; for (const auto target_length : params.sampling.target_lengths) { p.target_state.position.s = std::min( max_s, path_spline.frenet(initial_state.pose).s + std::max(0.0, target_length - base_length)); @@ -109,7 +109,7 @@ frenet_planner::SamplingParameters prepareSamplingParameters( return sampling_parameters; } -sampler_common::transform::Spline2D preparePathSpline( +autoware::sampler_common::transform::Spline2D preparePathSpline( const std::vector & path, const bool smooth_path) { std::vector x; @@ -151,4 +151,4 @@ sampler_common::transform::Spline2D preparePathSpline( } return {x, y}; } -} // namespace path_sampler +} // namespace autoware::path_sampler diff --git a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp similarity index 95% rename from planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp rename to planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp index 07bd4f32787de..d161d60bc724d 100644 --- a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "path_sampler/utils/trajectory_utils.hpp" +#include "autoware_path_sampler/utils/trajectory_utils.hpp" +#include "autoware_path_sampler/utils/geometry_utils.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "path_sampler/utils/geometry_utils.hpp" #include "tf2/utils.h" #include "autoware_planning_msgs/msg/path_point.hpp" @@ -31,7 +31,7 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { namespace trajectory_utils { @@ -107,4 +107,4 @@ void insertStopPoint( traj_points.insert(traj_points.begin() + stop_seg_idx + 1, additional_traj_point); } } // namespace trajectory_utils -} // namespace path_sampler +} // namespace autoware::path_sampler diff --git a/planning/sampling_based_planner/sampler_common/CMakeLists.txt b/planning/sampling_based_planner/autoware_sampler_common/CMakeLists.txt similarity index 81% rename from planning/sampling_based_planner/sampler_common/CMakeLists.txt rename to planning/sampling_based_planner/autoware_sampler_common/CMakeLists.txt index 5e669a705201e..d82f38d3ec0c0 100644 --- a/planning/sampling_based_planner/sampler_common/CMakeLists.txt +++ b/planning/sampling_based_planner/autoware_sampler_common/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(sampler_common) +project(autoware_sampler_common) find_package(autoware_cmake REQUIRED) autoware_package() @@ -8,7 +8,7 @@ find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED) -ament_auto_add_library(sampler_common SHARED +ament_auto_add_library(autoware_sampler_common SHARED DIRECTORY src/ ) @@ -22,7 +22,7 @@ if(BUILD_TESTING) ) target_link_libraries(test_sampler_common - sampler_common + autoware_sampler_common ) endif() diff --git a/planning/sampling_based_planner/sampler_common/README.md b/planning/sampling_based_planner/autoware_sampler_common/README.md similarity index 100% rename from planning/sampling_based_planner/sampler_common/README.md rename to planning/sampling_based_planner/autoware_sampler_common/README.md diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/footprint.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/footprint.hpp similarity index 73% rename from planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/footprint.hpp rename to planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/footprint.hpp index 061f922001a0f..52dd1a83bd99e 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/footprint.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/footprint.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ -#define SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ +#ifndef AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ +#define AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ -#include "sampler_common/structures.hpp" +#include "autoware_sampler_common/structures.hpp" -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { /// @brief Calculate the footprint of the given path /// @param path sequence of pose used to build the footprint /// @param constraints input constraint object containing vehicle footprint offsets /// @return the polygon footprint of the path MultiPoint2d buildFootprintPoints(const Path & path, const Constraints & constraints); -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints -#endif // SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ +#endif // AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/hard_constraint.hpp similarity index 73% rename from planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp rename to planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/hard_constraint.hpp index a314e83b065d7..6098e7c15d33d 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/hard_constraint.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ -#define SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ +#ifndef AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ +#define AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ -#include "sampler_common/structures.hpp" +#include "autoware_sampler_common/structures.hpp" #include -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { /// @brief Check if the path satisfies the hard constraints MultiPoint2d checkHardConstraints(Path & path, const Constraints & constraints); @@ -27,6 +27,6 @@ bool has_collision( const double min_distance = 0.0); bool satisfyMinMax(const std::vector & values, const double min, const double max); -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints -#endif // SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ +#endif // AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/map_constraints.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/map_constraints.hpp similarity index 80% rename from planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/map_constraints.hpp rename to planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/map_constraints.hpp index ed04d8104a904..7867efa1fa2db 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/map_constraints.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/map_constraints.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ -#define SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ +#ifndef AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ +#define AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ -#include "sampler_common/structures.hpp" +#include "autoware_sampler_common/structures.hpp" #include #include -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { struct MapConstraints { @@ -55,6 +55,6 @@ struct MapConstraints MultiPolygon2d drivable_polygons; std::vector polygon_costs; }; -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints -#endif // SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ +#endif // AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/soft_constraint.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/soft_constraint.hpp similarity index 74% rename from planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/soft_constraint.hpp rename to planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/soft_constraint.hpp index 9fdb9fe137e49..290b6b6674e21 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/soft_constraint.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/soft_constraint.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ -#define SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ +#ifndef AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ +#define AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { /// @brief calculate the curvature cost of the given path void calculateCurvatureCost(Path & path, const Constraints & constraints); @@ -30,6 +30,6 @@ void calculateLateralDeviationCost( /// @brief calculate the overall cost of the given path void calculateCost( Path & path, const Constraints & constraints, const transform::Spline2D & reference); -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints -#endif // SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ +#endif // AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp similarity index 98% rename from planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp rename to planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp index cef791a01df44..a3ba25b6594e9 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SAMPLER_COMMON__STRUCTURES_HPP_ -#define SAMPLER_COMMON__STRUCTURES_HPP_ +#ifndef AUTOWARE_SAMPLER_COMMON__STRUCTURES_HPP_ +#define AUTOWARE_SAMPLER_COMMON__STRUCTURES_HPP_ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" @@ -36,7 +36,7 @@ #include #include -namespace sampler_common +namespace autoware::sampler_common { using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::MultiPoint2d; @@ -367,6 +367,6 @@ struct ReusableTrajectory Configuration planning_configuration; // planning configuration at the end of the trajectory }; -} // namespace sampler_common +} // namespace autoware::sampler_common -#endif // SAMPLER_COMMON__STRUCTURES_HPP_ +#endif // AUTOWARE_SAMPLER_COMMON__STRUCTURES_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/transform/spline_transform.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/transform/spline_transform.hpp similarity index 88% rename from planning/sampling_based_planner/sampler_common/include/sampler_common/transform/spline_transform.hpp rename to planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/transform/spline_transform.hpp index ce66141832dbf..c5f78c29cb13b 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/transform/spline_transform.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/transform/spline_transform.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ -#define SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ +#ifndef AUTOWARE_SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ +#define AUTOWARE_SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ -#include "sampler_common/structures.hpp" +#include "autoware_sampler_common/structures.hpp" #include -namespace sampler_common::transform +namespace autoware::sampler_common::transform { -using sampler_common::FrenetPoint; +using autoware::sampler_common::FrenetPoint; class Spline { @@ -80,6 +80,6 @@ class Spline2D static std::vector arcLength( const std::vector & x, const std::vector & y); }; -} // namespace sampler_common::transform +} // namespace autoware::sampler_common::transform -#endif // SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ +#endif // AUTOWARE_SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/package.xml b/planning/sampling_based_planner/autoware_sampler_common/package.xml similarity index 94% rename from planning/sampling_based_planner/sampler_common/package.xml rename to planning/sampling_based_planner/autoware_sampler_common/package.xml index 321ca2fd43fef..a1462131d62ad 100644 --- a/planning/sampling_based_planner/sampler_common/package.xml +++ b/planning/sampling_based_planner/autoware_sampler_common/package.xml @@ -1,6 +1,6 @@ - sampler_common + autoware_sampler_common 0.0.1 Common classes and functions for sampling based planners Maxime CLEMENT diff --git a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/footprint.cpp b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/footprint.cpp similarity index 87% rename from planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/footprint.cpp rename to planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/footprint.cpp index 92c8ae65267db..1aafa598142a4 100644 --- a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/footprint.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/footprint.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "sampler_common/constraints/footprint.hpp" +#include "autoware_sampler_common/constraints/footprint.hpp" -#include "sampler_common/structures.hpp" +#include "autoware_sampler_common/structures.hpp" #include @@ -25,7 +25,7 @@ #include #include -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { namespace @@ -50,4 +50,4 @@ MultiPoint2d buildFootprintPoints(const Path & path, const Constraints & constra } return footprint; } -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints diff --git a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/hard_constraint.cpp b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/hard_constraint.cpp similarity index 89% rename from planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/hard_constraint.cpp rename to planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/hard_constraint.cpp index 7bb29c8837723..9901b3979259c 100644 --- a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/hard_constraint.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/hard_constraint.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "sampler_common/constraints/hard_constraint.hpp" +#include "autoware_sampler_common/constraints/hard_constraint.hpp" -#include "sampler_common/constraints/footprint.hpp" +#include "autoware_sampler_common/constraints/footprint.hpp" #include #include #include -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { bool satisfyMinMax(const std::vector & values, const double min, const double max) { @@ -56,4 +56,4 @@ MultiPoint2d checkHardConstraints(Path & path, const Constraints & constraints) } return footprint; } -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints diff --git a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/soft_constraint.cpp b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/soft_constraint.cpp similarity index 85% rename from planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/soft_constraint.cpp rename to planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/soft_constraint.cpp index b1d390e68c49f..71dff81929553 100644 --- a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/soft_constraint.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/soft_constraint.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "sampler_common/constraints/soft_constraint.hpp" +#include "autoware_sampler_common/constraints/soft_constraint.hpp" -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" #include -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { void calculateCurvatureCost(Path & path, const Constraints & constraints) { @@ -51,4 +51,4 @@ void calculateCost( calculateLengthCost(path, constraints); calculateLateralDeviationCost(path, constraints, reference); } -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints diff --git a/planning/sampling_based_planner/sampler_common/src/sampler_common/transform/spline_transform.cpp b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/transform/spline_transform.cpp similarity index 98% rename from planning/sampling_based_planner/sampler_common/src/sampler_common/transform/spline_transform.cpp rename to planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/transform/spline_transform.cpp index cf5f1e5478fe5..ce473b1768a54 100644 --- a/planning/sampling_based_planner/sampler_common/src/sampler_common/transform/spline_transform.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/transform/spline_transform.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include #include #include -namespace sampler_common::transform +namespace autoware::sampler_common::transform { Spline::Spline(const std::vector & base_index, const std::vector & base_value) { @@ -312,4 +312,4 @@ double Spline2D::yaw(const double s) const return std::atan2(y_vel, x_vel); } -} // namespace sampler_common::transform +} // namespace autoware::sampler_common::transform diff --git a/planning/sampling_based_planner/sampler_common/test/test_structures.cpp b/planning/sampling_based_planner/autoware_sampler_common/test/test_structures.cpp similarity index 96% rename from planning/sampling_based_planner/sampler_common/test/test_structures.cpp rename to planning/sampling_based_planner/autoware_sampler_common/test/test_structures.cpp index 006768c9325e2..77496209290c1 100644 --- a/planning/sampling_based_planner/sampler_common/test/test_structures.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/test/test_structures.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include TEST(Path, extendPath) { - using sampler_common::Path; + using autoware::sampler_common::Path; Path traj1; Path traj2; Path traj3 = traj1.extend(traj2); @@ -43,7 +43,7 @@ TEST(Path, extendPath) TEST(Trajectory, resample) { constexpr auto eps = 1e-6; - using sampler_common::Trajectory; + using autoware::sampler_common::Trajectory; Trajectory t; t.reserve(2); @@ -103,7 +103,7 @@ TEST(Trajectory, resample) TEST(Trajectory, resampleTime) { constexpr auto eps = 1e-6; - using sampler_common::Trajectory; + using autoware::sampler_common::Trajectory; Trajectory t; t.reserve(2); diff --git a/planning/sampling_based_planner/sampler_common/test/test_transform.cpp b/planning/sampling_based_planner/autoware_sampler_common/test/test_transform.cpp similarity index 85% rename from planning/sampling_based_planner/sampler_common/test/test_transform.cpp rename to planning/sampling_based_planner/autoware_sampler_common/test/test_transform.cpp index d7998d7b8879e..6c5f43e929766 100644 --- a/planning/sampling_based_planner/sampler_common/test/test_transform.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/test/test_transform.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include @@ -24,13 +24,13 @@ constexpr auto TOL = 1E-6; // 1µm tolerance TEST(splineTransform, makeSpline2D) { - sampler_common::transform::Spline2D spline1({0.0, 1.0, 2.0}, {0.0, 1.0, 2.0}); - sampler_common::transform::Spline2D spline2({-10.0, 5.0, -2.0}, {99.0, 3.0, -20.0}); + autoware::sampler_common::transform::Spline2D spline1({0.0, 1.0, 2.0}, {0.0, 1.0, 2.0}); + autoware::sampler_common::transform::Spline2D spline2({-10.0, 5.0, -2.0}, {99.0, 3.0, -20.0}); } TEST(splineTransform, toFrenet) { - sampler_common::transform::Spline2D spline({0.0, 1.0, 2.0}, {0.0, 0.0, 0.0}); + autoware::sampler_common::transform::Spline2D spline({0.0, 1.0, 2.0}, {0.0, 0.0, 0.0}); auto frenet = spline.frenet({0.0, 0.0}); EXPECT_NEAR(frenet.s, 0.0, TOL); EXPECT_NEAR(frenet.d, 0.0, TOL); @@ -56,7 +56,7 @@ TEST(splineTransform, toFrenet) EXPECT_NEAR(frenet.s, 1.0, TOL); EXPECT_NEAR(frenet.d, -1.0, TOL); // EDGE CASE 90 deg angle - sampler_common::transform::Spline2D spline2({0.0, 1.0, 2.0}, {0.0, 1.0, 0.0}); + autoware::sampler_common::transform::Spline2D spline2({0.0, 1.0, 2.0}, {0.0, 1.0, 0.0}); frenet = spline2.frenet({1.0, 2.0}); EXPECT_NEAR(frenet.s, 1.0, TOL); EXPECT_NEAR(frenet.d, 1.0, TOL); @@ -64,7 +64,7 @@ TEST(splineTransform, toFrenet) TEST(splineTransform, toCartesian) { - sampler_common::transform::Spline2D spline({0.0, 1.0, 2.0}, {0.0, 0.0, 0.0}); + autoware::sampler_common::transform::Spline2D spline({0.0, 1.0, 2.0}, {0.0, 0.0, 0.0}); auto cart = spline.cartesian({1.0, 0.0}); EXPECT_NEAR(cart.x(), 1.0, TOL); EXPECT_NEAR(cart.y(), 0.0, TOL); @@ -85,7 +85,7 @@ TEST(splineTransform, benchFrenet) auto y = 0.0; std::generate(xs.begin(), xs.end(), [&]() { return ++x; }); std::generate(ys.begin(), ys.end(), [&]() { return ++y; }); - sampler_common::transform::Spline2D spline(xs, ys); + autoware::sampler_common::transform::Spline2D spline(xs, ys); auto points_distribution = std::uniform_real_distribution(0.0, static_cast(size)); constexpr auto nb_iter = 1e3; for (auto iter = 0; iter < nb_iter; ++iter) { diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/path_generation.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/path_generation.hpp deleted file mode 100644 index 448400236c9f8..0000000000000 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/path_generation.hpp +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright 2023 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef PATH_SAMPLER__PATH_GENERATION_HPP_ -#define PATH_SAMPLER__PATH_GENERATION_HPP_ - -#include "bezier_sampler/bezier_sampling.hpp" -#include "frenet_planner/structures.hpp" -#include "path_sampler/parameters.hpp" -#include "sampler_common/constraints/hard_constraint.hpp" -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" - -#include - -#include - -namespace path_sampler -{ -/** - * @brief generate candidate paths for the given problem inputs - * @param [in] initial_state initial ego state - * @param [in] path_spline spline of the reference path - * @param [in] base_length base length of the reuse path (= 0.0 if not reusing a previous path) - * @param [in] params parameters - * @return candidate paths - */ -std::vector generateCandidatePaths( - const sampler_common::State & initial_state, - const sampler_common::transform::Spline2D & path_spline, const double base_length, - const Parameters & params); - -std::vector generateBezierPaths( - const sampler_common::State & initial_state, const double base_length, - const sampler_common::transform::Spline2D & path_spline, const Parameters & params); -std::vector generateFrenetPaths( - const sampler_common::State & initial_state, const double base_length, - const sampler_common::transform::Spline2D & path_spline, const Parameters & params); -} // namespace path_sampler - -#endif // PATH_SAMPLER__PATH_GENERATION_HPP_ diff --git a/sensing/imu_corrector/CMakeLists.txt b/sensing/imu_corrector/CMakeLists.txt index ded596a8a9898..b3be5be12b75d 100644 --- a/sensing/imu_corrector/CMakeLists.txt +++ b/sensing/imu_corrector/CMakeLists.txt @@ -4,27 +4,32 @@ project(imu_corrector) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(gyro_bias_estimation_module SHARED - src/gyro_bias_estimation_module.cpp -) - -ament_auto_add_executable(imu_corrector +ament_auto_add_library(${PROJECT_NAME} SHARED src/imu_corrector_core.cpp - src/imu_corrector_node.cpp ) -ament_auto_add_executable(gyro_bias_estimator +ament_auto_add_library(gyro_bias_estimator SHARED src/gyro_bias_estimator.cpp - src/gyro_bias_estimator_node.cpp + src/gyro_bias_estimation_module.cpp ) -target_link_libraries(gyro_bias_estimator gyro_bias_estimation_module) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "imu_corrector::ImuCorrector" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) + +rclcpp_components_register_node(gyro_bias_estimator + PLUGIN "imu_corrector::GyroBiasEstimator" + EXECUTABLE gyro_bias_estimator_node + EXECUTOR SingleThreadedExecutor +) function(add_testcase filepath) get_filename_component(filename ${filepath} NAME) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gmock(${test_name} ${filepath}) - target_link_libraries("${test_name}" gyro_bias_estimation_module) + target_link_libraries("${test_name}" gyro_bias_estimator) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() diff --git a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml index e16ccef446aba..100168b17171a 100644 --- a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml +++ b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml @@ -6,7 +6,7 @@ - + diff --git a/sensing/imu_corrector/launch/imu_corrector.launch.xml b/sensing/imu_corrector/launch/imu_corrector.launch.xml index d87d6e77c110d..8430f2a50a85c 100755 --- a/sensing/imu_corrector/launch/imu_corrector.launch.xml +++ b/sensing/imu_corrector/launch/imu_corrector.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index e99667ed1c4a6..d79626f66801b 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -23,8 +23,8 @@ namespace imu_corrector { -GyroBiasEstimator::GyroBiasEstimator() -: Node("gyro_bias_validator"), +GyroBiasEstimator::GyroBiasEstimator(const rclcpp::NodeOptions & options) +: rclcpp::Node("gyro_bias_validator", options), gyro_bias_threshold_(declare_parameter("gyro_bias_threshold")), angular_velocity_offset_x_(declare_parameter("angular_velocity_offset_x")), angular_velocity_offset_y_(declare_parameter("angular_velocity_offset_y")), @@ -244,3 +244,6 @@ void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusW } } // namespace imu_corrector + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::GyroBiasEstimator) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp index 3592ff1f7d3b4..671c18de78f86 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.hpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -42,7 +42,7 @@ class GyroBiasEstimator : public rclcpp::Node using Odometry = nav_msgs::msg::Odometry; public: - GyroBiasEstimator(); + explicit GyroBiasEstimator(const rclcpp::NodeOptions & options); private: void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); diff --git a/sensing/imu_corrector/src/gyro_bias_estimator_node.cpp b/sensing/imu_corrector/src/gyro_bias_estimator_node.cpp deleted file mode 100644 index a491957bbb57f..0000000000000 --- a/sensing/imu_corrector/src/gyro_bias_estimator_node.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "gyro_bias_estimator.hpp" - -#include - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/sensing/imu_corrector/src/imu_corrector_core.cpp b/sensing/imu_corrector/src/imu_corrector_core.cpp index a26c64925729c..c8e7698cb4e37 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.cpp +++ b/sensing/imu_corrector/src/imu_corrector_core.cpp @@ -53,8 +53,9 @@ geometry_msgs::msg::Vector3 transformVector3( namespace imu_corrector { -ImuCorrector::ImuCorrector() -: Node("imu_corrector"), output_frame_(declare_parameter("base_link", "base_link")) +ImuCorrector::ImuCorrector(const rclcpp::NodeOptions & options) +: rclcpp::Node("imu_corrector", options), + output_frame_(declare_parameter("base_link", "base_link")) { transform_listener_ = std::make_shared(this); @@ -123,3 +124,6 @@ void ImuCorrector::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m } } // namespace imu_corrector + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::ImuCorrector) diff --git a/sensing/imu_corrector/src/imu_corrector_core.hpp b/sensing/imu_corrector/src/imu_corrector_core.hpp index 7709c611ab714..4d5377274ae78 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.hpp +++ b/sensing/imu_corrector/src/imu_corrector_core.hpp @@ -34,7 +34,7 @@ class ImuCorrector : public rclcpp::Node using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; public: - ImuCorrector(); + explicit ImuCorrector(const rclcpp::NodeOptions & options); private: void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); diff --git a/sensing/imu_corrector/src/imu_corrector_node.cpp b/sensing/imu_corrector/src/imu_corrector_node.cpp deleted file mode 100644 index c1df5bee28439..0000000000000 --- a/sensing/imu_corrector/src/imu_corrector_node.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "imu_corrector_core.hpp" - -#include - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index 0cbb04345477e..ecb84c8a7f3b3 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -221,6 +221,14 @@ ament_auto_package(INSTALL_TO_SHARE ) if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest) + ament_lint_auto_find_test_dependencies() + ament_add_gtest(test_utilities + test/test_utilities.cpp + ) + target_link_libraries(test_utilities pointcloud_preprocessor_filter) + add_ros_test( test/test_distortion_corrector.py TIMEOUT "30" diff --git a/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md b/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md index bd2f7d98919d5..8e31dfb203d2b 100644 --- a/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md +++ b/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md @@ -10,6 +10,7 @@ The `vector_map_inside_area_filter` is a node that removes points inside the vec - Extract the vector map area that intersects with the bounding box of input points to reduce the calculation cost - Create the 2D polygon from the extracted vector map area - Remove input points inside the polygon +- If the z value is used for filtering, remove points that are below the z threshold ![vector_map_inside_area_filter_figure](./image/vector_map_inside_area_filter_overview.svg) @@ -35,5 +36,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, so please | Name | Type | Description | | -------------- | ------ | --------------------------- | | `polygon_type` | string | polygon type to be filtered | +| `use_z` | bool | use z value for filtering | +| `z_threshold` | float | z threshold for filtering | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp index 5de644fd8ffd1..654057e7bb8d7 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp @@ -26,6 +26,7 @@ #include #include +#include #include using K = CGAL::Exact_predicates_inexact_constructions_kernel; @@ -49,28 +50,30 @@ void to_cgal_polygon(const lanelet::BasicPolygon2d & polygon_in, PolygonCgal & p */ void remove_polygon_cgal_from_cloud( const sensor_msgs::msg::PointCloud2 & cloud_in, const PolygonCgal & polyline_polygon, - sensor_msgs::msg::PointCloud2 & cloud_out); + sensor_msgs::msg::PointCloud2 & cloud_out, const std::optional & max_z = std::nullopt); /** * @brief remove points in the given polygon */ void remove_polygon_cgal_from_cloud( const pcl::PointCloud & cloud_in, const PolygonCgal & polyline_polygon, - pcl::PointCloud & cloud_out); + pcl::PointCloud & cloud_out, const std::optional & max_z = std::nullopt); /** * @brief remove points in the given polygons */ void remove_polygon_cgal_from_cloud( const sensor_msgs::msg::PointCloud2 & cloud_in, - const std::vector & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out); + const std::vector & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out, + const std::optional & max_z = std::nullopt); /** * @brief remove points in the given polygons */ void remove_polygon_cgal_from_cloud( const pcl::PointCloud & cloud_in, - const std::vector & polyline_polygons, pcl::PointCloud & cloud_out); + const std::vector & polyline_polygons, pcl::PointCloud & cloud_out, + const std::optional & max_z = std::nullopt); /** * @brief return true if the given point is inside the at least one of the polygons diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp index 894768d385438..760ffbe1eb1dd 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp @@ -43,6 +43,8 @@ class VectorMapInsideAreaFilterComponent : public pointcloud_preprocessor::Filte // parameter std::string polygon_type_; + bool use_z_filter_ = false; + float z_threshold_; public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp index fe9fde7301fd7..ec84a2467db78 100644 --- a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp +++ b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp @@ -46,18 +46,19 @@ void to_cgal_polygon(const lanelet::BasicPolygon2d & polygon_in, PolygonCgal & p void remove_polygon_cgal_from_cloud( const sensor_msgs::msg::PointCloud2 & cloud_in, const PolygonCgal & polyline_polygon, - sensor_msgs::msg::PointCloud2 & cloud_out) + sensor_msgs::msg::PointCloud2 & cloud_out, const std::optional & max_z) { pcl::PointCloud pcl_output; for (sensor_msgs::PointCloud2ConstIterator iter_x(cloud_in, "x"), iter_y(cloud_in, "y"), iter_z(cloud_in, "z"); iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - // check if the point is inside the polygon - if ( - CGAL::bounded_side_2( - polyline_polygon.begin(), polyline_polygon.end(), PointCgal(*iter_x, *iter_y), K()) == - CGAL::ON_UNBOUNDED_SIDE) { + const bool within_max_z = max_z ? *iter_z <= *max_z : true; + const bool within_polygon = CGAL::bounded_side_2( + polyline_polygon.begin(), polyline_polygon.end(), + PointCgal(*iter_x, *iter_y), K()) == CGAL::ON_BOUNDED_SIDE; + // remove points within the polygon and max_z + if (!(within_max_z && within_polygon)) { pcl::PointXYZ p; p.x = *iter_x; p.y = *iter_y; @@ -72,17 +73,18 @@ void remove_polygon_cgal_from_cloud( void remove_polygon_cgal_from_cloud( const pcl::PointCloud & cloud_in, const PolygonCgal & polyline_polygon, - pcl::PointCloud & cloud_out) + pcl::PointCloud & cloud_out, const std::optional & max_z) { cloud_out.clear(); cloud_out.header = cloud_in.header; for (const auto & p : cloud_in) { - // check if the point is inside the polygon - if ( - CGAL::bounded_side_2( - polyline_polygon.begin(), polyline_polygon.end(), PointCgal(p.x, p.y), K()) == - CGAL::ON_UNBOUNDED_SIDE) { + const bool within_max_z = max_z ? p.z <= *max_z : true; + const bool within_polygon = CGAL::bounded_side_2( + polyline_polygon.begin(), polyline_polygon.end(), + PointCgal(p.x, p.y), K()) == CGAL::ON_BOUNDED_SIDE; + // remove points within the polygon and max_z + if (!(within_max_z && within_polygon)) { cloud_out.emplace_back(p); } } @@ -90,7 +92,8 @@ void remove_polygon_cgal_from_cloud( void remove_polygon_cgal_from_cloud( const sensor_msgs::msg::PointCloud2 & cloud_in, - const std::vector & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out) + const std::vector & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out, + const std::optional & max_z) { if (polyline_polygons.empty()) { cloud_out = cloud_in; @@ -101,9 +104,11 @@ void remove_polygon_cgal_from_cloud( for (sensor_msgs::PointCloud2ConstIterator iter_x(cloud_in, "x"), iter_y(cloud_in, "y"), iter_z(cloud_in, "z"); iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - // if the point is inside the polygon, skip inserting and check the next point - pcl::PointXYZ p(*iter_x, *iter_y, *iter_z); - if (point_within_cgal_polys(p, polyline_polygons)) { + const bool within_max_z = max_z ? *iter_z <= *max_z : true; + const pcl::PointXYZ p(*iter_x, *iter_y, *iter_z); + const bool within_polygon = point_within_cgal_polys(p, polyline_polygons); + // remove points within the polygon and max_z + if (within_max_z && within_polygon) { continue; } filtered_cloud.emplace_back(p); @@ -115,7 +120,8 @@ void remove_polygon_cgal_from_cloud( void remove_polygon_cgal_from_cloud( const pcl::PointCloud & cloud_in, - const std::vector & polyline_polygons, pcl::PointCloud & cloud_out) + const std::vector & polyline_polygons, pcl::PointCloud & cloud_out, + const std::optional & max_z) { if (polyline_polygons.empty()) { cloud_out = cloud_in; @@ -124,8 +130,10 @@ void remove_polygon_cgal_from_cloud( pcl::PointCloud filtered_cloud; for (const auto & p : cloud_in) { - // if the point is inside the polygon, skip inserting and check the next point - if (point_within_cgal_polys(p, polyline_polygons)) { + const bool within_max_z = max_z ? p.z <= *max_z : true; + const bool within_polygon = point_within_cgal_polys(p, polyline_polygons); + // remove points within the polygon and max_z + if (within_max_z && within_polygon) { continue; } filtered_cloud.emplace_back(p); diff --git a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp index 8fcf15326829f..6b4e90a697cd6 100644 --- a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp @@ -40,7 +40,8 @@ lanelet::ConstPolygons3d calcIntersectedPolygons( } pcl::PointCloud removePointsWithinPolygons( - const pcl::PointCloud::Ptr & cloud_in, const lanelet::ConstPolygons3d & polygons) + const pcl::PointCloud::Ptr & cloud_in, const lanelet::ConstPolygons3d & polygons, + const std::optional & z_threshold_) { std::vector cgal_polys; @@ -53,7 +54,7 @@ pcl::PointCloud removePointsWithinPolygons( pcl::PointCloud filtered_cloud; pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( - *cloud_in, cgal_polys, filtered_cloud); + *cloud_in, cgal_polys, filtered_cloud, z_threshold_); return filtered_cloud; } @@ -74,6 +75,10 @@ VectorMapInsideAreaFilterComponent::VectorMapInsideAreaFilterComponent( map_sub_ = this->create_subscription( "input/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&VectorMapInsideAreaFilterComponent::mapCallback, this, _1)); + + // Set parameters + use_z_filter_ = declare_parameter("use_z_filter", false); + z_threshold_ = declare_parameter("z_threshold", 0.0f); // defined in the base_link frame } void VectorMapInsideAreaFilterComponent::filter( @@ -99,7 +104,25 @@ void VectorMapInsideAreaFilterComponent::filter( const auto intersected_lanelets = calcIntersectedPolygons(bounding_box, polygon_lanelets_); // filter pointcloud by lanelet - const auto filtered_pc = removePointsWithinPolygons(pc_input, intersected_lanelets); + std::optional z_threshold_in_base_link = std::nullopt; + if (use_z_filter_) { + // assume z_max is defined in the base_link frame + const std::string base_link_frame = "base_link"; + z_threshold_in_base_link = z_threshold_; + if (input->header.frame_id != base_link_frame) { + try { + // get z difference from baselink to input frame + const auto transform = + tf_buffer_->lookupTransform(input->header.frame_id, base_link_frame, input->header.stamp); + *z_threshold_in_base_link += transform.transform.translation.z; + } catch (const tf2::TransformException & e) { + RCLCPP_WARN(get_logger(), "Failed to transform z_threshold to base_link frame"); + z_threshold_in_base_link = std::nullopt; + } + } + } + const auto filtered_pc = + removePointsWithinPolygons(pc_input, intersected_lanelets, z_threshold_in_base_link); // convert to ROS message pcl::toROSMsg(filtered_pc, output); diff --git a/sensing/pointcloud_preprocessor/test/test_utilities.cpp b/sensing/pointcloud_preprocessor/test/test_utilities.cpp new file mode 100644 index 0000000000000..68c86dfbd0505 --- /dev/null +++ b/sensing/pointcloud_preprocessor/test/test_utilities.cpp @@ -0,0 +1,125 @@ +// 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 "pointcloud_preprocessor/utility/utilities.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include + +constexpr double EPSILON = 1e-6; + +class RemovePolygonCgalFromCloudTest : public ::testing::Test +{ +protected: + PolygonCgal polygon; + sensor_msgs::msg::PointCloud2 cloud_out; + + virtual void SetUp() + { + // Set up a simple square polygon + polygon.push_back(PointCgal(0.0, 0.0)); + polygon.push_back(PointCgal(0.0, 1.0)); + polygon.push_back(PointCgal(1.0, 1.0)); + polygon.push_back(PointCgal(1.0, 0.0)); + polygon.push_back(PointCgal(0.0, 0.0)); + } + + void CreatePointCloud2(sensor_msgs::msg::PointCloud2 & cloud, double x, double y, double z) + { + pcl::PointCloud pcl_cloud; + pcl_cloud.push_back(pcl::PointXYZ(x, y, z)); + pcl::toROSMsg(pcl_cloud, cloud); + cloud.header.frame_id = "map"; + } +}; + +// Test case 1: without max_z, points inside the polygon are removed +TEST_F(RemovePolygonCgalFromCloudTest, PointsInsidePolygonAreRemoved) +{ + sensor_msgs::msg::PointCloud2 cloud_in; + CreatePointCloud2(cloud_in, 0.5, 0.5, 0.1); // point inside the polygon + + pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud(cloud_in, polygon, cloud_out); + + pcl::PointCloud pcl_output; + pcl::fromROSMsg(cloud_out, pcl_output); + + EXPECT_EQ(pcl_output.size(), 0); +} + +// Test case 2: without max_z, points outside the polygon remain +TEST_F(RemovePolygonCgalFromCloudTest, PointsOutsidePolygonRemain) +{ + sensor_msgs::msg::PointCloud2 cloud_in; + CreatePointCloud2(cloud_in, 1.5, 1.5, 0.1); // point outside the polygon + + pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud(cloud_in, polygon, cloud_out); + + pcl::PointCloud pcl_output; + pcl::fromROSMsg(cloud_out, pcl_output); + + EXPECT_EQ(pcl_output.size(), 1); + EXPECT_NEAR(pcl_output.points[0].x, 1.5, EPSILON); + EXPECT_NEAR(pcl_output.points[0].y, 1.5, EPSILON); + EXPECT_NEAR(pcl_output.points[0].z, 0.1, EPSILON); +} + +// Test case 3: with max_z, points inside the polygon and below max_z are removed +TEST_F(RemovePolygonCgalFromCloudTest, PointsBelowMaxZAreRemoved) +{ + sensor_msgs::msg::PointCloud2 cloud_in; + CreatePointCloud2(cloud_in, 0.5, 0.5, 0.1); // point inside the polygon, below max_z + + std::optional max_z = 1.0f; + pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( + cloud_in, polygon, cloud_out, max_z); + + pcl::PointCloud pcl_output; + pcl::fromROSMsg(cloud_out, pcl_output); + + EXPECT_EQ(pcl_output.size(), 0); +} + +// Test case 4: with max_z, points inside the polygon but above max_z remain +TEST_F(RemovePolygonCgalFromCloudTest, PointsAboveMaxZRemain) +{ + sensor_msgs::msg::PointCloud2 cloud_in; + CreatePointCloud2(cloud_in, 0.5, 0.5, 1.5); // point inside the polygon, above max_z + + std::optional max_z = 1.0f; + pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( + cloud_in, polygon, cloud_out, max_z); + + pcl::PointCloud pcl_output; + pcl::fromROSMsg(cloud_out, pcl_output); + + EXPECT_EQ(pcl_output.size(), 1); + EXPECT_NEAR(pcl_output.points[0].x, 0.5, EPSILON); + EXPECT_NEAR(pcl_output.points[0].y, 0.5, EPSILON); + EXPECT_NEAR(pcl_output.points[0].z, 1.5, EPSILON); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/sensing/vehicle_velocity_converter/CMakeLists.txt b/sensing/vehicle_velocity_converter/CMakeLists.txt index 4f4a14bd9f045..bb50fbff90c4b 100644 --- a/sensing/vehicle_velocity_converter/CMakeLists.txt +++ b/sensing/vehicle_velocity_converter/CMakeLists.txt @@ -4,11 +4,15 @@ project(vehicle_velocity_converter) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(vehicle_velocity_converter - src/vehicle_velocity_converter_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/vehicle_velocity_converter.cpp ) -ament_target_dependencies(vehicle_velocity_converter) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "VehicleVelocityConverter" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package( INSTALL_TO_SHARE diff --git a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp b/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp index 4a1a66b842892..8ef46609ee542 100644 --- a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp +++ b/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp @@ -28,7 +28,7 @@ class VehicleVelocityConverter : public rclcpp::Node { public: - VehicleVelocityConverter(); + explicit VehicleVelocityConverter(const rclcpp::NodeOptions & options); ~VehicleVelocityConverter() = default; private: diff --git a/sensing/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml b/sensing/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml index 6302c09b526f9..84e1838dc89eb 100644 --- a/sensing/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml +++ b/sensing/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/sensing/vehicle_velocity_converter/package.xml b/sensing/vehicle_velocity_converter/package.xml index 39780deaccc28..b32b51bc52e10 100644 --- a/sensing/vehicle_velocity_converter/package.xml +++ b/sensing/vehicle_velocity_converter/package.xml @@ -13,6 +13,7 @@ autoware_vehicle_msgs geometry_msgs rclcpp + rclcpp_components ament_lint_auto autoware_lint_common diff --git a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp index 360ec4cae58d5..2aece69f5283b 100644 --- a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp +++ b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp @@ -14,7 +14,8 @@ #include "vehicle_velocity_converter/vehicle_velocity_converter.hpp" -VehicleVelocityConverter::VehicleVelocityConverter() : Node("vehicle_velocity_converter") +VehicleVelocityConverter::VehicleVelocityConverter(const rclcpp::NodeOptions & options) +: rclcpp::Node("vehicle_velocity_converter", options) { // set covariance value for twist with covariance msg stddev_vx_ = declare_parameter("velocity_stddev_xx"); @@ -52,3 +53,6 @@ void VehicleVelocityConverter::callbackVelocityReport( twist_with_covariance_pub_->publish(twist_with_covariance_msg); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(VehicleVelocityConverter) diff --git a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter_node.cpp b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter_node.cpp deleted file mode 100644 index cee25f6e6c62a..0000000000000 --- a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter_node.cpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright 2021 TierIV -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp index 012bb55af3068..cd66cde64c9a5 100644 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp @@ -20,6 +20,8 @@ #include // Autoware +#include + #include #include #include @@ -56,33 +58,26 @@ class EmergencyHandler : public rclcpp::Node explicit EmergencyHandler(const rclcpp::NodeOptions & options); private: - // Subscribers + // Subscribers with callback rclcpp::Subscription::SharedPtr sub_hazard_status_stamped_; rclcpp::Subscription::SharedPtr sub_prev_control_command_; - rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr sub_control_mode_; - rclcpp::Subscription::SharedPtr - sub_mrm_comfortable_stop_status_; - rclcpp::Subscription::SharedPtr - sub_mrm_emergency_stop_status_; + // Subscribers without callback + tier4_autoware_utils::InterProcessPollingSubscriber sub_odom_{ + this, "~/input/odometry"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_control_mode_{this, "~/input/control_mode"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_mrm_comfortable_stop_status_{this, "~/input/mrm/comfortable_stop/status"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_mrm_emergency_stop_status_{this, "~/input/mrm/emergency_stop/status"}; autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_stamped_; autoware_control_msgs::msg::Control::ConstSharedPtr prev_control_command_; - nav_msgs::msg::Odometry::ConstSharedPtr odom_; - autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; - tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_comfortable_stop_status_; - tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_emergency_stop_status_; void onHazardStatusStamped( const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg); void onPrevControlCommand(const autoware_control_msgs::msg::Control::ConstSharedPtr msg); - void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - void onControlMode(const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); - void onMrmComfortableStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); - void onMrmEmergencyStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); // Publisher rclcpp::Publisher::SharedPtr pub_control_command_; @@ -135,9 +130,13 @@ class EmergencyHandler : public rclcpp::Node void updateMrmState(); void operateMrm(); autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); - bool isStopped(); + + bool isAutonomous(); bool isDrivingBackwards(); bool isEmergency(); + bool isStopped(); + bool isComfortableStopStatusAvailable(); + bool isEmergencyStopStatusAvailable(); }; #endif // EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_ diff --git a/system/emergency_handler/package.xml b/system/emergency_handler/package.xml index aa2090c86edb8..f26080fd8ef56 100644 --- a/system/emergency_handler/package.xml +++ b/system/emergency_handler/package.xml @@ -21,6 +21,7 @@ rclcpp_components std_msgs std_srvs + tier4_autoware_utils tier4_system_msgs ament_lint_auto diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp index ae3d60e32a445..b2ee12afc9c76 100644 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp @@ -30,24 +30,13 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) using std::placeholders::_1; - // Subscriber + // Subscribers with callback sub_hazard_status_stamped_ = create_subscription( "~/input/hazard_status", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onHazardStatusStamped, this, _1)); sub_prev_control_command_ = create_subscription( "~/input/prev_control_command", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onPrevControlCommand, this, _1)); - sub_odom_ = create_subscription( - "~/input/odometry", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onOdometry, this, _1)); - // subscribe control mode - sub_control_mode_ = create_subscription( - "~/input/control_mode", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onControlMode, this, _1)); - sub_mrm_comfortable_stop_status_ = create_subscription( - "~/input/mrm/comfortable_stop/status", rclcpp::QoS{1}, - std::bind(&EmergencyHandler::onMrmComfortableStopStatus, this, _1)); - sub_mrm_emergency_stop_status_ = create_subscription( - "~/input/mrm/emergency_stop/status", rclcpp::QoS{1}, - std::bind(&EmergencyHandler::onMrmEmergencyStopStatus, this, _1)); // Publisher pub_control_command_ = create_publisher( @@ -72,13 +61,6 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) client_mrm_emergency_stop_group_); // Initialize - odom_ = std::make_shared(); - control_mode_ = std::make_shared(); - prev_control_command_ = - autoware_control_msgs::msg::Control::ConstSharedPtr(new autoware_control_msgs::msg::Control); - mrm_comfortable_stop_status_ = - std::make_shared(); - mrm_emergency_stop_status_ = std::make_shared(); mrm_state_.stamp = this->now(); mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL; mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE; @@ -105,29 +87,6 @@ void EmergencyHandler::onPrevControlCommand( prev_control_command_ = autoware_control_msgs::msg::Control::ConstSharedPtr(control_command); } -void EmergencyHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - odom_ = msg; -} - -void EmergencyHandler::onControlMode( - const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) -{ - control_mode_ = msg; -} - -void EmergencyHandler::onMrmComfortableStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) -{ - mrm_comfortable_stop_status_ = msg; -} - -void EmergencyHandler::onMrmEmergencyStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) -{ - mrm_emergency_stop_status_ = msg; -} - autoware_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHazardCmdMsg() { using autoware_vehicle_msgs::msg::HazardLightsCommand; @@ -293,17 +252,14 @@ bool EmergencyHandler::isDataReady() return false; } - if ( - param_.use_comfortable_stop && mrm_comfortable_stop_status_->state == - tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + if (param_.use_comfortable_stop && !isComfortableStopStatusAvailable()) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), "waiting for mrm comfortable stop to become available..."); return false; } - if ( - mrm_emergency_stop_status_->state == tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + if (!isEmergencyStopStatusAvailable()) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), "waiting for mrm emergency stop to become available..."); @@ -381,7 +337,7 @@ void EmergencyHandler::updateMrmState() const bool is_emergency = isEmergency(); // Get mode - const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS; + const bool is_auto_mode = isAutonomous(); // State Machine if (mrm_state_.state == MrmState::NORMAL) { @@ -447,6 +403,14 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurre return mrm_state_.behavior; } +bool EmergencyHandler::isAutonomous() +{ + using autoware_vehicle_msgs::msg::ControlModeReport; + auto mode = sub_control_mode_.takeData(); + if (mode == nullptr) return false; + return mode->mode == ControlModeReport::AUTONOMOUS; +} + bool EmergencyHandler::isEmergency() { return hazard_status_stamped_->status.emergency || @@ -455,14 +419,32 @@ bool EmergencyHandler::isEmergency() bool EmergencyHandler::isStopped() { + auto odom = sub_odom_.takeData(); + if (odom == nullptr) return false; constexpr auto th_stopped_velocity = 0.001; - return (std::abs(odom_->twist.twist.linear.x) < th_stopped_velocity); + return (std::abs(odom->twist.twist.linear.x) < th_stopped_velocity); +} + +bool EmergencyHandler::isComfortableStopStatusAvailable() +{ + auto status = sub_mrm_comfortable_stop_status_.takeData(); + if (status == nullptr) return false; + return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; +} + +bool EmergencyHandler::isEmergencyStopStatusAvailable() +{ + auto status = sub_mrm_emergency_stop_status_.takeData(); + if (status == nullptr) return false; + return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; } bool EmergencyHandler::isDrivingBackwards() { + auto odom = sub_odom_.takeData(); + if (odom == nullptr) return false; constexpr auto th_moving_backwards = -0.001; - return odom_->twist.twist.linear.x < th_moving_backwards; + return odom->twist.twist.linear.x < th_moving_backwards; } #include diff --git a/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_node.cpp b/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_node.cpp deleted file mode 100644 index 03bad4198776d..0000000000000 --- a/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_node.cpp +++ /dev/null @@ -1,27 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index 7a160b6c531e9..33d8af5004c47 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -22,6 +22,8 @@ #include // Autoware +#include + #include #include #include @@ -66,39 +68,28 @@ class MrmHandler : public rclcpp::Node // type enum RequestType { CALL, CANCEL }; - // Subscribers - rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr sub_control_mode_; + // Subscribers with callback rclcpp::Subscription::SharedPtr sub_operation_mode_availability_; - rclcpp::Subscription::SharedPtr - sub_mrm_pull_over_status_; - rclcpp::Subscription::SharedPtr - sub_mrm_comfortable_stop_status_; - rclcpp::Subscription::SharedPtr - sub_mrm_emergency_stop_status_; - rclcpp::Subscription::SharedPtr - sub_operation_mode_state_; - - nav_msgs::msg::Odometry::ConstSharedPtr odom_; - autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; + // Subscribers without callback + tier4_autoware_utils::InterProcessPollingSubscriber sub_odom_{ + this, "~/input/odometry"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_control_mode_{this, "~/input/control_mode"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_mrm_pull_over_status_{this, "~/input/mrm/pull_over/status"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_mrm_comfortable_stop_status_{this, "~/input/mrm/comfortable_stop/status"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_mrm_emergency_stop_status_{this, "~/input/mrm/emergency_stop/status"}; + tier4_autoware_utils::InterProcessPollingSubscriber< + autoware_adapi_v1_msgs::msg::OperationModeState> + sub_operation_mode_state_{this, "~/input/api/operation_mode/state"}; + tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr operation_mode_availability_; - tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_pull_over_status_; - tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_comfortable_stop_status_; - tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_emergency_stop_status_; - autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr operation_mode_state_; - void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - void onControlMode(const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); void onOperationModeAvailability( const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg); - void onMrmPullOverStatus(const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); - void onMrmComfortableStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); - void onMrmEmergencyStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); - void onOperationModeState( - const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); // Publisher @@ -156,6 +147,10 @@ class MrmHandler : public rclcpp::Node bool isStopped(); bool isDrivingBackwards(); bool isEmergency() const; + bool isAutonomous(); + bool isPullOverStatusAvailable(); + bool isComfortableStopStatusAvailable(); + bool isEmergencyStopStatusAvailable(); bool isArrivedAtGoal(); }; diff --git a/system/mrm_handler/package.xml b/system/mrm_handler/package.xml index 2db22cecaa82d..5774cce3215a9 100644 --- a/system/mrm_handler/package.xml +++ b/system/mrm_handler/package.xml @@ -21,6 +21,7 @@ rclcpp_components std_msgs std_srvs + tier4_autoware_utils tier4_system_msgs ament_lint_auto diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index e24ce221a1050..44407c40c6787 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -36,28 +36,11 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" using std::placeholders::_1; - // Subscriber - sub_odom_ = create_subscription( - "~/input/odometry", rclcpp::QoS{1}, std::bind(&MrmHandler::onOdometry, this, _1)); - // subscribe control mode - sub_control_mode_ = create_subscription( - "~/input/control_mode", rclcpp::QoS{1}, std::bind(&MrmHandler::onControlMode, this, _1)); + // Subscribers with callback sub_operation_mode_availability_ = create_subscription( "~/input/operation_mode_availability", rclcpp::QoS{1}, std::bind(&MrmHandler::onOperationModeAvailability, this, _1)); - sub_mrm_pull_over_status_ = create_subscription( - "~/input/mrm/pull_over/status", rclcpp::QoS{1}, - std::bind(&MrmHandler::onMrmPullOverStatus, this, _1)); - sub_mrm_comfortable_stop_status_ = create_subscription( - "~/input/mrm/comfortable_stop/status", rclcpp::QoS{1}, - std::bind(&MrmHandler::onMrmComfortableStopStatus, this, _1)); - sub_mrm_emergency_stop_status_ = create_subscription( - "~/input/mrm/emergency_stop/status", rclcpp::QoS{1}, - std::bind(&MrmHandler::onMrmEmergencyStopStatus, this, _1)); - sub_operation_mode_state_ = create_subscription( - "~/input/api/operation_mode/state", rclcpp::QoS{1}, - std::bind(&MrmHandler::onOperationModeState, this, _1)); // Publisher pub_hazard_cmd_ = create_publisher( @@ -84,13 +67,6 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" client_mrm_emergency_stop_group_); // Initialize - odom_ = std::make_shared(); - control_mode_ = std::make_shared(); - mrm_pull_over_status_ = std::make_shared(); - mrm_comfortable_stop_status_ = - std::make_shared(); - mrm_emergency_stop_status_ = std::make_shared(); - operation_mode_state_ = std::make_shared(); mrm_state_.stamp = this->now(); mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL; mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE; @@ -102,17 +78,6 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" this, get_clock(), update_period_ns, std::bind(&MrmHandler::onTimer, this)); } -void MrmHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - odom_ = msg; -} - -void MrmHandler::onControlMode( - const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) -{ - control_mode_ = msg; -} - void MrmHandler::onOperationModeAvailability( const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg) { @@ -141,30 +106,6 @@ void MrmHandler::onOperationModeAvailability( operation_mode_availability_ = msg; } -void MrmHandler::onMrmPullOverStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) -{ - mrm_pull_over_status_ = msg; -} - -void MrmHandler::onMrmComfortableStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) -{ - mrm_comfortable_stop_status_ = msg; -} - -void MrmHandler::onMrmEmergencyStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) -{ - mrm_emergency_stop_status_ = msg; -} - -void MrmHandler::onOperationModeState( - const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg) -{ - operation_mode_state_ = msg; -} - void MrmHandler::publishHazardCmd() { using autoware_vehicle_msgs::msg::HazardLightsCommand; @@ -352,26 +293,21 @@ bool MrmHandler::isDataReady() return false; } - if ( - param_.use_pull_over && - mrm_pull_over_status_->state == tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + if (param_.use_pull_over && !isPullOverStatusAvailable()) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), "waiting for mrm pull over to become available..."); return false; } - if ( - param_.use_comfortable_stop && mrm_comfortable_stop_status_->state == - tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + if (param_.use_comfortable_stop && !isComfortableStopStatusAvailable()) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), "waiting for mrm comfortable stop to become available..."); return false; } - if ( - mrm_emergency_stop_status_->state == tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + if (!isEmergencyStopStatusAvailable()) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), "waiting for mrm emergency stop to become available..."); @@ -453,48 +389,48 @@ void MrmHandler::updateMrmState() // Check emergency const bool is_emergency = isEmergency(); + // Send recovery events if is not an emergency + if (!is_emergency) { + if (mrm_state_.state != MrmState::NORMAL) transitionTo(MrmState::NORMAL); + return; + } + // Get mode - const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS; + const bool is_auto_mode = isAutonomous(); // State Machine - if (mrm_state_.state == MrmState::NORMAL) { - // NORMAL - if (is_auto_mode && is_emergency) { - transitionTo(MrmState::MRM_OPERATING); - return; - } - } else { - // Emergency - // Send recovery events if "not emergency" - if (!is_emergency) { - transitionTo(MrmState::NORMAL); + switch (mrm_state_.state) { + case MrmState::NORMAL: + if (is_auto_mode) { + transitionTo(MrmState::MRM_OPERATING); + } return; - } - if (mrm_state_.state == MrmState::MRM_OPERATING) { - // TODO(TetsuKawa): Check MRC is accomplished - if (mrm_state_.behavior == MrmState::PULL_OVER) { - if (isStopped() && isArrivedAtGoal()) { - transitionTo(MrmState::MRM_SUCCEEDED); - return; - } - } else { - if (isStopped()) { - transitionTo(MrmState::MRM_SUCCEEDED); - return; - } + case MrmState::MRM_OPERATING: + if (!isStopped()) return; + if (mrm_state_.behavior != MrmState::PULL_OVER) { + transitionTo(MrmState::MRM_SUCCEEDED); + return; + } + if (isArrivedAtGoal()) { + transitionTo(MrmState::MRM_SUCCEEDED); } - } else if (mrm_state_.state == MrmState::MRM_SUCCEEDED) { - const auto current_mrm_behavior = getCurrentMrmBehavior(); - if (current_mrm_behavior != mrm_state_.behavior) { + return; + + case MrmState::MRM_SUCCEEDED: + if (mrm_state_.behavior != getCurrentMrmBehavior()) { transitionTo(MrmState::MRM_OPERATING); } - } else if (mrm_state_.state == MrmState::MRM_FAILED) { + return; + case MrmState::MRM_FAILED: // Do nothing(only checking common recovery events) - } else { + return; + + default: { const auto msg = "invalid state: " + std::to_string(mrm_state_.state); throw std::runtime_error(msg); } + return; } } @@ -581,14 +517,18 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB bool MrmHandler::isStopped() { + auto odom = sub_odom_.takeData(); + if (odom == nullptr) return false; constexpr auto th_stopped_velocity = 0.001; - return std::abs((odom_->twist.twist.linear.x < th_stopped_velocity) < th_stopped_velocity); + return (std::abs(odom->twist.twist.linear.x < th_stopped_velocity) < th_stopped_velocity); } bool MrmHandler::isDrivingBackwards() { + auto odom = sub_odom_.takeData(); + if (odom == nullptr) return false; constexpr auto th_moving_backwards = -0.001; - return odom_->twist.twist.linear.x < th_moving_backwards; + return odom->twist.twist.linear.x < th_moving_backwards; } bool MrmHandler::isEmergency() const @@ -597,11 +537,41 @@ bool MrmHandler::isEmergency() const is_operation_mode_availability_timeout; } +bool MrmHandler::isAutonomous() +{ + using autoware_vehicle_msgs::msg::ControlModeReport; + auto mode = sub_control_mode_.takeData(); + if (mode == nullptr) return false; + return mode->mode == ControlModeReport::AUTONOMOUS; +} + +bool MrmHandler::isPullOverStatusAvailable() +{ + auto status = sub_mrm_pull_over_status_.takeData(); + if (status == nullptr) return false; + return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; +} + +bool MrmHandler::isComfortableStopStatusAvailable() +{ + auto status = sub_mrm_comfortable_stop_status_.takeData(); + if (status == nullptr) return false; + return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; +} + +bool MrmHandler::isEmergencyStopStatusAvailable() +{ + auto status = sub_mrm_emergency_stop_status_.takeData(); + if (status == nullptr) return false; + return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; +} + bool MrmHandler::isArrivedAtGoal() { using autoware_adapi_v1_msgs::msg::OperationModeState; - - return operation_mode_state_->mode == OperationModeState::STOP; + auto state = sub_operation_mode_state_.takeData(); + if (state == nullptr) return false; + return state->mode == OperationModeState::STOP; } #include diff --git a/vehicle/steer_offset_estimator/CMakeLists.txt b/vehicle/autoware_steer_offset_estimator/CMakeLists.txt similarity index 86% rename from vehicle/steer_offset_estimator/CMakeLists.txt rename to vehicle/autoware_steer_offset_estimator/CMakeLists.txt index eec8b2e24bec7..80ae467ed1f85 100644 --- a/vehicle/steer_offset_estimator/CMakeLists.txt +++ b/vehicle/autoware_steer_offset_estimator/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(steer_offset_estimator) +project(autoware_steer_offset_estimator) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) @@ -18,7 +18,7 @@ ament_auto_add_library(steer_offset_estimator_node SHARED ) rclcpp_components_register_node(steer_offset_estimator_node - PLUGIN "steer_offset_estimator::SteerOffsetEstimatorNode" + PLUGIN "autoware::steer_offset_estimator::SteerOffsetEstimatorNode" EXECUTABLE steer_offset_estimator ) diff --git a/vehicle/steer_offset_estimator/Readme.md b/vehicle/autoware_steer_offset_estimator/Readme.md similarity index 100% rename from vehicle/steer_offset_estimator/Readme.md rename to vehicle/autoware_steer_offset_estimator/Readme.md diff --git a/vehicle/steer_offset_estimator/config/steer_offset_estimator.param.yaml b/vehicle/autoware_steer_offset_estimator/config/steer_offset_estimator.param.yaml similarity index 100% rename from vehicle/steer_offset_estimator/config/steer_offset_estimator.param.yaml rename to vehicle/autoware_steer_offset_estimator/config/steer_offset_estimator.param.yaml diff --git a/vehicle/steer_offset_estimator/image/kinematic_constraints.png b/vehicle/autoware_steer_offset_estimator/image/kinematic_constraints.png similarity index 100% rename from vehicle/steer_offset_estimator/image/kinematic_constraints.png rename to vehicle/autoware_steer_offset_estimator/image/kinematic_constraints.png diff --git a/vehicle/steer_offset_estimator/image/steer_offset.png b/vehicle/autoware_steer_offset_estimator/image/steer_offset.png similarity index 100% rename from vehicle/steer_offset_estimator/image/steer_offset.png rename to vehicle/autoware_steer_offset_estimator/image/steer_offset.png diff --git a/vehicle/steer_offset_estimator/include/steer_offset_estimator/steer_offset_estimator_node.hpp b/vehicle/autoware_steer_offset_estimator/include/autoware_steer_offset_estimator/steer_offset_estimator_node.hpp similarity index 87% rename from vehicle/steer_offset_estimator/include/steer_offset_estimator/steer_offset_estimator_node.hpp rename to vehicle/autoware_steer_offset_estimator/include/autoware_steer_offset_estimator/steer_offset_estimator_node.hpp index afd1d0520e951..4e03cbe0162fe 100644 --- a/vehicle/steer_offset_estimator/include/steer_offset_estimator/steer_offset_estimator_node.hpp +++ b/vehicle/autoware_steer_offset_estimator/include/autoware_steer_offset_estimator/steer_offset_estimator_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef STEER_OFFSET_ESTIMATOR__STEER_OFFSET_ESTIMATOR_NODE_HPP_ -#define STEER_OFFSET_ESTIMATOR__STEER_OFFSET_ESTIMATOR_NODE_HPP_ +#ifndef AUTOWARE_STEER_OFFSET_ESTIMATOR__STEER_OFFSET_ESTIMATOR_NODE_HPP_ +#define AUTOWARE_STEER_OFFSET_ESTIMATOR__STEER_OFFSET_ESTIMATOR_NODE_HPP_ #include "diagnostic_updater/diagnostic_updater.hpp" #include "rclcpp/rclcpp.hpp" @@ -24,7 +24,7 @@ #include -namespace steer_offset_estimator +namespace autoware::steer_offset_estimator { using geometry_msgs::msg::TwistStamped; using tier4_debug_msgs::msg::Float32Stamped; @@ -64,6 +64,6 @@ class SteerOffsetEstimatorNode : public rclcpp::Node public: explicit SteerOffsetEstimatorNode(const rclcpp::NodeOptions & node_options); }; -} // namespace steer_offset_estimator +} // namespace autoware::steer_offset_estimator -#endif // STEER_OFFSET_ESTIMATOR__STEER_OFFSET_ESTIMATOR_NODE_HPP_ +#endif // AUTOWARE_STEER_OFFSET_ESTIMATOR__STEER_OFFSET_ESTIMATOR_NODE_HPP_ diff --git a/vehicle/steer_offset_estimator/launch/steer_offset_estimator.launch.xml b/vehicle/autoware_steer_offset_estimator/launch/steer_offset_estimator.launch.xml similarity index 77% rename from vehicle/steer_offset_estimator/launch/steer_offset_estimator.launch.xml rename to vehicle/autoware_steer_offset_estimator/launch/steer_offset_estimator.launch.xml index 8b98537c75500..9ee05189a35d8 100644 --- a/vehicle/steer_offset_estimator/launch/steer_offset_estimator.launch.xml +++ b/vehicle/autoware_steer_offset_estimator/launch/steer_offset_estimator.launch.xml @@ -1,6 +1,6 @@ - + @@ -12,7 +12,7 @@ - + diff --git a/vehicle/steer_offset_estimator/package.xml b/vehicle/autoware_steer_offset_estimator/package.xml similarity index 95% rename from vehicle/steer_offset_estimator/package.xml rename to vehicle/autoware_steer_offset_estimator/package.xml index 636d901d71ef5..4246f8dee8a4e 100644 --- a/vehicle/steer_offset_estimator/package.xml +++ b/vehicle/autoware_steer_offset_estimator/package.xml @@ -1,6 +1,6 @@ - steer_offset_estimator + autoware_steer_offset_estimator 0.1.0 The steer_offset_estimator Taiki Tanaka diff --git a/vehicle/steer_offset_estimator/schema/steer_offset_estimator.schema.json b/vehicle/autoware_steer_offset_estimator/schema/steer_offset_estimator.schema.json similarity index 100% rename from vehicle/steer_offset_estimator/schema/steer_offset_estimator.schema.json rename to vehicle/autoware_steer_offset_estimator/schema/steer_offset_estimator.schema.json diff --git a/vehicle/steer_offset_estimator/src/steer_offset_estimator_node.cpp b/vehicle/autoware_steer_offset_estimator/src/steer_offset_estimator_node.cpp similarity index 95% rename from vehicle/steer_offset_estimator/src/steer_offset_estimator_node.cpp rename to vehicle/autoware_steer_offset_estimator/src/steer_offset_estimator_node.cpp index 86c76dc6f2f26..fdb5fdb404c0c 100644 --- a/vehicle/steer_offset_estimator/src/steer_offset_estimator_node.cpp +++ b/vehicle/autoware_steer_offset_estimator/src/steer_offset_estimator_node.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "steer_offset_estimator/steer_offset_estimator_node.hpp" +#include "autoware_steer_offset_estimator/steer_offset_estimator_node.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include #include -namespace steer_offset_estimator +namespace autoware::steer_offset_estimator { SteerOffsetEstimatorNode::SteerOffsetEstimatorNode(const rclcpp::NodeOptions & node_options) : Node("steer_offset_estimator", node_options) @@ -137,7 +137,7 @@ void SteerOffsetEstimatorNode::onTimer() pub_steer_offset_cov_->publish(std::move(cov_msg)); } } -} // namespace steer_offset_estimator +} // namespace autoware::steer_offset_estimator #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(steer_offset_estimator::SteerOffsetEstimatorNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::steer_offset_estimator::SteerOffsetEstimatorNode)